Diff: AHRS.h

Revision:

21:31e01d3e0143

Parent:

20:1182bc29c195

Child:

22:495a419e474c

--- a/AHRS.h	Wed Oct 09 13:45:36 2019 +0000
+++ b/AHRS.h	Mon Oct 21 17:14:27 2019 +0000
@@ -8,6 +8,8 @@
 #include "LSM9DS1_i2c.h"
 #include "Signal.h"
 #include "EKF.h"
+#include "EKF_RP.h"
+#include "EKF_RPY.h"
 #include "matrix.h"
 #include "Data_Logger.h"
 #include "Read_Xtern_Sensors.h"
@@ -23,29 +25,50 @@
     virtual ~AHRS();
 
     float getRoll(uint8_t ft)
-    {
-        if(ft ==1)
-            return ekf.get_est_state(0);
-        else
-            return Mahony_filter.getRollRadians();
+    {   
+        switch(ft){
+            case 1:
+                return ekf.get_est_state(0);
+            case 2:
+                return Mahony_filter.getRollRadians();
+            case 3:
+                return ekf_rp.get_est_state(0);
+            case 4:
+                return ekf_rpy.get_est_state(0);
+            default:
+            return 0.0;
+        }
     }
-
     float getPitch(uint8_t ft)
-    {
-        if(ft ==1)
-            return ekf.get_est_state(1);
-        else
-            return Mahony_filter.getPitchRadians();
+        {   
+        switch (ft){
+            case 1:
+                return ekf.get_est_state(1);
+            case 2:
+                return Mahony_filter.getPitchRadians();
+            case 3:
+                return ekf_rp.get_est_state(1);
+            case 4:
+                return ekf_rpy.get_est_state(1);
+            default:
+            return 0.0;
+        }
     }
-
-    float getYaw()
-    {
-        return 0.0;
+    float getYaw(uint8_t ft)
+        {   
+        switch (ft){
+            case 4:
+                return ekf_rpy.get_est_state(2);
+            default:
+                return 0.0;
+        }
     }
 
     LSM9DS1 imu;
     Mahony Mahony_filter;
     EKF ekf;
+    EKF_RP ekf_rp;
+    EKF_RPY ekf_rpy;
     LinearCharacteristics raw_gx2gx;
     LinearCharacteristics raw_gy2gy;
     LinearCharacteristics raw_gz2gz;