From 83f0a8114b2db6a4379847d36bd0426837fd426a Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 6 Dec 2016 17:30:48 -0800 Subject: [PATCH] Plane: use a common reset for uniform landing wipe --- ArduPlane/ArduPlane.cpp | 2 -- ArduPlane/altitude.cpp | 2 +- ArduPlane/commands_logic.cpp | 9 +++------ ArduPlane/system.cpp | 7 ++----- 4 files changed, 6 insertions(+), 14 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0aae899104..3700b5a343 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -569,8 +569,6 @@ void Plane::handle_auto_mode(void) if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) { steer_state.hold_course_cd = -1; } - landing.complete = false; - landing.pre_flare = false; calc_nav_roll(); calc_nav_pitch(); calc_throttle(); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index a8bbce5cab..177ed230f8 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -657,7 +657,7 @@ 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.initial_slope = landing.slope; + 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 95285e6b7b..880dccb702 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -13,10 +13,11 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) DataFlash.Log_Write_Mission_Cmd(mission, cmd); } + landing.reset(); + // special handling for nav vs non-nav commands if (AP_Mission::is_nav_cmd(cmd)) { // set land_complete to false to stop us zeroing the throttle - landing.init_start_nav_cmd(); auto_state.sink_rate = 0; // set takeoff_complete to true so we don't add extra elevator @@ -137,8 +138,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_DO_LAND_START: - //ensure go around hasn't been set - landing.commanded_go_around = false; + // handled in landing.reset() break; case MAV_CMD_DO_FENCE_ENABLE: @@ -382,7 +382,6 @@ void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd) void Plane::do_land(const AP_Mission::Mission_Command& cmd) { - landing.commanded_go_around = false; set_next_WP(cmd.content.location); // configure abort altitude and pitch @@ -398,8 +397,6 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) auto_state.takeoff_pitch_cd = 1000; } - landing.slope = 0; - // zero rangefinder state, start to accumulate good samples now memset(&rangefinder_state, 0, sizeof(rangefinder_state)); } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index b9981cb1c4..ddf64fa527 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -337,11 +337,8 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) // reset landing check auto_state.checked_for_autoland = false; - // reset go around command - landing.commanded_go_around = false; - - // not in pre-flare - landing.pre_flare = false; + // reset landing + landing.reset(); // zero locked course steer_state.locked_course_err = 0;