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  }*/