AP_Soaring: Override airspeed and flap angle.

ArduPlane/navigation.cpp

@@ -194,6 +194,27 @@ void Plane::calc_airspeed_errors()
 
 #endif // OFFBOARD_GUIDED == ENABLED
 
+#if HAL_SOARING_ENABLED
+    } else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
+        if (control_mode == &mode_thermal) {
+            float arspd = g2.soaring_controller.get_thermalling_target_airspeed();
+
+            if (arspd > 0) {
+                target_airspeed_cm = arspd * 100;
+            } else {
+                target_airspeed_cm = aparm.airspeed_cruise_cm;
+            }
+        } else if (control_mode == &mode_auto) {
+            float arspd = g2.soaring_controller.get_cruising_target_airspeed();
+
+            if (arspd > 0) {
+                target_airspeed_cm = arspd * 100;
+            } else {
+                target_airspeed_cm = aparm.airspeed_cruise_cm;
+            }
+        }
+#endif
+
     } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
         // Landing airspeed target
         target_airspeed_cm = landing.get_target_airspeed_cm();

ArduPlane/servos.cpp

@@ -629,6 +629,12 @@ void Plane::set_servos_flaps(void)
             auto_flap_percent = g.flap_1_percent;
         } //else flaps stay at default zero deflection
 
+#if HAL_SOARING_ENABLED
+        if (control_mode == &mode_thermal) {
+            auto_flap_percent = g2.soaring_controller.get_thermalling_flap();
+        }
+#endif
+
         /*
           special flap levels for takeoff and landing. This works
           better than speed based flaps as it leads to less

libraries/AP_Soaring/AP_Soaring.cpp

@@ -139,6 +139,29 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
     // @Units: deg
     AP_GROUPINFO("THML_BANK", 18, SoaringController, thermal_bank, 30.0),
 
+    // 19 reserved for POLAR_LEARN.
+
+    // @Param: THML_ARSPD
+    // @DisplayName: Specific setting for airspeed when thermalling.
+    // @Description: If non-zero this airspeed will be used when thermalling.
+    // @Range: 5 50
+    // @User: Advanced
+    AP_GROUPINFO("THML_ARSPD", 20, SoaringController, soar_thermal_airspeed, 0),
+
+    // @Param: CRSE_ARSPD
+    // @DisplayName: Specific setting for airspeed when cruising.
+    // @Description: If non-zero this airspeed will be used when cruising.
+    // @Range: 5 50
+    // @User: Advanced
+    AP_GROUPINFO("CRSE_ARSPD", 21, SoaringController, soar_cruise_airspeed, 0),
+
+    // @Param: THML_FLAP
+    // @DisplayName: Flap percent to be used during thermalling flight.
+    // @Description: This sets the flap when in LOITER with soaring active. Overrides the usual auto flap behaviour.
+    // @Range: 0 100
+    // @User: Advanced
+    AP_GROUPINFO("THML_FLAP", 22, SoaringController, soar_thermal_flap, 0),
+
     AP_GROUPEND
 };
 
@@ -469,4 +492,14 @@ float SoaringController::get_thermalling_radius() const
     return radius;
 }
 
+float SoaringController::get_thermalling_target_airspeed()
+{
+    return soar_thermal_airspeed;
+}
+
+float SoaringController::get_cruising_target_airspeed()
+{
+    return soar_cruise_airspeed;
+}
+
 #endif // HAL_SOARING_ENABLED

libraries/AP_Soaring/AP_Soaring.h

@@ -74,6 +74,9 @@ protected:
     AP_Float alt_cutoff;
     AP_Float max_drift;
     AP_Float thermal_bank;
+    AP_Float soar_thermal_airspeed;
+    AP_Float soar_cruise_airspeed;
+    AP_Float soar_thermal_flap;
 
 public:
     SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms);
@@ -135,6 +138,15 @@ public:
 
     float get_thermalling_radius() const;
 
+    float get_thermalling_target_airspeed();
+
+    float get_cruising_target_airspeed();
+
+    float get_thermalling_flap() const
+    {
+        return soar_thermal_flap;
+    }
+
 private:
 
     ActiveStatus _last_update_status;