Diff: AHRS.cpp

Revision:

28:21dfb161c67c

Parent:

26:1da7c6204775

--- a/AHRS.cpp	Tue Jan 14 14:24:03 2020 +0000
+++ b/AHRS.cpp	Mon Jan 20 12:41:13 2020 +0000
@@ -4,16 +4,22 @@
 
 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, I2C &i2c) : ekf(TS), Mahony_filter(TS), cf_yaw(1.0f,TS), ekf_rp(TS), ekf_rpy(TS), imu(i2c), dout3(PA_10)
+#if _BMI088 
+    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)
+#else
+    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)
+#endif
 {
     /* setup storage */
     this->Ts = TS;
     this->filtertype = filtertype;
     #if _LSM9DS
         imu_setup_LSM9DS(calib);
-    #else
+    #else 
         imu_setup_BMX055(calib);
+        #if _BMI088
+            imu_setup_BMI088(calib);
+        #endif
     #endif
 }
 // -----------------------------------------------------------------------------
@@ -127,7 +133,7 @@
             magnet[0] += imu.magX;
             magnet[1] += imu.magY;
             magnet[2] += imu.magZ;
-            wait_ms(10);
+            wait_ms(5);
         }
         /* calculate mean value */
         for(uint16_t i = 0; i < 3; i++) {
@@ -144,7 +150,7 @@
 
     /* 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_ay2ay.setup( 1.0f, accel[1]);  // 
     raw_az2az.setup( 1.0,  0.0f);  // do not remove gravity!
 
 // ========== ACHTUNG HIER BMX055 SENSOR
@@ -166,17 +172,63 @@
 // 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;
+    printf("initialized BMX055\r\n");
     
     // the thread starts
 
 }
+//------------------------------------------------------------------------------
+#if _BMI088 
+void AHRS::imu_setup_BMI088(bool calib){
+    float gyro[3], accel[3];
+    for(uint8_t i = 0; i < 3; i++)  
+        {
+        gyro[i]   = 0.0f;
+        accel[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++) {
+            imu2.readGyro();
+            imu2.readAccel();
+            gyro[0]   += imu2.gyroX;
+            gyro[1]   += imu2.gyroY;
+            gyro[2]   += imu2.gyroZ;
+            accel[0]  += imu2.accX;
+            accel[1]  += imu2.accY;
+            accel[2]  += imu2.accZ;
+            wait_ms(5);
+        }
+        /* calculate mean value */
+        for(uint16_t i = 0; i < 3; i++) {
+            gyro[i] /= (float)N;
+            accel[i] /= (float)N;
+            }
+    }
+// ========== ACHTUNG HIER BMI088 SENSOR
+    /* gyro */
+    raw_gx2gx88.setup( 1.0, gyro[0]);
+    raw_gy2gy88.setup( 1.0, gyro[1]);  // 
+    raw_gz2gz88.setup( 1.0, gyro[2]);
+
+    /* accel */
+    raw_ax2ax88.setup( 1.0f, accel[0]);  // use gain and offset here
+    raw_ay2ay88.setup( 1.0f, accel[1]);  // 
+    raw_az2az88.setup( 1.0,  0.0f);  // do not remove gravity!
+    printf("initialized BMI088\r\n");
+}
+#endif
 
 AHRS::~AHRS() {}
 
 void AHRS::update(void)
 {   
-    //dout3.write(1);
+    //
     switch(filtertype){
         case 1:
             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]);
@@ -234,13 +286,12 @@
         }
 }
 void AHRS::read_imu_sensors(void){
-    mutex.lock();
-    dout3.write(1);
+//    mutex.lock();
+dout1.write(1);
     imu.readGyro();
     imu.readAccel();
     imu.readMag();
-    dout3.write(0);
-    mutex.unlock();
+dout1.write(0);
     data.sens_gyr[0] = raw_gx2gx(imu.gyroX);
     data.sens_gyr[1] = raw_gy2gy(imu.gyroY);
     data.sens_gyr[2] = raw_gz2gz(imu.gyroZ);
@@ -250,6 +301,17 @@
     data.sens_mag[0] = raw_mx2mx(imu.magX);
     data.sens_mag[1] = raw_my2my(imu.magY);
     data.sens_mag[2] = raw_mz2mz(imu.magZ);
+//    mutex.unlock();
+#if _BMI088 
+    imu2.readGyro();
+    imu2.readAccel();
+    data.sens_gyr[3] = raw_gx2gx88(imu2.gyroX);
+    data.sens_gyr[4] = raw_gy2gy88(imu2.gyroY);
+    data.sens_gyr[5] = raw_gz2gz88(imu2.gyroZ);
+    data.sens_acc[3] = raw_ax2ax88(imu2.accX);
+    data.sens_acc[4] = raw_ay2ay88(imu2.accY);
+    data.sens_acc[5] = raw_az2az88(imu2.accZ);
+#endif
 }
 // ------------------- start controllers ----------------
 void AHRS::start_loop(void){