Diff: AHRS.cpp

Revision:

9:644266463f5f

Parent:

8:51062bb877f0

Child:

10:fd4e2436b311

diff -r 51062bb877f0 -r 644266463f5f AHRS.cpp
--- a/AHRS.cpp	Fri Jul 05 06:56:19 2019 +0000
+++ b/AHRS.cpp	Fri Jul 12 15:19:12 2019 +0000
@@ -8,40 +8,57 @@
 using namespace std;
 
 //OLD: AHRS::AHRS(uint8_t filtertype,float TS) : RPY_filter(TS), csAG(PA_15),csM(PD_2), spi(PC_12, PC_11, PC_10), imu(&spi, &csM, &csAG), thread(osPriorityBelowNormal, 4096){  
-AHRS::AHRS(uint8_t filtertype,float TS) :  imu(PC_9, PA_8, 0xD6, 0x3C), RPY_filter(TS), Mahony_filter(TS), thread(osPriorityBelowNormal, 4096){  
-       
-    thread.start(callback(this, &AHRS::update));
-    ticker.attach(callback(this, &AHRS::sendSignal), TS);
-/*    LinearCharacteristics raw_ax2ax(40.0/65536.0,-418.0);       // use gain and offset here
-    LinearCharacteristics raw_ay2ay(-40.0/65536.0,-307.0);      // y-axis reversed
-    LinearCharacteristics raw_az2az(-16350.0,16350,-10.0, 10.0);
-    LinearCharacteristics raw_gx2gx(-32767,32768,-500*PI/180.0, 500*PI/180.0);
-    LinearCharacteristics raw_gy2gy(-32767,32768, 500*PI/180.0,-500*PI/180.0);  // y-axis reversed (lefthanded system)
-    LinearCharacteristics raw_gz2gz(-32767,32768,-500*PI/180.0, 500*PI/180.0);
-    LinearCharacteristics int2magx( -32767,32768,100.0,-100.0);     // x-axis reversed
-    LinearCharacteristics int2magy( -32767,32768,100.0,-100.0);     // y-axis reversed*/
-    raw_ax2ax.setup( 1.0,0.15);       // use gain and offset here
-    raw_ay2ay.setup(-1.0,-0.31);      // y-axis reversed // was -1,0.0
+AHRS::AHRS(uint8_t filtertype,float TS,bool calib) :  imu(PC_9, PA_8, 0xD6, 0x3C), RPY_filter(TS), Mahony_filter(TS), thread(osPriorityBelowNormal, 4096){  
+    float g[3];
+    g[0]=0.0;g[1]=0.0;g[2]=0.0;   
+    float a[3];
+    a[0]=0.0;a[1]=0.0;a[2]=0.0; 
+    uint8_t N = 100;  
+    while (!imu.begin()) {
+        wait(1);
+        printf("Failed to communicate with LSM9DS1.\r\n");
+    }
+    if(calib){
+        wait_ms(500);
+        for(int k=0;k < N;k++)
+            {
+                imu.readGyro();
+                imu.readAccel();
+                g[0]+= imu.gyroX;
+                g[1]+= imu.gyroY;
+                g[2]+= imu.gyroZ;
+                a[0]+= imu.accX;
+                a[1]+= imu.accY;
+                a[2]+= imu.accZ;
+                wait_ms(10);
+                }
+        for(int k = 0;k<3;k++){
+            g[k] /= (float)N;
+            a[k] /= (float)N;
+            }
+        printf("Correct g: %1.5f %1.5f %1.5f a: %1.5f %1.5f %1.5f\r\n",g[0],g[1],g[2],a[0],a[1],a[2]); 
+        }
+    raw_gx2gx.setup( 1.0,g[0]);
+    raw_gy2gy.setup(-1.0,g[1]);  // y-axis reversed (lefthanded system)
+    raw_gz2gz.setup( 1.0,g[2]);
+    raw_ax2ax.setup( 1.0,a[0]);       // use gain and offset here
+    raw_ay2ay.setup(-1.0,a[1]);      // y-axis reversed // was -1,0.0
     raw_az2az.setup( 1.0,0.0);
-    raw_gx2gx.setup( 1.0,0.0);
-    raw_gy2gy.setup(-1.0,0.0);  // y-axis reversed (lefthanded system)
-    raw_gz2gz.setup( 1.0,0.0);
     int2magx.setup(  -1.0,0.0);     // x-axis reversed
     int2magy.setup(  -1.0,0.0);     // y-axis reversed
     int2magz.setup(  1.0,0.0);
     local_time = 0.0;
-    //
+// The Thread starts
+    thread.start(callback(this, &AHRS::update));
+    ticker.attach(callback(this, &AHRS::sendSignal), TS);
+
     
-    //matrix measurement(6,1,0.0);
-    while (!imu.begin()) {
-        wait(1);
-        printf("Failed to communicate with LSM9DS1.\r\n");
-    }
 
 }
 
 AHRS::~AHRS() {}
 
+
 void AHRS::update(void)
 {
     while(1){   
@@ -52,9 +69,9 @@
         imu.readGyro();
         matrix measurement(6,1,0.0);
         //Perform filter update
-        Mahony_filter.update(raw_gx2gx(imu.gyroX), raw_gy2gy(imu.gyroY),  raw_gz2gz(imu.gyroZ) ,
-                      raw_ax2ax(imu.accX), raw_ay2ay(imu.accY),  raw_az2az(imu.accZ),
-                      int2magx(imu.magX), int2magy(imu.magY),  int2magz(imu.magZ));
+        //Mahony_filter.update(raw_gx2gx(imu.gyroX), raw_gy2gy(imu.gyroY),  raw_gz2gz(imu.gyroZ) ,
+        //              raw_ax2ax(imu.accX), raw_ay2ay(imu.accY),  raw_az2az(imu.accZ),
+        //              int2magx(imu.magX), int2magy(imu.magY),  int2magz(imu.magZ));
         measurement.put_entry(0,0,raw_gx2gx(imu.gyroX));
         measurement.put_entry(1,0,raw_gy2gy(imu.gyroY));
         measurement.put_entry(3,0,raw_ax2ax(imu.accX));
@@ -69,14 +86,8 @@
         my_logger.data_vector[5] = raw_az2az(imu.accZ);
         my_logger.data_vector[6] = RPY_filter.get_est_state(0);
         my_logger.data_vector[7] = RPY_filter.get_est_state(1);
-        my_logger.data_vector[8] = Mahony_filter.getRollRadians();
-        my_logger.data_vector[9] = Mahony_filter.getPitchRadians();
-        
-        //printf("R %1.5f P %1.5f gx: %1.5f gy: %1.5f\r\n",my_logger.data_vector[6],my_logger.data_vector[7],my_logger.data_vector[0],my_logger.data_vector[1]); 
-        //printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\r\n",my_logger.data_vector[6],my_logger.data_vector[7],raw_gx2gx(imu.gyroX),raw_gy2gy(imu.gyroY),raw_gz2gz(imu.gyroZ),raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); 
-        //printf("ax: %1.4f ay: %1.4f az: %1.4f\r\n",raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); 
-        //printf("gx: %1.4f gy: %1.4f gz: %1.4f\r\n",raw_gx2gx(imu.gyroX),raw_gy2gy(imu.gyroY),raw_gz2gz(imu.gyroZ)); 
-        //printf("ax: %1.4f ay: %1.4f az: %1.4f\r\n",raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); 
+        my_logger.data_vector[8] = RPY_filter.get_est_state(4);
+        my_logger.data_vector[9] = RPY_filter.get_est_state(5);
         }       // while(1) (the thread)
         
 }