Diff: main.cpp

Revision:

3:788eecfd5ae9

Parent:

2:3881894aaff5

diff -r 3881894aaff5 -r 788eecfd5ae9 main.cpp
--- a/main.cpp	Thu Aug 06 12:25:49 2015 +0000
+++ b/main.cpp	Wed Jul 19 07:59:55 2017 +0000
@@ -33,39 +33,91 @@
 */
 
 #include "mbed.h"
-#include "HMC5883L.h"
-#include "MPU6050.h"
+#include "QMC5883L.h"
+#include "icm20602_i2c.h"
 #include "ledControl.h"
 
 Serial pc(USBTX,USBRX);    
-MPU6050 mpu6050;           
-HMC5883L hmc5883l;    
+ICM20602 icm20602;           
+QMC5883L qmc5883l;    
 Ticker toggler1;
 Ticker filter;   
 Ticker compass;        
 
+enum detect_orientation_return
+{
+    DETECT_ORIENTATION_UPSIDE_DOWN,
+    DETECT_ORIENTATION_RIGHTSIDE_UP,
+    DETECT_ORIENTATION_LEFT,
+    DETECT_ORIENTATION_RIGHT,
+    DETECT_ORIENTATION_TAIL_DOWN,
+    DETECT_ORIENTATION_NOSE_DOWN,
+    DETECT_ORIENTATION_ERROR
+};
+
 void toggle_led1();
 void toggle_led2();
-void compFilter();
-void tiltCompensatedAngle();
+//void compFilter();
+void read_IMU_data();
+void read_MAG_data();
+void IMU_calibration();
+void IMU_compensate();
+char detect_orientation(float acc_dete[3], float gyro_dete[3]);
+//void tiltCompensatedAngle();
 
 float pitchAngle = 0;
 float rollAngle = 0;
 float yawAngle = 0;
