Plane: Add THERMAL mode for thermal soaring.

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:

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;

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

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

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;
 }

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();

ArduPlane/mode.h

@@ -4,6 +4,7 @@
 #include <AP_Common/Location.h>
 #include <stdint.h>
 #include <AP_Common/Location.h>
+#include <AP_Soaring/AP_Soaring.h>
 
 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

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;
 }
 

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

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
 

libraries/AP_Soaring/AP_Soaring.h

@@ -16,6 +16,10 @@
 #include "Variometer.h"
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 
+#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