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;
+
 };

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -111,6 +111,9 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_heightrate_p(_param_fw_t_hrate_p.get());
 	_tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get());
 	_tecs.set_speedrate_p(_param_fw_t_srate_p.get());
+	_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
+	_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
+
 
 	// Landing slope
 	/* check if negative value for 2/3 of flare altitude is set for throttle cut */

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -408,6 +408,8 @@ private:
 		(ParamFloat<px4::params::FW_T_THRO_CONST>) _param_fw_t_thro_const,
 		(ParamFloat<px4::params::FW_T_TIME_CONST>) _param_fw_t_time_const,
 		(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
+		(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
+		(ParamFloat<px4::params::FW_T_TAS_R_TC>) _param_tas_rate_time_const,
 
 		(ParamFloat<px4::params::FW_THR_ALT_SCL>) _param_fw_thr_alt_scl,
 		(ParamFloat<px4::params::FW_THR_CRUISE>) _param_fw_thr_cruise,

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -737,3 +737,30 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f);
  * @group FW L1 Control
  */
 PARAM_DEFINE_INT32(FW_POSCTL_INV_ST, 0);
+ 
+/**
+ * Specific total energy rate first order filter time constant.
+ *
+ * This filter is applied to the specific total energy rate used for throttle damping.
+ *
+ * @min 0.0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f);
+
+
+/**
+ * True airspeed rate first order filter time constant.
+ *
+ * This filter is applied to the true airspeed rate.
+ *
+ * @min 0.0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.4f);