AP_TECS: disable bad descent for soaring

Yukong · Mar 13, 2017 · 5fdebe2 · 5fdebe2
1 parent 9f2f070
commit 5fdebe2
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 2 deletions.
diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
@@ -751,6 +751,11 @@ void AP_TECS::_detect_bad_descent(void)
     {
         _flags.badDescent = false;
     }
+
+    // when soaring is active we never trigger a bad descent
+    if (_soaring_controller.is_active() && _soaring_controller.get_throttle_suppressed()) {
+        _flags.badDescent = false;        
+    }
 }
 
 void AP_TECS::_update_pitch(void)

diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
@@ -25,13 +25,15 @@
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 #include <DataFlash/DataFlash.h>
 #include <AP_Landing/AP_Landing.h>
+#include <AP_Soaring/AP_Soaring.h>
 
 class AP_TECS : public AP_SpdHgtControl {
 public:
-    AP_TECS(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, const AP_Landing &landing) :
+    AP_TECS(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, const AP_Landing &landing, const SoaringController &soaring_controller) :
         _ahrs(ahrs),
         aparm(parms),
-        _landing(landing)
+        _landing(landing),
+        _soaring_controller(soaring_controller)
     {
         AP_Param::setup_object_defaults(this, var_info);
     }
@@ -133,6 +135,9 @@ class AP_TECS : public AP_SpdHgtControl {
 
     // reference to const AP_Landing to access it's params
     const AP_Landing &_landing;
+
+    // reference to const SoaringController to access its state
+    const SoaringController &_soaring_controller;
 
     // TECS tuning parameters
     AP_Float _hgtCompFiltOmega;