altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

AHRS.h@20:1182bc29c195, 2019-10-09 (annotated)

Committer:
altb2
Date:
Wed Oct 09 13:45:36 2019 +0000
Revision:
20:1182bc29c195
Parent:
19:42ea6dd68185
Child:
21:31e01d3e0143
Major changes for new structure of IndNav_QK3

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 4:3c21fb0c9e84 11 #include "matrix.h"
altb2 20:1182bc29c195 12 #include "Data_Logger.h"
altb2 20:1182bc29c195 13 #include "Read_Xtern_Sensors.h"
altb2 4:3c21fb0c9e84 14
altb2 20:1182bc29c195 15 extern DATA_Xchange data;
altb 3:6811c0ce95f6 16
pmic 17:f9eed26536d9 17 class AHRS
pmic 17:f9eed26536d9 18 {
altb 3:6811c0ce95f6 19 public:
pmic 17:f9eed26536d9 20
altb2 9:644266463f5f 21 AHRS(uint8_t,float,bool);
pmic 17:f9eed26536d9 22
altb 3:6811c0ce95f6 23 virtual ~AHRS();
pmic 17:f9eed26536d9 24
pmic 17:f9eed26536d9 25 float getRoll(uint8_t ft)
pmic 17:f9eed26536d9 26 {
altb2 8:51062bb877f0 27 if(ft ==1)
altb2 20:1182bc29c195 28 return ekf.get_est_state(0);
altb2 8:51062bb877f0 29 else
pmic 17:f9eed26536d9 30 return Mahony_filter.getRollRadians();
altb 3:6811c0ce95f6 31 }
pmic 17:f9eed26536d9 32
pmic 17:f9eed26536d9 33 float getPitch(uint8_t ft)
pmic 17:f9eed26536d9 34 {
altb2 8:51062bb877f0 35 if(ft ==1)
altb2 20:1182bc29c195 36 return ekf.get_est_state(1);
altb2 8:51062bb877f0 37 else
pmic 17:f9eed26536d9 38 return Mahony_filter.getPitchRadians();
altb 3:6811c0ce95f6 39 }
pmic 17:f9eed26536d9 40
pmic 17:f9eed26536d9 41 float getYaw()
pmic 17:f9eed26536d9 42 {
altb2 6:5824bd96b6cf 43 return 0.0;
altb 3:6811c0ce95f6 44 }
pmic 17:f9eed26536d9 45
altb 3:6811c0ce95f6 46 LSM9DS1 imu;
altb2 8:51062bb877f0 47 Mahony Mahony_filter;
altb2 20:1182bc29c195 48 EKF ekf;
altb 3:6811c0ce95f6 49 LinearCharacteristics raw_gx2gx;
altb 3:6811c0ce95f6 50 LinearCharacteristics raw_gy2gy;
altb 3:6811c0ce95f6 51 LinearCharacteristics raw_gz2gz;
altb2 4:3c21fb0c9e84 52 LinearCharacteristics raw_ax2ax;
altb2 4:3c21fb0c9e84 53 LinearCharacteristics raw_ay2ay;
altb2 4:3c21fb0c9e84 54 LinearCharacteristics raw_az2az;
pmic 17:f9eed26536d9 55 LinearCharacteristics raw_mx2mx;
pmic 17:f9eed26536d9 56 LinearCharacteristics raw_my2my;
pmic 17:f9eed26536d9 57 LinearCharacteristics raw_mz2mz;
altb2 20:1182bc29c195 58 void read_imu_sensors(void);
altb2 9:644266463f5f 59 float xyzUS[3];
altb2 10:fd4e2436b311 60 float xyzAS[3];
altb2 9:644266463f5f 61 float rxryrzUS[3];
altb2 10:fd4e2436b311 62 float v_xyzOF[2];
altb2 10:fd4e2436b311 63 float xyzOF[3];
altb2 11:b010622c0748 64 float xyzLIDAR[3];
pmic 17:f9eed26536d9 65 float magnet_cal_0[3];
altb2 20:1182bc29c195 66 void update();
altb2 20:1182bc29c195 67 void update_as_thread(void);
altb2 20:1182bc29c195 68 void start_loop(void);
altb 3:6811c0ce95f6 69 private:
pmic 17:f9eed26536d9 70
altb2 20:1182bc29c195 71 float local_time;
altb2 20:1182bc29c195 72 uint8_t filtertype;
altb 3:6811c0ce95f6 73 Signal signal;
altb 3:6811c0ce95f6 74 Thread thread;
altb 3:6811c0ce95f6 75 Ticker ticker;
altb 3:6811c0ce95f6 76 void sendSignal();
altb2 20:1182bc29c195 77 float Ts;
altb2 20:1182bc29c195 78 //DigitalOut dout3;
altb2 20:1182bc29c195 79
altb 3:6811c0ce95f6 80 };
altb2 20:1182bc29c195 81
altb2 20:1182bc29c195 82 #endif