Diff: AHRS.cpp

Revision:

8:51062bb877f0

Parent:

7:bfde7bd5fe31

Child:

9:644266463f5f

--- a/AHRS.cpp	Wed Jun 26 14:19:01 2019 +0000
+++ b/AHRS.cpp	Fri Jul 05 06:56:19 2019 +0000
@@ -8,7 +8,7 @@
 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), 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);
@@ -20,14 +20,14 @@
     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,-.31);       // use gain and offset here
+    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
     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
+    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;
     //
@@ -48,21 +48,19 @@
         thread.signal_wait(signal);
         imu.readAccel();
         //imu.readMag_calibrated();
-        //imu.readMag();
+        imu.readMag();
         imu.readGyro();
         matrix measurement(6,1,0.0);
         //Perform filter update
-        //RPY_filter.update(raw_gx2gx(imu.gx), raw_gy2gy(imu.gy),  raw_gz2gz(imu.gz) ,
-        //              raw_ax2ax(imu.ax), raw_ay2ay(imu.ay),  raw_az2az(imu.az),
-        //              int2magx(imu.mx), int2magy(imu.my),  int2magz(imu.mz));
+        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));
         measurement.put_entry(4,0,raw_ay2ay(imu.accY));
         RPY_filter.loop(&measurement);
-        //measurement.put_entry(0,0,10*sin(local_time));   my_logger.data_vector[8]=10*sin(local_time);    measurement.put_entry(1,0,-5*cos(local_time));    my_logger.data_vector[9]=-5*cos(local_time);   measurement.put_entry(3,0,sin(2*local_time));  measurement.put_entry(4,0,cos(3*local_time));
-        //if(local_time<=2.0) RPY_filter.loop(&measurement); if(local_time==0.0) printf("\r\n"); if(local_time<=0.2){ printf("%1.2f xk: ",local_time); for(int k=0;k<6;k++) printf("%1.5f ",RPY_filter.get_est_state(k)); printf("\r\n");}
-        //local_time += 0.01;
+
         my_logger.data_vector[0] = raw_gx2gx(imu.gyroX);
         my_logger.data_vector[1] = raw_gy2gy(imu.gyroY);
         my_logger.data_vector[2] = raw_gz2gz(imu.gyroZ);
@@ -71,8 +69,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] = RPY_filter.get_est_state(4);
-        my_logger.data_vector[9] = RPY_filter.get_est_state(5);
+        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));