fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 16:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Use heading fusion as default for startup and ground operation

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-01 15:35:17 +11:00
parent 57d4064d7b
commit 7c83ac4b6d
1 changed files with 9 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+9 -16
View File
@@ -105,9 +105,9 @@ void Ekf::controlFusionModes()
// or the more accurate 3-axis fusion
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
if (!_control_status.flags.armed) {
// always use 2D mag fusion for initial startup
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
// use heading fusion for initial startup
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
} else {
@@ -123,24 +123,17 @@ void Ekf::controlFusionModes()
_control_status.flags.mag_3D = true;
} else {
// always use 2D mag fusion when on the ground
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
// use heading fusion when on the ground
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
}
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
// always use yaw fusion unless tilt is over 45 deg, otherwise use 2D fusion
if (_R_prev(2, 2) > 0.7071f) {
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_2D = false;
} else {
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
}
// always use heading fusion
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) {