From 3e66dd10d77b1ef6780dec02192236035957f676 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 10 Jan 2017 12:47:31 -0700 Subject: [PATCH] Plane: Move landing stages inside AP_Landing and refactor --- ArduPlane/ArduPlane.cpp | 57 +++++++---------------- ArduPlane/commands_logic.cpp | 18 +++++++ ArduPlane/navigation.cpp | 2 +- libraries/AP_Landing/AP_Landing.cpp | 10 ++++ libraries/AP_Landing/AP_Landing.h | 10 ++-- libraries/AP_Landing/AP_Landing_Slope.cpp | 15 ++++-- 6 files changed, 64 insertions(+), 48 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index a953943456..f200e41e95 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -865,47 +865,17 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs) return; } - switch (fs) { - case AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH: - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude()); + if (fs == AP_Vehicle::FixedWing::FLIGHT_LAND) { landing.in_progress = true; -#if GEOFENCE_ENABLED == ENABLED - if (g.fence_autoenable == 1) { - if (! geofence_set_enabled(false, AUTO_TOGGLED)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)"); - } else { - gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)"); - } - } else if (g.fence_autoenable == 2) { - if (! geofence_set_floor_enabled(false)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)"); - } else { - gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); - } + } else { + landing.in_progress = false; + if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", + auto_state.takeoff_altitude_rel_cm/100); } -#endif - break; - - case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND: - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", auto_state.takeoff_altitude_rel_cm/100); - landing.in_progress = false; - break; - - case AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE: - case AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL: - landing.in_progress = true; - break; - - case AP_Vehicle::FixedWing::FLIGHT_NORMAL: - case AP_Vehicle::FixedWing::FLIGHT_VTOL: - case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: - landing.in_progress = false; - break; } - flight_stage = fs; - landing.stage = fs; if (should_log(MASK_LOG_MODE)) { Log_Write_Status(); @@ -975,10 +945,11 @@ void Plane::update_flight_stage(void) } else if (landing.get_abort_throttle_enable() && channel_throttle->get_control_in() >= 90) { plane.gcs_send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle"); set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); - } else if (landing.is_complete()) { - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL); - } else if (landing.pre_flare == true) { - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE); + } else { + set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND); + } +/* + // FIXME: lift this into AP_Landing_Slope } else if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH) { 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(); @@ -991,7 +962,7 @@ void Plane::update_flight_stage(void) } else { set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); } - } + }*/ } else if (quadplane.in_assisted_flight()) { set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); } else { @@ -1010,6 +981,10 @@ void Plane::update_flight_stage(void) set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); } + if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) { + landing.reset(); + } + // tell AHRS the airspeed to true airspeed ratio airspeed.set_EAS2TAS(barometer.get_EAS2TAS()); } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 5136173925..0913aca8c5 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -401,6 +401,24 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) // zero rangefinder state, start to accumulate good samples now memset(&rangefinder_state, 0, sizeof(rangefinder_state)); + + landing.do_land(cmd, relative_altitude()); + +#if GEOFENCE_ENABLED == ENABLED + if (g.fence_autoenable == 1) { + if (! geofence_set_enabled(false, AUTO_TOGGLED)) { + gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)"); + } else { + gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)"); + } + } else if (g.fence_autoenable == 2) { + if (! geofence_set_floor_enabled(false)) { + gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)"); + } else { + gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); + } + } +#endif } void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd) diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 88f8a9f868..f1c788e89e 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -95,7 +95,7 @@ void Plane::calc_airspeed_errors() } else if (control_mode == AUTO && landing.in_progress) { // Landing airspeed target - target_airspeed_cm = landing.get_target_airspeed_cm(flight_stage); + target_airspeed_cm = landing.get_target_airspeed_cm(); } else { // Normal airspeed target diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index a32a2b865b..86309af804 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -141,6 +141,16 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { AP_GROUPEND }; +void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) { + switch (type) { + case TYPE_STANDARD_GLIDE_SLOPE: + type_slope_do_land(cmd, relative_altitude); + break; + default: + // a incorrect type is handled in the verify_land + break; + } +} /* update navigation for landing. Called when on landing approach or diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index a70a79ec27..9275c0fc50 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -68,6 +68,7 @@ public: // TODO: TYPE_HELICAL, }; + void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); bool verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed); bool verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, @@ -157,10 +158,13 @@ private: AP_Int8 type; // Land Type STANDARD GLIDE SLOPE - enum slope_stage {SLOPE_APPROACH, - SLOPE_PREFLARE, - SLOPE_FINAL}; + enum slope_stage { + SLOPE_APPROACH, + SLOPE_PREFLARE, + SLOPE_FINAL + }; slope_stage type_slope_stage; + void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); void type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed); bool 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); diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index 4065a88eb9..5d8bde21bb 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -21,6 +21,12 @@ #include #include +void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) +{ + type_slope_stage = SLOPE_APPROACH; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude); +} + void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed) { // when aborting a landing, mimic the verify_takeoff with steering hold. Once @@ -38,6 +44,7 @@ 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 @@ -325,18 +332,20 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) { bool AP_Landing::type_slope_is_flaring(void) const { - return type_slope_stage == SLOPE_FINAL; + return in_progress && type_slope_stage == SLOPE_FINAL; } bool AP_Landing::type_slope_is_on_approach(void) const { - return (type_slope_stage == SLOPE_APPROACH || + return in_progress && + (type_slope_stage == SLOPE_APPROACH || type_slope_stage == SLOPE_PREFLARE); } bool AP_Landing::type_slope_is_expecting_impact(void) const { - return (type_slope_stage == SLOPE_PREFLARE || + return in_progress && + (type_slope_stage == SLOPE_PREFLARE || type_slope_stage == SLOPE_FINAL); }