altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

AHRS.h@21:31e01d3e0143, 2019-10-21 (annotated)

Committer:
altb2
Date:
Mon Oct 21 17:14:27 2019 +0000
Revision:
21:31e01d3e0143
Parent:
20:1182bc29c195
Child:
22:495a419e474c
Added EKF_RP, EKF_RPY from pmic, also EIgen Library

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