From 2b6f09cd0f51ca7e7d112f50b1bd2b8db54fc0b9 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 20 Jun 2025 13:01:41 -0800 Subject: [PATCH] reset terrain to rng before reset aid source --- .../aid_sources/range_finder/jake_range_height_control.cpp | 4 ++-- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp index 364ae63161..839986c27d 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp @@ -232,9 +232,9 @@ void Ekf::fuseRangeAsHeightAiding() // PX4_INFO("FIRST range terrain fusion, resetting terrain to range"); PX4_INFO("FIRST range terrain fusion, resetting terrain to range"); + resetTerrainToRng(_aid_src_rng_hgt); resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt); resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain); - resetTerrainToRng(_aid_src_rng_hgt); } } @@ -242,8 +242,8 @@ void Ekf::fuseRangeAsHeightAiding() if (!optical_flow_for_terrain && innovation_rejected && kinematically_consistent) { PX4_INFO("range terrain fusion, resetting terrain to range"); - resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt); resetTerrainToRng(_aid_src_rng_hgt); + resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt); } // Fuse diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index b01432e836..342cfcdd11 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -307,7 +307,7 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl) { - PX4_INFO("_ekfResetHandlerHagl"); + // PX4_INFO("_ekfResetHandlerHagl"); _dist_to_bottom_lock = NAN; }