diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c051d8bd40..2a2cbed9da 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -41,6 +41,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case Mode::Number::AUTO: case Mode::Number::RTL: case Mode::Number::LOITER: + case Mode::Number::THERMAL: case Mode::Number::AVOID_ADSB: case Mode::Number::GUIDED: case Mode::Number::CIRCLE: diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index e2ec3149c8..a933195450 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -83,6 +83,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) case Mode::Number::CIRCLE: case Mode::Number::TAKEOFF: case Mode::Number::QRTL: + case Mode::Number::THERMAL: rate_controlled = true; attitude_stabilized = true; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 56de153687..74b332febb 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -159,6 +159,7 @@ public: friend class ModeQAcro; friend class ModeQAutotune; friend class ModeTakeoff; + friend class ModeThermal; Plane(void); @@ -279,6 +280,9 @@ private: ModeQAcro mode_qacro; ModeQAutotune mode_qautotune; ModeTakeoff mode_takeoff; +#if SOARING_ENABLED == ENABLED + ModeThermal mode_thermal; +#endif // This is the state of the flight control system // There are multiple states defined such as MANUAL, FBW-A, AUTO diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 34b7f9fda5..6c237e577f 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -294,9 +294,6 @@ #define OSD_ENABLED DISABLED #endif -#ifndef SOARING_ENABLED - #define SOARING_ENABLED !HAL_MINIMIZE_FEATURES -#endif #ifndef OFFBOARD_GUIDED #define OFFBOARD_GUIDED !HAL_MINIMIZE_FEATURES #endif diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 33a0677b67..e2af5c81db 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -73,6 +73,11 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::TAKEOFF: ret = &mode_takeoff; break; + case Mode::Number::THERMAL: +#if SOARING_ENABLED == ENABLED + ret = &mode_thermal; +#endif + break; } return ret; } diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 4164bb1ae4..3970e28cd4 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -43,6 +43,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso case Mode::Number::AVOID_ADSB: case Mode::Number::GUIDED: case Mode::Number::LOITER: + case Mode::Number::THERMAL: if(g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) { failsafe.saved_mode_number = control_mode->mode_number(); failsafe.saved_mode_set = true; @@ -110,6 +111,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::AVOID_ADSB: case Mode::Number::GUIDED: case Mode::Number::LOITER: + case Mode::Number::THERMAL: if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 1fe7f621b2..00ffdff82a 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -4,6 +4,7 @@ #include #include #include +#include class Mode { @@ -39,6 +40,7 @@ public: QRTL = 21, QAUTOTUNE = 22, QACRO = 23, + THERMAL = 24, }; // Constructor @@ -521,3 +523,25 @@ protected: bool _enter() override; }; + +#if SOARING_ENABLED + +class ModeThermal: public Mode +{ +public: + + Number mode_number() const override { return Number::THERMAL; } + const char *name() const override { return "THERMAL"; } + const char *name4() const override { return "THML"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + + void navigate() override; + +protected: + + bool _enter() override; +}; + +#endif diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 645bb647d2..e2d4d1d736 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -9,13 +9,6 @@ bool ModeLoiter::_enter() plane.do_loiter_at_location(); plane.loiter_angle_reset(); -#if SOARING_ENABLED == ENABLED - if (plane.g2.soaring_controller.is_active()) { - plane.g2.soaring_controller.init_thermalling(); - plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path - } -#endif - return true; } diff --git a/ArduPlane/mode_thermal.cpp b/ArduPlane/mode_thermal.cpp new file mode 100644 index 0000000000..8ae1b685d3 --- /dev/null +++ b/ArduPlane/mode_thermal.cpp @@ -0,0 +1,37 @@ +#include "mode.h" +#include "Plane.h" + +#if SOARING_ENABLED == ENABLED + +bool ModeThermal::_enter() +{ + if (!plane.g2.soaring_controller.is_active()) { + return false; + } + + plane.throttle_allows_nudging = true; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = true; + plane.do_loiter_at_location(); + plane.loiter_angle_reset(); + + plane.g2.soaring_controller.init_thermalling(); + plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path + + return true; +} + +void ModeThermal::update() +{ + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + plane.calc_throttle(); +} + +void ModeThermal::navigate() +{ + // Zero indicates to use WP_LOITER_RAD + plane.update_loiter(0); +} + +#endif diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index 32ea20a678..2a5a9a517b 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -26,8 +26,8 @@ void Plane::update_soaring() { case Mode::Number::CRUISE: g2.soaring_controller.suppress_throttle(); break; - case Mode::Number::LOITER: - // Never use throttle in LOITER with soaring active. + case Mode::Number::THERMAL: + // Never use throttle in THERMAL with soaring active. g2.soaring_controller.set_throttle_suppressed(true); break; default: @@ -52,12 +52,12 @@ void Plane::update_soaring() { g2.soaring_controller.update_cruising(); if (g2.soaring_controller.check_thermal_criteria()) { - gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_loiter.name()); - set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED); + gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_thermal.name()); + set_mode(mode_thermal, ModeReason::SOARING_THERMAL_DETECTED); } break; - case Mode::Number::LOITER: { + case Mode::Number::THERMAL: { // Update thermal estimate and check for switch back to AUTO g2.soaring_controller.update_thermalling(); // Update estimate diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index ea833cb082..f51a0e2e1f 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -16,6 +16,10 @@ #include "Variometer.h" #include +#ifndef SOARING_ENABLED + #define SOARING_ENABLED !HAL_MINIMIZE_FEATURES +#endif + #define INITIAL_THERMAL_STRENGTH 2.0 #define INITIAL_THERMAL_RADIUS 80.0 #define INITIAL_STRENGTH_COVARIANCE 0.0049