From 6c0296ba1326af5eab21776268082f037e862fe7 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 9 Jan 2017 21:29:50 -0800 Subject: [PATCH] AP_Landing: shadow copy of plane flight_stage into landing .. and describe stages without using specific stage name --- ArduPlane/ArduPlane.cpp | 1 + libraries/AP_Landing/AP_Landing.cpp | 55 +++++++++++++++++++++-- libraries/AP_Landing/AP_Landing.h | 14 +++++- libraries/AP_Landing/AP_Landing_Slope.cpp | 26 ++++++++--- 4 files changed, 85 insertions(+), 11 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 633926ba75..a953943456 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -905,6 +905,7 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs) flight_stage = fs; + landing.stage = fs; if (should_log(MASK_LOG_MODE)) { Log_Write_Status(); diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 47409a9b66..852de98c74 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -150,23 +150,66 @@ bool AP_Landing::verify_land(const AP_Vehicle::FixedWing::FlightStage flight_sta const int32_t auto_state_takeoff_altitude_rel_cm, 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, bool &throttle_suppressed) { switch (type) { - default: case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_verify_land(flight_stage,prev_WP_loc, next_WP_loc, current_loc, auto_state_takeoff_altitude_rel_cm, height,sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range, throttle_suppressed); + default: + // returning TRUE while executing verify_land() will increment the + // mission index which in many cases will trigger an RTL for end-of-mission + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE"); + return true; } } void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float wp_distance, int32_t &target_altitude_offset_cm) { switch (type) { - default: 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; + } +} + +// return true while the aircraft should be in a flaring state +bool AP_Landing::is_flaring(void) const +{ + switch (type) { + case TYPE_STANDARD_GLIDE_SLOPE: + return type_slope_is_flaring(); + + default: + return false; + } +} + +// return true while the aircraft is performing a landing approach +bool AP_Landing::is_on_approach(void) const +{ + switch (type) { + case TYPE_STANDARD_GLIDE_SLOPE: + return type_slope_is_on_approach(); + + default: + return false; } } +// 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 +{ + switch (type) { + case TYPE_STANDARD_GLIDE_SLOPE: + return type_slope_is_expecting_impact(); + + default: + return false; + } +} + + /* a special glide slope calculation for the landing approach @@ -179,10 +222,12 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm) { switch (type) { - default: 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; } } @@ -332,9 +377,11 @@ int32_t AP_Landing::get_target_airspeed_cm(const AP_Vehicle::FixedWing::FlightSt bool AP_Landing::request_go_around(void) { switch (type) { - default: case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_request_go_around(); + + default: + return false; } } diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 99516df24b..f641440329 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -58,6 +58,9 @@ public: AP_Param::setup_object_defaults(this, var_info); } + + + // NOTE: make sure to update is_type_valid() enum Landing_Type { TYPE_STANDARD_GLIDE_SLOPE = 0, // TODO: TYPE_DEEPSTALL, @@ -71,6 +74,8 @@ public: void setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm); void check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state); bool request_go_around(void); + bool is_flaring(void) const; + bool is_on_approach(void) const; // helper functions @@ -92,9 +97,10 @@ public: 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; } - void set_initial_slope() { initial_slope = slope; } - + void set_initial_slope(void) { initial_slope = slope; } + bool is_expecting_impact(void) const; + AP_Vehicle::FixedWing::FlightStage stage; // Flag to indicate if we have landed. // Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown @@ -163,5 +169,9 @@ private: void type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm); void type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state); bool type_slope_request_go_around(void); + 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 5479e7ebe1..29a84a3a77 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -37,10 +37,10 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage // the altitude has been reached, restart the landing sequence if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { - throttle_suppressed = false; - complete = false; - pre_flare = false; - nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); + throttle_suppressed = false; + complete = false; + pre_flare = false; + nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); // see if we have reached abort altitude if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) { @@ -53,7 +53,7 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage } // make sure to return false so it leaves the mission index alone return false; - } +} /* Set land_complete (which starts the flare) under 3 conditions: @@ -298,4 +298,20 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo constrain_target_altitude_location_fn(loc, prev_WP_loc); } +bool AP_Landing::type_slope_is_flaring(void) const +{ + return (stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_LAND_FINAL); +} + +bool AP_Landing::type_slope_is_on_approach(void) const +{ + return (stage == AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH || + stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE); +} + +bool AP_Landing::type_slope_is_expecting_impact(void) const +{ + return (stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE || + stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL); +}