Hepta 2 / Mbed 2 deprecated HEPTA_9axis

HEPTA2 9軸センサー

Dependencies:   Hepta2_9axis mbed

HeptaMPU9250.cpp@0:91fc70cdd15d, 2017-07-19 (annotated)

Committer:
hepta2ume
Date:
Wed Jul 19 07:35:07 2017 +0000
Revision:
0:91fc70cdd15d
9?????(HEPTA?)_0719;

Who changed what in which revision?

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