mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 17:03:35 +08:00
ekf2: add dedicated EKF2_CONFIG_TERRAIN in kconfig
- new estimator_aid_src_terrain_range_finder for HAGL RNG
This commit is contained in:
@@ -123,17 +123,17 @@ void Ekf::controlMagFusion()
|
||||
|
||||
bool Ekf::checkHaglYawResetReq() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// We need to reset the yaw angle after climbing away from the ground to enable
|
||||
// recovery from ground level magnetic interference.
|
||||
if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
|
||||
// Check if height has increased sufficiently to be away from ground magnetic anomalies
|
||||
// and request a yaw reset if not already requested.
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
static constexpr float mag_anomalies_max_hagl = 1.5f;
|
||||
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
||||
return above_mag_anomalies;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
}
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user