altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

AHRS.h@22:495a419e474c, 2019-10-28 (annotated)

Committer:
altb2
Date:
Mon Oct 28 07:52:43 2019 +0000
Revision:
22:495a419e474c
Parent:
21:31e01d3e0143
Child:
25:fe14dbcef82d
EKF_RPY and EKF_RP from pmic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 20:1182bc29c195 1 #ifndef AHRS_H_
altb2 20:1182bc29c195 2 #define AHRS_H_
altb2 20:1182bc29c195 3
altb2 20:1182bc29c195 4
altb 3:6811c0ce95f6 5 #include "Mahony.h"
altb 3:6811c0ce95f6 6 #include "MadgwickAHRS.h"
altb 3:6811c0ce95f6 7 #include "LinearCharacteristics.h"
altb2 4:3c21fb0c9e84 8 #include "LSM9DS1_i2c.h"
altb 3:6811c0ce95f6 9 #include "Signal.h"
altb2 19:42ea6dd68185 10 #include "EKF.h"
altb2 21:31e01d3e0143 11 #include "EKF_RP.h"
altb2 21:31e01d3e0143 12 #include "EKF_RPY.h"
altb2 22:495a419e474c 13 #include "CF_Yaw.h"
altb2 4:3c21fb0c9e84 14 #include "matrix.h"
altb2 20:1182bc29c195 15 #include "Data_Logger.h"
altb2 20:1182bc29c195 16 #include "Read_Xtern_Sensors.h"
altb2 4:3c21fb0c9e84 17
altb2 20:1182bc29c195 18 extern DATA_Xchange data;
altb 3:6811c0ce95f6 19
pmic 17:f9eed26536d9 20 class AHRS
pmic 17:f9eed26536d9 21 {
altb 3:6811c0ce95f6 22 public:
pmic 17:f9eed26536d9 23
altb2 9:644266463f5f 24 AHRS(uint8_t,float,bool);
pmic 17:f9eed26536d9 25
altb 3:6811c0ce95f6 26 virtual ~AHRS();
pmic 17:f9eed26536d9 27
pmic 17:f9eed26536d9 28 float getRoll(uint8_t ft)
altb2 21:31e01d3e0143 29 {
altb2 21:31e01d3e0143 30 switch(ft){
altb2 21:31e01d3e0143 31 case 1:
altb2 21:31e01d3e0143 32 return ekf.get_est_state(0);
altb2 21:31e01d3e0143 33 case 2:
altb2 21:31e01d3e0143 34 return Mahony_filter.getRollRadians();
altb2 21:31e01d3e0143 35 case 3:
altb2 21:31e01d3e0143 36 return ekf_rp.get_est_state(0);
altb2 21:31e01d3e0143 37 case 4:
altb2 21:31e01d3e0143 38 return ekf_rpy.get_est_state(0);
altb2 21:31e01d3e0143 39 default:
altb2 21:31e01d3e0143 40 return 0.0;
altb2 21:31e01d3e0143 41 }
altb 3:6811c0ce95f6 42 }
pmic 17:f9eed26536d9 43 float getPitch(uint8_t ft)
altb2 21:31e01d3e0143 44 {
altb2 21:31e01d3e0143 45 switch (ft){
altb2 21:31e01d3e0143 46 case 1:
altb2 21:31e01d3e0143 47 return ekf.get_est_state(1);
altb2 21:31e01d3e0143 48 case 2:
altb2 21:31e01d3e0143 49 return Mahony_filter.getPitchRadians();
altb2 21:31e01d3e0143 50 case 3:
altb2 21:31e01d3e0143 51 return ekf_rp.get_est_state(1);
altb2 21:31e01d3e0143 52 case 4:
altb2 21:31e01d3e0143 53 return ekf_rpy.get_est_state(1);
altb2 21:31e01d3e0143 54 default:
altb2 21:31e01d3e0143 55 return 0.0;
altb2 21:31e01d3e0143 56 }
altb 3:6811c0ce95f6 57 }
altb2 21:31e01d3e0143 58 float getYaw(uint8_t ft)
altb2 21:31e01d3e0143 59 {
altb2 21:31e01d3e0143 60 switch (ft){
altb2 21:31e01d3e0143 61 case 4:
altb2 21:31e01d3e0143 62 return ekf_rpy.get_est_state(2);
altb2 21:31e01d3e0143 63 default:
altb2 21:31e01d3e0143 64 return 0.0;
altb2 21:31e01d3e0143 65 }
altb 3:6811c0ce95f6 66 }
pmic 17:f9eed26536d9 67
altb 3:6811c0ce95f6 68 LSM9DS1 imu;
altb2 8:51062bb877f0 69 Mahony Mahony_filter;
altb2 20:1182bc29c195 70 EKF ekf;
altb2 21:31e01d3e0143 71 EKF_RP ekf_rp;
altb2 21:31e01d3e0143 72 EKF_RPY ekf_rpy;
altb2 22:495a419e474c 73 CF_Yaw cf_yaw;
altb2 22:495a419e474c 74
altb 3:6811c0ce95f6 75 LinearCharacteristics raw_gx2gx;
altb 3:6811c0ce95f6 76 LinearCharacteristics raw_gy2gy;
altb 3:6811c0ce95f6 77 LinearCharacteristics raw_gz2gz;
altb2 4:3c21fb0c9e84 78 LinearCharacteristics raw_ax2ax;
altb2 4:3c21fb0c9e84 79 LinearCharacteristics raw_ay2ay;
altb2 4:3c21fb0c9e84 80 LinearCharacteristics raw_az2az;
pmic 17:f9eed26536d9 81 LinearCharacteristics raw_mx2mx;
pmic 17:f9eed26536d9 82 LinearCharacteristics raw_my2my;
pmic 17:f9eed26536d9 83 LinearCharacteristics raw_mz2mz;
altb2 20:1182bc29c195 84 void read_imu_sensors(void);
altb2 9:644266463f5f 85 float xyzUS[3];
altb2 10:fd4e2436b311 86 float xyzAS[3];
altb2 9:644266463f5f 87 float rxryrzUS[3];
altb2 10:fd4e2436b311 88 float v_xyzOF[2];
altb2 10:fd4e2436b311 89 float xyzOF[3];
altb2 11:b010622c0748 90 float xyzLIDAR[3];
pmic 17:f9eed26536d9 91 float magnet_cal_0[3];
altb2 20:1182bc29c195 92 void update();
altb2 20:1182bc29c195 93 void update_as_thread(void);
altb2 20:1182bc29c195 94 void start_loop(void);
altb 3:6811c0ce95f6 95 private:
pmic 17:f9eed26536d9 96
altb2 20:1182bc29c195 97 float local_time;
altb2 20:1182bc29c195 98 uint8_t filtertype;
altb 3:6811c0ce95f6 99 Signal signal;
altb 3:6811c0ce95f6 100 Thread thread;
altb 3:6811c0ce95f6 101 Ticker ticker;
altb 3:6811c0ce95f6 102 void sendSignal();
altb2 20:1182bc29c195 103 float Ts;
altb2 22:495a419e474c 104 // //DigitalOut dout3;
altb2 20:1182bc29c195 105
altb 3:6811c0ce95f6 106 };
altb2 20:1182bc29c195 107
altb2 20:1182bc29c195 108 #endif