AHRS
Dependencies: Eigen
Diff: AHRS.h
- Revision:
- 21:31e01d3e0143
- Parent:
- 20:1182bc29c195
- Child:
- 22:495a419e474c
diff -r 1182bc29c195 -r 31e01d3e0143 AHRS.h --- 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;