From 394fbb1a503d6123dd00d168cf8586f84428ac8e Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 7 Jun 2019 08:08:28 -0700 Subject: [PATCH] Plane: unified soaring exit behavior: auto heads to next wp, cruise continues cruise heading, FBW to home --- ArduPlane/commands_logic.cpp | 2 +- ArduPlane/mode.h | 4 ++- ArduPlane/mode_loiter.cpp | 14 ++++++++-- ArduPlane/soaring.cpp | 54 +++++++++++++++++++----------------- 4 files changed, 45 insertions(+), 29 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 6912dfc569..8d7810d022 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1078,5 +1078,5 @@ bool Plane::verify_loiter_heading(bool init) loiter.sum_cd = 0; } - return plane.mode_loiter.headingLinedUp(next_WP_loc, next_nav_cmd.content.location); + return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location); } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 8810c7fa4a..9290651b69 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -173,7 +173,9 @@ public: // methods that affect movement of the vehicle in this mode void update() override; - bool headingLinedUp(const Location loiterCenterLoc, const Location targetLoc); + bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc); + bool isHeadingLinedUp(const float bearing); + bool isHeadingLinedUp(const int32_t bearing_cd); protected: diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index e09e91abb6..9313cf379e 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -26,7 +26,7 @@ void ModeLoiter::update() plane.calc_throttle(); } -bool ModeLoiter::headingLinedUp(const Location loiterCenterLoc, const Location targetLoc) +bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc) { const uint16_t loiterRadius = abs(plane.aparm.loiter_radius); if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) { @@ -37,9 +37,19 @@ bool ModeLoiter::headingLinedUp(const Location loiterCenterLoc, const Location t return true; } - // Bearing in degrees + // Bearing in centi-degrees const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc); + return isHeadingLinedUp(bearing_cd); +} +bool ModeLoiter::isHeadingLinedUp(const float bearing) +{ + const int32_t bearing_cd = bearing*100; + return isHeadingLinedUp(bearing_cd); +} + +bool ModeLoiter::isHeadingLinedUp(const int32_t bearing_cd) +{ // get current heading. const int32_t heading_cd = plane.gps.ground_course_cd(); diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index 3a2044448e..613a4b6c05 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -73,33 +73,37 @@ void Plane::update_soaring() { const char* strTooLow = "Soaring: Too low, restoring "; const char* strTooWeak = "Soaring: Thermal ended, restoring "; - // Exit as soon as thermal state estimate deteriorates - switch (previous_mode->mode_number()) { + // Exit as soon as thermal state estimate deteriorates and we're lined up to next target + switch (previous_mode->mode_number()){ case Mode::Number::FLY_BY_WIRE_B: { + const bool homeIsSet = AP::ahrs().home_is_set(); + const bool headingLinedupToHome = homeIsSet && plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home()); + if (homeIsSet && !headingLinedupToHome) { + break; + } switch (loiterStatus) { - case SoaringController::LoiterStatus::ALT_TOO_HIGH: { - const bool homeIsSet = AP::ahrs().home_is_set(); - const bool headingLinedupToHome = homeIsSet && plane.mode_loiter.headingLinedUp(next_WP_loc, AP::ahrs().get_home()); - if (!homeIsSet || headingLinedupToHome) { - gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_fbwb.name()); - set_mode(mode_fbwb, ModeReason::SOARING_ALT_TOO_HIGH); - } - } + case SoaringController::LoiterStatus::ALT_TOO_HIGH: + gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_fbwb.name()); + set_mode(mode_fbwb, ModeReason::SOARING_ALT_TOO_HIGH); break; - default: case SoaringController::LoiterStatus::ALT_TOO_LOW: gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_rtl.name()); set_mode(mode_rtl, ModeReason::SOARING_ALT_TOO_LOW); break; + default: case SoaringController::LoiterStatus::THERMAL_WEAK: gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_fbwb.name()); set_mode(mode_fbwb, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED); break; } // switch louterStatus - } // case FBWB break; + } case Mode::Number::CRUISE: { + const bool headingLinedupToCruise = plane.mode_loiter.isHeadingLinedUp(cruise_state.locked_heading_cd); + if (cruise_state.locked_heading && !headingLinedupToCruise) { + break; + } // return to cruise with old ground course const CruiseState cruise = cruise_state; switch (loiterStatus) { @@ -107,11 +111,11 @@ void Plane::update_soaring() { gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_cruise.name()); set_mode(mode_cruise, ModeReason::SOARING_ALT_TOO_HIGH); break; - default: case SoaringController::LoiterStatus::ALT_TOO_LOW: gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_rtl.name()); set_mode(mode_rtl, ModeReason::SOARING_ALT_TOO_LOW); break; + default: case SoaringController::LoiterStatus::THERMAL_WEAK: gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_cruise.name()); set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED); @@ -122,31 +126,31 @@ void Plane::update_soaring() { break; } // case Cruise - case Mode::Number::AUTO: + case Mode::Number::AUTO: { + //Get the lat/lon of next Nav waypoint after this one: + AP_Mission::Mission_Command next_nav_cmd; + const bool nextWpisValid = mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd); + const bool headingLinedupToWP = nextWpisValid && plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location); + if (nextWpisValid && !headingLinedupToWP) { + break; + } switch (loiterStatus) { case SoaringController::LoiterStatus::ALT_TOO_HIGH: gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_auto.name()); set_mode(mode_auto, ModeReason::SOARING_ALT_TOO_HIGH); break; - default: case SoaringController::LoiterStatus::ALT_TOO_LOW: gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_auto.name()); set_mode(mode_auto, ModeReason::SOARING_ALT_TOO_LOW); break; - case SoaringController::LoiterStatus::THERMAL_WEAK: { - //Get the lat/lon of next Nav waypoint after this one: - AP_Mission::Mission_Command next_nav_cmd; - const bool nextWpisValid = mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd); - const bool headingLinedupToWP = nextWpisValid && plane.mode_loiter.headingLinedUp(next_WP_loc, next_nav_cmd.content.location); - if (!nextWpisValid || headingLinedupToWP) { + default: + case SoaringController::LoiterStatus::THERMAL_WEAK: gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name()); set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED); - } - } break; - } // switch loiterStatus + } // switch loiterStatus break; - + } // case AUTO default: // all other modes break; } // switch previous_mode