From 2d02948fbca5a1a9a816eda8af3b0cd2118c4a61 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 15 Nov 2023 12:35:12 -0800 Subject: [PATCH] Plane: implement LAND_RNGFND_HLD to hold off corrections --- ArduPlane/commands_logic.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 823f1e4da93bb5..27539fbc8ccc27 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -252,12 +252,22 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed); } else { + float height = height_above_target(); + // use rangefinder to correct if possible - float height = height_above_target() - rangefinder_correction(); + if (landing.get_rangefinder_holdoff_distance() <= 0) || // rangefinder holdoff is disabled (never hold off, always use rtanegfinder) + wp_proportion >= 1 || // or we have reached the land waypoint + is_flaring() || // or we are flaring + current_loc.get_distance(next_WP_loc) >= rangefinder_holdoff_before_land_point) // or we've reached the holdoff distance + { + height -= rangefinder_correction(); + } + // for flare calculations we don't want to use the terrain // correction as otherwise we will flare early on rising // ground height -= auto_state.terrain_correction; + return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc, height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), g.rangefinder_landing && rangefinder_state.in_range);