mirror of https://github.com/ArduPilot/ardupilot
Plane: add reason to exit Thermal loiter and adjust LOW alt reaon to cause RTL
This commit is contained in:
parent
52ae093a97
commit
f6396d7a6d
|
@ -43,6 +43,9 @@ void Plane::update_soaring() {
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (control_mode->mode_number()) {
|
switch (control_mode->mode_number()) {
|
||||||
|
default:
|
||||||
|
// nothing to do
|
||||||
|
break;
|
||||||
case Mode::Number::AUTO:
|
case Mode::Number::AUTO:
|
||||||
case Mode::Number::FLY_BY_WIRE_B:
|
case Mode::Number::FLY_BY_WIRE_B:
|
||||||
case Mode::Number::CRUISE:
|
case Mode::Number::CRUISE:
|
||||||
|
@ -50,50 +53,101 @@ void Plane::update_soaring() {
|
||||||
g2.soaring_controller.update_cruising();
|
g2.soaring_controller.update_cruising();
|
||||||
|
|
||||||
if (g2.soaring_controller.check_thermal_criteria()) {
|
if (g2.soaring_controller.check_thermal_criteria()) {
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter");
|
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_loiter.name());
|
||||||
set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED);
|
set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case Mode::Number::LOITER:
|
case Mode::Number::LOITER: {
|
||||||
// Update thermal estimate and check for switch back to AUTO
|
// Update thermal estimate and check for switch back to AUTO
|
||||||
g2.soaring_controller.update_thermalling(); // Update estimate
|
g2.soaring_controller.update_thermalling(); // Update estimate
|
||||||
|
|
||||||
if (g2.soaring_controller.check_cruise_criteria()) {
|
const SoaringController::LoiterStatus loiterStatus = g2.soaring_controller.check_cruise_criteria();
|
||||||
|
|
||||||
|
if (loiterStatus == SoaringController::LoiterStatus::THERMAL_GOOD_TO_KEEP_LOITERING ||
|
||||||
|
loiterStatus == SoaringController::LoiterStatus::SOARING_DISABLED) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
const char* strTooHigh = "Soaring: Too high, restoring ";
|
||||||
|
const char* strTooLow = "Soaring: Too high, restoring ";
|
||||||
|
const char* strTooWeak = "Soaring: Thermal ended, restoring ";
|
||||||
|
|
||||||
// Exit as soon as thermal state estimate deteriorates
|
// Exit as soon as thermal state estimate deteriorates
|
||||||
switch (previous_mode->mode_number()) {
|
switch (previous_mode->mode_number()) {
|
||||||
case Mode::Number::FLY_BY_WIRE_B:
|
case Mode::Number::FLY_BY_WIRE_B: {
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL");
|
switch (loiterStatus) {
|
||||||
set_mode(mode_rtl, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
|
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;
|
||||||
|
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
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case Mode::Number::CRUISE: {
|
case Mode::Number::CRUISE: {
|
||||||
// return to cruise with old ground course
|
// return to cruise with old ground course
|
||||||
CruiseState cruise = cruise_state;
|
const CruiseState cruise = cruise_state;
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE");
|
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;
|
||||||
|
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;
|
||||||
|
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);
|
set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
|
||||||
|
break;
|
||||||
|
} // switch loiterStatus
|
||||||
cruise_state = cruise;
|
cruise_state = cruise;
|
||||||
set_target_altitude_current();
|
set_target_altitude_current();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case Mode::Number::AUTO:
|
case Mode::Number::AUTO:
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO");
|
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:
|
||||||
|
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name());
|
||||||
set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
|
set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
|
||||||
break;
|
break;
|
||||||
|
} // switch loiterStatus
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
} // switch previous_mode
|
||||||
} else {
|
break;
|
||||||
|
} // loiter case
|
||||||
|
}} // switch control_mode
|
||||||
|
|
||||||
|
|
||||||
|
if (control_mode == &mode_loiter) {
|
||||||
// still in thermal - need to update the wp location
|
// still in thermal - need to update the wp location
|
||||||
g2.soaring_controller.get_target(next_WP_loc);
|
g2.soaring_controller.get_target(next_WP_loc);
|
||||||
|
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
default:
|
|
||||||
// nothing to do
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // SOARING_ENABLED
|
#endif // SOARING_ENABLED
|
||||||
|
|
|
@ -53,4 +53,6 @@ enum class ModeReason : uint8_t {
|
||||||
UNAVAILABLE,
|
UNAVAILABLE,
|
||||||
AUTOROTATION_START,
|
AUTOROTATION_START,
|
||||||
AUTOROTATION_BAILOUT,
|
AUTOROTATION_BAILOUT,
|
||||||
|
SOARING_ALT_TOO_HIGH,
|
||||||
|
SOARING_ALT_TOO_LOW,
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue