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.
main.cpp
00001 /* mbed Microcontroller Library 00002 * Copyright (c) 2018 ARM Limited 00003 * SPDX-License-Identifier: Apache-2.0 00004 */ 00005 00006 #include "mbed.h" 00007 #include "ak8963.h" 00008 #include <stddef.h> 00009 #include <stdio.h> // This ert_main.c example uses printf/fflush 00010 #include "LAAP.h" // Model's header file 00011 #include "rtwtypes.h" 00012 00013 static LAAPModelClass rtObj; // Instance of model class 00014 00015 void rt_OneStep(void); 00016 void rt_OneStep(void) 00017 { 00018 static boolean_T OverrunFlag = false; 00019 00020 // Disable interrupts here 00021 00022 // Check for overrun 00023 if (OverrunFlag) { 00024 rtmSetErrorStatus(rtObj.getRTM(), "Overrun"); 00025 return; 00026 } 00027 00028 OverrunFlag = true; 00029 00030 // Save FPU context here (if necessary) 00031 // Re-enable timer or interrupt here 00032 // Set model inputs here 00033 00034 // Step the model 00035 rtObj.step(); 00036 00037 // Get model outputs here 00038 00039 // Indicate task complete 00040 OverrunFlag = false; 00041 00042 // Disable interrupts here 00043 // Restore FPU context here (if necessary) 00044 // Enable interrupts here 00045 } 00046 /* 00047 #define AK8963_ADDRESS 0x0C 00048 #define WHO_AM_I_AK8963 0x00 // should return 0x48 00049 #define INFO 0x01 00050 #define AK8963_ST1 0x02 // data ready status bit 0 00051 #define AK8963_XOUT_L 0x03 // data 00052 #define AK8963_XOUT_H 0x04 00053 #define AK8963_YOUT_L 0x05 00054 #define AK8963_YOUT_H 0x06 00055 #define AK8963_ZOUT_L 0x07 00056 #define AK8963_ZOUT_H 0x08 00057 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 00058 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 00059 #define AK8963_ASTC 0x0C // Self test control 00060 #define AK8963_I2CDIS 0x0F // I2C disable 00061 #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value 00062 #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value 00063 #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value 00064 */ 00065 #define USER_CTRL 0x6A; 00066 #define I2C_MST_EN 0x20; 00067 00068 #define MPU6050_ADDR 0xD0 00069 #define MPU6050_SMPLRT_DIV 0x19 00070 #define MPU6050_CONFIG 0x1a 00071 #define MPU6050_GYRO_CONFIG 0x1b 00072 #define MPU6050_ACCEL_CONFIG 0x1c 00073 #define MPU6050_WHO_AM_I 0x75 00074 #define MPU6050_PWR_MGMT_1 0x6b 00075 #define ACCEL_XOUT_H_REG 0x3B 00076 #define Alfa_comp 0.96 00077 00078 double Ax, Ay, Az, Gx, Gy, Gz,Mx,My,Mz; 00079 00080 double H_senx,H_seny,H_senz; 00081 char name[1]; 00082 00083 int16_t Mag_X_RAW = 0; 00084 int16_t Mag_Y_RAW = 0; 00085 int16_t Mag_Z_RAW = 0; 00086 00087 double avgx,avgy,avgz; 00088 double rate,bias=0; 00089 double angle; 00090 00091 double pitch_acc=0, roll_acc=0; 00092 double pitch_gyro=0, roll_gyro=0; 00093 double pitch=0, roll=0,yaw=0; 00094 double dtx,dtx1,dtx2,dt=0,dt1,dt2; 00095 //global variables for meaured pwm values 00096 uint16_t Rip_chnl_n[6],Rip_chnl_p[6]; 00097 00098 int distx,disty,distz; 00099 int distxo,distyo,distzo; 00100 int velocityx,velocityy,velocityz; 00101 00102 //////////////////////To measure pwm using interrupts and timer/////////////////////////////// 00103 InterruptIn ch1(PG_2); 00104 InterruptIn ch2(PG_3); 00105 InterruptIn ch3(A4); 00106 InterruptIn ch4(A5); 00107 InterruptIn ch5(PG_0); 00108 InterruptIn ch6(PG_1); 00109 /////////////////////////////////////////////////////////////////////////////////////////// 00110 //////////////////// pwm out for ESC inputs//////////////////////////////////////////////// 00111 PwmOut u1(PE_9); 00112 PwmOut u2(PE_11); 00113 PwmOut u3(PE_12); 00114 PwmOut u4(PE_14); 00115 ///////////////////////////I2C///////////////////////////////////////////////////////////// 00116 I2C i2c(PF_15,PF_14); 00117 ////////////////////////////////////////////////////////////////////////////////////////// 00118 ///////////////////////////////LIDAR serial ports///////////////////////////////////////// 00119 RawSerial rawz(PD_5,PD_6); 00120 RawSerial rawx(PC_10,PC_11); 00121 RawSerial rawy(PC_12,PD_2); 00122 ////////////////////////////////////////////////////////////////////////////////////////// 00123 //////////blue light indicator for error in i2c/////////////////////////////////////////// 00124 DigitalOut myled(LED2); 00125 ////////////////////////////////////////////////////////////////////////////////////////// 00126 Serial pc(SERIAL_TX, SERIAL_RX); 00127 00128 Timeout onestep; 00129 00130 Timer timer1,timer2; 00131 ////////////////////////////////function initializations///////////////////////////////// 00132 void stop_counter(); 00133 void start_counter(); 00134 void counters_init(); 00135 00136 void dist_x(); 00137 void dist_y(); 00138 void dist_z(); 00139 00140 void mpu_init(); 00141 void mpu_read(); 00142 void get_angle(); 00143 void mpu_calibrate(); 00144 void Mag_init(); 00145 void Mag_read(); 00146 00147 onestep.attach_us(&rt_OneStep(),100); 00148 00149 int main() 00150 { 00151 counters_init(); 00152 00153 pc.baud(115200); 00154 00155 rawx.baud(115200); 00156 rawy.baud(115200); 00157 rawz.baud(115200); 00158 00159 rawx.attach(&dist_x); 00160 rawy.attach(&dist_y); 00161 rawz.attach(&dist_z); 00162 00163 mpu_init(); 00164 //Mag_init(); 00165 /*AK8963 mag(&i2c, AK8963::SLAVE_ADDR_1); 00166 if(mag.checkConnection() != AK8963::SUCCESS){ 00167 pc.printf("check connection"); 00168 00169 } 00170 if(mag.setOperationMode(AK8963::MODE_CONTINUOUS_1) != AK8963::SUCCESS){ 00171 while(1){pc.printf("failed continious mode");} 00172 }*/ 00173 timer1.start(); //////////////timer for channel 00174 timer2.start(); /////////////timer for dt to calculate dt 00175 00176 //mpu_calibrate(); 00177 00178 00179 u1.period_us(20000); 00180 u2.period_us(20000); 00181 u3.period_us(20000); 00182 u4.period_us(20000); 00183 00184 00185 while(timer2.read_ms()<5000) 00186 { u1.write((float)rtObj.getu1_()); 00187 u2.write((float)rtObj.getu2_()); 00188 u3.write((float)rtObj.getu3 ()); 00189 u4.write((float)rtObj.getu4 ()); 00190 00191 } 00192 00193 00194 00195 00196 while (true) 00197 { int i; 00198 //int statusAK8963= AK8963::NOT_DATA_READY; 00199 00200 00201 get_angle(); 00202 00203 /* if(statusAK8963 == AK8963::DATA_READY){ 00204 AK8963::MagneticVector mag1; 00205 mag.getMagneticVector(&mag1); 00206 pc.printf("%5.1f,%5.1f,%5.1f\n",mag1.mx,mag1.my,mag1.mz); 00207 statusAK8963 = AK8963::NOT_DATA_READY; 00208 } else if (statusAK8963 == AK8963::NOT_DATA_READY){ 00209 } 00210 00211 00212 for(int i=1000;i<2000;i++){ 00213 u1.write((float)i/20000.0); 00214 u2.write((float)i/20000.0); 00215 u3.write((float)i/20000.0); 00216 u4.write((float)i/20000.0); 00217 wait(0.0001);} 00218 for(int i=2000;i>1000;i--){ 00219 u1.write((float)i/20000.0); 00220 u2.write((float)i/20000.0); 00221 u3.write((float)i/20000.0); 00222 u4.write((float)i/20000.0); 00223 wait(0.0001);}*/ 00224 // Mag_read(); 00225 //for(i=0;i<6;i++) 00226 //pc.printf("%d\t",Rip_chnl_n[i]); 00227 // pc.printf("\n"); 00228 /* pc.printf("pitch = %f\t",pitch); 00229 pc.printf("roll = %f\t",roll); 00230 pc.printf("yaw = %f\t",yaw); 00231 */ 00232 pc.printf("x = %d\t",distx); 00233 pc.printf("y = %d\t",disty); 00234 pc.printf("z = %d\n",distz); 00235 00236 00237 00238 //pc.printf("magx = %d\t",name[0]); 00239 //pc.printf("raw = %d\t",Mag_X_RAW); 00240 //pc.printf("mx = %f\t",Mx); 00241 //wait(0.01); 00242 // 00243 } 00244 } 00245 00246 00247 ///////////////////////////////////////////////////////////////functions////////////////////////////////////////////////////////// 00248 00249 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00250 00251 //////////////////////////////********************************functions for interrupts*****************************//////////////// 00252 void start_counter_1() 00253 { 00254 Rip_chnl_p[0]=timer1.read_us(); 00255 } 00256 void start_counter_2(){ 00257 Rip_chnl_p[1]=timer1.read_us(); 00258 } 00259 void start_counter_3(){ 00260 Rip_chnl_p[2]=timer1.read_us(); 00261 } 00262 void start_counter_4(){ 00263 Rip_chnl_p[3]=timer1.read_us(); 00264 } 00265 void start_counter_5(){ 00266 Rip_chnl_p[4]=timer1.read_us(); 00267 } 00268 void start_counter_6(){ 00269 Rip_chnl_p[5]=timer1.read_us(); 00270 } 00271 void stop_counter_1(){ 00272 uint32_t x; 00273 x=timer1.read_us(); 00274 Rip_chnl_n[0]=x-Rip_chnl_p[0]; 00275 } 00276 void stop_counter_2(){ 00277 uint32_t x; 00278 x=timer1.read_us(); 00279 Rip_chnl_n[1]=x-Rip_chnl_p[1]; 00280 } 00281 void stop_counter_3(){ 00282 uint32_t x; 00283 x=timer1.read_us(); 00284 Rip_chnl_n[2]=x-Rip_chnl_p[2]; 00285 } 00286 void stop_counter_4(){ 00287 uint32_t x; 00288 x=timer1.read_us(); 00289 Rip_chnl_n[3]=x-Rip_chnl_p[3]; 00290 } 00291 void stop_counter_5(){ 00292 uint32_t x; 00293 x=timer1.read_us(); 00294 Rip_chnl_n[4]=x-Rip_chnl_p[4]; 00295 } 00296 void stop_counter_6(){ 00297 uint32_t x; 00298 x=timer1.read_us(); 00299 Rip_chnl_n[5]=x-Rip_chnl_p[5]; 00300 } 00301 00302 void counters_init(){ 00303 ch1.rise(&start_counter_1); 00304 ch2.rise(&start_counter_2); 00305 ch3.rise(&start_counter_3); 00306 ch4.rise(&start_counter_4); 00307 ch5.rise(&start_counter_5); 00308 ch6.rise(&start_counter_6); 00309 ch1.fall(&stop_counter_1); 00310 ch2.fall(&stop_counter_2); 00311 ch3.fall(&stop_counter_3); 00312 ch4.fall(&stop_counter_4); 00313 ch5.fall(&stop_counter_5); 00314 ch6.fall(&stop_counter_6); 00315 } 00316 00317 00318 //////////////////////////////***********************************LIDAR********************************************//////////////// 00319 void dist_x() ///////////*******use this function to get distance it has object of rawserial as argument*******//////////// 00320 { //int distance;//actual distance measurements of LiDAR 00321 int strength;//signal strength of LiDAR 00322 int check;//save check value 00323 int i; 00324 int uartx[9];//save data measured by LiDAR 00325 00326 if (rawx.readable())//check if serial port has data input 00327 { 00328 if(rawx.getc()==0x59)//assess data package frame header 0x59 00329 { 00330 uartx[0]=0x59; 00331 if(rawx.getc()==0x59)//assess data package frame header 0x59 00332 { 00333 uartx[1]=0x59; 00334 for(i=2;i<9;i++)//save data in array 00335 { 00336 uartx[i]=rawx.getc(); 00337 } 00338 check=uartx[0]+uartx[1]+uartx[2]+uartx[3]+uartx[4]+uartx[5]+uartx[6]+uartx[7]; 00339 if(uartx[8]==(check&0xff))//verify the received data as per protocol 00340 { 00341 00342 00343 strength=uartx[4]+uartx[5]*256;//calculate signal strength value 00344 distx=uartx[2]+uartx[3]*256;//calculate distance value in cm 00345 //velocityx = distx - distxo; ////velocity in meters 00346 //distxo = distx; 00347 } 00348 } 00349 } 00350 } 00351 00352 } 00353 void dist_y() ///////////*******use this function to get distance it has object of rawserial as argument*******//////////// 00354 { 00355 int strength;//signal strength of LiDAR 00356 int check;//save check value 00357 int i; 00358 int uarty[9];//save data measured by LiDAR 00359 00360 if (rawy.readable())//check if serial port has data input 00361 { 00362 if(rawy.getc()==0x59)//assess data package frame header 0x59 00363 { 00364 uarty[0]=0x59; 00365 if(rawy.getc()==0x59)//assess data package frame header 0x59 00366 { 00367 uarty[1]=0x59; 00368 for(i=2;i<9;i++)//save data in array 00369 { 00370 uarty[i]=rawy.getc(); 00371 } 00372 check=uarty[0]+uarty[1]+uarty[2]+uarty[3]+uarty[4]+uarty[5]+uarty[6]+uarty[7]; 00373 if(uarty[8]==(check&0xff))//verify the received data as per protocol 00374 { 00375 00376 strength=uarty[4]+uarty[5]*256;//calculate signal strength value 00377 disty=uarty[2]+uarty[3]*256;//calculate distance value 00378 //velocityy = disty - distyo; ////velocity in meters 00379 //distyo = disty; 00380 } 00381 00382 } 00383 } 00384 } 00385 00386 } 00387 void dist_z() ///////////*******use this function to get distance it has object of rawserial as argument*******//////////// 00388 { 00389 int strength;//signal strength of LiDAR 00390 int check;//save check value 00391 int i; 00392 int uartz[9];//save data measured by LiDAR 00393 00394 if (rawz.readable())//check if serial port has data input 00395 { 00396 if(rawz.getc()==0x59)//assess data package frame header 0x59 00397 { 00398 uartz[0]=0x59; 00399 if(rawz.getc()==0x59)//assess data package frame header 0x59 00400 { 00401 uartz[1]=0x59; 00402 for(i=2;i<9;i++)//save data in array 00403 { 00404 uartz[i]=rawz.getc(); 00405 } 00406 check=uartz[0]+uartz[1]+uartz[2]+uartz[3]+uartz[4]+uartz[5]+uartz[6]+uartz[7]; 00407 if(uartz[8]==(check&0xff))//verify the received data as per protocol 00408 { 00409 strength=uartz[4]+uartz[5]*256;//calculate signal strength value 00410 distz=uartz[2]+uartz[3]*256;//calculate distance value 00411 //velocityz = distz - distzo; ////velocity in meters 00412 //distzo = distz; 00413 } 00414 } 00415 } 00416 } 00417 00418 } 00419 //////////////////////////////////**********velocity**************////////////////////////////////////// 00420 00421 00422 /////////////////////////////////***********************mpu****************************/////////////////////////////// 00423 00424 void mpu_init() 00425 { 00426 i2c.frequency(100000); 00427 char data_write[2]; 00428 00429 data_write[0] = MPU6050_PWR_MGMT_1; 00430 data_write[1] = 0x00; 00431 00432 int status = i2c.write(MPU6050_ADDR, data_write, 2, 0); 00433 00434 if (status != 0) 00435 { // Error 00436 while (1) 00437 { 00438 myled = !myled; 00439 wait(0.2); 00440 } 00441 } 00442 data_write[0] = USER_CTRL; 00443 data_write[1] = I2C_MST_EN; 00444 i2c.write(MPU6050_ADDR, data_write, 2, 0); 00445 00446 data_write[0] = 0x37;//INT_PIN_CFG; 00447 data_write[1] = 0x02; 00448 i2c.write(MPU6050_ADDR, data_write, 2, 0); 00449 00450 data_write[0] = MPU6050_SMPLRT_DIV; 00451 data_write[1] = 0x07; 00452 i2c.write(MPU6050_ADDR, data_write, 2, 0); 00453 00454 data_write[0] = MPU6050_ACCEL_CONFIG; 00455 data_write[1] = 0x00; 00456 i2c.write(MPU6050_ADDR, data_write, 2, 0); 00457 data_write[0] = 0x1d; 00458 data_write[1] = 0x06; 00459 i2c.write(MPU6050_ADDR, data_write, 2, 0); 00460 data_write[0] = MPU6050_CONFIG; 00461 data_write[1] = 0x00; 00462 i2c.write(MPU6050_ADDR, data_write, 2, 0); 00463 00464 data_write[0] = MPU6050_GYRO_CONFIG; 00465 data_write[1] = 0x00; 00466 i2c.write(MPU6050_ADDR, data_write, 2, 0); 00467 wait(0.2); 00468 } 00469 00470 void mpu_read(){ 00471 char Rec_Data[14]; 00472 int16_t Accel_X_RAW = 0; 00473 int16_t Accel_Y_RAW = 0; 00474 int16_t Accel_Z_RAW = 0; 00475 int16_t Temp_raw = 0; 00476 int16_t Gyro_X_RAW = 0; 00477 int16_t Gyro_Y_RAW = 0; 00478 int16_t Gyro_Z_RAW = 0; 00479 char data_write[1]; 00480 00481 // Read 1 BYTES of data starting from ACCEL_XOUT_H register 00482 data_write[0] = ACCEL_XOUT_H_REG; 00483 i2c.write(MPU6050_ADDR, data_write, 1, 1); // no stop 00484 i2c.read(MPU6050_ADDR, Rec_Data, 14); 00485 Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1]); 00486 Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3]); 00487 Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5]); 00488 00489 Temp_raw = (int16_t)(Rec_Data[6] << 8 | Rec_Data [7]); 00490 00491 Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data [9]); 00492 Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data [11]); 00493 Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data [13]); 00494 /*** convert the RAW values into acceleration in 'g' 00495 we have to divide according to the Full scale value set in FS_SEL 00496 I have configured FS_SEL = 0. So I am dividing by 16384.0 00497 for more details check ACCEL_CONFIG Register ****/ 00498 00499 Ax = Accel_X_RAW/16384.0; 00500 Ay = Accel_Y_RAW/16384.0; 00501 Az = Accel_Z_RAW/16384.0; 00502 00503 Gx = Gyro_X_RAW/131.0; 00504 Gy = Gyro_Y_RAW/131.0; 00505 Gz = Gyro_Z_RAW/131.0; 00506 00507 } 00508 00509 00510 void get_angle(){ 00511 00512 mpu_read(); 00513 dt2=timer2.read_ms(); ////////****very imp self note check the order of dt1 and dt2*********/////////// 00514 dt=dt2-dt1; 00515 dt=dt/1000; 00516 00517 roll_acc = (atan2((Ay),sqrt(pow((Ax),2) + pow((Az),2))))*(90*7/11); 00518 pitch_acc = ((atan2((-Ax),sqrt(pow((Ay),2) + pow((Az),2))))*(90*7/11)); 00519 00520 /* COMPLIMENTARY FILTER*/ 00521 pitch = Alfa_comp*(pitch + Gy*dt) + (1-Alfa_comp)*pitch_acc ; 00522 00523 roll = Alfa_comp*(roll + Gx*dt) + (1-Alfa_comp)*roll_acc ; 00524 if(abs(Gz)>1) 00525 yaw += Gz*dt; 00526 dt1=timer2.read_ms(); 00527 00528 } 00529 void mpu_calibrate(void){ 00530 int i; 00531 for(i=0;i<=3000;i++) 00532 { 00533 //dt2=timer2.read_ms(); 00534 if(i>0){ 00535 get_angle();} 00536 00537 // dt1=timer2.read_ms(); 00538 //dt=dt1-dt2; 00539 00540 avgx+=roll; 00541 avgy+=pitch; 00542 //avgz+=yaw; 00543 } 00544 avgx=avgx/3000; 00545 avgy=avgy/3000; 00546 // avgz=avgz/2000; 00547 } 00548 /*void Mag_init(){ 00549 char raw[3]; 00550 char data_write[2]; 00551 00552 ///////////////////////////////////////// Power down magnetometer/////////////////////////////////////////////////////// 00553 data_write[0] = AK8963_CNTL; 00554 data_write[1] = 0x00; 00555 00556 int status = i2c.write(AK8963_ADDRESS, data_write, 2, 0); 00557 if (status != 0) 00558 { // Error 00559 while (1) 00560 { 00561 myled = !myled; 00562 wait(0.2); 00563 } 00564 } 00565 00566 wait(0.01); 00567 data_write[0] = 0x6B; //////////////////rest magnetometer 00568 data_write[1] = 0x80; 00569 00570 i2c.write(MPU6050_ADDR, data_write, 2, 0); 00571 00572 00573 wait(0.01); 00574 ///////////////////////////////////////// Enter Fuse ROM access mode/////////////////////////////////////////////////// 00575 data_write[0] = AK8963_CNTL; 00576 data_write[1] = 0x0F; 00577 i2c.write(AK8963_ADDRESS, data_write, 2, 0); 00578 00579 wait(0.01); 00580 ///////////////////////////////////////// Read the x-, y-, and z-axis calibration values////////////////////////////// 00581 data_write[0] = AK8963_ASAX; 00582 i2c.write(AK8963_ADDRESS, data_write, 1, 0); 00583 i2c.read(AK8963_ADDRESS, raw, 3); 00584 // Return x-axis sensitivity adjustment values, etc. 00585 H_senx = (float)(raw[0] - 128)/256.0f + 1.0f; 00586 H_seny = (float)(raw[1] - 128)/256.0f + 1.0f; 00587 H_senz = (float)(raw[2] - 128)/256.0f + 1.0f; 00588 00589 data_write[0] = AK8963_CNTL; 00590 data_write[1] = 0x16; 00591 i2c.write(AK8963_ADDRESS, data_write, 2, 0); 00592 wait(0.01); 00593 data_write[0] = 0x0B; 00594 data_write[1] = 0x01; 00595 i2c.write(AK8963_ADDRESS, data_write, 2, 0); 00596 wait(0.01); 00597 // Configure the magnetometer for continuous read and highest resolution 00598 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, 00599 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates 00600 00601 } 00602 void Mag_read(){ 00603 00604 char data_write[1]; 00605 char x[1]; 00606 char rawData[7]; 00607 /////////////read name/////////////////////////////////// 00608 data_write[0] = WHO_AM_I_AK8963; 00609 i2c.write(AK8963_ADDRESS, data_write, 1, 0); 00610 i2c.read(AK8963_ADDRESS, name, 1); 00611 00612 ////////////////////////////////////// 00613 data_write[0] = AK8963_ST1; 00614 i2c.write(AK8963_ADDRESS, data_write, 1, 0); 00615 i2c.read(AK8963_ADDRESS, x, 1); 00616 00617 if(x[0] & 0x01) { // wait for magnetometer data ready bit to be set 00618 data_write[0] = AK8963_XOUT_L;// Read the six raw data and ST2 registers sequentially into data array 00619 i2c.write(AK8963_ADDRESS, data_write, 1, 1); 00620 i2c.read(AK8963_ADDRESS, rawData, 7); 00621 00622 uint8_t c = rawData[6]; // End data read by reading ST2 register 00623 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data 00624 Mag_X_RAW = (int16_t)((rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value 00625 Mag_Y_RAW = (int16_t)((rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian 00626 Mag_Z_RAW = (int16_t)((rawData[5] << 8) | rawData[4]) ; 00627 } 00628 } 00629 Mx = Mag_X_RAW*H_senx; 00630 My = Mag_Y_RAW*H_seny; 00631 Mz = Mag_Z_RAW*H_senz; 00632 }*/
Generated on Wed Jul 13 2022 23:35:41 by
