Diff: AHRS.cpp

Revision:

26:1da7c6204775

Parent:

25:fe14dbcef82d

Child:

28:21dfb161c67c

--- a/AHRS.cpp	Mon Jan 06 12:49:38 2020 +0000
+++ b/AHRS.cpp	Fri Jan 10 16:00:47 2020 +0000
@@ -16,6 +16,7 @@
         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++)  
@@ -63,18 +64,18 @@
     }
 
     /* gyro */
+    // ========== ACHTUNG HIER LSM9DS SENSOR
     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]);
 
     // pmic, 23.09.2019
     // calibration for acc and mag needs to be redone on real copter pes board with power on and spinning motors (no propellers)
-
     /* 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!
-
+// ========== ACHTUNG HIER LSM9DS SENSOR
     /* magnet */
     /* Caibration on 21.10.2019 QK2 delivers:
     A=[ 0.9904   0   0; 0.0605    1.0158  0;0.0397   -0.0199    0.9938]
@@ -135,7 +136,7 @@
             magnet[i] /= (float)N;
             }
     }
-
+// ========== ACHTUNG HIER BMX055 SENSOR
     /* gyro */
     raw_gx2gx.setup( 1.0, gyro[0]);
     raw_gy2gy.setup( 1.0, gyro[1]);  // y-axis reversed (lefthanded system)
@@ -146,15 +147,19 @@
     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!
 
+// ========== ACHTUNG HIER BMX055 SENSOR
     /* magnet */
     // diag(A0) =  0.8047    0.8447    0.8474
+    // diag(A0) = 0.7485    0.8074    0.8892    //10.1.2020
     // 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);     // 
+    // b0       = 0.2656    0.4564    0.9155    // 10.1.20
+    //  1.0150    1.0843    1.1814, 0.2826    0.4353    0.9392   aFMA 10.1.2020
+    raw_mx2mx.setup(1.0150f,0.2826f);
+    raw_my2my.setup(1.0843f,0.4353f);
+    raw_mz2mz.setup(1.1814f,0.9392f);      // */
 /*    raw_mx2mx.setup( 1.0f,0.0f);
     raw_my2my.setup( 1.0f,0.0f);
-    raw_mz2mz.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]);