Diff: AHRS.cpp

Revision:

19:42ea6dd68185

Parent:

17:f9eed26536d9

Child:

20:1182bc29c195

--- a/AHRS.cpp	Mon Sep 23 14:23:11 2019 +0000
+++ b/AHRS.cpp	Wed Oct 02 15:30:15 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, bool calib) :  imu(PC_9, PA_8, 0xD6, 0x3C), RPY_filter(TS), Mahony_filter(TS), thread(osPriorityBelowNormal, 4096)
+AHRS::AHRS(uint8_t filtertype, float TS, bool calib) :  imu(PC_9, PA_8, 0xD6, 0x3C), ekf(TS), Mahony_filter(TS), thread(osPriorityBelowNormal, 4096)
 {
     /* setup storage */
     float gyro[3], accel[3], magnet[3];
@@ -62,20 +62,25 @@
     // calibration for acc and mag needs to be redone on real copter pes board with power on and spinning motors (no propellers)
 
     /* accel */
-    raw_ax2ax.setup( 1.0032920, accel[0]);  // use gain and offset here
-    raw_ay2ay.setup(-0.9854960, accel[1]);  // y-axis reversed // was -1,0.0
-    raw_az2az.setup( 1.0113879,     0.0f);  // do not remove gravity!
+    raw_ax2ax.setup( 1.0f, accel[0]);  // use gain and offset here
+    raw_ay2ay.setup(-1.0f, accel[1]);  // y-axis reversed // was -1,0.0
+    raw_az2az.setup( 1.0,  0.0f);  // do not remove gravity!
 
     /* magnet */
-    raw_mx2mx.setup( 0.9708198f, -0.1047768f);
-    raw_my2my.setup( 1.0038431f,  0.2345627f);
-    raw_mz2mz.setup(-1.0261139f,  0.0650053f);
+    /* Caibration on 25.9.2019 QK2 delivers:
+    A=[ 0.9904   0   0; 0.0605    1.0158  0;0.0397   -0.0199    0.9938]
+    b0= [0.4160    0.1587   -0.0998], see T:\T-IMS-IndNav\01_Technisches\09_PES_Board_IMU\meas_mag_aFMA\calib_mag_QK2.m
+    */
+    raw_mx2mx.setup( 0.9904f,0.4160f);
+    raw_my2my.setup( 1.0158f, 0.1587f);
+    raw_mz2mz.setup(-0.9938f, -0.0998f);
     this->magnet_cal_0[0] = raw_mx2mx(magnet[0]);
     this->magnet_cal_0[1] = raw_my2my(magnet[1]);
     this->magnet_cal_0[2] = raw_mz2mz(magnet[2]);
 
     local_time = 0.0;
-
+    
+    this->filtertype = filtertype;
     // the thread starts
     thread.start(callback(this, &AHRS::update));
     ticker.attach(callback(this, &AHRS::sendSignal), TS);
@@ -86,45 +91,11 @@
 
 void AHRS::update(void)
 {
-    while(1) {
-
-        thread.signal_wait(signal);
-        imu.readGyro();
-        imu.readAccel();
-        imu.readMag();
-        matrix measurement(6, 1, 0.0f);
-        //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));
-        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);
-
-        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);
-        my_logger.data_vector[3] = raw_ax2ax(imu.accX);
-        my_logger.data_vector[4] = raw_ay2ay(imu.accY);
-        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);
-
-        // pmic, 23.09.2019
-        // dono about logger extension, please adapt
-        // my_logger.data_vector[10] = raw_mx2mx(imu.magX);
-        // my_logger.data_vector[11] = raw_my2my(imu.magY);
-        // my_logger.data_vector[12] = raw_mz2mz(imu.magZ);
+    if(filtertype !=1)
+        Mahony_filter.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_acc[2]);
+    else
+        ekf.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_acc[2]);
 
     }       // while(1) (the thread)
 
 }
-
-void AHRS::sendSignal()
-{
-    thread.signal_set(signal);
-}
\ No newline at end of file