From 4d557f95ea01149d7d35812969cb5471612ec449 Mon Sep 17 00:00:00 2001 From: Leonard Hall <leonardthall@gmail.com> Date: Sun, 8 Jan 2023 19:27:06 +1030 Subject: [PATCH] Copter: Use filtered and corrected range finder in surface tracking --- ArduCopter/surface_tracking.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index 63f48902ad..669e7c646a 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -17,7 +17,7 @@ void Copter::SurfaceTracking::update_surface_offset() // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m. curr_surface_alt_above_origin_cm is 7m (or 700cm) RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f; - const float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_position_z_up_cm() - dir * rf_state.alt_cm; + const float curr_surface_alt_above_origin_cm = rf_state.inertial_alt_cm - dir * rf_state.alt_cm_filt.get(); // update position controller target offset to the surface's alt above the EKF origin copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm);