Diff: AHRS.cpp

Revision:

25:fe14dbcef82d

Parent:

23:71996bfe68eb

Child:

26:1da7c6204775

--- a/AHRS.cpp	Wed Nov 13 11:40:46 2019 +0000
+++ b/AHRS.cpp	Mon Jan 06 12:49:38 2020 +0000
@@ -5,10 +5,18 @@
 using namespace std;
 
 //OLD: AHRS::AHRS(uint8_t filtertype, float TS) : RPY_filter(TS), csAG(PA_15),csM(PD_2), spi(PC_12, PC_11, PC_10), imu(&spi, &csM, &csAG), thread(osPriorityBelowNormal, 4096){
-AHRS::AHRS(uint8_t filtertype, float TS, bool calib) :  imu(PC_9, PA_8, 0xD6, 0x3C), ekf(TS), Mahony_filter(TS), cf_yaw(1.0f,TS), ekf_rp(TS), ekf_rpy(TS)//, dout3(PA_10)
+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), dout3(PA_10)
 {
     /* setup storage */
     this->Ts = TS;
+    this->filtertype = filtertype;
+    #if _LSM9DS
+        imu_setup_LSM9DS(calib);
+    #else
+        imu_setup_BMX055(calib);
+    #endif
+}
+void AHRS::imu_setup_LSM9DS(bool calib){
     float gyro[3], accel[3], magnet[3];
     for(uint8_t i = 0; i < 3; i++)  
         {
@@ -86,10 +94,78 @@
     ekf_rpy.set_m0(this->magnet_cal_0[0],this->magnet_cal_0[1],this->magnet_cal_0[2]);
     local_time = 0.0;
     
-    this->filtertype = filtertype;
     // the thread starts
 
 }
+//------------------------------------------------------------------------------
+void AHRS::imu_setup_BMX055(bool calib){
+    float gyro[3], accel[3], magnet[3];
+    for(uint8_t i = 0; i < 3; i++)  
+        {
+        gyro[i]   = 0.0f;
+        accel[i]  = 0.0f;
+        magnet[i] = 0.0f;
+        }
+
+    /* take mean value of N samples */
+    uint8_t N = 200;
+    if(calib) 
+        {
+        wait_ms(500);
+        /* read and cumsum data */
+        for(uint16_t i = 0; i < N; i++) {
+            imu.readGyro();
+            imu.readAccel();
+            imu.readMag();
+            gyro[0]   += imu.gyroX;
+            gyro[1]   += imu.gyroY;
+            gyro[2]   += imu.gyroZ;
+            accel[0]  += imu.accX;
+            accel[1]  += imu.accY;
+            accel[2]  += imu.accZ;
+            magnet[0] += imu.magX;
+            magnet[1] += imu.magY;
+            magnet[2] += imu.magZ;
+            wait_ms(10);
+        }
+        /* calculate mean value */
+        for(uint16_t i = 0; i < 3; i++) {
+            gyro[i] /= (float)N;
+            accel[i] /= (float)N;
+            magnet[i] /= (float)N;
+            }
+    }
+
+    /* gyro */
+    raw_gx2gx.setup( 1.0, gyro[0]);
+    raw_gy2gy.setup( 1.0, gyro[1]);  // y-axis reversed (lefthanded system)
+    raw_gz2gz.setup( 1.0, gyro[2]);
+
+    /* accel */
+    raw_ax2ax.setup( 1.0f, accel[0]);  // use gain and offset here
+    raw_ay2ay.setup( 1.0f, accel[1]);  // y-axis reversed // was -1,0.0
+    raw_az2az.setup( 1.0,  0.0f);  // do not remove gravity!
+
+    /* magnet */
+    // diag(A0) =  0.8047    0.8447    0.8474
+    // b0       = -0.1195    0.2009   -0.3133, calibration at desk 19.12.2019
+    raw_mx2mx.setup( 0.8047f,-0.1195f);
+    raw_my2my.setup( 0.8447f, 0.2009f);
+    raw_mz2mz.setup( 0.8474f,-0.3133f);     // 
+/*    raw_mx2mx.setup( 1.0f,0.0f);
+    raw_my2my.setup( 1.0f,0.0f);
+    raw_mz2mz.setup( 1.0f,0.0f);         */
+    this->magnet_cal_0[0] = raw_mx2mx(magnet[0]);
+    this->magnet_cal_0[1] = raw_my2my(magnet[1]);
+    this->magnet_cal_0[2] = raw_mz2mz(magnet[2]);
+// set EKF_RPY magnet values now:
+    ekf_rpy.set_m0(this->magnet_cal_0[0],this->magnet_cal_0[1],this->magnet_cal_0[2]);
+    local_time = 0.0;
+    
+    // the thread starts
+
+}
+
 
 AHRS::~AHRS() {}
 
@@ -153,9 +229,13 @@
         }
 }
 void AHRS::read_imu_sensors(void){
+    mutex.lock();
+    dout3.write(1);
     imu.readGyro();
     imu.readAccel();
     imu.readMag();
+    dout3.write(0);
+    mutex.unlock();
     data.sens_gyr[0] = raw_gx2gx(imu.gyroX);
     data.sens_gyr[1] = raw_gy2gy(imu.gyroY);
     data.sens_gyr[2] = raw_gz2gz(imu.gyroZ);