diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index f32a241eee..b1fb69d6c6 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -559,6 +559,9 @@ private: // are we doing loiter mode as a VTOL? bool vtol_loiter; + + // how much correction have we added for terrain data + float terrain_correction; } auto_state; struct { diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index f999dc2c25..69c8358b91 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -595,6 +595,7 @@ void Plane::rangefinder_terrain_correction(float &height) } float correction = (terrain_amsl1 - terrain_amsl2); height += correction; + auto_state.terrain_correction = correction; #endif } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index c548a72bab..6082878585 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -230,7 +230,11 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret } else { // use rangefinder to correct if possible - const float height = height_above_target() - rangefinder_correction(); + float height = height_above_target() - 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(), rangefinder_state.in_range); }