ekf2: add dedicated EKF2_CONFIG_TERRAIN in kconfig

- new estimator_aid_src_terrain_range_finder for HAGL RNG
This commit is contained in:
Daniel Agar
2023-09-15 10:02:09 -04:00
parent 845b01a00d
commit 10b54d08fc
14 changed files with 367 additions and 253 deletions
+2 -2
View File
@@ -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;
}