From 9a79b79f1ebfe8f6b4346e5ef19451ad7f744b21 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 18 Nov 2016 14:05:45 -0800 Subject: [PATCH] AP_Landing: port more from plane --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/Plane.h | 3 --- ArduPlane/commands_logic.cpp | 6 +++--- ArduPlane/landing.cpp | 26 +------------------------- ArduPlane/system.cpp | 2 +- libraries/AP_Landing/AP_Landing.cpp | 27 +++++++++++++++++++++++++++ libraries/AP_Landing/AP_Landing.h | 8 ++++++++ 8 files changed, 42 insertions(+), 34 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 912123b62f..18500b1b39 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -972,7 +972,7 @@ void Plane::update_flight_stage(void) } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { if ((g.land_abort_throttle_enable && channel_throttle->get_control_in() >= 90) || - auto_state.commanded_go_around || + landing.commanded_go_around || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){ // abort mode is sticky, it must complete while executing NAV_LAND set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c0a6a4e5de..275ce38b5d 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1360,7 +1360,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) if (!is_zero(packet.param1)) { plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100; } - plane.auto_state.commanded_go_around = true; + plane.landing.commanded_go_around = true; result = MAV_RESULT_ACCEPTED; plane.gcs_send_text(MAV_SEVERITY_INFO,"Go around command accepted"); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 9bfeff07f6..d64068fc3a 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -472,9 +472,6 @@ private: // in FBWA taildragger takeoff mode bool fbwa_tdrag_takeoff_mode:1; - // denotes if a go-around has been commanded for landing - bool commanded_go_around:1; - // Altitude threshold to complete a takeoff command in autonomous modes. Centimeters // are we in idle mode? used for balloon launch to stop servo // movement until altitude is reached diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 318e0f981f..dd70a17e96 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -25,7 +25,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) auto_state.takeoff_complete = true; // if a go around had been commanded, clear it now. - auto_state.commanded_go_around = false; + landing.commanded_go_around = false; // start non-idle auto_state.idle_mode = false; @@ -145,7 +145,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_LAND_START: //ensure go around hasn't been set - auto_state.commanded_go_around = false; + landing.commanded_go_around = false; break; case MAV_CMD_DO_FENCE_ENABLE: @@ -383,7 +383,7 @@ void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd) void Plane::do_land(const AP_Mission::Mission_Command& cmd) { - auto_state.commanded_go_around = false; + landing.commanded_go_around = false; set_next_WP(cmd.content.location); // configure abort altitude and pitch diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index 84c9f51c69..cda3c8fa41 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -157,31 +157,7 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void) // re-calculate auto_state.land_slope with updated prev_WP_loc setup_landing_glide_slope(); - if (rangefinder_state.correction >= 0) { // we're too low or object is below us - // correction positive means we're too low so we should continue on with - // the newly computed shallower slope instead of pitching/throttling up - - } else if (aparm.land_slope_recalc_steep_threshold_to_abort > 0) { - // correction negative means we're too high and need to point down (and speed up) to re-align - // to land on target. A large negative correction means we would have to dive down a lot and will - // generating way too much speed that we can not bleed off in time. It is better to remember - // the large baro altitude offset and abort the landing to come around again with the correct altitude - // offset and "perfect" slope. - - // calculate projected slope with projected alt - float new_slope_deg = degrees(atan(landing.slope)); - float initial_slope_deg = degrees(atan(landing.initial_slope)); - - // is projected slope too steep? - if (new_slope_deg - initial_slope_deg > aparm.land_slope_recalc_steep_threshold_to_abort) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)", - (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg)); - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!"); - landing.alt_offset = rangefinder_state.correction; - auto_state.commanded_go_around = 1; - aparm.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once - } - } + landing.check_if_need_to_abort(rangefinder_state); } /* diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 861e5094a5..85b7ff3a0a 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -338,7 +338,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) landing.checked_for_autoland = false; // reset go around command - auto_state.commanded_go_around = false; + landing.commanded_go_around = false; // not in pre-flare landing.pre_flare = false; diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 3db9315fd3..d6689bccc9 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -187,4 +187,31 @@ Location AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, cons return loc; } +void AP_Landing::check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state) +{ + if (rangefinder_state.correction >= 0) { // we're too low or object is below us + // correction positive means we're too low so we should continue on with + // the newly computed shallower slope instead of pitching/throttling up + } else if (aparm.land_slope_recalc_steep_threshold_to_abort > 0 && !has_aborted_due_to_slope_recalc) { + // correction negative means we're too high and need to point down (and speed up) to re-align + // to land on target. A large negative correction means we would have to dive down a lot and will + // generating way too much speed that we can not bleed off in time. It is better to remember + // the large baro altitude offset and abort the landing to come around again with the correct altitude + // offset and "perfect" slope. + + // calculate projected slope with projected alt + float new_slope_deg = degrees(atan(slope)); + float initial_slope_deg = degrees(atan(initial_slope)); + + // is projected slope too steep? + if (new_slope_deg - initial_slope_deg > aparm.land_slope_recalc_steep_threshold_to_abort) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)", + (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg)); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!"); + alt_offset = rangefinder_state.correction; + commanded_go_around = true; + has_aborted_due_to_slope_recalc = true; // only allow this once. + } + } +} diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 72f9940ec6..fde4794758 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -19,6 +19,7 @@ #include #include #include +#include /// @class AP_Landing /// @brief Class managing ArduPlane landing methods @@ -39,6 +40,7 @@ public: bool jump_to_landing_sequence(void); Location setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc); + void check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state); static const struct AP_Param::GroupInfo var_info[]; @@ -67,8 +69,14 @@ public: // have we checked for an auto-land? bool checked_for_autoland; + // denotes if a go-around has been commanded for landing + bool commanded_go_around; + + private: + bool has_aborted_due_to_slope_recalc; + AP_Mission &mission; AP_AHRS &ahrs; AP_SpdHgtControl *SpdHgt_Controller;