AHRS.cpp

00001 #include "AHRS.h"
00002 
00003 #define PI 3.141592653589793
00004 
00005 using namespace std;
00006 
00007 #if _BMI088 
00008     AHRS::AHRS(uint8_t filtertype, float TS, bool calib, I2C &i2c) : ekf(TS), Mahony_filter(TS), cf_yaw(1.0f,TS), ekf_rp(TS), ekf_rpy(TS), imu(i2c), imu2(i2c), dout1(PA_10)
00009 #else
00010     AHRS::AHRS(uint8_t filtertype, float TS, bool calib, I2C &i2c) : ekf(TS), Mahony_filter(TS), cf_yaw(1.0f,TS), ekf_rp(TS), ekf_rpy(TS), imu(i2c), dout1(PA_10)
00011 #endif
00012 {
00013     /* setup storage */
00014     this->Ts = TS;
00015     this->filtertype = filtertype;
00016     #if _LSM9DS
00017         imu_setup_LSM9DS(calib);
00018     #else 
00019         imu_setup_BMX055(calib);
00020         #if _BMI088
00021             imu_setup_BMI088(calib);
00022         #endif
00023     #endif
00024 }
00025 // -----------------------------------------------------------------------------
00026 void AHRS::imu_setup_LSM9DS(bool calib){
00027     float gyro[3], accel[3], magnet[3];
00028     for(uint8_t i = 0; i < 3; i++)  
00029         {
00030         gyro[i]   = 0.0f;
00031         accel[i]  = 0.0f;
00032         magnet[i] = 0.0f;
00033         }
00034 
00035     /* inform user if imu is not responsive */
00036     while (!imu.begin()) 
00037         {
00038         wait(1);
00039         printf("Failed to communicate with LSM9DS1.\r\n");
00040         }
00041 
00042     /* take mean value of N samples */
00043     uint8_t N = 200;
00044     if(calib) 
00045         {
00046         wait_ms(500);
00047         /* read and cumsum data */
00048         for(uint16_t i = 0; i < N; i++) {
00049             imu.readGyro();
00050             imu.readAccel();
00051             imu.readMag();
00052             gyro[0]   += imu.gyroX;
00053             gyro[1]   += imu.gyroY;
00054             gyro[2]   += imu.gyroZ;
00055             accel[0]  += imu.accX;
00056             accel[1]  += imu.accY;
00057             accel[2]  += imu.accZ;
00058             magnet[0] += imu.magX;
00059             magnet[1] += imu.magY;
00060             magnet[2] += imu.magZ;
00061             wait_ms(10);
00062         }
00063         /* calculate mean value */
00064         for(uint16_t i = 0; i < 3; i++) {
00065             gyro[i] /= (float)N;
00066             accel[i] /= (float)N;
00067             magnet[i] /= (float)N;
00068             }
00069         printf("Correct gyro: %1.5f %1.5f %1.5f accel: %1.5f %1.5f %1.5f magnet: %1.5f %1.5f %1.5f\r\n", gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnet[0], magnet[1], magnet[2]);
00070     }
00071 
00072     /* gyro */
00073     // ========== ACHTUNG HIER LSM9DS SENSOR
00074     raw_gx2gx.setup( 1.0, gyro[0]);
00075     raw_gy2gy.setup(-1.0, gyro[1]);  // y-axis reversed (lefthanded system)
00076     raw_gz2gz.setup( 1.0, gyro[2]);
00077 
00078     // pmic, 23.09.2019
00079     // calibration for acc and mag needs to be redone on real copter pes board with power on and spinning motors (no propellers)
00080     /* accel */
00081     raw_ax2ax.setup( 1.0f, accel[0]);  // use gain and offset here
00082     raw_ay2ay.setup(-1.0f, accel[1]);  // y-axis reversed // was -1,0.0
00083     raw_az2az.setup( 1.0,  0.0f);  // do not remove gravity!
00084 // ========== ACHTUNG HIER LSM9DS SENSOR
00085     /* magnet */
00086     /* Caibration on 21.10.2019 QK2 delivers:
00087     A=[ 0.9904   0   0; 0.0605    1.0158  0;0.0397   -0.0199    0.9938]
00088     b0= [0.4160    0.1587   -0.0998], //see T:\T-IMS-IndNav\01_Technisches\09_PES_Board_IMU\meas_mag_aFMA\calib_magn_Okt19_aFMA.m
00089     */
00090     raw_mx2mx.setup( 0.9837f,0.252f);
00091     raw_my2my.setup( 1.0298f,0.1675f);
00092     raw_mz2mz.setup(-0.9864f,-0.1392f);     // achtung, bei gain(3) Vorzeichen drehen, b0 beibehalten!!!
00093 //    raw_mx2mx.setup( 1.0f,0.0f);
00094 //    raw_my2my.setup( 1.0f,0.0f);
00095 //    raw_mz2mz.setup( 1.0f,0.0f);     // achtung, bei b0(3) Vorzeichen drehen!!!
00096     
00097     this->magnet_cal_0[0] = raw_mx2mx(magnet[0]);
00098     this->magnet_cal_0[1] = raw_my2my(magnet[1]);
00099     this->magnet_cal_0[2] = raw_mz2mz(magnet[2]);
00100 // set EKF_RPY magnet values now:
00101     ekf_rpy.set_m0(this->magnet_cal_0[0],this->magnet_cal_0[1],this->magnet_cal_0[2]);
00102     local_time = 0.0;
00103     
00104     // the thread starts
00105 
00106 }
00107 //------------------------------------------------------------------------------
00108 void AHRS::imu_setup_BMX055(bool calib){
00109     float gyro[3], accel[3], magnet[3];
00110     for(uint8_t i = 0; i < 3; i++)  
00111         {
00112         gyro[i]   = 0.0f;
00113         accel[i]  = 0.0f;
00114         magnet[i] = 0.0f;
00115         }
00116 
00117     /* take mean value of N samples */
00118     uint8_t N = 200;
00119     if(calib) 
00120         {
00121         wait_ms(500);
00122         /* read and cumsum data */
00123         for(uint16_t i = 0; i < N; i++) {
00124             imu.readGyro();
00125             imu.readAccel();
00126             imu.readMag();
00127             gyro[0]   += imu.gyroX;
00128             gyro[1]   += imu.gyroY;
00129             gyro[2]   += imu.gyroZ;
00130             accel[0]  += imu.accX;
00131             accel[1]  += imu.accY;
00132             accel[2]  += imu.accZ;
00133             magnet[0] += imu.magX;
00134             magnet[1] += imu.magY;
00135             magnet[2] += imu.magZ;
00136             wait_ms(5);
00137         }
00138         /* calculate mean value */
00139         for(uint16_t i = 0; i < 3; i++) {
00140             gyro[i] /= (float)N;
00141             accel[i] /= (float)N;
00142             magnet[i] /= (float)N;
00143             }
00144     }
00145 // ========== ACHTUNG HIER BMX055 SENSOR
00146     /* gyro */
00147     raw_gx2gx.setup( 1.0, gyro[0]);
00148     raw_gy2gy.setup( 1.0, gyro[1]);  // y-axis reversed (lefthanded system)
00149     raw_gz2gz.setup( 1.0, gyro[2]);
00150 
00151     /* accel */
00152     raw_ax2ax.setup( 1.0f, accel[0]);  // use gain and offset here
00153     raw_ay2ay.setup( 1.0f, accel[1]);  // 
00154     raw_az2az.setup( 1.0,  0.0f);  // do not remove gravity!
00155 
00156 // ========== ACHTUNG HIER BMX055 SENSOR
00157     /* magnet */
00158     // diag(A0) =  0.8047    0.8447    0.8474
00159     // diag(A0) = 0.7485    0.8074    0.8892    //10.1.2020
00160     // b0       = -0.1195    0.2009   -0.3133, calibration at desk 19.12.2019
00161     // b0       = 0.2656    0.4564    0.9155    // 10.1.20
00162     //  1.0150    1.0843    1.1814, 0.2826    0.4353    0.9392   aFMA 10.1.2020
00163     raw_mx2mx.setup(1.0150f,0.2826f);
00164     raw_my2my.setup(1.0843f,0.4353f);
00165     raw_mz2mz.setup(1.1814f,0.9392f);      // */
00166 /*    raw_mx2mx.setup( 1.0f,0.0f);
00167     raw_my2my.setup( 1.0f,0.0f);
00168     raw_mz2mz.setup( 1.0f,0.0f); //        */
00169     this->magnet_cal_0[0] = raw_mx2mx(magnet[0]);
00170     this->magnet_cal_0[1] = raw_my2my(magnet[1]);
00171     this->magnet_cal_0[2] = raw_mz2mz(magnet[2]);
00172 // set EKF_RPY magnet values now:
00173     ekf_rpy.set_m0(this->magnet_cal_0[0],this->magnet_cal_0[1],this->magnet_cal_0[2]);
00174     local_time = 0.0;
00175     printf("initialized BMX055\r\n");
00176     
00177     // the thread starts
00178 
00179 }
00180 //------------------------------------------------------------------------------
00181 #if _BMI088 
00182 void AHRS::imu_setup_BMI088(bool calib){
00183     float gyro[3], accel[3];
00184     for(uint8_t i = 0; i < 3; i++)  
00185         {
00186         gyro[i]   = 0.0f;
00187         accel[i]  = 0.0f;
00188         }
00189 
00190     /* take mean value of N samples */
00191     uint8_t N = 200;
00192     if(calib) 
00193         {
00194         wait_ms(500);
00195         /* read and cumsum data */
00196         for(uint16_t i = 0; i < N; i++) {
00197             imu2.readGyro();
00198             imu2.readAccel();
00199             gyro[0]   += imu2.gyroX;
00200             gyro[1]   += imu2.gyroY;
00201             gyro[2]   += imu2.gyroZ;
00202             accel[0]  += imu2.accX;
00203             accel[1]  += imu2.accY;
00204             accel[2]  += imu2.accZ;
00205             wait_ms(5);
00206         }
00207         /* calculate mean value */
00208         for(uint16_t i = 0; i < 3; i++) {
00209             gyro[i] /= (float)N;
00210             accel[i] /= (float)N;
00211             }
00212     }
00213 // ========== ACHTUNG HIER BMI088 SENSOR
00214     /* gyro */
00215     raw_gx2gx88.setup( 1.0, gyro[0]);
00216     raw_gy2gy88.setup( 1.0, gyro[1]);  // 
00217     raw_gz2gz88.setup( 1.0, gyro[2]);
00218 
00219     /* accel */
00220     raw_ax2ax88.setup( 1.0f, accel[0]);  // use gain and offset here
00221     raw_ay2ay88.setup( 1.0f, accel[1]);  // 
00222     raw_az2az88.setup( 1.0,  0.0f);  // do not remove gravity!
00223     printf("initialized BMI088\r\n");
00224 }
00225 #endif
00226 
00227 AHRS::~AHRS() {}
00228 
00229 void AHRS::update(void)
00230 {   
00231     //
00232     switch(filtertype){
00233         case 1:
00234             ekf.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_acc[2]);
00235             cf_yaw.update(data.sens_gyr[2],data.sens_mag[0],data.sens_mag[1],data.sens_mag[2]);
00236             data.est_RPY[0] = ekf.get_est_state(0);
00237             data.est_RPY[1] = ekf.get_est_state(1);
00238             //data.est_Vxyz[0] = ekf.get_est_state(4);
00239             //data.est_Vxyz[1] = ekf.get_est_state(5);
00240             break;
00241         case 2:
00242             Mahony_filter.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_acc[2],0.0f,0.0f,0.0f);
00243             data.est_RPY[0] = Mahony_filter.getRollRadians();
00244             data.est_RPY[1] = Mahony_filter.getPitchRadians();
00245             break;
00246         case 3:
00247             ekf_rp.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_acc[0],data.sens_acc[1]);
00248 
00249             cf_yaw.update(data.sens_gyr[2],data.sens_mag[0],data.sens_mag[1],data.sens_mag[2]);
00250             data.est_RPY[0] = ekf_rp.get_est_state(0);
00251             data.est_RPY[1] = ekf_rp.get_est_state(1);
00252             break;
00253         case 4:
00254             ekf_rpy.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_mag[0],data.sens_mag[1]);
00255             data.est_RPY[0] = ekf_rpy.get_est_state(0);
00256             data.est_RPY[1] = ekf_rpy.get_est_state(1);
00257             data.est_RPY[2] = ekf_rpy.get_est_state(2);
00258             data.est_RPY[3] = ekf_rpy.get_est_state(3);
00259             data.est_RPY[4] = ekf_rpy.get_est_state(4);
00260             data.est_RPY[5] = ekf_rpy.get_est_state(5);
00261             data.est_RPY[6] = ekf_rpy.get_est_state(6);
00262             data.est_RPY[7] = ekf_rpy.get_est_state(7);
00263             break;
00264         }
00265  //   dout3.write(0);
00266 }
00267 void AHRS::update_as_thread(void)
00268 {   
00269     while(1)
00270         {
00271         thread.signal_wait(signal);
00272 //        dout3.write(1);
00273         if(filtertype !=1)
00274             {
00275             Mahony_filter.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_acc[2],0.0f,0.0f,0.0f);
00276             data.est_RPY[0] = Mahony_filter.getRollRadians();
00277             data.est_RPY[1] = Mahony_filter.getPitchRadians();
00278             }
00279         else
00280             {
00281             ekf.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_acc[2]);
00282             data.est_RPY[0] = ekf.get_est_state(0);
00283             data.est_RPY[1] = ekf.get_est_state(1);
00284             }
00285 //            dout3.write(0);
00286         }
00287 }
00288 void AHRS::read_imu_sensors(void){
00289 //    mutex.lock();
00290 dout1.write(1);
00291     imu.readGyro();
00292     imu.readAccel();
00293     imu.readMag();
00294 dout1.write(0);
00295     data.sens_gyr[0] = raw_gx2gx(imu.gyroX);
00296     data.sens_gyr[1] = raw_gy2gy(imu.gyroY);
00297     data.sens_gyr[2] = raw_gz2gz(imu.gyroZ);
00298     data.sens_acc[0] = raw_ax2ax(imu.accX);
00299     data.sens_acc[1] = raw_ay2ay(imu.accY);
00300     data.sens_acc[2] = raw_az2az(imu.accZ);
00301     data.sens_mag[0] = raw_mx2mx(imu.magX);
00302     data.sens_mag[1] = raw_my2my(imu.magY);
00303     data.sens_mag[2] = raw_mz2mz(imu.magZ);
00304 //    mutex.unlock();
00305 #if _BMI088 
00306     imu2.readGyro();
00307     imu2.readAccel();
00308     data.sens_gyr[3] = raw_gx2gx88(imu2.gyroX);
00309     data.sens_gyr[4] = raw_gy2gy88(imu2.gyroY);
00310     data.sens_gyr[5] = raw_gz2gz88(imu2.gyroZ);
00311     data.sens_acc[3] = raw_ax2ax88(imu2.accX);
00312     data.sens_acc[4] = raw_ay2ay88(imu2.accY);
00313     data.sens_acc[5] = raw_az2az88(imu2.accZ);
00314 #endif
00315 }
00316 // ------------------- start controllers ----------------
00317 void AHRS::start_loop(void){
00318     thread.start(callback(this, &AHRS::update_as_thread));
00319     ticker.attach(callback(this, &AHRS::sendSignal), Ts);
00320 }
00321 
00322 // this is for realtime OS
00323 void AHRS::sendSignal() {
00324     thread.signal_set(signal);
00325 }