TECS: added alpha filter classes to energy rate and tas derivative

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/tecs/TECS.cpp

@@ -91,10 +91,11 @@ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &ro
 	if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) {
 		// Assuming the vehicle is flying X axis forward, use the X axis measured acceleration
 		// compensated for gravity to estimate the rate of change of speed
-		float speed_deriv_raw = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
+		const float speed_deriv_raw = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
 
 		// Apply some noise filtering
-		_speed_derivative = 0.95f * _speed_derivative + 0.05f * speed_deriv_raw;
+		_TAS_rate_filter.update(speed_deriv_raw);
+		_speed_derivative = _TAS_rate_filter.getState();
 
 	} else {
 		_speed_derivative = 0.0f;
@@ -271,7 +272,8 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::
 
 	// Calculate the total energy rate error, applying a first order IIR filter
 	// to reduce the effect of accelerometer noise
-	_STE_rate_error = 0.2f * (STE_rate_setpoint - _SPE_rate - _SKE_rate) + 0.8f * _STE_rate_error;
+	_STE_rate_error_filter.update(-_SPE_rate - _SKE_rate);
+	_STE_rate_error = _STE_rate_error_filter.getState();
 
 	// Calculate the throttle demand
 	if (_underspeed_detected) {
@@ -526,6 +528,14 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 		_uncommanded_descent_recovery = false;
 	}
 
+	// filter specific energy rate error using first order filter with 0.5 second time constant
+	_STE_rate_error_filter.setParameters(DT_DEFAULT, _STE_rate_time_const);
+	_STE_rate_error_filter.reset(0.0f);
+
+	// filter true airspeed rate using first order filter with 0.5 second time constant
+	_TAS_rate_filter.setParameters(DT_DEFAULT, _speed_derivative_time_const);
+	_TAS_rate_filter.reset(0.0f);
+
 	_states_initialized = true;
 }
 

src/lib/tecs/TECS.hpp

@@ -41,6 +41,7 @@
 
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
+#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
 
 class TECS
 {
@@ -127,6 +128,10 @@ public:
 
 	void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; }
 
+	void set_ste_rate_time_const(float time_const) { _STE_rate_time_const = time_const; }
+	void set_speed_derivative_time_constant(float time_const) { _speed_derivative_time_const = time_const; }
+
+
 	// TECS status
 	uint64_t timestamp() { return _pitch_update_timestamp; }
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
@@ -199,6 +204,8 @@ private:
 	float _indicated_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
 	float _indicated_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
 	float _throttle_slewrate{0.0f};					///< throttle demand slew rate limit (1/sec)
+	float _STE_rate_time_const{0.1f};				///< filter time constant for specific total energy rate (damping path) (s)
+	float _speed_derivative_time_const{0.01f};		///< speed derivative filter time constant (s)
 
 	// controller outputs
 	float _throttle_setpoint{0.0f};					///< normalized throttle demand (0..1)
@@ -324,4 +331,10 @@ private:
 	 */
 	void _update_STE_rate_lim();
 
+	float _param_filt_speed_deriv{0.95f};
+
+	AlphaFilter<float> _STE_rate_error_filter;
+
+	AlphaFilter<float> _TAS_rate_filter;
+
 };