From e5d580412ecc97a908b9da2ea9c6318013901f3b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 3 Nov 2020 07:59:29 +1100 Subject: [PATCH] Plane: fixed usage of rangefinder in landing with RNGFND_LANDING=0 this fixes two places where rangefinder can affect landing when RNGFND_LANDING=0 --- ArduPlane/altitude.cpp | 4 +++- ArduPlane/commands_logic.cpp | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 1293d5ceb3..9a0af4c5b5 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -702,7 +702,9 @@ void Plane::rangefinder_height_update(void) if (now - rangefinder_state.last_correction_time_ms > 5000) { rangefinder_state.correction = correction; rangefinder_state.initial_correction = correction; - landing.set_initial_slope(); + if (g.rangefinder_landing) { + landing.set_initial_slope(); + } rangefinder_state.last_correction_time_ms = now; } else { rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 04a20ec9bb..5c73f28404 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -233,7 +233,8 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret // 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(), rangefinder_state.in_range); + 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); } case MAV_CMD_NAV_LOITER_UNLIM: