AHRS
Dependencies: Eigen
Diff: AHRS.cpp
- Revision:
- 26:1da7c6204775
- Parent:
- 25:fe14dbcef82d
- Child:
- 28:21dfb161c67c
diff -r fe14dbcef82d -r 1da7c6204775 AHRS.cpp --- 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]);