diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index e936e0e43c..19d7581e80 100644 --- 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 index 9d817942da..281dcd3e12 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -25,13 +25,15 @@ #include #include #include +#include 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 @@ private: // 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;