From bdafc2c0250def24065aa3ec62065209fe7fe89c Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 12 Jan 2017 15:59:11 -0800 Subject: [PATCH] AP_Landing: re/move complete flag into backend the complete flag was only ever true during FLIGHT_LAND_FINAL so we just check for that now instead --- ArduPlane/is_flying.cpp | 1 - libraries/AP_Landing/AP_Landing.cpp | 22 +++++++++++++--------- libraries/AP_Landing/AP_Landing.h | 7 ++----- libraries/AP_Landing/AP_Landing_Slope.cpp | 14 +++++++++----- 4 files changed, 24 insertions(+), 20 deletions(-) diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index cb4d39e4ff..d0462f0209 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -286,7 +286,6 @@ void Plane::crash_detection_update(void) if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { disarm_motors(); } - landing.complete = true; if (crashed_near_land_waypoint) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); } else { diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index c098148796..e75299aef3 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -143,7 +143,6 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) { - complete = false; commanded_go_around = false; switch (type) { @@ -183,7 +182,6 @@ bool AP_Landing::verify_abort_landing(const Location &prev_WP_loc, Location &nex case TYPE_STANDARD_GLIDE_SLOPE: type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed); break; - default: break; } @@ -208,7 +206,6 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing case TYPE_STANDARD_GLIDE_SLOPE: type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm); break; - default: break; } @@ -224,7 +221,6 @@ bool AP_Landing::is_flaring(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_flaring(); - default: return false; } @@ -240,7 +236,6 @@ bool AP_Landing::is_on_approach(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_on_approach(); - default: return false; } @@ -256,7 +251,6 @@ bool AP_Landing::is_expecting_impact(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_expecting_impact(); - default: return false; } @@ -278,7 +272,6 @@ void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Lo case TYPE_STANDARD_GLIDE_SLOPE: type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm); break; - default: break; } @@ -386,7 +379,6 @@ int32_t AP_Landing::get_target_airspeed_cm(void) switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_get_target_airspeed_cm(); - default: return SpdHgt_Controller->get_land_airspeed(); } @@ -401,7 +393,6 @@ bool AP_Landing::request_go_around(void) switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_request_go_around(); - default: return false; } @@ -413,4 +404,17 @@ void AP_Landing::handle_flight_stage_change(const bool _in_landing_stage) commanded_go_around = false; } +/* + * returns true when a landing is complete, usually used to disable throttle + */ +bool AP_Landing::is_complete(void) const +{ + switch (type) { + case TYPE_STANDARD_GLIDE_SLOPE: + return type_slope_is_complete(); + default: + return true; + } +} + diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index f9de245c23..a99329f5a8 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -97,14 +97,10 @@ public: int8_t get_flap_percent(void) const { return flap_percent; } int8_t get_throttle_slewrate(void) const { return throttle_slewrate; } bool is_commanded_go_around(void) const { return commanded_go_around; } - bool is_complete(void) const { return complete; } + bool is_complete(void) const; void set_initial_slope(void) { initial_slope = slope; } bool is_expecting_impact(void) const; - // Flag to indicate if we have landed. - // Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown - bool complete; - // landing altitude offset (meters) float alt_offset; @@ -176,6 +172,7 @@ private: void type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state); int32_t type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd); bool type_slope_request_go_around(void); + bool type_slope_is_complete(void) const; bool type_slope_is_flaring(void) const; bool type_slope_is_on_approach(void) const; bool type_slope_is_expecting_impact(void) const; diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index 8f10103b18..a53cb6b4d9 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -38,7 +38,6 @@ void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Lo // when aborting a landing, mimic the verify_takeoff with steering hold. Once // the altitude has been reached, restart the landing sequence throttle_suppressed = false; - complete = false; nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); } @@ -94,7 +93,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n (!rangefinder_state_in_range && wp_proportion >= 1) || probably_crashed) { - if (!complete) { + if (type_slope_stage != SLOPE_STAGE_FINAL) { post_stats = true; if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)ahrs.get_gps().ground_speed()); @@ -104,7 +103,6 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n (double)ahrs.get_gps().ground_speed(), (double)get_distance(current_loc, next_WP_loc)); } - complete = true; type_slope_stage = SLOPE_STAGE_FINAL; } @@ -118,7 +116,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n aparm.min_gndspeed_cm.load(); aparm.throttle_cruise.load(); } - } else if (!complete && type_slope_stage == SLOPE_STAGE_APPROACH && pre_flare_airspeed > 0) { + } else if (type_slope_stage == SLOPE_STAGE_APPROACH && pre_flare_airspeed > 0) { bool reached_pre_flare_alt = pre_flare_alt > 0 && (height <= pre_flare_alt); bool reached_pre_flare_sec = pre_flare_sec > 0 && (height <= sink_rate * pre_flare_sec); if (reached_pre_flare_alt || reached_pre_flare_sec) { @@ -145,7 +143,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n } // check if we should auto-disarm after a confirmed landing - if (complete) { + if (type_slope_stage == SLOPE_STAGE_FINAL) { disarm_if_autoland_complete_fn(); } @@ -371,3 +369,9 @@ bool AP_Landing::type_slope_is_expecting_impact(void) const return (type_slope_stage == SLOPE_STAGE_PREFLARE || type_slope_stage == SLOPE_STAGE_FINAL); } + +bool AP_Landing::type_slope_is_complete(void) const +{ + return (type_slope_stage == SLOPE_STAGE_FINAL); +} +