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

ekf2: simplify mag yaw reset request when transitioning to mag enabled

Browse Source
This commit is contained in:
Daniel Agar
2022-07-21 09:24:28 -04:00
parent f254b55523
commit 4fee059696
2 changed files with 11 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+11 -20
View File
@@ -86,17 +86,18 @@ void Ekf::controlMagFusion()
return;
}
_mag_yaw_reset_req |= otherHeadingSourcesHaveStopped();
_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;
if (noOtherYawAidingThanMag() && mag_data_ready) {
if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D;
// Determine if we should use simple magnetic heading fusion which works better when
// there are large external disturbances or the more accurate 3-axis fusion
switch (_params.mag_fusion_type) {
default:
/* fallthrough */
// FALLTHROUGH
case MagFuseType::AUTO:
selectMagAuto();
break;
@@ -113,6 +114,12 @@ void Ekf::controlMagFusion()
break;
}
const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
if (!mag_enabled_previously && mag_enabled) {
_mag_yaw_reset_req = true;
}
if (_control_status.flags.in_air) {
checkHaglYawResetReq();
runInAirYawReset(mag_sample.mag);
@@ -133,12 +140,6 @@ void Ekf::controlMagFusion()
}
}
bool Ekf::noOtherYawAidingThanMag() const
{
// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used
return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw;
}
void Ekf::checkHaglYawResetReq()
{
// We need to reset the yaw angle after climbing away from the ground to enable
@@ -378,13 +379,3 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
}
}
}
bool Ekf::otherHeadingSourcesHaveStopped()
{
// detect rising edge of noOtherYawAidingThanMag()
bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
_non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
return result;
}