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

reset terrain to rng before reset aid source

Browse Source
This commit is contained in:
Jacob Dahl
2025-06-24 11:11:02 -08:00
parent 1386ec2436
commit 2b6f09cd0f
2 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp
+2 -2
View File
@@ -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");
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); resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain); 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 if (!optical_flow_for_terrain && innovation_rejected
&& kinematically_consistent) { && kinematically_consistent) {
PX4_INFO("range terrain fusion, resetting terrain to range"); PX4_INFO("range terrain fusion, resetting terrain to range");
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
resetTerrainToRng(_aid_src_rng_hgt); resetTerrainToRng(_aid_src_rng_hgt);
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
} }
// Fuse // Fuse
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+1 -1
View File
@@ -307,7 +307,7 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl) void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl)
{ {
PX4_INFO("_ekfResetHandlerHagl"); // PX4_INFO("_ekfResetHandlerHagl");
_dist_to_bottom_lock = NAN; _dist_to_bottom_lock = NAN;
} }