Plane: added a soaring controller to Arduplane

ArduPlane/ArduPlane.cpp

@@ -76,6 +76,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(Log_Write_Fast,         25,    300),
     SCHED_TASK(update_logging1,        10,    300),
     SCHED_TASK(update_logging2,        10,    300),
+    SCHED_TASK(update_soaring,         50,    400),
     SCHED_TASK(parachute_check,        10,    200),
     SCHED_TASK(terrain_update,         10,    200),
     SCHED_TASK(update_is_flying_5Hz,    5,    100),

ArduPlane/GCS_Mavlink.cpp

@@ -252,7 +252,7 @@ void Plane::send_vfr_hud(mavlink_channel_t chan)
         (ahrs.yaw_sensor / 100) % 360,
         abs(throttle_percentage()),
         current_loc.alt / 100.0f,
-        barometer.get_climb_rate());
+        (g2.soaring_controller.is_active() ? g2.soaring_controller.get_vario_reading() : barometer.get_climb_rate()));
 }
 
 /*

ArduPlane/Parameters.cpp

@@ -1213,11 +1213,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/RC_Channel/RC_Channels.cpp
     AP_SUBGROUPINFO(rc_channels, "RC", 7, ParametersG2, RC_Channels),
     
+    // @Group: SOAR_
+    // @Path: ../libraries/AP_Soaring/AP_Soaring.cpp
+    AP_SUBGROUPINFO(soaring_controller, "SOAR_", 8, ParametersG2, SoaringController),
+  
     AP_GROUPEND
 };
 
 ParametersG2::ParametersG2(void) :
-    ice_control(plane.rpm_sensor, plane.ahrs)
+    ice_control(plane.rpm_sensor, plane.ahrs),
+    soaring_controller(plane.ahrs, plane.TECS_controller, plane.aparm)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

ArduPlane/Parameters.h

@@ -542,6 +542,9 @@ public:
 
     // whether to enforce acceptance of packets only from sysid_my_gcs
     AP_Int8 sysid_enforce;
+    
+    // ArduSoar parameters
+    SoaringController soaring_controller;
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -76,6 +76,7 @@
 #include <AP_NavEKF3/AP_NavEKF3.h>
 #include <AP_Mission/AP_Mission.h>     // Mission command library
 
+#include <AP_Soaring/AP_Soaring.h>
 #include <AP_Notify/AP_Notify.h>      // Notify library
 #include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
 
@@ -216,7 +217,7 @@ private:
 #endif
 
     AP_L1_Control L1_controller {ahrs};
-    AP_TECS TECS_controller {ahrs, aparm, landing};
+    AP_TECS TECS_controller {ahrs, aparm, landing, g2.soaring_controller};
 
     // Attitude to servo controllers
     AP_RollController  rollController {ahrs, aparm, DataFlash};
@@ -410,7 +411,7 @@ private:
     } acro_state;
 
     // CRUISE controller state
-    struct {
+    struct CruiseState {
         bool locked_heading;
         int32_t locked_heading_cd;
         uint32_t lock_timer_ms;
@@ -1091,6 +1092,7 @@ private:
 #endif
     void accel_cal_update(void);
     void update_soft_armed();
+    void update_soaring();
 
     // support for AP_Avoidance custom flight mode, AVOID_ADSB
     bool avoid_adsb_init(bool ignore_checks);

ArduPlane/defines.h

@@ -80,6 +80,10 @@ enum mode_reason_t {
     MODE_REASON_FENCE_BREACH,
     MODE_REASON_AVOIDANCE,
     MODE_REASON_AVOIDANCE_RECOVERY,
+    MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING,
+    MODE_REASON_SOARING_THERMAL_DETECTED,
+    MODE_REASON_SOARING_IN_THERMAL,
+    MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED
 };
 
 // type of stick mixing enabled
@@ -135,7 +139,9 @@ enum log_messages {
     LOG_STATUS_MSG,
     LOG_OPTFLOW_MSG,
     LOG_QTUN_MSG,
-    LOG_PARAMTUNE_MSG
+    LOG_PARAMTUNE_MSG,
+    LOG_THERMAL_MSG,
+    LOG_VARIO_MSG
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)

ArduPlane/make.inc

@@ -63,3 +63,4 @@ LIBRARIES += AP_Stats
 LIBRARIES += AP_Landing
 LIBRARIES += AP_Beacon
 LIBRARIES += PID
+LIBRARIES += AP_Soaring

ArduPlane/servos.cpp

@@ -477,6 +477,14 @@ void Plane::set_servos_controlled(void)
         // ask quadplane code for forward throttle
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, quadplane.forward_throttle_pct());
     }
+
+    // suppress throttle when soaring is active
+    if ((control_mode == FLY_BY_WIRE_B || control_mode == CRUISE ||
+        control_mode == AUTO || control_mode == LOITER) &&
+        g2.soaring_controller.is_active() &&
+        g2.soaring_controller.get_throttle_suppressed()) {
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
+    }
 }
 
 /*

ArduPlane/soaring.cpp

@@ -0,0 +1,96 @@
+#include "Plane.h"
+
+/*
+*  ArduSoar support function
+*
+*  Peter Braswell, Samuel Tabor, Andrey Kolobov, and Iain Guilliard
+*/
+void Plane::update_soaring() {
+    
+    if (!g2.soaring_controller.is_active()) {
+        return;
+    }
+    
+    g2.soaring_controller.update_vario();
+
+    // Check for throttle suppression change.
+    switch (control_mode){
+    case AUTO:
+        g2.soaring_controller.suppress_throttle();
+        break;
+    case FLY_BY_WIRE_B:
+    case CRUISE:
+        if (!g2.soaring_controller.suppress_throttle()) {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: forcing RTL");
+            set_mode(RTL, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING);
+        }
+        break;
+    case LOITER:
+        // Do nothing. We will switch back to auto/rtl before enabling throttle.
+        break;
+    default:
+        // This does not affect the throttle since suppressed is only checked in the above three modes. 
+        // It ensures that the soaring always starts with throttle suppressed though.
+        g2.soaring_controller.set_throttle_suppressed(true);
+        break;
+    }
+
+    // Nothing to do if we are in powered flight
+    if (!g2.soaring_controller.get_throttle_suppressed() && aparm.throttle_max > 0) {
+        return;
+    }
+
+    switch (control_mode){
+    case AUTO:
+    case FLY_BY_WIRE_B:
+    case CRUISE:
+        // Test for switch into thermalling mode
+        g2.soaring_controller.update_cruising();
+
+        if (g2.soaring_controller.check_thermal_criteria()) {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter");
+            set_mode(LOITER, MODE_REASON_SOARING_THERMAL_DETECTED);
+        }
+        break;
+
+    case LOITER:
+        // Update thermal estimate and check for switch back to AUTO
+        g2.soaring_controller.update_thermalling();  // Update estimate
+
+        if (g2.soaring_controller.check_cruise_criteria()) {
+            // Exit as soon as thermal state estimate deteriorates
+            switch (previous_mode) {
+            case FLY_BY_WIRE_B:
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL");
+                set_mode(RTL, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
+                break;
+
+            case CRUISE: {
+                // return to cruise with old ground course
+                CruiseState cruise = cruise_state;
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE");
+                set_mode(CRUISE, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
+                cruise_state = cruise;
+                set_target_altitude_current();
+                break;
+            }
+
+            case AUTO:
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO");
+                set_mode(AUTO, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
+                break;
+
+            default:
+                break;
+            }
+        } else {
+            // still in thermal - need to update the wp location
+            g2.soaring_controller.get_target(next_WP_loc);
+        }
+        break;
+    default:
+        // nothing to do
+        break;
+    }
+}
+

ArduPlane/system.cpp

@@ -417,12 +417,20 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
         auto_navigation_mode = false;
         cruise_state.locked_heading = false;
         cruise_state.lock_timer_ms = 0;
+        
+        // for ArduSoar soaring_controller
+        g2.soaring_controller.init_cruising();
+        
         set_target_altitude_current();
         break;
 
     case FLY_BY_WIRE_B:
         auto_throttle_mode = true;
         auto_navigation_mode = false;
+        
+        // for ArduSoar soaring_controller
+        g2.soaring_controller.init_cruising();
+
         set_target_altitude_current();
         break;
 
@@ -444,6 +452,8 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
         next_WP_loc = prev_WP_loc = current_loc;
         // start or resume the mission, based on MIS_AUTORESET
         mission.start_or_resume();
+		
+        g2.soaring_controller.init_cruising();
         break;
 
     case RTL:
@@ -457,6 +467,13 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
         auto_throttle_mode = true;
         auto_navigation_mode = true;
         do_loiter_at_location();
+		
+        if (g2.soaring_controller.is_active() &&
+            g2.soaring_controller.suppress_throttle()) {
+			g2.soaring_controller.init_thermalling();
+			g2.soaring_controller.get_target(next_WP_loc); // ahead on flight path
+		}
+		
         break;
 
     case AVOID_ADSB:

ArduPlane/wscript

@@ -34,6 +34,7 @@ def build(bld):
             'AP_Landing',
             'AP_Beacon',
             'PID',
+            'AP_Soaring'
         ],
     )