Masahiko Yamazaki / Hepta6axis

For Hepta-Sat Lite

Hepta9axis.cpp@5:098423fb0371, 2017-09-05 (annotated)

Committer:
HEPTA
Date:
Tue Sep 05 12:46:12 2017 +0000
Revision:
5:098423fb0371
Parent:
4:01941772f493
Child:
6:fdfca3ffecd5
Hepta 9axis Sensor;

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
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 {
hepta2ume 0:5aaec0996753 31 //x軸加速度
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));
umeume 2:306058b9d04e 49 *ax = (acc_ax)*2/32764*9.81;
hepta2ume 0:5aaec0996753 50
hepta2ume 0:5aaec0996753 51
hepta2ume 0:5aaec0996753 52 //Y軸加速度
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));
hepta2ume 0:5aaec0996753 70 *ay = (acc_ay)*2/32764*9.81;
hepta2ume 0:5aaec0996753 71
hepta2ume 0:5aaec0996753 72
hepta2ume 0:5aaec0996753 73 //z軸加速度
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));
HEPTA 4:01941772f493 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 {
hepta2ume 0:5aaec0996753 97 //x軸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
hepta2ume 0:5aaec0996753 117 //y軸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
hepta2ume 0:5aaec0996753 137 //z軸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
hepta2ume 0:5aaec0996753 233 //////////////16進数表記/////////////////////////////////////////
HEPTA 5:098423fb0371 234 void Hepta9axis::sen_gyro_u16(char* gx_u16,char* gy_u16,char* gz_u16)
hepta2ume 0:5aaec0996753 235 {
hepta2ume 0:5aaec0996753 236 //x軸GYRO
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
hepta2ume 0:5aaec0996753 260 //y軸GYRO
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
hepta2ume 0:5aaec0996753 283 //z軸GYRO
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 {
umeume 2:306058b9d04e 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
hepta2ume 0:5aaec0996753 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];
hepta2ume 0:5aaec0996753 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 }