From 7bf1fe1277ef2e6bfaae880f66ad27fa818464d9 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 12 Nov 2021 17:55:01 +0000
Subject: [PATCH] AP_Soaring: remove SpdHgt and use TECS direct

---
 libraries/AP_Soaring/AP_Soaring.cpp | 10 +++++-----
 libraries/AP_Soaring/AP_Soaring.h   |  6 +++---
 libraries/AP_Soaring/Variometer.h   |  2 +-
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
index 77d7735cf3..1c213d10ca 100644
--- a/libraries/AP_Soaring/AP_Soaring.cpp
+++ b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -142,8 +142,8 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
     AP_GROUPEND
 };
 
-SoaringController::SoaringController(AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms) :
-    _spdHgt(spdHgt),
+SoaringController::SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms) :
+    _tecs(tecs),
     _vario(parms),
     _aparm(parms),
     _throttle_suppressed(true)
@@ -169,7 +169,7 @@ bool SoaringController::suppress_throttle()
         set_throttle_suppressed(true);
 
         // Zero the pitch integrator - the nose is currently raised to climb, we need to go back to glide.
-        _spdHgt.reset_pitch_I();
+        _tecs.reset_pitch_I();
 
         _cruise_start_time_us = AP_HAL::micros64();
 
@@ -411,7 +411,7 @@ void SoaringController::set_throttle_suppressed(bool suppressed)
     _throttle_suppressed = suppressed;
 
     // Let the TECS know.
-    _spdHgt.set_gliding_requested_flag(suppressed);
+    _tecs.set_gliding_requested_flag(suppressed);
 }
 
 bool SoaringController::check_drift(Vector2f prev_wp, Vector2f next_wp)
@@ -463,7 +463,7 @@ bool SoaringController::check_drift(Vector2f prev_wp, Vector2f next_wp)
 float SoaringController::get_thermalling_radius() const
 {
     // Thermalling radius is controlled by parameter SOAR_THML_BANK and true target airspeed.
-    const float target_aspd = _spdHgt.get_target_airspeed() * AP::ahrs().get_EAS2TAS();
+    const float target_aspd = _tecs.get_target_airspeed() * AP::ahrs().get_EAS2TAS();
     const float radius = (target_aspd*target_aspd) / (GRAVITY_MSS * tanf(thermal_bank*DEG_TO_RAD));
 
     return radius;
diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h
index 9a772f6fd6..9efd3ca110 100644
--- a/libraries/AP_Soaring/AP_Soaring.h
+++ b/libraries/AP_Soaring/AP_Soaring.h
@@ -14,7 +14,7 @@
 #include <AP_Math/AP_Math.h>
 #include "ExtendedKalmanFilter.h"
 #include "Variometer.h"
-#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
+#include <AP_TECS/AP_TECS.h>
 
 #ifndef HAL_SOARING_ENABLED
  #define HAL_SOARING_ENABLED !HAL_MINIMIZE_FEATURES
@@ -30,7 +30,7 @@
 
 class SoaringController {
     ExtendedKalmanFilter _ekf{};
-    AP_SpdHgtControl &_spdHgt;
+    AP_TECS &_tecs;
     Variometer _vario;
     const AP_Vehicle::FixedWing &_aparm;
 
@@ -77,7 +77,7 @@ protected:
     AP_Float thermal_bank;
 
 public:
-    SoaringController(AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms);
+    SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms);
 
     enum class LoiterStatus {
         DISABLED,
diff --git a/libraries/AP_Soaring/Variometer.h b/libraries/AP_Soaring/Variometer.h
index 3010630142..db571f0856 100644
--- a/libraries/AP_Soaring/Variometer.h
+++ b/libraries/AP_Soaring/Variometer.h
@@ -7,8 +7,8 @@ Manages the estimation of aircraft total energy, drag and vertical air velocity.
 
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Param/AP_Param.h>
-#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 #include <Filter/AverageFilter.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 
 class Variometer {