fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 05:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mag_fusion: mag heading and 3d modes are mutually exclusive

Browse Source 
There is no need to check if 3d fusion is active when in mag heading
fusion mode since this is impossible
This commit is contained in:
bresch
2020-08-20 16:25:41 +02:00
committed by Paul Riseborough
parent 9797e4d28f
commit 264c8c4e86
1 changed files with 2 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+2 -14
View File
@@ -727,13 +727,7 @@ void Ekf::fuseHeading()
if (_control_status.flags.mag_hdg) {
// Rotate the measurements into earth frame using the zero yaw angle
const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
@@ -784,13 +778,7 @@ void Ekf::fuseHeading()
// rotate the magnetometer measurements into earth frame using a zero yaw angle
const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();