ardupilot/ArduPlane/soaring.cpp

60 lines
1.8 KiB
C++

#include "Plane.h"
#if HAL_SOARING_ENABLED
/*
* ArduSoar support function
*
* Peter Braswell, Samuel Tabor, Andrey Kolobov, and Iain Guilliard
*/
void Plane::update_soaring() {
// Check if soaring is active. Also sets throttle suppressed
// status on active state changes.
plane.g2.soaring_controller.update_active_state();
if (!g2.soaring_controller.is_active()) {
return;
}
g2.soaring_controller.update_vario();
if (control_mode == &mode_thermal) {
// We are currently thermalling; suppress throttle and update
// the thermal mode.
// Never use throttle in THERMAL with soaring active.
g2.soaring_controller.set_throttle_suppressed(true);
// Update THERMAL mode soaring logic.
mode_thermal.update_soaring();
return;
}
if (control_mode->does_automatic_thermal_switch()) {
// We might decide to start thermalling; if we're not under
// powered flight then check for thermals and potentially
// switch modes.
// Check for throttle suppression change.
if (g2.soaring_controller.suppress_throttle()) {
// Throttle is suppressed, perform cruising modes update and check for mode switch.
// Cruising modes update.
g2.soaring_controller.update_cruising();
// Test for switch into THERMAL mode
if (g2.soaring_controller.check_thermal_criteria()) {
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_thermal.name());
set_mode(mode_thermal, ModeReason::SOARING_THERMAL_DETECTED);
}
}
return;
}
// We are not thermalling and won't start to from this mode. Allow throttle.
g2.soaring_controller.set_throttle_suppressed(false);
}
#endif // SOARING_ENABLED