altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

AHRS.h@19:42ea6dd68185, 2019-10-02 (annotated)

Committer:
altb2
Date:
Wed Oct 02 15:30:15 2019 +0000
Revision:
19:42ea6dd68185
Parent:
17:f9eed26536d9
Child:
20:1182bc29c195
major changes for QK3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 3:6811c0ce95f6 1 #include "Mahony.h"
altb 3:6811c0ce95f6 2 #include "MadgwickAHRS.h"
altb 3:6811c0ce95f6 3 #include "LinearCharacteristics.h"
altb2 4:3c21fb0c9e84 4 #include "LSM9DS1_i2c.h"
altb 3:6811c0ce95f6 5 #include "Signal.h"
altb2 19:42ea6dd68185 6 #include "EKF.h"
altb2 4:3c21fb0c9e84 7 #include "matrix.h"
altb2 6:5824bd96b6cf 8 #include "data_logger.h"
altb2 4:3c21fb0c9e84 9
altb2 6:5824bd96b6cf 10 extern data_logger my_logger;
altb 3:6811c0ce95f6 11
pmic 17:f9eed26536d9 12 class AHRS
pmic 17:f9eed26536d9 13 {
altb 3:6811c0ce95f6 14 public:
pmic 17:f9eed26536d9 15
altb2 9:644266463f5f 16 AHRS(uint8_t,float,bool);
pmic 17:f9eed26536d9 17
altb 3:6811c0ce95f6 18 virtual ~AHRS();
pmic 17:f9eed26536d9 19
pmic 17:f9eed26536d9 20 float getRoll(uint8_t ft)
pmic 17:f9eed26536d9 21 {
altb2 8:51062bb877f0 22 if(ft ==1)
pmic 17:f9eed26536d9 23 return RPY_filter.get_est_state(0);
altb2 8:51062bb877f0 24 else
pmic 17:f9eed26536d9 25 return Mahony_filter.getRollRadians();
altb 3:6811c0ce95f6 26 }
pmic 17:f9eed26536d9 27
pmic 17:f9eed26536d9 28 float getPitch(uint8_t ft)
pmic 17:f9eed26536d9 29 {
altb2 8:51062bb877f0 30 if(ft ==1)
pmic 17:f9eed26536d9 31 return RPY_filter.get_est_state(1);
altb2 8:51062bb877f0 32 else
pmic 17:f9eed26536d9 33 return Mahony_filter.getPitchRadians();
altb 3:6811c0ce95f6 34 }
pmic 17:f9eed26536d9 35
pmic 17:f9eed26536d9 36 float getYaw()
pmic 17:f9eed26536d9 37 {
altb2 6:5824bd96b6cf 38 return 0.0;
altb 3:6811c0ce95f6 39 }
pmic 17:f9eed26536d9 40
altb 3:6811c0ce95f6 41 LSM9DS1 imu;
altb2 8:51062bb877f0 42 Mahony Mahony_filter;
altb2 19:42ea6dd68185 43 EKF RPY_filter;
altb 3:6811c0ce95f6 44 LinearCharacteristics raw_gx2gx;
altb 3:6811c0ce95f6 45 LinearCharacteristics raw_gy2gy;
altb 3:6811c0ce95f6 46 LinearCharacteristics raw_gz2gz;
altb2 4:3c21fb0c9e84 47 LinearCharacteristics raw_ax2ax;
altb2 4:3c21fb0c9e84 48 LinearCharacteristics raw_ay2ay;
altb2 4:3c21fb0c9e84 49 LinearCharacteristics raw_az2az;
pmic 17:f9eed26536d9 50 LinearCharacteristics raw_mx2mx;
pmic 17:f9eed26536d9 51 LinearCharacteristics raw_my2my;
pmic 17:f9eed26536d9 52 LinearCharacteristics raw_mz2mz;
altb2 9:644266463f5f 53 float xyzUS[3];
altb2 10:fd4e2436b311 54 float xyzAS[3];
altb2 9:644266463f5f 55 float rxryrzUS[3];
altb2 10:fd4e2436b311 56 float v_xyzOF[2];
altb2 10:fd4e2436b311 57 float xyzOF[3];
altb2 11:b010622c0748 58 float xyzLIDAR[3];
pmic 17:f9eed26536d9 59 float magnet_cal_0[3];
pmic 17:f9eed26536d9 60
altb 3:6811c0ce95f6 61 private:
pmic 17:f9eed26536d9 62
altb 3:6811c0ce95f6 63 Signal signal;
altb 3:6811c0ce95f6 64 Thread thread;
altb 3:6811c0ce95f6 65 Ticker ticker;
pmic 17:f9eed26536d9 66 Mutex mutex; // mutex to lock critical sections
altb 3:6811c0ce95f6 67 void sendSignal();
altb2 7:bfde7bd5fe31 68 float local_time;
altb 3:6811c0ce95f6 69 void update();
altb2 19:42ea6dd68185 70 uint8_t filtertype;
altb 3:6811c0ce95f6 71 };