altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

AHRS.h@25:fe14dbcef82d, 2020-01-06 (annotated)

Committer:
altb2
Date:
Mon Jan 06 12:49:38 2020 +0000
Revision:
25:fe14dbcef82d
Parent:
22:495a419e474c
Child:
28:21dfb161c67c
implement BMX055

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