AHRS.h

00001 #ifndef AHRS_H_
00002 #define AHRS_H_
00003 
00004 
00005 #include "Mahony.h"
00006 #include "MadgwickAHRS.h"
00007 #include "LinearCharacteristics.h"
00008 #include "LSM9DS1_i2c.h"
00009 #include "BMX055.h"
00010 #include "BMI088.h"
00011 
00012 #include "Signal.h"
00013 #include "EKF.h"
00014 #include "EKF_RP.h"
00015 #include "EKF_RPY.h"
00016 #include "CF_Yaw.h"
00017 #include "matrix.h"
00018 #include "Data_Logger.h"
00019 #include "Read_Xtern_Sensors.h"
00020 
00021 extern DATA_Xchange data;
00022 
00023 class AHRS
00024 {
00025 public:
00026 
00027     AHRS(uint8_t,float,bool,I2C &);
00028     
00029     virtual ~AHRS();
00030 
00031     float getRoll(uint8_t ft)
00032     {   
00033         switch(ft){
00034             case 1:
00035                 return ekf.get_est_state(0);
00036             case 2:
00037                 return Mahony_filter.getRollRadians();
00038             case 3:
00039                 return ekf_rp.get_est_state(0);
00040             case 4:
00041                 return ekf_rpy.get_est_state(0);
00042             default:
00043             return 0.0;
00044         }
00045     }
00046     float getPitch(uint8_t ft)
00047         {   
00048         switch (ft){
00049             case 1:
00050                 return ekf.get_est_state(1);
00051             case 2:
00052                 return Mahony_filter.getPitchRadians();
00053             case 3:
00054                 return ekf_rp.get_est_state(1);
00055             case 4:
00056                 return ekf_rpy.get_est_state(1);
00057             default:
00058             return 0.0;
00059         }
00060     }
00061     float getYaw(uint8_t ft)
00062         {   
00063         switch (ft){
00064             case 4:
00065                 return ekf_rpy.get_est_state(2);
00066             default:
00067                 return 0.0;
00068         }
00069     }
00070     #if _LSM9DS
00071         LSM9DS1 imu;
00072     #else
00073         BMX055 imu;
00074         #if _BMI088    
00075             BMI088 imu2;
00076         #endif
00077     #endif
00078     Mahony Mahony_filter;
00079     EKF ekf;
00080     EKF_RP ekf_rp;
00081     EKF_RPY ekf_rpy;
00082     CF_Yaw cf_yaw;
00083   
00084     LinearCharacteristics raw_gx2gx;
00085     LinearCharacteristics raw_gy2gy;
00086     LinearCharacteristics raw_gz2gz;
00087     LinearCharacteristics raw_ax2ax;
00088     LinearCharacteristics raw_ay2ay;
00089     LinearCharacteristics raw_az2az;
00090     LinearCharacteristics raw_gx2gx88;
00091     LinearCharacteristics raw_gy2gy88;
00092     LinearCharacteristics raw_gz2gz88;
00093     LinearCharacteristics raw_ax2ax88;
00094     LinearCharacteristics raw_ay2ay88;
00095     LinearCharacteristics raw_az2az88;
00096     LinearCharacteristics raw_mx2mx;
00097     LinearCharacteristics raw_my2my;
00098     LinearCharacteristics raw_mz2mz;
00099     void read_imu_sensors(void);
00100     float xyzUS[3];
00101     float xyzAS[3];
00102     float rxryrzUS[3];
00103     float v_xyzOF[2];
00104     float xyzOF[3];
00105     float xyzLIDAR[3];
00106     float xyzRS[3];
00107     float magnet_cal_0[3];
00108     void update();
00109     void update_as_thread(void);
00110     void start_loop(void);
00111 private:
00112     float local_time;
00113     uint8_t filtertype;
00114     Signal signal;
00115     Thread thread;
00116     Ticker ticker;
00117     void sendSignal();
00118     void imu_setup_LSM9DS(bool calib);
00119     void imu_setup_BMX055(bool calib);
00120     void imu_setup_BMI088(bool calib);
00121     float Ts;
00122     DigitalOut dout1;
00123     Mutex mutex;
00124 };
00125 
00126 #endif