Masahiko Yamazaki / Hepta6axis

For Hepta-Sat Lite

Hepta9axis.cpp@3:d5eed0bb962e, 2017-09-04 (annotated)

Committer:
HEPTA
Date:
Mon Sep 04 11:45:42 2017 +0000
Revision:
3:d5eed0bb962e
Parent:
2:306058b9d04e
Child:
4:01941772f493
Hepta9axis 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));
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
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));
hepta2ume 0:5aaec0996753 115 *gx = (gyro_ax)*0.00763-1.6;
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();
umeume 2:306058b9d04e 182 /*
HEPTA 3:d5eed0bb962e 183 n_axis.start();
HEPTA 3:d5eed0bb962e 184 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 185 n_axis.write(0x09);
HEPTA 3:d5eed0bb962e 186 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 187 n_axis.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
HEPTA 3:d5eed0bb962e 193 n_axis.start();
hepta2ume 0:5aaec0996753 194
HEPTA 3:d5eed0bb962e 195 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 196 n_axis.write(0x05);
HEPTA 3:d5eed0bb962e 197 n_axis.start();
HEPTA 3:d5eed0bb962e 198 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 199 myl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 200 n_axis.stop();
HEPTA 3:d5eed0bb962e 201 n_axis.start();
HEPTA 3:d5eed0bb962e 202 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 203 n_axis.write(0x06);
HEPTA 3:d5eed0bb962e 204 n_axis.start();
HEPTA 3:d5eed0bb962e 205 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 206 myh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 207 n_axis.stop();
umeume 2:306058b9d04e 208 /*
HEPTA 3:d5eed0bb962e 209 n_axis.start();
HEPTA 3:d5eed0bb962e 210 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 211 n_axis.write(0x09);
HEPTA 3:d5eed0bb962e 212 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 213 n_axis.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
HEPTA 3:d5eed0bb962e 219 n_axis.start();
hepta2ume 0:5aaec0996753 220
HEPTA 3:d5eed0bb962e 221 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 222 n_axis.write(0x07);
HEPTA 3:d5eed0bb962e 223 n_axis.start();
HEPTA 3:d5eed0bb962e 224 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 225 mzl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 226 n_axis.stop();
HEPTA 3:d5eed0bb962e 227 n_axis.start();
HEPTA 3:d5eed0bb962e 228 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 229 n_axis.write(0x08);
HEPTA 3:d5eed0bb962e 230 n_axis.start();
HEPTA 3:d5eed0bb962e 231 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 232 mzh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 233 n_axis.stop();
umeume 2:306058b9d04e 234
HEPTA 3:d5eed0bb962e 235 n_axis.start();
HEPTA 3:d5eed0bb962e 236 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 237 n_axis.write(0x09);
HEPTA 3:d5eed0bb962e 238 n_axis.start();
HEPTA 3:d5eed0bb962e 239 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 240 n_axis.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 /*
HEPTA 3:d5eed0bb962e 244 n_axis.start();
HEPTA 3:d5eed0bb962e 245 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 246 n_axis.write(0x0a);
HEPTA 3:d5eed0bb962e 247 n_axis.write(0x12);
HEPTA 3:d5eed0bb962e 248 n_axis.stop();
umeume 2:306058b9d04e 249
HEPTA 3:d5eed0bb962e 250 n_axis.start();
HEPTA 3:d5eed0bb962e 251 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 252 n_axis.write(0x03);
HEPTA 3:d5eed0bb962e 253 n_axis.start();
HEPTA 3:d5eed0bb962e 254 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 255 mxl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 256 n_axis.stop();
HEPTA 3:d5eed0bb962e 257 n_axis.start();
HEPTA 3:d5eed0bb962e 258 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 259 n_axis.write(0x04);
HEPTA 3:d5eed0bb962e 260 n_axis.start();
HEPTA 3:d5eed0bb962e 261 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 262 mxh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 263 n_axis.stop();
umeume 2:306058b9d04e 264
HEPTA 3:d5eed0bb962e 265 n_axis.start();
HEPTA 3:d5eed0bb962e 266 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 267 n_axis.write(0x09);
HEPTA 3:d5eed0bb962e 268 n_axis.start();
HEPTA 3:d5eed0bb962e 269 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 270 n_axis.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
HEPTA 3:d5eed0bb962e 277 n_axis.start();
umeume 2:306058b9d04e 278
HEPTA 3:d5eed0bb962e 279 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 280 n_axis.write(0x05);
HEPTA 3:d5eed0bb962e 281 n_axis.start();
HEPTA 3:d5eed0bb962e 282 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 283 myl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 284 n_axis.stop();
HEPTA 3:d5eed0bb962e 285 n_axis.start();
HEPTA 3:d5eed0bb962e 286 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 287 n_axis.write(0x06);
HEPTA 3:d5eed0bb962e 288 n_axis.start();
HEPTA 3:d5eed0bb962e 289 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 290 myh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 291 n_axis.stop();
umeume 2:306058b9d04e 292
HEPTA 3:d5eed0bb962e 293 n_axis.start();
HEPTA 3:d5eed0bb962e 294 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 295 n_axis.write(0x09);
HEPTA 3:d5eed0bb962e 296 n_axis.start();
HEPTA 3:d5eed0bb962e 297 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 298 n_axis.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
HEPTA 3:d5eed0bb962e 302 n_axis.start();
umeume 2:306058b9d04e 303
HEPTA 3:d5eed0bb962e 304 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 305 n_axis.write(0x07);
HEPTA 3:d5eed0bb962e 306 n_axis.start();
HEPTA 3:d5eed0bb962e 307 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 308 mzl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 309 n_axis.stop();
HEPTA 3:d5eed0bb962e 310 n_axis.start();
HEPTA 3:d5eed0bb962e 311 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 312 n_axis.write(0x08);
HEPTA 3:d5eed0bb962e 313 n_axis.start();
HEPTA 3:d5eed0bb962e 314 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 315 mzh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 316 n_axis.stop();
umeume 2:306058b9d04e 317
HEPTA 3:d5eed0bb962e 318 n_axis.start();
HEPTA 3:d5eed0bb962e 319 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 320 n_axis.write(0x09);
HEPTA 3:d5eed0bb962e 321 n_axis.start();
HEPTA 3:d5eed0bb962e 322 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 323 n_axis.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
HEPTA 3:d5eed0bb962e 334 n_axis.start();
HEPTA 3:d5eed0bb962e 335 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 336 n_axis.write(0x43);
HEPTA 3:d5eed0bb962e 337 n_axis.start();
HEPTA 3:d5eed0bb962e 338 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 339 gxh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 340 n_axis.stop();
umeume 2:306058b9d04e 341
HEPTA 3:d5eed0bb962e 342 n_axis.start();
HEPTA 3:d5eed0bb962e 343 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 344 n_axis.write(0x44);
HEPTA 3:d5eed0bb962e 345 n_axis.start();
HEPTA 3:d5eed0bb962e 346 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 347 gxl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 348 n_axis.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
HEPTA 3:d5eed0bb962e 358 n_axis.start();
HEPTA 3:d5eed0bb962e 359 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 360 n_axis.write(0x45);
HEPTA 3:d5eed0bb962e 361 n_axis.start();
HEPTA 3:d5eed0bb962e 362 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 363 gyh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 364 n_axis.stop();
umeume 2:306058b9d04e 365
HEPTA 3:d5eed0bb962e 366 n_axis.start();
HEPTA 3:d5eed0bb962e 367 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 368 n_axis.write(0x46);
HEPTA 3:d5eed0bb962e 369 n_axis.start();
HEPTA 3:d5eed0bb962e 370 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 371 gyl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 372 n_axis.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
HEPTA 3:d5eed0bb962e 381 n_axis.start();
HEPTA 3:d5eed0bb962e 382 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 383 n_axis.write(0x47);
HEPTA 3:d5eed0bb962e 384 n_axis.start();
HEPTA 3:d5eed0bb962e 385 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 386 gzh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 387 n_axis.stop();
umeume 2:306058b9d04e 388
HEPTA 3:d5eed0bb962e 389 n_axis.start();
HEPTA 3:d5eed0bb962e 390 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 391 n_axis.write(0x48);
HEPTA 3:d5eed0bb962e 392 n_axis.start();
HEPTA 3:d5eed0bb962e 393 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 394 gzl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 395 n_axis.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軸加速度
HEPTA 3:d5eed0bb962e 409 n_axis.start();
HEPTA 3:d5eed0bb962e 410 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 411 n_axis.write(0x3B);
HEPTA 3:d5eed0bb962e 412 n_axis.start();
HEPTA 3:d5eed0bb962e 413 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 414 xh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 415 n_axis.stop();
hepta2ume 0:5aaec0996753 416
HEPTA 3:d5eed0bb962e 417 n_axis.start();
HEPTA 3:d5eed0bb962e 418 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 419 n_axis.write(0x3C);
HEPTA 3:d5eed0bb962e 420 n_axis.start();
HEPTA 3:d5eed0bb962e 421 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 422 xl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 423 n_axis.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軸加速度
HEPTA 3:d5eed0bb962e 433 n_axis.start();
HEPTA 3:d5eed0bb962e 434 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 435 n_axis.write(0x3D);
HEPTA 3:d5eed0bb962e 436 n_axis.start();
HEPTA 3:d5eed0bb962e 437 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 438 yh = n_axis.read(0);
umeume 2:306058b9d04e 439
HEPTA 3:d5eed0bb962e 440 n_axis.stop();
HEPTA 3:d5eed0bb962e 441 n_axis.start();
HEPTA 3:d5eed0bb962e 442 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 443 n_axis.write(0x3E);
HEPTA 3:d5eed0bb962e 444 n_axis.start();
HEPTA 3:d5eed0bb962e 445 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 446 yl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 447 n_axis.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軸加速度
HEPTA 3:d5eed0bb962e 456 n_axis.start();
HEPTA 3:d5eed0bb962e 457 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 458 n_axis.write(0x3F);
HEPTA 3:d5eed0bb962e 459 n_axis.start();
HEPTA 3:d5eed0bb962e 460 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 461 zh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 462 n_axis.stop();
umeume 2:306058b9d04e 463
HEPTA 3:d5eed0bb962e 464 n_axis.start();
HEPTA 3:d5eed0bb962e 465 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 466 n_axis.write(0x40);
HEPTA 3:d5eed0bb962e 467 n_axis.start();
HEPTA 3:d5eed0bb962e 468 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 469 zl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 470 n_axis.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 {
HEPTA 3:d5eed0bb962e 483 n_axis.start();
HEPTA 3:d5eed0bb962e 484 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 485 n_axis.write(0x0a);
HEPTA 3:d5eed0bb962e 486 n_axis.write(0x12);
HEPTA 3:d5eed0bb962e 487 n_axis.stop();
umeume 2:306058b9d04e 488
HEPTA 3:d5eed0bb962e 489 n_axis.start();
HEPTA 3:d5eed0bb962e 490 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 491 n_axis.write(0x03);
HEPTA 3:d5eed0bb962e 492 n_axis.start();
HEPTA 3:d5eed0bb962e 493 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 494 mxl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 495 n_axis.stop();
HEPTA 3:d5eed0bb962e 496 n_axis.start();
HEPTA 3:d5eed0bb962e 497 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 498 n_axis.write(0x04);
HEPTA 3:d5eed0bb962e 499 n_axis.start();
HEPTA 3:d5eed0bb962e 500 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 501 mxh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 502 n_axis.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
HEPTA 3:d5eed0bb962e 513 n_axis.start();
hepta2ume 0:5aaec0996753 514
HEPTA 3:d5eed0bb962e 515 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 516 n_axis.write(0x05);
HEPTA 3:d5eed0bb962e 517 n_axis.start();
HEPTA 3:d5eed0bb962e 518 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 519 myl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 520 n_axis.stop();
HEPTA 3:d5eed0bb962e 521 n_axis.start();
HEPTA 3:d5eed0bb962e 522 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 523 n_axis.write(0x06);
HEPTA 3:d5eed0bb962e 524 n_axis.start();
HEPTA 3:d5eed0bb962e 525 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 526 myh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 527 n_axis.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
HEPTA 3:d5eed0bb962e 537 n_axis.start();
hepta2ume 0:5aaec0996753 538
HEPTA 3:d5eed0bb962e 539 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 540 n_axis.write(0x07);
HEPTA 3:d5eed0bb962e 541 n_axis.start();
HEPTA 3:d5eed0bb962e 542 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 543 mzl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 544 n_axis.stop();
HEPTA 3:d5eed0bb962e 545 n_axis.start();
HEPTA 3:d5eed0bb962e 546 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 547 n_axis.write(0x08);
HEPTA 3:d5eed0bb962e 548 n_axis.start();
HEPTA 3:d5eed0bb962e 549 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 550 mzh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 551 n_axis.stop();
umeume 2:306058b9d04e 552
HEPTA 3:d5eed0bb962e 553 n_axis.start();
HEPTA 3:d5eed0bb962e 554 n_axis.write(addr_compus);
HEPTA 3:d5eed0bb962e 555 n_axis.write(0x09);
HEPTA 3:d5eed0bb962e 556 n_axis.start();
HEPTA 3:d5eed0bb962e 557 n_axis.write(addr_compus|0x01);
HEPTA 3:d5eed0bb962e 558 n_axis.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 }