From b2d63c5049e8b8a4f4975e8a78a506b4cd7dc901 Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Fri, 26 Jun 2020 18:04:19 +0100 Subject: [PATCH] Plane: Refactor soaring.cpp. --- ArduPlane/Plane.h | 4 +- ArduPlane/soaring.cpp | 198 +++++++++++++++++------------------------- 2 files changed, 84 insertions(+), 118 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 9682ef2e2d..f0d1eee6a3 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -783,7 +783,7 @@ private: float rudder_dt; // soaring mode-change timer - uint32_t soaring_mode_timer; + uint32_t soaring_mode_timer_ms; // terrain disable for non AUTO modes, set with an RC Option switch bool non_auto_terrain_disable; @@ -1072,6 +1072,8 @@ private: // soaring.cpp #if SOARING_ENABLED == ENABLED void update_soaring(); + bool soaring_exit_heading_aligned() const; + void soaring_restore_mode(const char *reason, ModeReason modereason, Mode &exit_mode); #endif // reverse_thrust.cpp diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index 57fab787fd..e813d5a750 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -66,156 +66,120 @@ void Plane::update_soaring() { // Update thermal estimate and check for switch back to AUTO g2.soaring_controller.update_thermalling(); // Update estimate + // Thermalling is done in a home-relative coordinate system, so we need home to be set. + Vector3f position; + if (!ahrs.get_relative_position_NED_home(position)) { + return; + } + + // Check distance to home against MAX_RADIUS. + if (g2.soaring_controller.max_radius >= 0 && + powf(position.x,2)+powf(position.y,2) > powf(g2.soaring_controller.max_radius,2) && + previous_mode->mode_number()!=Mode::Number::AUTO) { + // Some other loiter status, and outside of maximum soaring radius, and previous mode wasn't AUTO + gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Outside SOAR_MAX_RADIUS, RTL"); + set_mode(mode_rtl, ModeReason::SOARING_DRIFT_EXCEEDED); + break; + } + + // If previous mode was AUTO and there was a previous NAV command, we can use previous and next wps for drift calculation + // with respect to the desired direction of travel. If these vectors are zero, drift will be calculated from thermal start + // position only, without taking account of the desired direction of travel. Vector2f prev_wp, next_wp; - // If previous mode was AUTO and there was a previous NAV command, we can use previous and next wps for drift calculation. - if (previous_mode->mode_number() == Mode::Number::AUTO) { + if (previous_mode == &mode_auto) { AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd(); AP_Mission::Mission_Command prev_nav_cmd; if (!(mission.get_next_nav_cmd(mission.get_prev_nav_cmd_with_wp_index(), prev_nav_cmd) && - prev_nav_cmd.content.location.get_vector_xy_from_origin_NE(prev_wp) && + prev_nav_cmd.content.location.get_vector_xy_from_origin_NE(prev_wp) && current_nav_cmd.content.location.get_vector_xy_from_origin_NE(next_wp))) { - prev_wp.x = 0.0; - prev_wp.y = 0.0; - next_wp.x = 0.0; - next_wp.y = 0.0; + prev_wp.zero(); + next_wp.zero(); } } + // Get the status of the soaring controller cruise checks. const SoaringController::LoiterStatus loiterStatus = g2.soaring_controller.check_cruise_criteria(prev_wp/100, next_wp/100); if (loiterStatus == SoaringController::LoiterStatus::GOOD_TO_KEEP_LOITERING) { // Reset loiter angle, so that the loiter exit heading criteria // only starts expanding when we're ready to exit. plane.loiter.sum_cd = 0; - plane.soaring_mode_timer = AP_HAL::millis(); + plane.soaring_mode_timer_ms = AP_HAL::millis(); + + //update the wp location + g2.soaring_controller.get_target(next_WP_loc); + break; } // Some other loiter status, we need to think about exiting loiter. - uint32_t timer = AP_HAL::millis() - plane.soaring_mode_timer; + const uint32_t time_in_loiter_ms = AP_HAL::millis() - plane.soaring_mode_timer_ms; - // Check distance to home. - Vector3f position; - if (!ahrs.get_relative_position_NED_home(position)) { - return; - } else if (g2.soaring_controller.max_radius >= 0 && - powf(position.x,2)+powf(position.y,2) > powf(g2.soaring_controller.max_radius,2) && - previous_mode->mode_number()!=Mode::Number::AUTO) { - // Some other loiter status, and outside of maximum soaring radius, and previous mode wasn't AUTO - gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Outside SOAR_MAX_RADIUS, RTL"); - set_mode(mode_rtl, ModeReason::SOARING_DRIFT_EXCEEDED); + if (!soaring_exit_heading_aligned() && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && time_in_loiter_ms < 20000) { + // Heading not lined up, and not timed out or in a condition requiring immediate exit. + break; } - const char* strTooHigh = "Soaring: Too high, restoring "; - const char* strTooLow = "Soaring: Too low, restoring "; - const char* strTooWeak = "Soaring: Thermal ended, restoring "; - const char* strTooFar = "Soaring: Drifted too far, restoring "; - - // 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 && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) { - break; - } - switch (loiterStatus) { - 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; - 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; - case SoaringController::LoiterStatus::DRIFT_EXCEEDED: - gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_fbwb.name()); - set_mode(mode_fbwb, ModeReason::SOARING_DRIFT_EXCEEDED); - break; - } // switch louterStatus - break; - } + // Heading lined up and loiter status not good to continue. Need to switch mode. - case Mode::Number::CRUISE: { - const bool headingLinedupToCruise = plane.mode_loiter.isHeadingLinedUp_cd(cruise_state.locked_heading_cd); - if (cruise_state.locked_heading && !headingLinedupToCruise && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) { - break; - } - // return to cruise with old ground course - const CruiseState cruise = cruise_state; - switch (loiterStatus) { - case SoaringController::LoiterStatus::ALT_TOO_HIGH: - gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_cruise.name()); - set_mode(mode_cruise, ModeReason::SOARING_ALT_TOO_HIGH); - break; - 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); - break; - case SoaringController::LoiterStatus::DRIFT_EXCEEDED: - gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_cruise.name()); - set_mode(mode_cruise, ModeReason::SOARING_DRIFT_EXCEEDED); - break; - } // switch loiterStatus - cruise_state = cruise; - set_target_altitude_current(); + // Determine appropriate mode. + Mode* exit_mode = previous_mode; + + if (loiterStatus == SoaringController::LoiterStatus::ALT_TOO_LOW && + ((previous_mode == &mode_cruise) || (previous_mode == &mode_fbwb))) { + exit_mode = &mode_rtl; + } + + // Print message and set mode. + switch (loiterStatus) { + case SoaringController::LoiterStatus::ALT_TOO_HIGH: + soaring_restore_mode("Too high", ModeReason::SOARING_ALT_TOO_HIGH, *exit_mode); break; - } // case Cruise - - case Mode::Number::AUTO: { - //Get the lat/lon of next Nav waypoint after this one: - AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd();; - const bool headingLinedupToWP = plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location); - if (!headingLinedupToWP && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) { - 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; - 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; - 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; - case SoaringController::LoiterStatus::DRIFT_EXCEEDED: - gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_auto.name()); - set_mode(mode_auto, ModeReason::SOARING_DRIFT_EXCEEDED); - break; - } // switch loiterStatus + case SoaringController::LoiterStatus::ALT_TOO_LOW: + soaring_restore_mode("Too low", ModeReason::SOARING_ALT_TOO_LOW, *exit_mode); + break; + default: + case SoaringController::LoiterStatus::THERMAL_WEAK: + soaring_restore_mode("Thermal ended", ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED, *exit_mode); break; - } // case AUTO - default: // all other modes + case SoaringController::LoiterStatus::DRIFT_EXCEEDED: + soaring_restore_mode("Drifted too far", ModeReason::SOARING_DRIFT_EXCEEDED, *exit_mode); break; - } // switch previous_mode + } // switch loiterStatus + break; - } // case loiter + + } // case loiter } // switch control_mode +} - if (control_mode == &mode_loiter) { - // still in thermal - need to update the wp location - g2.soaring_controller.get_target(next_WP_loc); - +bool Plane::soaring_exit_heading_aligned() const +{ + // Return true if the current heading is aligned with the next objective. + // If home is not set, or heading not locked, return true to avoid delaying mode change. + switch (previous_mode->mode_number()) { + case Mode::Number::AUTO: { + //Get the lat/lon of next Nav waypoint after this one: + AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd();; + return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location); } + case Mode::Number::FLY_BY_WIRE_B: + return (!AP::ahrs().home_is_set() || plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home())); + case Mode::Number::CRUISE: + return (!cruise_state.locked_heading || plane.mode_loiter.isHeadingLinedUp_cd(cruise_state.locked_heading_cd)); + default: + break; + } + return true; +} +void Plane::soaring_restore_mode(const char *reason, ModeReason modereason, Mode &exit_mode) +{ + gcs().send_text(MAV_SEVERITY_INFO, "Soaring: %s, restoring %s", reason, exit_mode.name()); + set_mode(exit_mode, modereason); } #endif // SOARING_ENABLED