Diff: AHRS.cpp

Revision:

20:1182bc29c195

Parent:

19:42ea6dd68185

Child:

21:31e01d3e0143

--- a/AHRS.cpp	Wed Oct 02 15:30:15 2019 +0000
+++ b/AHRS.cpp	Wed Oct 09 13:45:36 2019 +0000
@@ -1,32 +1,36 @@
 #include "AHRS.h"
 #include "Mahony.h"
 #include "MadgwickAHRS.h"
-#include "ekf.h"
+#include "EKF.h"
 
 #define PI 3.141592653589793
 
 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), ekf(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)//, dout3(PA_10)
 {
     /* setup storage */
+    this->Ts = TS;
     float gyro[3], accel[3], magnet[3];
-    for(uint8_t i = 0; i < 3; i++)  {
+    for(uint8_t i = 0; i < 3; i++)  
+        {
         gyro[i]   = 0.0f;
         accel[i]  = 0.0f;
         magnet[i] = 0.0f;
-    }
+        }
 
     /* inform user if imu is not responsive */
-    while (!imu.begin()) {
+    while (!imu.begin()) 
+        {
         wait(1);
         printf("Failed to communicate with LSM9DS1.\r\n");
-    }
+        }
 
     /* take mean value of N samples */
     uint8_t N = 200;
-    if(calib) {
+    if(calib) 
+        {
         wait_ms(500);
         /* read and cumsum data */
         for(uint16_t i = 0; i < N; i++) {
@@ -49,7 +53,7 @@
             gyro[i] /= (float)N;
             accel[i] /= (float)N;
             magnet[i] /= (float)N;
-        }
+            }
         printf("Correct gyro: %1.5f %1.5f %1.5f accel: %1.5f %1.5f %1.5f magnet: %1.5f %1.5f %1.5f\r\n", gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnet[0], magnet[1], magnet[2]);
     }
 
@@ -82,20 +86,70 @@
     
     this->filtertype = filtertype;
     // the thread starts
-    thread.start(callback(this, &AHRS::update));
-    ticker.attach(callback(this, &AHRS::sendSignal), TS);
 
 }
 
 AHRS::~AHRS() {}
 
 void AHRS::update(void)
-{
+{   
+    //dout3.write(1);
     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]);
+        {
+        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],0.0f,0.0f,0.0f);
+        data.est_RPY[0] = Mahony_filter.getRollRadians();
+        data.est_RPY[1] = Mahony_filter.getPitchRadians();
+        }
     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]);
+        data.est_RPY[0] = ekf.get_est_state(0);
+        data.est_RPY[1] = ekf.get_est_state(1);
+        }
+ //   dout3.write(0);
+}
+void AHRS::update_as_thread(void)
+{   
+    while(1)
+        {
+        thread.signal_wait(signal);
+//        dout3.write(1);
+        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],0.0f,0.0f,0.0f);
+            data.est_RPY[0] = Mahony_filter.getRollRadians();
+            data.est_RPY[1] = Mahony_filter.getPitchRadians();
+            }
+        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]);
+            data.est_RPY[0] = ekf.get_est_state(0);
+            data.est_RPY[1] = ekf.get_est_state(1);
+            }
+//            dout3.write(0);
+        }
+}
+void AHRS::read_imu_sensors(void){
+    imu.readGyro();
+    imu.readAccel();
+    imu.readMag();
+    data.sens_gyr[0] = raw_gx2gx(imu.gyroX);
+    data.sens_gyr[1] = raw_gy2gy(imu.gyroY);
+    data.sens_gyr[2] = raw_gz2gz(imu.gyroZ);
+    data.sens_acc[0] = raw_ax2ax(imu.accX);
+    data.sens_acc[1] = raw_ay2ay(imu.accY);
+    data.sens_acc[2] = raw_az2az(imu.accZ);
+    data.sens_mag[0] = raw_mx2mx(imu.magX);
+    data.sens_mag[1] = raw_my2my(imu.magY);
+    data.sens_mag[2] = raw_mz2mz(imu.magZ);
+}
+// ------------------- start controllers ----------------
+void AHRS::start_loop(void){
+    thread.start(callback(this, &AHRS::update_as_thread));
+    ticker.attach(callback(this, &AHRS::sendSignal), Ts);
+}
 
-    }       // while(1) (the thread)
-
-}
+// this is for realtime OS
+void AHRS::sendSignal() {
+    thread.signal_set(signal);
+}
\ No newline at end of file