Masahiko Yamazaki / Hepta6axis

For Hepta-Sat Lite

Hepta9axis.cpp@2:306058b9d04e, 2017-08-23 (annotated)

Committer:
umeume
Date:
Wed Aug 23 06:18:26 2017 +0000
Revision:
2:306058b9d04e
Parent:
1:5b35162b3b6a
Child:
3:d5eed0bb962e
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hepta2ume 0:5aaec0996753 1 #include"Hepta9axis.h"
hepta2ume 0:5aaec0996753 2 #include"mbed.h"
hepta2ume 0:5aaec0996753 3
umeume 2:306058b9d04e 4 Hepta9axis::Hepta9axis(PinName sda, PinName scl, int aaddr, int agaddr) : _9axis(sda,scl),addr_accel_gyro(aaddr),addr_compus(agaddr)
hepta2ume 0:5aaec0996753 5 {
umeume 2:306058b9d04e 6 _9axis.frequency(100000);
hepta2ume 0:5aaec0996753 7 cmd[0]=0x6B;
hepta2ume 0:5aaec0996753 8 cmd[1]=0x00;
umeume 2:306058b9d04e 9 _9axis.write(addr_accel_gyro,cmd,2);
hepta2ume 0:5aaec0996753 10 cmd[0] = 0x37;
hepta2ume 0:5aaec0996753 11 cmd[1] = 0x02;
umeume 2:306058b9d04e 12 _9axis.write(addr_accel_gyro,cmd,2);
umeume 2:306058b9d04e 13 _9axis.stop();
hepta2ume 0:5aaec0996753 14 }
hepta2ume 0:5aaec0996753 15
umeume 2:306058b9d04e 16 void Hepta9axis::setup()
hepta2ume 0:5aaec0996753 17 {
hepta2ume 0:5aaec0996753 18
umeume 2:306058b9d04e 19 _9axis.frequency(100000);
hepta2ume 0:5aaec0996753 20 cmd[0]=0x6B;
hepta2ume 0:5aaec0996753 21 cmd[1]=0x00;
umeume 2:306058b9d04e 22 _9axis.write(addr_accel_gyro,cmd,2);
hepta2ume 0:5aaec0996753 23 cmd[0] = 0x37;
hepta2ume 0:5aaec0996753 24 cmd[1] = 0x02;
umeume 2:306058b9d04e 25 _9axis.write(addr_accel_gyro,cmd,2);
umeume 2:306058b9d04e 26 _9axis.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 {
hepta2ume 0:5aaec0996753 31 //x軸加速度
umeume 2:306058b9d04e 32 _9axis.start();
umeume 2:306058b9d04e 33 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 34 _9axis.write(0x3B);
umeume 2:306058b9d04e 35 _9axis.start();
umeume 2:306058b9d04e 36 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 37 xh = _9axis.read(0);
umeume 2:306058b9d04e 38 _9axis.stop();
hepta2ume 0:5aaec0996753 39
umeume 2:306058b9d04e 40 _9axis.start();
umeume 2:306058b9d04e 41 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 42 _9axis.write(0x3C);
umeume 2:306058b9d04e 43 _9axis.start();
umeume 2:306058b9d04e 44 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 45 xl = _9axis.read(0);
umeume 2:306058b9d04e 46 _9axis.stop();
hepta2ume 0:5aaec0996753 47
hepta2ume 0:5aaec0996753 48 double acc_ax = short((xh<<8) | (xl));
umeume 2:306058b9d04e 49 *ax = (acc_ax)*2/32764*9.81;
hepta2ume 0:5aaec0996753 50
hepta2ume 0:5aaec0996753 51
hepta2ume 0:5aaec0996753 52 //Y軸加速度
umeume 2:306058b9d04e 53 _9axis.start();
umeume 2:306058b9d04e 54 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 55 _9axis.write(0x3D);
umeume 2:306058b9d04e 56 _9axis.start();
umeume 2:306058b9d04e 57 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 58 yh = _9axis.read(0);
umeume 2:306058b9d04e 59
umeume 2:306058b9d04e 60 _9axis.stop();
umeume 2:306058b9d04e 61 _9axis.start();
umeume 2:306058b9d04e 62 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 63 _9axis.write(0x3E);
umeume 2:306058b9d04e 64 _9axis.start();
umeume 2:306058b9d04e 65 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 66 yl = _9axis.read(0);
umeume 2:306058b9d04e 67
umeume 2:306058b9d04e 68 _9axis.stop();
hepta2ume 0:5aaec0996753 69 double acc_ay = short((yh<<8) | (yl));
hepta2ume 0:5aaec0996753 70 *ay = (acc_ay)*2/32764*9.81;
hepta2ume 0:5aaec0996753 71
hepta2ume 0:5aaec0996753 72
hepta2ume 0:5aaec0996753 73 //z軸加速度
umeume 2:306058b9d04e 74 _9axis.start();
umeume 2:306058b9d04e 75 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 76 _9axis.write(0x3F);
umeume 2:306058b9d04e 77 _9axis.start();
umeume 2:306058b9d04e 78 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 79 zh = _9axis.read(0);
umeume 2:306058b9d04e 80 _9axis.stop();
umeume 2:306058b9d04e 81
umeume 2:306058b9d04e 82 _9axis.start();
umeume 2:306058b9d04e 83 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 84 _9axis.write(0x40);
umeume 2:306058b9d04e 85 _9axis.start();
umeume 2:306058b9d04e 86 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 87 zl = _9axis.read(0);
umeume 2:306058b9d04e 88 _9axis.stop();
umeume 2:306058b9d04e 89
hepta2ume 0:5aaec0996753 90 double acc_az = short((zh<<8) | (zl));
hepta2ume 0:5aaec0996753 91 *az = (acc_az)*2/32764*9.81-0.1;
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 {
hepta2ume 0:5aaec0996753 97 //x軸GYRO
umeume 2:306058b9d04e 98 _9axis.start();
umeume 2:306058b9d04e 99 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 100 _9axis.write(0x43);
umeume 2:306058b9d04e 101 _9axis.start();
umeume 2:306058b9d04e 102 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 103 gxh = _9axis.read(0);
umeume 2:306058b9d04e 104 _9axis.stop();
umeume 2:306058b9d04e 105
umeume 2:306058b9d04e 106 _9axis.start();
umeume 2:306058b9d04e 107 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 108 _9axis.write(0x44);
umeume 2:306058b9d04e 109 _9axis.start();
umeume 2:306058b9d04e 110 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 111 gxl = _9axis.read(0);
umeume 2:306058b9d04e 112 _9axis.stop();
umeume 2:306058b9d04e 113
hepta2ume 0:5aaec0996753 114 double gyro_ax = short((gxh<<8) | (gxl));
hepta2ume 0:5aaec0996753 115 *gx = (gyro_ax)*0.00763-1.6;
hepta2ume 0:5aaec0996753 116
hepta2ume 0:5aaec0996753 117 //y軸GYRO
umeume 2:306058b9d04e 118 _9axis.start();
umeume 2:306058b9d04e 119 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 120 _9axis.write(0x45);
umeume 2:306058b9d04e 121 _9axis.start();
umeume 2:306058b9d04e 122 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 123 gyh = _9axis.read(0);
umeume 2:306058b9d04e 124 _9axis.stop();
umeume 2:306058b9d04e 125
umeume 2:306058b9d04e 126 _9axis.start();
umeume 2:306058b9d04e 127 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 128 _9axis.write(0x46);
umeume 2:306058b9d04e 129 _9axis.start();
umeume 2:306058b9d04e 130 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 131 gyl = _9axis.read(0);
umeume 2:306058b9d04e 132 _9axis.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
hepta2ume 0:5aaec0996753 137 //z軸GYRO
umeume 2:306058b9d04e 138 _9axis.start();
umeume 2:306058b9d04e 139 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 140 _9axis.write(0x47);
umeume 2:306058b9d04e 141 _9axis.start();
umeume 2:306058b9d04e 142 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 143 gzh = _9axis.read(0);
umeume 2:306058b9d04e 144 _9axis.stop();
umeume 2:306058b9d04e 145
umeume 2:306058b9d04e 146 _9axis.start();
umeume 2:306058b9d04e 147 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 148 _9axis.write(0x48);
umeume 2:306058b9d04e 149 _9axis.start();
umeume 2:306058b9d04e 150 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 151 gzl = _9axis.read(0);
umeume 2:306058b9d04e 152 _9axis.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
umeume 2:306058b9d04e 162 _9axis.start();
umeume 2:306058b9d04e 163 _9axis.write(addr_compus);
umeume 2:306058b9d04e 164 _9axis.write(0x0A);
umeume 2:306058b9d04e 165 _9axis.write(0x12);
umeume 2:306058b9d04e 166 _9axis.stop();
umeume 2:306058b9d04e 167
umeume 2:306058b9d04e 168 _9axis.start();
umeume 2:306058b9d04e 169 _9axis.write(addr_compus);
umeume 2:306058b9d04e 170 _9axis.write(0x03);
umeume 2:306058b9d04e 171 _9axis.start();
umeume 2:306058b9d04e 172 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 173 mxl = _9axis.read(0);
umeume 2:306058b9d04e 174 _9axis.stop();
umeume 2:306058b9d04e 175 _9axis.start();
umeume 2:306058b9d04e 176 _9axis.write(addr_compus);
umeume 2:306058b9d04e 177 _9axis.write(0x04);
umeume 2:306058b9d04e 178 _9axis.start();
umeume 2:306058b9d04e 179 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 180 mxh = _9axis.read(0);
umeume 2:306058b9d04e 181 _9axis.stop();
umeume 2:306058b9d04e 182 /*
umeume 2:306058b9d04e 183 _9axis.start();
umeume 2:306058b9d04e 184 _9axis.write(addr_compus);
umeume 2:306058b9d04e 185 _9axis.write(0x09);
umeume 2:306058b9d04e 186 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 187 _9axis.stop();
umeume 2:306058b9d04e 188 */
hepta2ume 0:5aaec0996753 189 double mg_x = short((mxh<<8) | (mxl));
hepta2ume 0:5aaec0996753 190 *mx = ( mg_x)*0.15;
hepta2ume 0:5aaec0996753 191 //pc.printf("MGX = %f\t",mx);
hepta2ume 0:5aaec0996753 192
umeume 2:306058b9d04e 193 _9axis.start();
hepta2ume 0:5aaec0996753 194
umeume 2:306058b9d04e 195 _9axis.write(addr_compus);
umeume 2:306058b9d04e 196 _9axis.write(0x05);
umeume 2:306058b9d04e 197 _9axis.start();
umeume 2:306058b9d04e 198 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 199 myl = _9axis.read(0);
umeume 2:306058b9d04e 200 _9axis.stop();
umeume 2:306058b9d04e 201 _9axis.start();
umeume 2:306058b9d04e 202 _9axis.write(addr_compus);
umeume 2:306058b9d04e 203 _9axis.write(0x06);
umeume 2:306058b9d04e 204 _9axis.start();
umeume 2:306058b9d04e 205 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 206 myh = _9axis.read(0);
umeume 2:306058b9d04e 207 _9axis.stop();
umeume 2:306058b9d04e 208 /*
umeume 2:306058b9d04e 209 _9axis.start();
umeume 2:306058b9d04e 210 _9axis.write(addr_compus);
umeume 2:306058b9d04e 211 _9axis.write(0x09);
umeume 2:306058b9d04e 212 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 213 _9axis.stop();
umeume 2:306058b9d04e 214 */
hepta2ume 0:5aaec0996753 215 double mg_y = short((myh<<8) | (myl));
hepta2ume 0:5aaec0996753 216 *my = ( mg_y)*0.15;
hepta2ume 0:5aaec0996753 217 // pc.printf("MAGY = %f\t",my);
hepta2ume 0:5aaec0996753 218
umeume 2:306058b9d04e 219 _9axis.start();
hepta2ume 0:5aaec0996753 220
umeume 2:306058b9d04e 221 _9axis.write(addr_compus);
umeume 2:306058b9d04e 222 _9axis.write(0x07);
umeume 2:306058b9d04e 223 _9axis.start();
umeume 2:306058b9d04e 224 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 225 mzl = _9axis.read(0);
umeume 2:306058b9d04e 226 _9axis.stop();
umeume 2:306058b9d04e 227 _9axis.start();
umeume 2:306058b9d04e 228 _9axis.write(addr_compus);
umeume 2:306058b9d04e 229 _9axis.write(0x08);
umeume 2:306058b9d04e 230 _9axis.start();
umeume 2:306058b9d04e 231 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 232 mzh = _9axis.read(0);
umeume 2:306058b9d04e 233 _9axis.stop();
umeume 2:306058b9d04e 234
umeume 2:306058b9d04e 235 _9axis.start();
umeume 2:306058b9d04e 236 _9axis.write(addr_compus);
umeume 2:306058b9d04e 237 _9axis.write(0x09);
umeume 2:306058b9d04e 238 _9axis.start();
umeume 2:306058b9d04e 239 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 240 _9axis.stop();
hepta2ume 0:5aaec0996753 241 double mg_z= short((mzh<<8) | (mzl));
hepta2ume 0:5aaec0996753 242 *mz = ( mg_z)*0.15;
umeume 2:306058b9d04e 243 /*
umeume 2:306058b9d04e 244 _9axis.start();
umeume 2:306058b9d04e 245 _9axis.write(addr_compus);
umeume 2:306058b9d04e 246 _9axis.write(0x0a);
umeume 2:306058b9d04e 247 _9axis.write(0x12);
umeume 2:306058b9d04e 248 _9axis.stop();
umeume 2:306058b9d04e 249
umeume 2:306058b9d04e 250 _9axis.start();
umeume 2:306058b9d04e 251 _9axis.write(addr_compus);
umeume 2:306058b9d04e 252 _9axis.write(0x03);
umeume 2:306058b9d04e 253 _9axis.start();
umeume 2:306058b9d04e 254 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 255 mxl = _9axis.read(0);
umeume 2:306058b9d04e 256 _9axis.stop();
umeume 2:306058b9d04e 257 _9axis.start();
umeume 2:306058b9d04e 258 _9axis.write(addr_compus);
umeume 2:306058b9d04e 259 _9axis.write(0x04);
umeume 2:306058b9d04e 260 _9axis.start();
umeume 2:306058b9d04e 261 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 262 mxh = _9axis.read(0);
umeume 2:306058b9d04e 263 _9axis.stop();
umeume 2:306058b9d04e 264
umeume 2:306058b9d04e 265 _9axis.start();
umeume 2:306058b9d04e 266 _9axis.write(addr_compus);
umeume 2:306058b9d04e 267 _9axis.write(0x09);
umeume 2:306058b9d04e 268 _9axis.start();
umeume 2:306058b9d04e 269 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 270 _9axis.stop();
umeume 2:306058b9d04e 271
umeume 2:306058b9d04e 272
umeume 2:306058b9d04e 273 double mg_x = short((mxh<<8) | (mxl));
umeume 2:306058b9d04e 274 *mx = ( mg_x)*0.15;
umeume 2:306058b9d04e 275
umeume 2:306058b9d04e 276
umeume 2:306058b9d04e 277 _9axis.start();
umeume 2:306058b9d04e 278
umeume 2:306058b9d04e 279 _9axis.write(addr_compus);
umeume 2:306058b9d04e 280 _9axis.write(0x05);
umeume 2:306058b9d04e 281 _9axis.start();
umeume 2:306058b9d04e 282 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 283 myl = _9axis.read(0);
umeume 2:306058b9d04e 284 _9axis.stop();
umeume 2:306058b9d04e 285 _9axis.start();
umeume 2:306058b9d04e 286 _9axis.write(addr_compus);
umeume 2:306058b9d04e 287 _9axis.write(0x06);
umeume 2:306058b9d04e 288 _9axis.start();
umeume 2:306058b9d04e 289 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 290 myh = _9axis.read(0);
umeume 2:306058b9d04e 291 _9axis.stop();
umeume 2:306058b9d04e 292
umeume 2:306058b9d04e 293 _9axis.start();
umeume 2:306058b9d04e 294 _9axis.write(addr_compus);
umeume 2:306058b9d04e 295 _9axis.write(0x09);
umeume 2:306058b9d04e 296 _9axis.start();
umeume 2:306058b9d04e 297 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 298 _9axis.stop();
umeume 2:306058b9d04e 299 double mg_y = short((myh<<8) | (myl));
umeume 2:306058b9d04e 300 *my = ( mg_y)*0.15;
umeume 2:306058b9d04e 301
umeume 2:306058b9d04e 302 _9axis.start();
umeume 2:306058b9d04e 303
umeume 2:306058b9d04e 304 _9axis.write(addr_compus);
umeume 2:306058b9d04e 305 _9axis.write(0x07);
umeume 2:306058b9d04e 306 _9axis.start();
umeume 2:306058b9d04e 307 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 308 mzl = _9axis.read(0);
umeume 2:306058b9d04e 309 _9axis.stop();
umeume 2:306058b9d04e 310 _9axis.start();
umeume 2:306058b9d04e 311 _9axis.write(addr_compus);
umeume 2:306058b9d04e 312 _9axis.write(0x08);
umeume 2:306058b9d04e 313 _9axis.start();
umeume 2:306058b9d04e 314 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 315 mzh = _9axis.read(0);
umeume 2:306058b9d04e 316 _9axis.stop();
umeume 2:306058b9d04e 317
umeume 2:306058b9d04e 318 _9axis.start();
umeume 2:306058b9d04e 319 _9axis.write(addr_compus);
umeume 2:306058b9d04e 320 _9axis.write(0x09);
umeume 2:306058b9d04e 321 _9axis.start();
umeume 2:306058b9d04e 322 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 323 _9axis.stop();
umeume 2:306058b9d04e 324
umeume 2:306058b9d04e 325 double mg_z= short((mzh<<8) | (mzl));
umeume 2:306058b9d04e 326 *mz = ( mg_z)*0.15;*/
hepta2ume 0:5aaec0996753 327 //pc.printf("MAGZ = %f\r\n",mz);
hepta2ume 0:5aaec0996753 328 }
hepta2ume 0:5aaec0996753 329
hepta2ume 0:5aaec0996753 330 //////////////16進数表記/////////////////////////////////////////
umeume 2:306058b9d04e 331 void Hepta9axis::sen_gyro_u16(char* gx_u16,char* gy_u16,char* gz_u16, int *dsize)
hepta2ume 0:5aaec0996753 332 {
hepta2ume 0:5aaec0996753 333 //x軸GYRO
umeume 2:306058b9d04e 334 _9axis.start();
umeume 2:306058b9d04e 335 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 336 _9axis.write(0x43);
umeume 2:306058b9d04e 337 _9axis.start();
umeume 2:306058b9d04e 338 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 339 gxh = _9axis.read(0);
umeume 2:306058b9d04e 340 _9axis.stop();
umeume 2:306058b9d04e 341
umeume 2:306058b9d04e 342 _9axis.start();
umeume 2:306058b9d04e 343 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 344 _9axis.write(0x44);
umeume 2:306058b9d04e 345 _9axis.start();
umeume 2:306058b9d04e 346 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 347 gxl = _9axis.read(0);
umeume 2:306058b9d04e 348 _9axis.stop();
umeume 2:306058b9d04e 349
hepta2ume 0:5aaec0996753 350 sprintf( g1, "%02X", ((gxh)) & 0xFF);
hepta2ume 0:5aaec0996753 351 sprintf( g2, "%02X", ((gxl)) & 0xFF);
hepta2ume 0:5aaec0996753 352 gx_u16[0]=g1[0];
hepta2ume 0:5aaec0996753 353 gx_u16[1]=g1[1];
hepta2ume 0:5aaec0996753 354 gx_u16[2]=g2[0];
hepta2ume 0:5aaec0996753 355 gx_u16[3]=g2[1];
hepta2ume 0:5aaec0996753 356
hepta2ume 0:5aaec0996753 357 //y軸GYRO
umeume 2:306058b9d04e 358 _9axis.start();
umeume 2:306058b9d04e 359 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 360 _9axis.write(0x45);
umeume 2:306058b9d04e 361 _9axis.start();
umeume 2:306058b9d04e 362 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 363 gyh = _9axis.read(0);
umeume 2:306058b9d04e 364 _9axis.stop();
umeume 2:306058b9d04e 365
umeume 2:306058b9d04e 366 _9axis.start();
umeume 2:306058b9d04e 367 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 368 _9axis.write(0x46);
umeume 2:306058b9d04e 369 _9axis.start();
umeume 2:306058b9d04e 370 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 371 gyl = _9axis.read(0);
umeume 2:306058b9d04e 372 _9axis.stop();
hepta2ume 1:5b35162b3b6a 373 sprintf( g1, "%02X", (gyh) & 0xFF);
hepta2ume 1:5b35162b3b6a 374 sprintf( g2, "%02X", (gyl) & 0xFF);
hepta2ume 0:5aaec0996753 375 gy_u16[0]=g1[0];
hepta2ume 0:5aaec0996753 376 gy_u16[1]=g1[1];
hepta2ume 0:5aaec0996753 377 gy_u16[2]=g2[0];
hepta2ume 0:5aaec0996753 378 gy_u16[3]=g2[1];
umeume 2:306058b9d04e 379
hepta2ume 0:5aaec0996753 380 //z軸GYRO
umeume 2:306058b9d04e 381 _9axis.start();
umeume 2:306058b9d04e 382 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 383 _9axis.write(0x47);
umeume 2:306058b9d04e 384 _9axis.start();
umeume 2:306058b9d04e 385 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 386 gzh = _9axis.read(0);
umeume 2:306058b9d04e 387 _9axis.stop();
umeume 2:306058b9d04e 388
umeume 2:306058b9d04e 389 _9axis.start();
umeume 2:306058b9d04e 390 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 391 _9axis.write(0x48);
umeume 2:306058b9d04e 392 _9axis.start();
umeume 2:306058b9d04e 393 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 394 gzl = _9axis.read(0);
umeume 2:306058b9d04e 395 _9axis.stop();
umeume 2:306058b9d04e 396
hepta2ume 0:5aaec0996753 397 sprintf( g1, "%02X", ((gzh)) & 0xFF);
hepta2ume 0:5aaec0996753 398 sprintf( g2, "%02X", ((gzl)) & 0xFF);
hepta2ume 0:5aaec0996753 399 gz_u16[0]=g1[0];
hepta2ume 0:5aaec0996753 400 gz_u16[1]=g1[1];
hepta2ume 0:5aaec0996753 401 gz_u16[2]=g2[0];
hepta2ume 0:5aaec0996753 402 gz_u16[3]=g2[1];
hepta2ume 0:5aaec0996753 403 *dsize = 4;
umeume 2:306058b9d04e 404 }
umeume 2:306058b9d04e 405
umeume 2:306058b9d04e 406 void Hepta9axis::sen_acc_u16(char* ax_u16,char* ay_u16,char* az_u16, int *dsize)
hepta2ume 0:5aaec0996753 407 {
umeume 2:306058b9d04e 408 //x軸加速度
umeume 2:306058b9d04e 409 _9axis.start();
umeume 2:306058b9d04e 410 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 411 _9axis.write(0x3B);
umeume 2:306058b9d04e 412 _9axis.start();
umeume 2:306058b9d04e 413 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 414 xh = _9axis.read(0);
umeume 2:306058b9d04e 415 _9axis.stop();
hepta2ume 0:5aaec0996753 416
umeume 2:306058b9d04e 417 _9axis.start();
umeume 2:306058b9d04e 418 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 419 _9axis.write(0x3C);
umeume 2:306058b9d04e 420 _9axis.start();
umeume 2:306058b9d04e 421 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 422 xl = _9axis.read(0);
umeume 2:306058b9d04e 423 _9axis.stop();
umeume 2:306058b9d04e 424
hepta2ume 0:5aaec0996753 425 sprintf( a1, "%02X", ((xh)) & 0xFF);
hepta2ume 0:5aaec0996753 426 sprintf( a2, "%02X", ((xl)) & 0xFF);
hepta2ume 0:5aaec0996753 427 ax_u16[0]=a1[0];
hepta2ume 0:5aaec0996753 428 ax_u16[1]=a1[1];
hepta2ume 0:5aaec0996753 429 ax_u16[2]=a2[0];
hepta2ume 0:5aaec0996753 430 ax_u16[3]=a2[1];
hepta2ume 0:5aaec0996753 431
hepta2ume 0:5aaec0996753 432 //Y軸加速度
umeume 2:306058b9d04e 433 _9axis.start();
umeume 2:306058b9d04e 434 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 435 _9axis.write(0x3D);
umeume 2:306058b9d04e 436 _9axis.start();
umeume 2:306058b9d04e 437 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 438 yh = _9axis.read(0);
umeume 2:306058b9d04e 439
umeume 2:306058b9d04e 440 _9axis.stop();
umeume 2:306058b9d04e 441 _9axis.start();
umeume 2:306058b9d04e 442 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 443 _9axis.write(0x3E);
umeume 2:306058b9d04e 444 _9axis.start();
umeume 2:306058b9d04e 445 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 446 yl = _9axis.read(0);
umeume 2:306058b9d04e 447 _9axis.stop();
umeume 2:306058b9d04e 448
hepta2ume 0:5aaec0996753 449 sprintf( a1, "%02X", ((yh)) & 0xFF);
hepta2ume 0:5aaec0996753 450 sprintf( a2, "%02X", ((yl)) & 0xFF);
hepta2ume 0:5aaec0996753 451 ay_u16[0]=a1[0];
hepta2ume 0:5aaec0996753 452 ay_u16[1]=a1[1];
hepta2ume 0:5aaec0996753 453 ay_u16[2]=a2[0];
hepta2ume 0:5aaec0996753 454 ay_u16[3]=a2[1];
hepta2ume 0:5aaec0996753 455 //z軸加速度
umeume 2:306058b9d04e 456 _9axis.start();
umeume 2:306058b9d04e 457 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 458 _9axis.write(0x3F);
umeume 2:306058b9d04e 459 _9axis.start();
umeume 2:306058b9d04e 460 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 461 zh = _9axis.read(0);
umeume 2:306058b9d04e 462 _9axis.stop();
umeume 2:306058b9d04e 463
umeume 2:306058b9d04e 464 _9axis.start();
umeume 2:306058b9d04e 465 _9axis.write(addr_accel_gyro);
umeume 2:306058b9d04e 466 _9axis.write(0x40);
umeume 2:306058b9d04e 467 _9axis.start();
umeume 2:306058b9d04e 468 _9axis.write(addr_accel_gyro|0x01);
umeume 2:306058b9d04e 469 zl = _9axis.read(0);
umeume 2:306058b9d04e 470 _9axis.stop();
umeume 2:306058b9d04e 471
hepta2ume 0:5aaec0996753 472 sprintf( a1, "%02X", ((zh)) & 0xFF);
hepta2ume 0:5aaec0996753 473 sprintf( a2, "%02X", ((zl)) & 0xFF);
hepta2ume 0:5aaec0996753 474 az_u16[0]=a1[0];
hepta2ume 0:5aaec0996753 475 az_u16[1]=a1[1];
hepta2ume 0:5aaec0996753 476 az_u16[2]=a2[0];
hepta2ume 0:5aaec0996753 477 az_u16[3]=a2[1];
hepta2ume 0:5aaec0996753 478 *dsize = 4;
umeume 2:306058b9d04e 479 }
umeume 2:306058b9d04e 480
umeume 2:306058b9d04e 481 void Hepta9axis::sen_mag_u16(char* mx_u16,char* my_u16,char* mz_u16, int *dsize)
hepta2ume 0:5aaec0996753 482 {
umeume 2:306058b9d04e 483 _9axis.start();
umeume 2:306058b9d04e 484 _9axis.write(addr_compus);
umeume 2:306058b9d04e 485 _9axis.write(0x0a);
umeume 2:306058b9d04e 486 _9axis.write(0x12);
umeume 2:306058b9d04e 487 _9axis.stop();
umeume 2:306058b9d04e 488
umeume 2:306058b9d04e 489 _9axis.start();
umeume 2:306058b9d04e 490 _9axis.write(addr_compus);
umeume 2:306058b9d04e 491 _9axis.write(0x03);
umeume 2:306058b9d04e 492 _9axis.start();
umeume 2:306058b9d04e 493 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 494 mxl = _9axis.read(0);
umeume 2:306058b9d04e 495 _9axis.stop();
umeume 2:306058b9d04e 496 _9axis.start();
umeume 2:306058b9d04e 497 _9axis.write(addr_compus);
umeume 2:306058b9d04e 498 _9axis.write(0x04);
umeume 2:306058b9d04e 499 _9axis.start();
umeume 2:306058b9d04e 500 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 501 mxh = _9axis.read(0);
umeume 2:306058b9d04e 502 _9axis.stop();
umeume 2:306058b9d04e 503
hepta2ume 0:5aaec0996753 504 sprintf( m1, "%02X", ((mxh)) & 0xFF);
hepta2ume 0:5aaec0996753 505 sprintf( m2, "%02X", ((mxl)) & 0xFF);
hepta2ume 0:5aaec0996753 506 mx_u16[0]=m1[0];
hepta2ume 0:5aaec0996753 507 mx_u16[1]=m1[1];
hepta2ume 0:5aaec0996753 508 mx_u16[2]=m2[0];
hepta2ume 0:5aaec0996753 509 mx_u16[3]=m2[1];
umeume 2:306058b9d04e 510
hepta2ume 0:5aaec0996753 511 //pc.printf("MGX = %f\t",mx);
hepta2ume 0:5aaec0996753 512
umeume 2:306058b9d04e 513 _9axis.start();
hepta2ume 0:5aaec0996753 514
umeume 2:306058b9d04e 515 _9axis.write(addr_compus);
umeume 2:306058b9d04e 516 _9axis.write(0x05);
umeume 2:306058b9d04e 517 _9axis.start();
umeume 2:306058b9d04e 518 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 519 myl = _9axis.read(0);
umeume 2:306058b9d04e 520 _9axis.stop();
umeume 2:306058b9d04e 521 _9axis.start();
umeume 2:306058b9d04e 522 _9axis.write(addr_compus);
umeume 2:306058b9d04e 523 _9axis.write(0x06);
umeume 2:306058b9d04e 524 _9axis.start();
umeume 2:306058b9d04e 525 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 526 myh = _9axis.read(0);
umeume 2:306058b9d04e 527 _9axis.stop();
umeume 2:306058b9d04e 528
hepta2ume 0:5aaec0996753 529 sprintf( m1, "%02X", ((myh)) & 0xFF);
hepta2ume 0:5aaec0996753 530 sprintf( m2, "%02X", ((myl)) & 0xFF);
hepta2ume 0:5aaec0996753 531 my_u16[0]=m1[0];
hepta2ume 0:5aaec0996753 532 my_u16[1]=m1[1];
hepta2ume 0:5aaec0996753 533 my_u16[2]=m2[0];
hepta2ume 0:5aaec0996753 534 my_u16[3]=m2[1];
hepta2ume 0:5aaec0996753 535 // pc.printf("MAGY = %f\t",my);
hepta2ume 0:5aaec0996753 536
umeume 2:306058b9d04e 537 _9axis.start();
hepta2ume 0:5aaec0996753 538
umeume 2:306058b9d04e 539 _9axis.write(addr_compus);
umeume 2:306058b9d04e 540 _9axis.write(0x07);
umeume 2:306058b9d04e 541 _9axis.start();
umeume 2:306058b9d04e 542 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 543 mzl = _9axis.read(0);
umeume 2:306058b9d04e 544 _9axis.stop();
umeume 2:306058b9d04e 545 _9axis.start();
umeume 2:306058b9d04e 546 _9axis.write(addr_compus);
umeume 2:306058b9d04e 547 _9axis.write(0x08);
umeume 2:306058b9d04e 548 _9axis.start();
umeume 2:306058b9d04e 549 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 550 mzh = _9axis.read(0);
umeume 2:306058b9d04e 551 _9axis.stop();
umeume 2:306058b9d04e 552
umeume 2:306058b9d04e 553 _9axis.start();
umeume 2:306058b9d04e 554 _9axis.write(addr_compus);
umeume 2:306058b9d04e 555 _9axis.write(0x09);
umeume 2:306058b9d04e 556 _9axis.start();
umeume 2:306058b9d04e 557 _9axis.write(addr_compus|0x01);
umeume 2:306058b9d04e 558 _9axis.stop();
umeume 2:306058b9d04e 559
hepta2ume 0:5aaec0996753 560 sprintf( m1, "%02X", ((mzh)) & 0xFF);
hepta2ume 0:5aaec0996753 561 sprintf( m2, "%02X", ((mzl)) & 0xFF);
hepta2ume 0:5aaec0996753 562 mz_u16[0]=m1[0];
hepta2ume 0:5aaec0996753 563 mz_u16[1]=m1[1];
hepta2ume 0:5aaec0996753 564 mz_u16[2]=m2[0];
hepta2ume 0:5aaec0996753 565 mz_u16[3]=m2[1];
hepta2ume 0:5aaec0996753 566 *dsize = 4;
umeume 2:306058b9d04e 567 }