Masahiko Yamazaki / Hepta6axis

For Hepta-Sat Lite

Hepta9axis.cpp@1:5b35162b3b6a, 2017-08-05 (annotated)

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