diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 3579fe42b8..0361c32fe4 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -88,12 +88,14 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float // Update and average speed rate of change if airspeed is being measured if (PX4_ISFINITE(equivalent_airspeed) && airspeed_sensor_enabled()) { + _tas_rate_raw = speed_deriv_forward; // Apply some noise filtering _TAS_rate_filter.update(speed_deriv_forward); - _speed_derivative = _TAS_rate_filter.getState(); + _tas_rate_filtered = _TAS_rate_filter.getState(); } else { - _speed_derivative = 0.0f; + _tas_rate_raw = 0.0f; + _tas_rate_filtered = 0.0f; } if (!_in_air) { @@ -141,7 +143,7 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva // Update TAS state _tas_rate_state = _tas_rate_state + tas_rate_state_input * dt; - float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f; + float tas_state_input = _tas_rate_state + _tas_rate_raw + tas_error * _tas_estimate_freq * 1.4142f; _tas_state = _tas_state + tas_state_input * dt; // Limit the TAS state to a minimum of 3 m/s @@ -260,7 +262,7 @@ void TECS::_update_energy_estimates() // Calculate specific energy rates in units of (m**2/sec**3) _SPE_rate = _vert_vel_state * CONSTANTS_ONE_G; // potential energy rate of change - _SKE_rate = _tas_state * _speed_derivative;// kinetic energy rate of change + _SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change } void TECS::_update_throttle_setpoint(const float throttle_cruise) diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index f3ced9bfa3..207991a973 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -149,7 +149,7 @@ public: float get_EAS_setpoint() { return _EAS_setpoint; }; float TAS_rate_setpoint() { return _TAS_rate_setpoint; } - float speed_derivative() { return _speed_derivative; } + float speed_derivative() { return _tas_rate_filtered; } float STE_error() { return _STE_error; } float STE_rate_error() { return _STE_rate_error; } @@ -239,7 +239,7 @@ private: float _pitch_integ_state{0.0f}; ///< pitch integrator state (rad) float _last_throttle_setpoint{0.0f}; ///< throttle demand rate limiter state (1/sec) float _last_pitch_setpoint{0.0f}; ///< pitch demand rate limiter state (rad/sec) - float _speed_derivative{0.0f}; ///< rate of change of speed along X axis (m/sec**2) + float _tas_rate_filtered{0.0f}; ///< low pass filtered rate of change of speed along X axis (m/sec**2) // speed demand calculations float _EAS{0.0f}; ///< equivalent airspeed (m/sec) @@ -250,6 +250,7 @@ private: float _EAS_setpoint{0.0f}; ///< Equivalent airspeed demand (m/sec) float _TAS_setpoint_adj{0.0f}; ///< true airspeed demand tracked by the TECS algorithm (m/sec) float _TAS_rate_setpoint{0.0f}; ///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2) + float _tas_rate_raw{0.0f}; ///< true airspeed rate, calculated as inertial acceleration in body X direction // height demand calculations float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m)