altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

AHRS.h@28:21dfb161c67c, 2020-01-20 (annotated)

Committer:
altb2
Date:
Mon Jan 20 12:41:13 2020 +0000
Revision:
28:21dfb161c67c
Parent:
25:fe14dbcef82d 
read also BMI088, only for datalogging

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