+float acc[3];
+float gyro[3];
+float acc_comp[3];
+float gyro_comp[3];
+float acc_off[3];
+float gyro_off[3];
+float mag[3];
+float IMU_tmp;
+float mag_tmp;
+const char cali = 'c';
+const char coll = ' ';
+char orientation;
 
 int main() 
 {
+    
     pc.baud(9600);                                 // baud rate: 9600
-    mpu6050.whoAmI();                              // Communication test: WHO_AM_I register reading 
-    mpu6050.calibrate(accelBias,gyroBias);         // Calibrate MPU6050 and load biases into bias registers
-    mpu6050.init();                                // Initialize the sensor
-    hmc5883l.init();
-    filter.attach(&compFilter,    0.005);              // Call the complementaryFilter func.  every 5 ms (200 Hz sampling period)
-    compass.attach(&tiltCompensatedAngle, 0.015);      // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period)
+    icm20602.whoAmI();                              // Communication test: WHO_AM_I register reading 
+    qmc5883l.ChipID();
+//    icm20602.calibrate(accelBias,gyroBias);         // Calibrate MPU6050 and load biases into bias registers
+//    filter.attach(&compFilter,    2);              // Call the complementaryFilter func.  every 5 ms (200 Hz sampling period)
+//    compass.attach(&tiltCompensatedAngle, 0.015);      // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period)  
+    
+    icm20602.init();
+    qmc5883l.init();
     while(1) 
-    {
-        pc.printf("%.1f,%.1f,%.1f\r\n",rollAngle,pitchAngle,yawAngle);  // send data to matlab
-        wait_ms(40);
+    {       
+//        pc.putc(pc.getc());
+//        pc.printf("%c\n",pc.getc());
+        //if(pc.getc()==cali)
+//        {   
+//            pc.printf("calibrate IMU!\n");
+//            IMU_calibration();
+////            break;
+//        }else{  
+//        while(1)  {
+            read_IMU_data();
+            read_MAG_data(); 
+            IMU_compensate();          
+    //        pc.printf("%.5f,%.5f\r\n",aRes,gRes);  // send data to matlab
+    pc.printf("original data:%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2],IMU_tmp,mag[0],mag[1],mag[2],mag_tmp); 
+           // pc.printf("original acc:%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]);  // send data to matlab
+//            pc.printf("original gyro:%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]);  // send data to matlab
+//            pc.printf("compensated acc:%.5f,%.5f,%.5f\r\n",acc_comp[0],acc_comp[1],acc_comp[2]);  // send data to matlab
+//            pc.printf("compensated gyro:%.5f,%.5f,%.5f\r\n",gyro_comp[0],gyro_comp[1],gyro_comp[2]);  // send data to matlab
+    //        pc.printf("mag:%.5f,%.5f,%.5f\r\n",mag[0],mag[1],mag[2]);  // send data to matlab
+           // wait_ms(400);
+//            ledToggle(3);
+        //    }
+//        }
     }
 }
 
@@ -73,40 +125,158 @@
 void toggle_led2() {ledToggle(2);}
 
 /* This function is created to avoid address error that caused from Ticker.attach func */ 
-void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
-
+//void compFilter() {icm20602.complementaryFilter(&pitchAngle, &rollAngle);}
+//void read_imu() {icm20602.read_IMU_data(&acc[3], &gyro[3]);}
 /* Tilt compensated compass data */
 // Works well for tilt in +/- 50 deg range
-void tiltCompensatedAngle() 
-{   
-    float mag_Data[3], Xh, Yh;
-    hmc5883l.readMagData(mag_Data);
-    
-    Xh = mag_Data[0] * cos(rollAngle*PI/180) - mag_Data[2] * sin(rollAngle*PI/180) ;
+//void tiltCompensatedAngle() 
+//{   
+//    float mag_Data[3], Xh, Yh;
+//    hmc5883l.readMagData(mag_Data);
+//    
+//    Xh = mag_Data[0] * cos(rollAngle*PI/180) - mag_Data[2] * sin(rollAngle*PI/180) ;
+//    
+//    Yh = mag_Data[0] * sin(pitchAngle*PI/180) * sin(rollAngle*PI/180) + 
+//    mag_Data[1] * cos(pitchAngle*PI/180) -
+//    mag_Data[2] * sin(pitchAngle*PI/180) * cos(rollAngle*PI/180) ;
+//    
+//    /* Calculate the compensated heading angle */
+//    double heading = atan2(Yh, Xh);
+//    
+//    // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
+//    // declinationAngle can be found here http://www.magnetic-declination.com/
+//    // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
+//    float declinationAngle = 0.096;
+//    heading += declinationAngle;
+//    
+//    // Correct for when signs are reversed.
+//    if(heading < 0)
+//        heading += 2*PI;
+//    
+//    // Check for wrap due to addition of declination.
+//    if(heading > 2*PI)
+//        heading -= 2*PI;
+//    
+//    /* Convert radian to degrees */
+//    heading = heading * 180 / PI;  
+//    
+//    yawAngle = heading; 
+//}
+void read_IMU_data() {
     
-    Yh = mag_Data[0] * sin(pitchAngle*PI/180) * sin(rollAngle*PI/180) + 
-    mag_Data[1] * cos(pitchAngle*PI/180) -
-    mag_Data[2] * sin(pitchAngle*PI/180) * cos(rollAngle*PI/180) ;
+    acc[0] = icm20602.getAccXvalue() * IMU_ONE_G * aRes;
+    acc[1] = icm20602.getAccYvalue() * IMU_ONE_G * aRes;
+    acc[2] = icm20602.getAccZvalue() * IMU_ONE_G * aRes;
+    gyro[0] = icm20602.getGyrXvalue() * gRes;
+    gyro[1] = icm20602.getGyrYvalue() * gRes;
+    gyro[2] = icm20602.getGyrZvalue() * gRes;
+    IMU_tmp = icm20602.getIMUTemp() / 262.144;//326.8;
+//    pc.printf("acc0:%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]);  // send data to matlab
+}
+void read_MAG_data() {
     
-    /* Calculate the compensated heading angle */
-    double heading = atan2(Yh, Xh);
+    mag[0] = qmc5883l.getMagXvalue() * mRes;
+    mag[1] = qmc5883l.getMagYvalue() * mRes;
+    mag[2] = qmc5883l.getMagZvalue() * mRes;
+    mag_tmp = qmc5883l.getMagTemp() /262.144;// 100;
+//    pc.printf("mag:%.5f,%.5f,%.5f\r\n",mag[0],mag[1],mag[2]);  // send data to matlab
+}
+
+void IMU_calibration(){
+    
+    int ii,jj,i=1,counter=0,timer=10;
+    float acc_cal[6][3];
+    float gyro_cal[6][3];
+    //float acc_sum[3];
+//    float gyro_sum[3];
+    pc.printf("put the IMU still!\n");
     
-    // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
-    // declinationAngle can be found here http://www.magnetic-declination.com/
-    // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
-    float declinationAngle = 0.096;
-    heading += declinationAngle;
-    
-    // Correct for when signs are reversed.
-    if(heading < 0)
-        heading += 2*PI;
+    for(i=0;i<6;i++){
+
+        if(pc.getc()==coll){           
+            pc.printf("%dst side start\n",i+1); 
+            float acc_sum[3]={0};
+            float gyro_sum[3]={0};
+            while(counter < timer){
+                read_IMU_data();
+                acc_sum[0] += acc[0];
+                acc_sum[1] += acc[1];
+                acc_sum[2] += acc[2];
+                
+                gyro_sum[0] += gyro[0];
+                gyro_sum[1] += gyro[1];
+                gyro_sum[2] += gyro[2];
+                
+                counter++;
+                 //pc.printf(" acc :%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]);  // send data to matlab
+//    pc.printf(" gyro :%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]);  // send data to matlab
+//    pc.printf(" acc sum:%.5f,%.5f,%.5f\r\n",acc_sum[0],acc_sum[1],acc_sum[2]);  // send data to matlab
+//    pc.printf(" gyro sum:%.5f,%.5f,%.5f\r\n",gyro_sum[0],gyro_sum[1],gyro_sum[2]);  // send data to matlab
+//                pc.printf("%d----\n",counter); 
+            }
+//pc.printf(" acc :%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]);  // send data to matlab
+//    pc.printf(" gyro :%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]);  // send data to matlab            
+            acc_cal[i][0] = acc_sum[0]/timer;
+            acc_cal[i][1] = acc_sum[1]/timer;
+            acc_cal[i][2] = acc_sum[2]/timer;
+            
+            gyro_cal[i][0] = gyro_sum[0]/timer;
+            gyro_cal[i][1] = gyro_sum[1]/timer;
+            gyro_cal[i][2] = gyro_sum[2]/timer;
+            pc.printf("%d--counter\n",counter); 
+            counter = 0;
+            //acc_sum[0] =0.0;acc_sum[1] =0.0;acc_sum[2] =0.0;
+//            gyro_sum[0] = 0.0;gyro_sum[1] = 0.0;gyro_sum[2] = 0.0;
+//pc.printf(" acc sum:%.5f,%.5f,%.5f\r\n",acc_cal[i][0],acc_cal[i][1],acc_cal[i][2]);  // send data to matlab
+//    pc.printf(" gyro sum:%.5f,%.5f,%.5f\r\n",gyro_cal[i][0],gyro_cal[i][1],gyro_cal[i][2]);  // send data to matlab
+            orientation = detect_orientation(acc_cal[i],gyro_cal[i]);
+        
+            pc.printf("%cst side completed\n",orientation); 
+        }
+    }
+    //calculate the offset and scale
+    for(ii = 0;ii<3;ii++)
+    {
+        for(jj=0;jj<6;jj++){
+            acc_off[ii] += acc_cal[jj][ii];
+            gyro_off[ii] += gyro_cal[jj][ii];
+        }
+       // pc.printf(" acc offset sum:%.5f,%.5f,%.5f\r\n",acc_off[0],acc_off[1],acc_off[2]);  // send data to matlab
+//        pc.printf(" gyro offset sum:%.5f,%.5f,%.5f\r\n",gyro_off[0],gyro_off[1],gyro_off[2]);  // send data to matlab
+        acc_off[ii]/=6;
+        gyro_off[ii]/=6;
+    } 
+    //pc.printf(" acc offset:%.5f,%.5f,%.5f\r\n",acc_off[0],acc_off[1],acc_off[2]);  // send data to matlab
+//    pc.printf(" gyro offset:%.5f,%.5f,%.5f\r\n",gyro_off[0],gyro_off[1],gyro_off[2]);  // send data to matlab
+    return;     
+}
+
+char detect_orientation(float acc_dete[3], float gyro_dete[3])
+{
+    if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2] - IMU_ONE_G) < 1.0){
+        return  DETECT_ORIENTATION_UPSIDE_DOWN;//[0 0 g]   
+    }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2] + IMU_ONE_G) < 1.0){
+        return  DETECT_ORIENTATION_RIGHTSIDE_UP;//[0 0 -g]   
+    }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1] - IMU_ONE_G) < 1.0 && fabsf(acc_dete[2]) < 1.0){
+        return  DETECT_ORIENTATION_LEFT;//[0 g 0]   
+    }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1] + IMU_ONE_G) < 1.0 && fabsf(acc_dete[2]) < 1.0){
+        return  DETECT_ORIENTATION_RIGHT;//[0 -g 0]   
+    }else if(fabsf(acc_dete[0] - IMU_ONE_G) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2]) < 1.0){
+        return  DETECT_ORIENTATION_TAIL_DOWN;//[g 0 0]   
+    }else if(fabsf(acc_dete[0] + IMU_ONE_G) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2]) < 1.0){
+        return  DETECT_ORIENTATION_NOSE_DOWN;//[-g 0 0]   
+    }else{
+        return DETECT_ORIENTATION_ERROR;
+    }
+}
+
+void IMU_compensate()
+{
+    int k;
+    for(k=0;k<3;k++)
+    {
+        acc_comp[k] = acc[k] - acc_off[k];
+        gyro_comp[k] = gyro[k] - gyro_off[k];
+    }
     
-    // Check for wrap due to addition of declination.
-    if(heading > 2*PI)
-        heading -= 2*PI;
-    
-    /* Convert radian to degrees */
-    heading = heading * 180 / PI;  
-    
-    yawAngle = heading; 
-}
+}
\ No newline at end of file