dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Daniel Agar 2022-03-21 14:16:17 -04:00
parent f254b55523
commit 4fee059696
2 changed files with 11 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/ekf.h
View File

@ -449,7 +449,6 @@ private:
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements
bool _non_mag_yaw_aiding_running_prev{false}; ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS).
bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited
SquareMatrix24f P{}; ///< state covariance matrix
@ -848,9 +847,6 @@ private:
// control fusion of magnetometer observations
void controlMagFusion();
bool noOtherYawAidingThanMag() const;
bool otherHeadingSourcesHaveStopped();
void checkHaglYawResetReq();
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }

31
src/modules/ekf2/EKF/mag_control.cpp
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;
}