diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 813aabd249..d4e6822e70 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -761,11 +761,6 @@ 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) @@ -946,7 +941,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t ptchMinCO_cd, int16_t throttle_nudge, float hgt_afe, - float load_factor) + float load_factor, + bool soaring_active) { // Calculate time in seconds since last update uint64_t now = AP_HAL::micros64(); @@ -1072,6 +1068,11 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // Detect bad descent due to demanded airspeed being too high _detect_bad_descent(); + // when soaring is active we never trigger a bad descent + if (soaring_active) { + _flags.badDescent = false; + } + // Calculate pitch demand _update_pitch(); diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 28c02637ca..ec3b67ded1 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -25,15 +25,13 @@ #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, const SoaringController &soaring_controller) + AP_TECS(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, const AP_Landing &landing) : _ahrs(ahrs) , aparm(parms) , _landing(landing) - , _soaring_controller(soaring_controller) { AP_Param::setup_object_defaults(this, var_info); } @@ -45,7 +43,7 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(void); + void update_50hz(void) override; // Update the control loop calculations void update_pitch_throttle(int32_t hgt_dem_cm, @@ -55,47 +53,48 @@ public: int32_t ptchMinCO_cd, int16_t throttle_nudge, float hgt_afe, - float load_factor); + float load_factor, + bool soaring_active) override; // demanded throttle in percentage // should return -100 to 100, usually positive unless reverse thrust is enabled via _THRminf < 0 - int32_t get_throttle_demand(void) { + int32_t get_throttle_demand(void) override { return int32_t(_throttle_dem * 100.0f); } // demanded pitch angle in centi-degrees // should return between -9000 to +9000 - int32_t get_pitch_demand(void) { + int32_t get_pitch_demand(void) override { return int32_t(_pitch_dem * 5729.5781f); } // Rate of change of velocity along X body axis in m/s^2 - float get_VXdot(void) { + float get_VXdot(void) override { return _vel_dot; } // return current target airspeed - float get_target_airspeed(void) const { + float get_target_airspeed(void) const override { return _TAS_dem / _ahrs.get_EAS2TAS(); } // return maximum climb rate - float get_max_climbrate(void) const { + float get_max_climbrate(void) const override { return _maxClimbRate; } // added to let SoaringContoller reset pitch integrator to zero - void reset_pitch_I(void) { + void reset_pitch_I(void) override { _integSEB_state = 0.0f; } // return landing sink rate - float get_land_sinkrate(void) const { + float get_land_sinkrate(void) const override { return _land_sink; } // return landing airspeed - float get_land_airspeed(void) const { + float get_land_airspeed(void) const override { return _landAirspeed; } @@ -105,7 +104,7 @@ public: } // set path_proportion - void set_path_proportion(float path_proportion) { + void set_path_proportion(float path_proportion) override { _path_proportion = constrain_float(path_proportion, 0.0f, 1.0f); } @@ -140,9 +139,6 @@ 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; AP_Float _spdCompFiltOmega;