diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 93b8d43c8f..6a232882b7 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -340,8 +340,8 @@ void AP_TECS::_update_speed(float load_factor) float EAS2TAS = _ahrs.get_EAS2TAS(); _TAS_dem = _EAS_dem * EAS2TAS; - _TASmax = aparm.airspeed_max * EAS2TAS; - _TASmin = aparm.airspeed_min * EAS2TAS; + _TASmax = airspeed.get_airspeed_max() * EAS2TAS; + _TASmin = airspeed.get_airspeed_min() * EAS2TAS; if (aparm.stall_prevention) { // when stall prevention is active we raise the mimimum @@ -368,7 +368,7 @@ void AP_TECS::_update_speed(float load_factor) // airspeed is not being used and set speed rate to zero if (!_ahrs.airspeed_sensor_enabled() || !_ahrs.airspeed_estimate(&_EAS)) { // If no airspeed available use average of min and max - _EAS = 0.5f * (aparm.airspeed_min.get() + (float)aparm.airspeed_max.get()); + _EAS = 0.5f * (airspeed.get_airspeed_min() + airspeed.get_airspeed_max()); } // Implement a second order complementary filter to obtain a diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 673499ab5d..814ec07c78 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -121,6 +121,7 @@ private: AP_AHRS &_ahrs; const AP_Vehicle::FixedWing &aparm; + AP_Airspeed airspeed; // TECS tuning parameters AP_Float _hgtCompFiltOmega;