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

EKF: Add method to fuse horizontal magnetometer data

Browse Source 
This method is more suitable than a raw heading measurement because it works across a full range of pitch angles.
It has been made the default for ground operation.
This commit is contained in:
Paul Riseborough
2016-02-20 14:42:17 +11:00
parent 6df6ac0023
commit a711632017
5 changed files with 239 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+26 -7
View File
@@ -103,36 +103,55 @@ 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 simple mag fusion for initial startup
_control_status.flags.mag_hdg = true;
// always use 2D mag fusion for initial startup
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
_control_status.flags.mag_3D = false;
} else {
if (_control_status.flags.in_air) {
// always use 3-axis mag fusion when airborne
// always use 3D mag fusion when airborne
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = true;
} else {
// always use simple heading fusion when on the ground
_control_status.flags.mag_hdg = true;
// always use 2D mag fusion when on the ground
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
_control_status.flags.mag_3D = false;
}
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
// always use simple heading fusion
_control_status.flags.mag_hdg = true;
// 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;
}
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) {
// always use 2D mag fusion
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// always use 3-axis mag fusion
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = true;
} else {
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
}