HEPTA-Sat Training 2017~2018
/
Hepta9axis
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: Lab9-01_All_transmit Lab9-03_Thermal_chamber 3daf572bcae1 Team ... more
Fork of
Hepta9axis
by 
CLTP 8
Embed:
(wiki syntax)
Show/hide line numbers
Hepta9axis.cpp
00001 #include"Hepta9axis.h" 00002 #include"mbed.h" 00003 00004 Hepta9axis::Hepta9axis(PinName sda, PinName scl, int aaddr, int agaddr) : n_axis(sda,scl),addr_accel_gyro(aaddr),addr_compus(agaddr) 00005 { 00006 n_axis.frequency(100000); 00007 cmd[0]=0x6B; 00008 cmd[1]=0x00; 00009 n_axis.write(addr_accel_gyro,cmd,2); 00010 cmd[0] = 0x37; 00011 cmd[1] = 0x02; 00012 n_axis.write(addr_accel_gyro,cmd,2); 00013 n_axis.stop(); 00014 } 00015 00016 void Hepta9axis::setup() 00017 { 00018 00019 n_axis.frequency(100000); 00020 cmd[0]=0x6B; 00021 cmd[1]=0x00; 00022 n_axis.write(addr_accel_gyro,cmd,2); 00023 cmd[0] = 0x37; 00024 cmd[1] = 0x02; 00025 n_axis.write(addr_accel_gyro,cmd,2); 00026 n_axis.stop(); 00027 } 00028 00029 void Hepta9axis::sen_acc(float *ax,float *ay,float *az) 00030 { 00031 //x軸加速度 00032 n_axis.start(); 00033 n_axis.write(addr_accel_gyro); 00034 n_axis.write(0x3B); 00035 n_axis.start(); 00036 n_axis.write(addr_accel_gyro|0x01); 00037 xh = n_axis.read(0); 00038 n_axis.stop(); 00039 00040 n_axis.start(); 00041 n_axis.write(addr_accel_gyro); 00042 n_axis.write(0x3C); 00043 n_axis.start(); 00044 n_axis.write(addr_accel_gyro|0x01); 00045 xl = n_axis.read(0); 00046 n_axis.stop(); 00047 00048 double acc_ax = short((xh<<8) | (xl)); 00049 *ax = (acc_ax)*2/32764*9.81; 00050 00051 00052 //Y軸加速度 00053 n_axis.start(); 00054 n_axis.write(addr_accel_gyro); 00055 n_axis.write(0x3D); 00056 n_axis.start(); 00057 n_axis.write(addr_accel_gyro|0x01); 00058 yh = n_axis.read(0); 00059 00060 n_axis.stop(); 00061 n_axis.start(); 00062 n_axis.write(addr_accel_gyro); 00063 n_axis.write(0x3E); 00064 n_axis.start(); 00065 n_axis.write(addr_accel_gyro|0x01); 00066 yl = n_axis.read(0); 00067 00068 n_axis.stop(); 00069 double acc_ay = short((yh<<8) | (yl)); 00070 *ay = (acc_ay)*2/32764*9.81; 00071 00072 00073 //z軸加速度 00074 n_axis.start(); 00075 n_axis.write(addr_accel_gyro); 00076 n_axis.write(0x3F); 00077 n_axis.start(); 00078 n_axis.write(addr_accel_gyro|0x01); 00079 zh = n_axis.read(0); 00080 n_axis.stop(); 00081 00082 n_axis.start(); 00083 n_axis.write(addr_accel_gyro); 00084 n_axis.write(0x40); 00085 n_axis.start(); 00086 n_axis.write(addr_accel_gyro|0x01); 00087 zl = n_axis.read(0); 00088 n_axis.stop(); 00089 00090 double acc_az = short((zh<<8) | (zl)); 00091 *az = (acc_az)*2/32764*9.81; 00092 00093 } 00094 00095 void Hepta9axis::sen_gyro(float *gx,float *gy,float *gz) 00096 { 00097 //x軸GYRO 00098 n_axis.start(); 00099 n_axis.write(addr_accel_gyro); 00100 n_axis.write(0x43); 00101 n_axis.start(); 00102 n_axis.write(addr_accel_gyro|0x01); 00103 gxh = n_axis.read(0); 00104 n_axis.stop(); 00105 00106 n_axis.start(); 00107 n_axis.write(addr_accel_gyro); 00108 n_axis.write(0x44); 00109 n_axis.start(); 00110 n_axis.write(addr_accel_gyro|0x01); 00111 gxl = n_axis.read(0); 00112 n_axis.stop(); 00113 00114 double gyro_ax = short((gxh<<8) | (gxl)); 00115 *gx = (gyro_ax)*0.00763; 00116 00117 //y軸GYRO 00118 n_axis.start(); 00119 n_axis.write(addr_accel_gyro); 00120 n_axis.write(0x45); 00121 n_axis.start(); 00122 n_axis.write(addr_accel_gyro|0x01); 00123 gyh = n_axis.read(0); 00124 n_axis.stop(); 00125 00126 n_axis.start(); 00127 n_axis.write(addr_accel_gyro); 00128 n_axis.write(0x46); 00129 n_axis.start(); 00130 n_axis.write(addr_accel_gyro|0x01); 00131 gyl = n_axis.read(0); 00132 n_axis.stop(); 00133 00134 double gyro_ay = short((gyh<<8) | (gyl)); 00135 *gy = (gyro_ay)*0.00763; 00136 00137 //z軸GYRO 00138 n_axis.start(); 00139 n_axis.write(addr_accel_gyro); 00140 n_axis.write(0x47); 00141 n_axis.start(); 00142 n_axis.write(addr_accel_gyro|0x01); 00143 gzh = n_axis.read(0); 00144 n_axis.stop(); 00145 00146 n_axis.start(); 00147 n_axis.write(addr_accel_gyro); 00148 n_axis.write(0x48); 00149 n_axis.start(); 00150 n_axis.write(addr_accel_gyro|0x01); 00151 gzl = n_axis.read(0); 00152 n_axis.stop(); 00153 00154 double gyro_az = short((gzh<<8) | (gzl)); 00155 *gz = (gyro_az)*0.00763; 00156 00157 } 00158 00159 void Hepta9axis::sen_mag(float *mx,float *my,float *mz) 00160 { 00161 00162 n_axis.start(); 00163 n_axis.write(addr_compus); 00164 n_axis.write(0x0A); 00165 n_axis.write(0x12); 00166 n_axis.stop(); 00167 00168 n_axis.start(); 00169 n_axis.write(addr_compus); 00170 n_axis.write(0x03); 00171 n_axis.start(); 00172 n_axis.write(addr_compus|0x01); 00173 mxl = n_axis.read(0); 00174 n_axis.stop(); 00175 n_axis.start(); 00176 n_axis.write(addr_compus); 00177 n_axis.write(0x04); 00178 n_axis.start(); 00179 n_axis.write(addr_compus|0x01); 00180 mxh = n_axis.read(0); 00181 n_axis.stop(); 00182 00183 double mg_x = short((mxh<<8) | (mxl)); 00184 *mx = ( mg_x)*0.15; 00185 00186 n_axis.start(); 00187 00188 n_axis.write(addr_compus); 00189 n_axis.write(0x05); 00190 n_axis.start(); 00191 n_axis.write(addr_compus|0x01); 00192 myl = n_axis.read(0); 00193 n_axis.stop(); 00194 n_axis.start(); 00195 n_axis.write(addr_compus); 00196 n_axis.write(0x06); 00197 n_axis.start(); 00198 n_axis.write(addr_compus|0x01); 00199 myh = n_axis.read(0); 00200 n_axis.stop(); 00201 00202 double mg_y = short((myh<<8) | (myl)); 00203 *my = ( mg_y)*0.15; 00204 00205 n_axis.start(); 00206 00207 n_axis.write(addr_compus); 00208 n_axis.write(0x07); 00209 n_axis.start(); 00210 n_axis.write(addr_compus|0x01); 00211 mzl = n_axis.read(0); 00212 n_axis.stop(); 00213 n_axis.start(); 00214 n_axis.write(addr_compus); 00215 n_axis.write(0x08); 00216 n_axis.start(); 00217 n_axis.write(addr_compus|0x01); 00218 mzh = n_axis.read(0); 00219 n_axis.stop(); 00220 00221 n_axis.start(); 00222 n_axis.write(addr_compus); 00223 n_axis.write(0x09); 00224 n_axis.start(); 00225 n_axis.write(addr_compus|0x01); 00226 st2 = n_axis.read(0); 00227 n_axis.stop(); 00228 double mg_z= short((mzh<<8) | (mzl)); 00229 *mz = ( mg_z)*0.15; 00230 00231 } 00232 00233 //////////////16進数表記///////////////////////////////////////// 00234 void Hepta9axis::sen_gyro_u16(char* gx_u16,char* gy_u16,char* gz_u16) 00235 { 00236 //x軸GYRO 00237 n_axis.start(); 00238 n_axis.write(addr_accel_gyro); 00239 n_axis.write(0x43); 00240 n_axis.start(); 00241 n_axis.write(addr_accel_gyro|0x01); 00242 gxh = n_axis.read(0); 00243 n_axis.stop(); 00244 00245 n_axis.start(); 00246 n_axis.write(addr_accel_gyro); 00247 n_axis.write(0x44); 00248 n_axis.start(); 00249 n_axis.write(addr_accel_gyro|0x01); 00250 gxl = n_axis.read(0); 00251 n_axis.stop(); 00252 00253 sprintf( g1, "%02X", ((gxh)) & 0xFF); 00254 sprintf( g2, "%02X", ((gxl)) & 0xFF); 00255 gx_u16[0]=g1[0]; 00256 gx_u16[1]=g1[1]; 00257 gx_u16[2]=g2[0]; 00258 gx_u16[3]=g2[1]; 00259 00260 //y軸GYRO 00261 n_axis.start(); 00262 n_axis.write(addr_accel_gyro); 00263 n_axis.write(0x45); 00264 n_axis.start(); 00265 n_axis.write(addr_accel_gyro|0x01); 00266 gyh = n_axis.read(0); 00267 n_axis.stop(); 00268 00269 n_axis.start(); 00270 n_axis.write(addr_accel_gyro); 00271 n_axis.write(0x46); 00272 n_axis.start(); 00273 n_axis.write(addr_accel_gyro|0x01); 00274 gyl = n_axis.read(0); 00275 n_axis.stop(); 00276 sprintf( g1, "%02X", (gyh) & 0xFF); 00277 sprintf( g2, "%02X", (gyl) & 0xFF); 00278 gy_u16[0]=g1[0]; 00279 gy_u16[1]=g1[1]; 00280 gy_u16[2]=g2[0]; 00281 gy_u16[3]=g2[1]; 00282 00283 //z軸GYRO 00284 n_axis.start(); 00285 n_axis.write(addr_accel_gyro); 00286 n_axis.write(0x47); 00287 n_axis.start(); 00288 n_axis.write(addr_accel_gyro|0x01); 00289 gzh = n_axis.read(0); 00290 n_axis.stop(); 00291 00292 n_axis.start(); 00293 n_axis.write(addr_accel_gyro); 00294 n_axis.write(0x48); 00295 n_axis.start(); 00296 n_axis.write(addr_accel_gyro|0x01); 00297 gzl = n_axis.read(0); 00298 n_axis.stop(); 00299 00300 sprintf( g1, "%02X", ((gzh)) & 0xFF); 00301 sprintf( g2, "%02X", ((gzl)) & 0xFF); 00302 gz_u16[0]=g1[0]; 00303 gz_u16[1]=g1[1]; 00304 gz_u16[2]=g2[0]; 00305 gz_u16[3]=g2[1]; 00306 //*dsize = 4; 00307 } 00308 00309 void Hepta9axis::sen_acc_u16(char* ax_u16,char* ay_u16,char* az_u16) 00310 { 00311 //x軸加速度 00312 n_axis.start(); 00313 n_axis.write(addr_accel_gyro); 00314 n_axis.write(0x3B); 00315 n_axis.start(); 00316 n_axis.write(addr_accel_gyro|0x01); 00317 xh = n_axis.read(0); 00318 n_axis.stop(); 00319 00320 n_axis.start(); 00321 n_axis.write(addr_accel_gyro); 00322 n_axis.write(0x3C); 00323 n_axis.start(); 00324 n_axis.write(addr_accel_gyro|0x01); 00325 xl = n_axis.read(0); 00326 n_axis.stop(); 00327 00328 sprintf( a1, "%02X", ((xh)) & 0xFF); 00329 sprintf( a2, "%02X", ((xl)) & 0xFF); 00330 ax_u16[0]=a1[0]; 00331 ax_u16[1]=a1[1]; 00332 ax_u16[2]=a2[0]; 00333 ax_u16[3]=a2[1]; 00334 00335 //Y軸加速度 00336 n_axis.start(); 00337 n_axis.write(addr_accel_gyro); 00338 n_axis.write(0x3D); 00339 n_axis.start(); 00340 n_axis.write(addr_accel_gyro|0x01); 00341 yh = n_axis.read(0); 00342 00343 n_axis.stop(); 00344 n_axis.start(); 00345 n_axis.write(addr_accel_gyro); 00346 n_axis.write(0x3E); 00347 n_axis.start(); 00348 n_axis.write(addr_accel_gyro|0x01); 00349 yl = n_axis.read(0); 00350 n_axis.stop(); 00351 00352 sprintf( a1, "%02X", ((yh)) & 0xFF); 00353 sprintf( a2, "%02X", ((yl)) & 0xFF); 00354 ay_u16[0]=a1[0]; 00355 ay_u16[1]=a1[1]; 00356 ay_u16[2]=a2[0]; 00357 ay_u16[3]=a2[1]; 00358 //z軸加速度 00359 n_axis.start(); 00360 n_axis.write(addr_accel_gyro); 00361 n_axis.write(0x3F); 00362 n_axis.start(); 00363 n_axis.write(addr_accel_gyro|0x01); 00364 zh = n_axis.read(0); 00365 n_axis.stop(); 00366 00367 n_axis.start(); 00368 n_axis.write(addr_accel_gyro); 00369 n_axis.write(0x40); 00370 n_axis.start(); 00371 n_axis.write(addr_accel_gyro|0x01); 00372 zl = n_axis.read(0); 00373 n_axis.stop(); 00374 00375 sprintf( a1, "%02X", ((zh)) & 0xFF); 00376 sprintf( a2, "%02X", ((zl)) & 0xFF); 00377 az_u16[0]=a1[0]; 00378 az_u16[1]=a1[1]; 00379 az_u16[2]=a2[0]; 00380 az_u16[3]=a2[1]; 00381 //*dsize = 4; 00382 } 00383 00384 void Hepta9axis::sen_mag_u16(char* mx_u16,char* my_u16,char* mz_u16) 00385 { 00386 n_axis.start(); 00387 n_axis.write(addr_compus); 00388 n_axis.write(0x0a); 00389 n_axis.write(0x12); 00390 n_axis.stop(); 00391 00392 n_axis.start(); 00393 n_axis.write(addr_compus); 00394 n_axis.write(0x03); 00395 n_axis.start(); 00396 n_axis.write(addr_compus|0x01); 00397 mxl = n_axis.read(0); 00398 n_axis.stop(); 00399 n_axis.start(); 00400 n_axis.write(addr_compus); 00401 n_axis.write(0x04); 00402 n_axis.start(); 00403 n_axis.write(addr_compus|0x01); 00404 mxh = n_axis.read(0); 00405 n_axis.stop(); 00406 00407 sprintf( m1, "%02X", ((mxh)) & 0xFF); 00408 sprintf( m2, "%02X", ((mxl)) & 0xFF); 00409 mx_u16[0]=m1[0]; 00410 mx_u16[1]=m1[1]; 00411 mx_u16[2]=m2[0]; 00412 mx_u16[3]=m2[1]; 00413 00414 n_axis.start(); 00415 00416 n_axis.write(addr_compus); 00417 n_axis.write(0x05); 00418 n_axis.start(); 00419 n_axis.write(addr_compus|0x01); 00420 myl = n_axis.read(0); 00421 n_axis.stop(); 00422 n_axis.start(); 00423 n_axis.write(addr_compus); 00424 n_axis.write(0x06); 00425 n_axis.start(); 00426 n_axis.write(addr_compus|0x01); 00427 myh = n_axis.read(0); 00428 n_axis.stop(); 00429 00430 sprintf( m1, "%02X", ((myh)) & 0xFF); 00431 sprintf( m2, "%02X", ((myl)) & 0xFF); 00432 my_u16[0]=m1[0]; 00433 my_u16[1]=m1[1]; 00434 my_u16[2]=m2[0]; 00435 my_u16[3]=m2[1]; 00436 00437 n_axis.start(); 00438 00439 n_axis.write(addr_compus); 00440 n_axis.write(0x07); 00441 n_axis.start(); 00442 n_axis.write(addr_compus|0x01); 00443 mzl = n_axis.read(0); 00444 n_axis.stop(); 00445 n_axis.start(); 00446 n_axis.write(addr_compus); 00447 n_axis.write(0x08); 00448 n_axis.start(); 00449 n_axis.write(addr_compus|0x01); 00450 mzh = n_axis.read(0); 00451 n_axis.stop(); 00452 00453 n_axis.start(); 00454 n_axis.write(addr_compus); 00455 n_axis.write(0x09); 00456 n_axis.start(); 00457 n_axis.write(addr_compus|0x01); 00458 st2 = n_axis.read(0); 00459 n_axis.stop(); 00460 00461 sprintf( m1, "%02X", ((mzh)) & 0xFF); 00462 sprintf( m2, "%02X", ((mzl)) & 0xFF); 00463 mz_u16[0]=m1[0]; 00464 mz_u16[1]=m1[1]; 00465 mz_u16[2]=m2[0]; 00466 mz_u16[3]=m2[1]; 00467 //*dsize = 4; 00468 }
Generated on Wed Jul 13 2022 11:40:23 by
1.7.2