HEPTA-Sat Training 2019
9axis-sensor library
Hepta9axis.cpp@9:de758e9f9e8e, 2019-11-04 (annotated)
- Committer:
- HeptaSatTraining2019
- Date:
- Mon Nov 04 16:01:31 2019 +0000
- Revision:
- 9:de758e9f9e8e
- Parent:
- 7:3a5b7c76eb48
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| hepta2ume | 0:5aaec0996753 | 1 | #include"Hepta9axis.h" |
| hepta2ume | 0:5aaec0996753 | 2 | #include"mbed.h" |
| hepta2ume | 0:5aaec0996753 | 3 | |
| HEPTA | 3:d5eed0bb962e | 4 | Hepta9axis::Hepta9axis(PinName sda, PinName scl, int aaddr, int agaddr) : n_axis(sda,scl),addr_accel_gyro(aaddr),addr_compus(agaddr) |
| hepta2ume | 0:5aaec0996753 | 5 | { |
| HEPTA | 3:d5eed0bb962e | 6 | n_axis.frequency(100000); |
| hepta2ume | 0:5aaec0996753 | 7 | cmd[0]=0x6B; |
| hepta2ume | 0:5aaec0996753 | 8 | cmd[1]=0x00; |
| HEPTA | 3:d5eed0bb962e | 9 | n_axis.write(addr_accel_gyro,cmd,2); |
| hepta2ume | 0:5aaec0996753 | 10 | cmd[0] = 0x37; |
| hepta2ume | 0:5aaec0996753 | 11 | cmd[1] = 0x02; |
| HEPTA | 3:d5eed0bb962e | 12 | n_axis.write(addr_accel_gyro,cmd,2); |
| HEPTA | 3:d5eed0bb962e | 13 | n_axis.stop(); |
| hepta2ume | 0:5aaec0996753 | 14 | } |
| hepta2ume | 0:5aaec0996753 | 15 | |
| umeume | 2:306058b9d04e | 16 | void Hepta9axis::setup() |
| hepta2ume | 0:5aaec0996753 | 17 | { |
| hepta2ume | 0:5aaec0996753 | 18 | |
| HEPTA | 3:d5eed0bb962e | 19 | n_axis.frequency(100000); |
| hepta2ume | 0:5aaec0996753 | 20 | cmd[0]=0x6B; |
| hepta2ume | 0:5aaec0996753 | 21 | cmd[1]=0x00; |
| HEPTA | 3:d5eed0bb962e | 22 | n_axis.write(addr_accel_gyro,cmd,2); |
| hepta2ume | 0:5aaec0996753 | 23 | cmd[0] = 0x37; |
| hepta2ume | 0:5aaec0996753 | 24 | cmd[1] = 0x02; |
| HEPTA | 3:d5eed0bb962e | 25 | n_axis.write(addr_accel_gyro,cmd,2); |
| HEPTA | 3:d5eed0bb962e | 26 | n_axis.stop(); |
| hepta2ume | 0:5aaec0996753 | 27 | } |
| hepta2ume | 0:5aaec0996753 | 28 | |
| umeume | 2:306058b9d04e | 29 | void Hepta9axis::sen_acc(float *ax,float *ay,float *az) |
| hepta2ume | 0:5aaec0996753 | 30 | { |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 31 | //x-axis accel |
| HEPTA | 3:d5eed0bb962e | 32 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 33 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 34 | n_axis.write(0x3B); |
| HEPTA | 3:d5eed0bb962e | 35 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 36 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 37 | xh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 38 | n_axis.stop(); |
| hepta2ume | 0:5aaec0996753 | 39 | |
| HEPTA | 3:d5eed0bb962e | 40 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 41 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 42 | n_axis.write(0x3C); |
| HEPTA | 3:d5eed0bb962e | 43 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 44 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 45 | xl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 46 | n_axis.stop(); |
| hepta2ume | 0:5aaec0996753 | 47 | |
| hepta2ume | 0:5aaec0996753 | 48 | double acc_ax = short((xh<<8) | (xl)); |
| HeptaSatTraining2019 | 9:de758e9f9e8e | 49 | *ax = -(acc_ax)*2/32764*9.81; |
| hepta2ume | 0:5aaec0996753 | 50 | |
| hepta2ume | 0:5aaec0996753 | 51 | |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 52 | //y-axis |
| HEPTA | 3:d5eed0bb962e | 53 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 54 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 55 | n_axis.write(0x3D); |
| HEPTA | 3:d5eed0bb962e | 56 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 57 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 58 | yh = n_axis.read(0); |
| umeume | 2:306058b9d04e | 59 | |
| HEPTA | 3:d5eed0bb962e | 60 | n_axis.stop(); |
| HEPTA | 3:d5eed0bb962e | 61 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 62 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 63 | n_axis.write(0x3E); |
| HEPTA | 3:d5eed0bb962e | 64 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 65 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 66 | yl = n_axis.read(0); |
| umeume | 2:306058b9d04e | 67 | |
| HEPTA | 3:d5eed0bb962e | 68 | n_axis.stop(); |
| hepta2ume | 0:5aaec0996753 | 69 | double acc_ay = short((yh<<8) | (yl)); |
| HeptaSatTraining2019 | 9:de758e9f9e8e | 70 | *ay = -(acc_ay)*2/32764*9.81; |
| hepta2ume | 0:5aaec0996753 | 71 | |
| hepta2ume | 0:5aaec0996753 | 72 | |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 73 | //z-axis |
| HEPTA | 3:d5eed0bb962e | 74 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 75 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 76 | n_axis.write(0x3F); |
| HEPTA | 3:d5eed0bb962e | 77 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 78 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 79 | zh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 80 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 81 | |
| HEPTA | 3:d5eed0bb962e | 82 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 83 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 84 | n_axis.write(0x40); |
| HEPTA | 3:d5eed0bb962e | 85 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 86 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 87 | zl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 88 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 89 | |
| hepta2ume | 0:5aaec0996753 | 90 | double acc_az = short((zh<<8) | (zl)); |
| HeptaSatTraining2019 | 9:de758e9f9e8e | 91 | *az = -(acc_az)*2/32764*(-9.81); |
| hepta2ume | 0:5aaec0996753 | 92 | |
| hepta2ume | 0:5aaec0996753 | 93 | } |
| hepta2ume | 0:5aaec0996753 | 94 | |
| umeume | 2:306058b9d04e | 95 | void Hepta9axis::sen_gyro(float *gx,float *gy,float *gz) |
| hepta2ume | 0:5aaec0996753 | 96 | { |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 97 | //x-axis gyro |
| HEPTA | 3:d5eed0bb962e | 98 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 99 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 100 | n_axis.write(0x43); |
| HEPTA | 3:d5eed0bb962e | 101 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 102 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 103 | gxh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 104 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 105 | |
| HEPTA | 3:d5eed0bb962e | 106 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 107 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 108 | n_axis.write(0x44); |
| HEPTA | 3:d5eed0bb962e | 109 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 110 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 111 | gxl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 112 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 113 | |
| hepta2ume | 0:5aaec0996753 | 114 | double gyro_ax = short((gxh<<8) | (gxl)); |
| HEPTA | 4:01941772f493 | 115 | *gx = (gyro_ax)*0.00763; |
| hepta2ume | 0:5aaec0996753 | 116 | |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 117 | //y-axis gyro |
| HEPTA | 3:d5eed0bb962e | 118 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 119 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 120 | n_axis.write(0x45); |
| HEPTA | 3:d5eed0bb962e | 121 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 122 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 123 | gyh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 124 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 125 | |
| HEPTA | 3:d5eed0bb962e | 126 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 127 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 128 | n_axis.write(0x46); |
| HEPTA | 3:d5eed0bb962e | 129 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 130 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 131 | gyl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 132 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 133 | |
| hepta2ume | 0:5aaec0996753 | 134 | double gyro_ay = short((gyh<<8) | (gyl)); |
| hepta2ume | 0:5aaec0996753 | 135 | *gy = (gyro_ay)*0.00763; |
| hepta2ume | 0:5aaec0996753 | 136 | |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 137 | //z-axis gyro |
| HEPTA | 3:d5eed0bb962e | 138 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 139 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 140 | n_axis.write(0x47); |
| HEPTA | 3:d5eed0bb962e | 141 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 142 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 143 | gzh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 144 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 145 | |
| HEPTA | 3:d5eed0bb962e | 146 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 147 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 148 | n_axis.write(0x48); |
| HEPTA | 3:d5eed0bb962e | 149 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 150 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 151 | gzl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 152 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 153 | |
| hepta2ume | 0:5aaec0996753 | 154 | double gyro_az = short((gzh<<8) | (gzl)); |
| hepta2ume | 0:5aaec0996753 | 155 | *gz = (gyro_az)*0.00763; |
| hepta2ume | 0:5aaec0996753 | 156 | |
| hepta2ume | 0:5aaec0996753 | 157 | } |
| hepta2ume | 0:5aaec0996753 | 158 | |
| umeume | 2:306058b9d04e | 159 | void Hepta9axis::sen_mag(float *mx,float *my,float *mz) |
| hepta2ume | 0:5aaec0996753 | 160 | { |
| hepta2ume | 0:5aaec0996753 | 161 | |
| HEPTA | 3:d5eed0bb962e | 162 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 163 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 164 | n_axis.write(0x0A); |
| HEPTA | 3:d5eed0bb962e | 165 | n_axis.write(0x12); |
| HEPTA | 3:d5eed0bb962e | 166 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 167 | |
| HEPTA | 3:d5eed0bb962e | 168 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 169 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 170 | n_axis.write(0x03); |
| HEPTA | 3:d5eed0bb962e | 171 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 172 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 173 | mxl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 174 | n_axis.stop(); |
| HEPTA | 3:d5eed0bb962e | 175 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 176 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 177 | n_axis.write(0x04); |
| HEPTA | 3:d5eed0bb962e | 178 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 179 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 180 | mxh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 181 | n_axis.stop(); |
| HEPTA | 4:01941772f493 | 182 | |
| hepta2ume | 0:5aaec0996753 | 183 | double mg_x = short((mxh<<8) | (mxl)); |
| hepta2ume | 0:5aaec0996753 | 184 | *mx = ( mg_x)*0.15; |
| umeume | 2:306058b9d04e | 185 | |
| HEPTA | 3:d5eed0bb962e | 186 | n_axis.start(); |
| umeume | 2:306058b9d04e | 187 | |
| HEPTA | 3:d5eed0bb962e | 188 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 189 | n_axis.write(0x05); |
| HEPTA | 3:d5eed0bb962e | 190 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 191 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 192 | myl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 193 | n_axis.stop(); |
| HEPTA | 3:d5eed0bb962e | 194 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 195 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 196 | n_axis.write(0x06); |
| HEPTA | 3:d5eed0bb962e | 197 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 198 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 199 | myh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 200 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 201 | |
| umeume | 2:306058b9d04e | 202 | double mg_y = short((myh<<8) | (myl)); |
| umeume | 2:306058b9d04e | 203 | *my = ( mg_y)*0.15; |
| umeume | 2:306058b9d04e | 204 | |
| HEPTA | 3:d5eed0bb962e | 205 | n_axis.start(); |
| umeume | 2:306058b9d04e | 206 | |
| HEPTA | 3:d5eed0bb962e | 207 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 208 | n_axis.write(0x07); |
| HEPTA | 3:d5eed0bb962e | 209 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 210 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 211 | mzl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 212 | n_axis.stop(); |
| HEPTA | 3:d5eed0bb962e | 213 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 214 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 215 | n_axis.write(0x08); |
| HEPTA | 3:d5eed0bb962e | 216 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 217 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 218 | mzh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 219 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 220 | |
| HEPTA | 3:d5eed0bb962e | 221 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 222 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 223 | n_axis.write(0x09); |
| HEPTA | 3:d5eed0bb962e | 224 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 225 | n_axis.write(addr_compus|0x01); |
| HEPTA | 4:01941772f493 | 226 | st2 = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 227 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 228 | double mg_z= short((mzh<<8) | (mzl)); |
| HEPTA | 4:01941772f493 | 229 | *mz = ( mg_z)*0.15; |
| HEPTA | 4:01941772f493 | 230 | |
| hepta2ume | 0:5aaec0996753 | 231 | } |
| hepta2ume | 0:5aaec0996753 | 232 | |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 233 | //////////////For HEX///////////////////////////////////////// |
| HEPTA | 5:098423fb0371 | 234 | void Hepta9axis::sen_gyro_u16(char* gx_u16,char* gy_u16,char* gz_u16) |
| hepta2ume | 0:5aaec0996753 | 235 | { |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 236 | //x |
| HEPTA | 3:d5eed0bb962e | 237 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 238 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 239 | n_axis.write(0x43); |
| HEPTA | 3:d5eed0bb962e | 240 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 241 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 242 | gxh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 243 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 244 | |
| HEPTA | 3:d5eed0bb962e | 245 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 246 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 247 | n_axis.write(0x44); |
| HEPTA | 3:d5eed0bb962e | 248 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 249 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 250 | gxl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 251 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 252 | |
| hepta2ume | 0:5aaec0996753 | 253 | sprintf( g1, "%02X", ((gxh)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 254 | sprintf( g2, "%02X", ((gxl)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 255 | gx_u16[0]=g1[0]; |
| hepta2ume | 0:5aaec0996753 | 256 | gx_u16[1]=g1[1]; |
| hepta2ume | 0:5aaec0996753 | 257 | gx_u16[2]=g2[0]; |
| hepta2ume | 0:5aaec0996753 | 258 | gx_u16[3]=g2[1]; |
| hepta2ume | 0:5aaec0996753 | 259 | |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 260 | //y |
| HEPTA | 3:d5eed0bb962e | 261 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 262 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 263 | n_axis.write(0x45); |
| HEPTA | 3:d5eed0bb962e | 264 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 265 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 266 | gyh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 267 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 268 | |
| HEPTA | 3:d5eed0bb962e | 269 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 270 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 271 | n_axis.write(0x46); |
| HEPTA | 3:d5eed0bb962e | 272 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 273 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 274 | gyl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 275 | n_axis.stop(); |
| hepta2ume | 1:5b35162b3b6a | 276 | sprintf( g1, "%02X", (gyh) & 0xFF); |
| hepta2ume | 1:5b35162b3b6a | 277 | sprintf( g2, "%02X", (gyl) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 278 | gy_u16[0]=g1[0]; |
| hepta2ume | 0:5aaec0996753 | 279 | gy_u16[1]=g1[1]; |
| hepta2ume | 0:5aaec0996753 | 280 | gy_u16[2]=g2[0]; |
| hepta2ume | 0:5aaec0996753 | 281 | gy_u16[3]=g2[1]; |
| umeume | 2:306058b9d04e | 282 | |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 283 | //z |
| HEPTA | 3:d5eed0bb962e | 284 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 285 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 286 | n_axis.write(0x47); |
| HEPTA | 3:d5eed0bb962e | 287 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 288 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 289 | gzh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 290 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 291 | |
| HEPTA | 3:d5eed0bb962e | 292 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 293 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 294 | n_axis.write(0x48); |
| HEPTA | 3:d5eed0bb962e | 295 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 296 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 297 | gzl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 298 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 299 | |
| hepta2ume | 0:5aaec0996753 | 300 | sprintf( g1, "%02X", ((gzh)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 301 | sprintf( g2, "%02X", ((gzl)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 302 | gz_u16[0]=g1[0]; |
| hepta2ume | 0:5aaec0996753 | 303 | gz_u16[1]=g1[1]; |
| hepta2ume | 0:5aaec0996753 | 304 | gz_u16[2]=g2[0]; |
| hepta2ume | 0:5aaec0996753 | 305 | gz_u16[3]=g2[1]; |
| HEPTA | 5:098423fb0371 | 306 | //*dsize = 4; |
| umeume | 2:306058b9d04e | 307 | } |
| umeume | 2:306058b9d04e | 308 | |
| HEPTA | 5:098423fb0371 | 309 | void Hepta9axis::sen_acc_u16(char* ax_u16,char* ay_u16,char* az_u16) |
| hepta2ume | 0:5aaec0996753 | 310 | { |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 311 | //x |
| HEPTA | 3:d5eed0bb962e | 312 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 313 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 314 | n_axis.write(0x3B); |
| HEPTA | 3:d5eed0bb962e | 315 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 316 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 317 | xh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 318 | n_axis.stop(); |
| hepta2ume | 0:5aaec0996753 | 319 | |
| HEPTA | 3:d5eed0bb962e | 320 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 321 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 322 | n_axis.write(0x3C); |
| HEPTA | 3:d5eed0bb962e | 323 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 324 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 325 | xl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 326 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 327 | |
| hepta2ume | 0:5aaec0996753 | 328 | sprintf( a1, "%02X", ((xh)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 329 | sprintf( a2, "%02X", ((xl)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 330 | ax_u16[0]=a1[0]; |
| hepta2ume | 0:5aaec0996753 | 331 | ax_u16[1]=a1[1]; |
| hepta2ume | 0:5aaec0996753 | 332 | ax_u16[2]=a2[0]; |
| hepta2ume | 0:5aaec0996753 | 333 | ax_u16[3]=a2[1]; |
| hepta2ume | 0:5aaec0996753 | 334 | |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 335 | //y |
| HEPTA | 3:d5eed0bb962e | 336 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 337 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 338 | n_axis.write(0x3D); |
| HEPTA | 3:d5eed0bb962e | 339 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 340 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 341 | yh = n_axis.read(0); |
| umeume | 2:306058b9d04e | 342 | |
| HEPTA | 3:d5eed0bb962e | 343 | n_axis.stop(); |
| HEPTA | 3:d5eed0bb962e | 344 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 345 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 346 | n_axis.write(0x3E); |
| HEPTA | 3:d5eed0bb962e | 347 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 348 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 349 | yl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 350 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 351 | |
| hepta2ume | 0:5aaec0996753 | 352 | sprintf( a1, "%02X", ((yh)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 353 | sprintf( a2, "%02X", ((yl)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 354 | ay_u16[0]=a1[0]; |
| hepta2ume | 0:5aaec0996753 | 355 | ay_u16[1]=a1[1]; |
| hepta2ume | 0:5aaec0996753 | 356 | ay_u16[2]=a2[0]; |
| hepta2ume | 0:5aaec0996753 | 357 | ay_u16[3]=a2[1]; |
| HeptaSatTraining2019 | 6:fdfca3ffecd5 | 358 | //z |
| HEPTA | 3:d5eed0bb962e | 359 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 360 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 361 | n_axis.write(0x3F); |
| HEPTA | 3:d5eed0bb962e | 362 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 363 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 364 | zh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 365 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 366 | |
| HEPTA | 3:d5eed0bb962e | 367 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 368 | n_axis.write(addr_accel_gyro); |
| HEPTA | 3:d5eed0bb962e | 369 | n_axis.write(0x40); |
| HEPTA | 3:d5eed0bb962e | 370 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 371 | n_axis.write(addr_accel_gyro|0x01); |
| HEPTA | 3:d5eed0bb962e | 372 | zl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 373 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 374 | |
| hepta2ume | 0:5aaec0996753 | 375 | sprintf( a1, "%02X", ((zh)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 376 | sprintf( a2, "%02X", ((zl)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 377 | az_u16[0]=a1[0]; |
| hepta2ume | 0:5aaec0996753 | 378 | az_u16[1]=a1[1]; |
| hepta2ume | 0:5aaec0996753 | 379 | az_u16[2]=a2[0]; |
| hepta2ume | 0:5aaec0996753 | 380 | az_u16[3]=a2[1]; |
| HEPTA | 5:098423fb0371 | 381 | //*dsize = 4; |
| umeume | 2:306058b9d04e | 382 | } |
| umeume | 2:306058b9d04e | 383 | |
| HEPTA | 5:098423fb0371 | 384 | void Hepta9axis::sen_mag_u16(char* mx_u16,char* my_u16,char* mz_u16) |
| hepta2ume | 0:5aaec0996753 | 385 | { |
| HEPTA | 3:d5eed0bb962e | 386 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 387 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 388 | n_axis.write(0x0a); |
| HEPTA | 3:d5eed0bb962e | 389 | n_axis.write(0x12); |
| HEPTA | 3:d5eed0bb962e | 390 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 391 | |
| HEPTA | 3:d5eed0bb962e | 392 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 393 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 394 | n_axis.write(0x03); |
| HEPTA | 3:d5eed0bb962e | 395 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 396 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 397 | mxl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 398 | n_axis.stop(); |
| HEPTA | 3:d5eed0bb962e | 399 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 400 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 401 | n_axis.write(0x04); |
| HEPTA | 3:d5eed0bb962e | 402 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 403 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 404 | mxh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 405 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 406 | |
| hepta2ume | 0:5aaec0996753 | 407 | sprintf( m1, "%02X", ((mxh)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 408 | sprintf( m2, "%02X", ((mxl)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 409 | mx_u16[0]=m1[0]; |
| hepta2ume | 0:5aaec0996753 | 410 | mx_u16[1]=m1[1]; |
| hepta2ume | 0:5aaec0996753 | 411 | mx_u16[2]=m2[0]; |
| hepta2ume | 0:5aaec0996753 | 412 | mx_u16[3]=m2[1]; |
| umeume | 2:306058b9d04e | 413 | |
| HEPTA | 3:d5eed0bb962e | 414 | n_axis.start(); |
| hepta2ume | 0:5aaec0996753 | 415 | |
| HEPTA | 3:d5eed0bb962e | 416 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 417 | n_axis.write(0x05); |
| HEPTA | 3:d5eed0bb962e | 418 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 419 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 420 | myl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 421 | n_axis.stop(); |
| HEPTA | 3:d5eed0bb962e | 422 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 423 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 424 | n_axis.write(0x06); |
| HEPTA | 3:d5eed0bb962e | 425 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 426 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 427 | myh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 428 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 429 | |
| hepta2ume | 0:5aaec0996753 | 430 | sprintf( m1, "%02X", ((myh)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 431 | sprintf( m2, "%02X", ((myl)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 432 | my_u16[0]=m1[0]; |
| hepta2ume | 0:5aaec0996753 | 433 | my_u16[1]=m1[1]; |
| hepta2ume | 0:5aaec0996753 | 434 | my_u16[2]=m2[0]; |
| hepta2ume | 0:5aaec0996753 | 435 | my_u16[3]=m2[1]; |
| hepta2ume | 0:5aaec0996753 | 436 | |
| HEPTA | 3:d5eed0bb962e | 437 | n_axis.start(); |
| hepta2ume | 0:5aaec0996753 | 438 | |
| HEPTA | 3:d5eed0bb962e | 439 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 440 | n_axis.write(0x07); |
| HEPTA | 3:d5eed0bb962e | 441 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 442 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 443 | mzl = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 444 | n_axis.stop(); |
| HEPTA | 3:d5eed0bb962e | 445 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 446 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 447 | n_axis.write(0x08); |
| HEPTA | 3:d5eed0bb962e | 448 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 449 | n_axis.write(addr_compus|0x01); |
| HEPTA | 3:d5eed0bb962e | 450 | mzh = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 451 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 452 | |
| HEPTA | 3:d5eed0bb962e | 453 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 454 | n_axis.write(addr_compus); |
| HEPTA | 3:d5eed0bb962e | 455 | n_axis.write(0x09); |
| HEPTA | 3:d5eed0bb962e | 456 | n_axis.start(); |
| HEPTA | 3:d5eed0bb962e | 457 | n_axis.write(addr_compus|0x01); |
| HEPTA | 4:01941772f493 | 458 | st2 = n_axis.read(0); |
| HEPTA | 3:d5eed0bb962e | 459 | n_axis.stop(); |
| umeume | 2:306058b9d04e | 460 | |
| hepta2ume | 0:5aaec0996753 | 461 | sprintf( m1, "%02X", ((mzh)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 462 | sprintf( m2, "%02X", ((mzl)) & 0xFF); |
| hepta2ume | 0:5aaec0996753 | 463 | mz_u16[0]=m1[0]; |
| hepta2ume | 0:5aaec0996753 | 464 | mz_u16[1]=m1[1]; |
| hepta2ume | 0:5aaec0996753 | 465 | mz_u16[2]=m2[0]; |
| hepta2ume | 0:5aaec0996753 | 466 | mz_u16[3]=m2[1]; |
| HEPTA | 5:098423fb0371 | 467 | //*dsize = 4; |
| umeume | 2:306058b9d04e | 468 | } |