From 2ccd91d88f8b99d96e65ec07c46e269cf28379df Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 10 Jan 2017 22:18:09 -0800 Subject: [PATCH] AP_Landing: rename stage enum --- ArduPlane/ArduPlane.cpp | 2 +- libraries/AP_Landing/AP_Landing.cpp | 23 +++++++++++ libraries/AP_Landing/AP_Landing.h | 9 +++-- libraries/AP_Landing/AP_Landing_Slope.cpp | 48 +++++++++++++---------- 4 files changed, 56 insertions(+), 26 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 41815e23d3..194400c3e7 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -865,7 +865,7 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs) return; } - landing.set_in_progress(fs == AP_Vehicle::FixedWing::FLIGHT_LAND); + landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND); if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 36d9922fe0..a48f40c96d 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -217,6 +217,10 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing // return true while the aircraft should be in a flaring state bool AP_Landing::is_flaring(void) const { + if (!in_progress) { + return false; + } + switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_flaring(); @@ -229,6 +233,10 @@ bool AP_Landing::is_flaring(void) const // return true while the aircraft is performing a landing approach bool AP_Landing::is_on_approach(void) const { + if (!in_progress) { + return false; + } + switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_on_approach(); @@ -241,6 +249,10 @@ bool AP_Landing::is_on_approach(void) const // return true when at the last stages of a land when an impact with the ground is expected soon bool AP_Landing::is_expecting_impact(void) const { + if (!in_progress) { + return false; + } + switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_expecting_impact(); @@ -356,6 +368,11 @@ float AP_Landing::head_wind(void) */ int32_t AP_Landing::get_target_airspeed_cm(void) { + if (!in_progress) { + // not landing, use regular cruise airspeed + return aparm.airspeed_cruise_cm; + } + switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_get_target_airspeed_cm(); @@ -380,4 +397,10 @@ bool AP_Landing::request_go_around(void) } } +void AP_Landing::handle_flight_stage_change(const bool _in_landing_stage) +{ + in_progress = _in_landing_stage; + commanded_go_around = false; +} + diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 0427a58335..7bed3654ff 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -78,7 +78,7 @@ public: bool request_go_around(void); bool is_flaring(void) const; bool is_on_approach(void) const; - void set_in_progress(const bool _in_progress) { in_progress = _in_progress; } + void handle_flight_stage_change(const bool _in_landing_stage); // helper functions bool restart_landing_sequence(void); @@ -157,9 +157,10 @@ private: // Land Type STANDARD GLIDE SLOPE enum slope_stage { - SLOPE_APPROACH, - SLOPE_PREFLARE, - SLOPE_FINAL + SLOPE_STAGE_NORMAL, + SLOPE_STAGE_APPROACH, + SLOPE_STAGE_PREFLARE, + SLOPE_STAGE_FINAL }; slope_stage type_slope_stage; void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index 61a8118abb..051df28c29 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -29,7 +29,7 @@ void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, cons // once landed, post some landing statistics to the GCS post_stats = false; - type_slope_stage = SLOPE_APPROACH; + type_slope_stage = SLOPE_STAGE_NORMAL; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude); } @@ -50,11 +50,24 @@ void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Lo bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Stage: %d Complete: %d", type_slope_stage, complete); // we don't 'verify' landing in the sense that it never completes, // so we don't verify command completion. Instead we use this to // adjust final landing parameters + // determine stage + if (type_slope_stage == SLOPE_STAGE_NORMAL) { + const bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale(); + const bool on_flight_line = fabsf(nav_controller->crosstrack_error()) < 5.0f && !nav_controller->data_is_stale(); + const bool below_prev_WP = current_loc.alt < prev_WP_loc.alt; + if ((mission.get_prev_nav_cmd_id() == MAV_CMD_NAV_LOITER_TO_ALT) || + (wp_proportion >= 0 && heading_lined_up && on_flight_line) || + (wp_proportion > 0.15f && heading_lined_up && below_prev_WP) || + (wp_proportion > 0.5f)) { + type_slope_stage = SLOPE_STAGE_APPROACH; + } + } + + /* Set land_complete (which starts the flare) under 3 conditions: 1) we are within LAND_FLARE_ALT meters of the landing altitude 2) we are within LAND_FLARE_SEC of the landing point vertically @@ -92,7 +105,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n (double)get_distance(current_loc, next_WP_loc)); } complete = true; - type_slope_stage = SLOPE_FINAL; + type_slope_stage = SLOPE_STAGE_FINAL; } @@ -105,11 +118,11 @@ 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_PREFLARE && pre_flare_airspeed > 0) { + } else if (!complete && 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) { - type_slope_stage = SLOPE_PREFLARE; + type_slope_stage = SLOPE_STAGE_PREFLARE; } } @@ -296,27 +309,22 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo } int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) { - int32_t target_airspeed_cm = aparm.airspeed_cruise_cm; - - if (!in_progress) { - // not landing, use regular cruise airspeed - return target_airspeed_cm; - } // we're landing, check for custom approach and // pre-flare airspeeds. Also increase for head-winds const float land_airspeed = SpdHgt_Controller->get_land_airspeed(); + int32_t target_airspeed_cm = aparm.airspeed_cruise_cm; switch (type_slope_stage) { - case SLOPE_APPROACH: + case SLOPE_STAGE_APPROACH: if (land_airspeed >= 0) { target_airspeed_cm = land_airspeed * 100; } break; - case SLOPE_PREFLARE: - case SLOPE_FINAL: + case SLOPE_STAGE_PREFLARE: + case SLOPE_STAGE_FINAL: if (pre_flare_airspeed > 0) { // if we just preflared then continue using the pre-flare airspeed during final flare target_airspeed_cm = pre_flare_airspeed * 100; @@ -338,20 +346,18 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) { bool AP_Landing::type_slope_is_flaring(void) const { - return in_progress && type_slope_stage == SLOPE_FINAL; + return (type_slope_stage == SLOPE_STAGE_FINAL); } bool AP_Landing::type_slope_is_on_approach(void) const { - return in_progress && - (type_slope_stage == SLOPE_APPROACH || - type_slope_stage == SLOPE_PREFLARE); + return (type_slope_stage == SLOPE_STAGE_APPROACH || + type_slope_stage == SLOPE_STAGE_PREFLARE); } bool AP_Landing::type_slope_is_expecting_impact(void) const { - return in_progress && - (type_slope_stage == SLOPE_PREFLARE || - type_slope_stage == SLOPE_FINAL); + return (type_slope_stage == SLOPE_STAGE_PREFLARE || + type_slope_stage == SLOPE_STAGE_FINAL); }