TECS: Combine both airspeed and airspeed derivative filters in TECS into one MIMO filter using a steady state Kalman filter.

msg/TecsStatus.msg

@@ -18,21 +18,11 @@ float32 true_airspeed_filtered
 float32 true_airspeed_derivative_sp
 float32 true_airspeed_derivative
 float32 true_airspeed_derivative_raw
-float32 true_airspeed_innovation
 
-float32 total_energy_error
-float32 energy_distribution_error
-float32 total_energy_rate_error
-float32 energy_distribution_rate_error
-
-float32 total_energy
 float32 total_energy_rate
-float32 total_energy_balance
 float32 total_energy_balance_rate
 
-float32 total_energy_sp
 float32 total_energy_rate_sp
-float32 total_energy_balance_sp
 float32 total_energy_balance_rate_sp
 
 float32 throttle_integ

src/lib/tecs/TECS.hpp

@@ -39,6 +39,7 @@
 
 #pragma once
 
+#include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
@@ -66,9 +67,10 @@ public:
 	 *
 	 */
 	struct Param {
-		float equivalent_airspeed_trim; 	///< the trim value of the equivalent airspeed om [m/s].
-		float airspeed_estimate_freq; 		///< cross-over frequency of the equivalent airspeed complementary filter [rad/sec].
-		float speed_derivative_time_const;	///< speed derivative filter time constant [s].
+		float equivalent_airspeed_trim; 	///< the trim value of the equivalent airspeed [m/s].
+		float airspeed_measurement_std_dev; 	///< airspeed measurement standard deviation in [m/s].
+		float airspeed_rate_measurement_std_dev;///< airspeed rate measurement standard deviation in [m/s²].
+		float airspeed_rate_noise_std_dev; 	///< standard deviation on the airspeed rate deviation in the model in [m/s²].
 	};
 
 	/**
@@ -108,7 +110,6 @@ public:
 
 private:
 	// States
-	AlphaFilter<float> _airspeed_rate_filter;				///< Alpha filter for airspeed rate
 	AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f};	///< Complimentary filter state
 };
 
@@ -176,8 +177,6 @@ private:
 	// State
 	VelocitySmoothing
 	_alt_control_traj_generator;		///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded.
-
-	// Output
 	float _alt_rate_ref; 			///< Altitude rate reference in [m/s].
 };
 
@@ -235,10 +234,12 @@ public:
 	struct DebugOutput {
 		float altitude_rate_control;			///< Altitude rate setpoint from altitude control loop [m/s].
 		float true_airspeed_derivative_control;		///< Airspeed rate setpoint from airspeed control loop [m/s²].
-		float total_energy_rate_error;			///< Total energy rate error [m²/s³].
+		float total_energy_rate_estimate;		///< Total energy rate estimate [m²/s³].
 		float total_energy_rate_sp;			///< Total energy rate setpoint [m²/s³].
-		float energy_balance_rate_error;		///< Energy balance rate error [m²/s³].
+		float energy_balance_rate_estimate;		///< Energy balance rate estimate [m²/s³].
 		float energy_balance_rate_sp;			///< Energy balance rate setpoint [m²/s³].
+		float pitch_integrator;				///< Pitch control integrator state [-].
+		float throttle_integrator;			///< Throttle control integrator state [-].
 	};
 
 	/**
@@ -278,7 +279,7 @@ public:
 	 * @brief Initialization of the state.
 	 *
 	 */
-	void initialize();
+	void initialize(const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag);
 	/**
 	 * @brief Update state and output.
 	 *
@@ -297,9 +298,9 @@ public:
 	/**
 	 * @brief Get the percent of the undersped.
 	 *
-	 * @return Percentage of detected undersped.
+	 * @return Ratio of detected undersped [0,1].
 	 */
-	float getPercentUndersped() const {return _percent_undersped;};
+	float getRatioUndersped() const {return _ratio_undersped;};
 	/**
 	 * @brief Get the throttle setpoint.
 	 *
@@ -323,33 +324,35 @@ public:
 	 *
 	 * @return the debug outpus struct.
 	 */
-	DebugOutput getDebugOutput() const {return _debug_output;};
+	const DebugOutput &getDebugOutput() const { return _debug_output; }
 
 private:
 	/**
-	 * @brief Specific total energy limit.
+	 * @brief Specific total energy rate limit.
 	 *
 	 */
-	struct STELimit {
+	struct STERateLimit {
 		float STE_rate_max;	///< Maximum specific total energy rate limit [m²/s³].
 		float STE_rate_min;	///< Minimal specific total energy rate limit [m²/s³].
 	};
 
 	/**
-	 * @brief Calculated specific energy.
+	 * @brief Control values.
+	 * setpoint and current state estimate value as input to a controller.
 	 *
 	 */
-	struct SpecificEnergy {
-		struct {
-			float rate;		///< Specific kinetic energy rate in [m²/s³].
-			float rate_setpoint;	///< Specific kinetic energy setpoint rate in [m²/s³].
-			float rate_error;	///< Specific kinetic energy rate error in [m²/s³].
-		} ske;				///< Specific kinetic energy.
-		struct {
-			float rate;		///< Specific potential energy rate in [m²/s³].
-			float rate_setpoint;	///< Specific potential energy setpoint rate in [m²/s³].
-			float rate_error;	///< Specific potential energy rate error in [m²/s³].
-		} spe;				///< Specific potential energy rate.
+	struct ControlValues {
+		float setpoint;		///< Control setpoint.
+		float estimate;		///< Control estimate of current state value.
+	};
+
+	/**
+	 * @brief Calculated specific energy rates.
+	 *
+	 */
+	struct SpecificEnergyRates {
+		ControlValues ske_rate;	///< Specific kinetic energy rate [m²/s³].
+		ControlValues spe_rate;	///< Specific potential energy rate [m²/s³].
 	};
 
 	/**
@@ -371,15 +374,22 @@ private:
 	};
 
 private:
+	/**
+	 * @brief Get control error from etpoint and estimate
+	 *
+	 * @param val is the current control setpoint and estimate.
+	 * @return error value
+	 */
+	static inline constexpr float _getControlError(TECSControl::ControlValues val) {return (val.setpoint - val.estimate);};
 	/**
 	 * @brief Calculate specific total energy rate limits.
 	 *
 	 * @param[in] param are the control parametes.
-	 * @return Specific total energy rate limits.
+	 * @return Specific total energy rate limits in [m²/s³].
 	 */
-	STELimit _calculateTotalEnergyRateLimit(const Param &param) const;
+	STERateLimit _calculateTotalEnergyRateLimit(const Param &param) const;
 	/**
-	 * @brief Airspeed control loop.
+	 * @brief calculate airspeed control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
 	 * @param input	is the current input measurment of the UAS.
@@ -387,24 +397,25 @@ private:
 	 * @param flag	is the control flags.
 	 * @return controlled airspeed rate setpoint in [m/s²].
 	 */
-	float _airspeedControl(const Setpoint &setpoint, const Input &input, const Param &param, const Flag &flag) const;
+	float _calcAirspeedControlOutput(const Setpoint &setpoint, const Input &input, const Param &param,
+					 const Flag &flag) const;
 	/**
-	 * @brief Altitude control loop.
+	 * @brief calculate altitude control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
 	 * @param input is the current input measurment of the UAS.
 	 * @param param is the control parameters.
 	 * @return controlled altitude rate setpoint in [m/s].
 	 */
-	float _altitudeControl(const Setpoint &setpoint, const Input &input, const Param &param) const;
+	float _calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param &param) const;
 	/**
-	 * @brief Update energy balance.
+	 * @brief Calculate specific energy rates.
 	 *
 	 * @param control_setpoint is the controlles altitude and airspeed rate setpoints.
 	 * @param input is the current input measurment of the UAS.
-	 * @return Specific energy rates.
+	 * @return Specific energy rates in [m²/s³].
 	 */
-	SpecificEnergy _updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const;
+	SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const;
 	/**
 	 * @brief Detect underspeed.
 	 *
@@ -422,37 +433,108 @@ private:
 	 */
 	SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param &param, const Flag &flag);
 	/**
-	 * @brief Update controlled pitch setpoint.
+	 * @brief Calculate pitch control.
 	 *
 	 * @param dt is the update time intervall in [s].
-	 * @param input is the current input measurment of the UAS.
-	 * @param se is the calculated specific energy.
+	 * @param input is the current input measurement of the UAS.
+	 * @param specific_energy_rate is the calculated specific energy.
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
 	 */
-	void _updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param &param, const Flag &flag);
+	void _calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rate,
+			       const Param &param,
+			       const Flag &flag);
+
+	/**
+	 * @brief Calculate pitch control specific energy balance rates.
+	 *
+	 * @param weight is the weighting use of the potential and kinetic energy.
+	 * @param specific_energy_rate is the specific energy rates in [m²/s³].
+	 * @return specific energy balance rate values in [m²/s³].
+	 */
+	ControlValues _calcPitchControlSebRate(const SpecificEnergyWeighting &weight,
+					       const SpecificEnergyRates &specific_energy_rate) const;
+
+	/**
+	 * @brief Calculate the pitch control update function.
+	 * Update the states of the pitch control
+	 *
+	 * @param dt is the update time intervall in [s].
+	 * @param seb_rate is the specific energy balance rate in [m²/s³].
+	 * @param param is the control parameters.
+	 */
+	void _calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param &param);
+
+	/**
+	 * @brief Calculate the pitch control output function.
+	 *
+	 * @param input is the current input measurement of the UAS.
+	 * @param seb_rate is the specific energy balance rate in [m²/s³].
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 * @return pitch setpoint angle [rad].
+	 */
+	float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
+				      const Flag &flag) const;
+
 	/**
 	 * @brief Update controlled throttle setpoint.
 	 *
 	 * @param dt is the update time intervall in [s].
-	 * @param se is the calculated specific energy.
+	 * @param specific_energy_rate is the calculated specific energy.
+	 * @param flag is the control flags.
+	 */
+	void _calcThrottleControl(float dt, const SpecificEnergyRates &specific_energy_rate, const Param &param,
+				  const Flag &flag);
+
+	/**
+	 * @brief Calculate throttle control specific total energy
+	 *
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param specific_energy_rate is the specific energy rates in [m²/s³].
+	 * @param param is the control parameters.
+	 * @return specific total energy rate values in [m²/s³]
+	 */
+	ControlValues _calcThrottleControlSteRate(const STERateLimit &limit, const SpecificEnergyRates &specific_energy_rate,
+			const Param &param) const;
+
+	/**
+	 * @brief Calculate the throttle control update function.
+	 * Update the throttle control states.
+	 *
+	 * @param dt is the update time intervall in [s].
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param ste_rate is the specific total energy rates in [m²/s³].
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
 	 */
-	void _updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param &param, const Flag &flag);
+	void _calcThrottleControlUpdate(float dt, const STERateLimit &limit, const ControlValues &ste_rate, const Param &param,
+					const Flag &flag);
+
+	/**
+	 * @brief Calculate the throttle control output function.
+	 *
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param ste_rate is the specific total energy rates in [m²/s³].
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 * @return throttle setpoin in [0,1].
+	 */
+	float _calcThrottleControlOutput(const STERateLimit &limit, const ControlValues &ste_rate, const Param &param,
+					 const Flag &flag) const;
 
 private:
 	// State
-	AlphaFilter<float> _ste_rate_error_filter;		///< Low pass filter for the specific total energy rate.
+	AlphaFilter<float> _ste_rate_estimate_filter;		///< Low pass filter for the specific total energy rate.
 	float _pitch_integ_state{0.0f};				///< Pitch integrator state [rad].
 	float _throttle_integ_state{0.0f};			///< Throttle integrator state.
 
 
 	// Output
-	DebugOutput _debug_output;
+	DebugOutput _debug_output;				///< Debug output.
 	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [rad].
-	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint.
-	float _percent_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
+	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint [0,1].
+	float _ratio_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
 	float _ste_rate{0.0f};					///< Specific total energy rate [m²/s³].
 };
 
@@ -466,12 +548,13 @@ public:
 		ECL_TECS_MODE_CLIMBOUT
 	};
 
-	struct DebugOutput : TECSControl::DebugOutput {
+	struct DebugOutput {
+		TECSControl::DebugOutput control;
 		float true_airspeed_filtered;
 		float true_airspeed_derivative;
 		float altitude_sp;
-		float altitude_rate;
-		float altitude_rate_setpoint;
+		float altitude_rate_alt_ref;
+		float altitude_rate_feedforward;
 		enum ECL_TECS_MODE tecs_mode;
 	};
 public:
@@ -484,19 +567,19 @@ public:
 	TECS(TECS &&) = delete;
 	TECS &operator=(TECS &&) = delete;
 
-	DebugOutput getStatus() const {return _debug_status;};
+	const DebugOutput &getStatus() const { return _debug_status; }
 
 	/**
 	 * Get the current airspeed status
 	 *
 	 * @return true if airspeed is enabled for control
 	 */
-	bool airspeed_sensor_enabled() { return _airspeed_enabled; }
+	bool airspeed_sensor_enabled() { return _control_flag.airspeed_enabled; }
 
 	/**
 	 * Set the airspeed enable state
 	 */
-	void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; }
+	void enable_airspeed(bool enabled) { _control_flag.airspeed_enabled = enabled; }
 
 	/**
 	 * @brief Update the control loop calculations
@@ -505,22 +588,26 @@ public:
 	void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
 		    float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
 		    float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
-		    const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
+		    float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
 
 	/**
 	 * @brief Initialize the control loop
 	 *
 	 */
-	void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed);
+	void initialize(float altitude, float altitude_rate, float equivalent_airspeed, float eas_to_tas);
 
 	void resetIntegrals()
 	{
 		_control.resetIntegrals();
 	}
 
-	void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; };
+	void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; };
 
 	// // setters for parameters
+	void set_airspeed_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_measurement_std_dev = std_dev;};
+	void set_airspeed_rate_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_measurement_std_dev = std_dev;};
+	void set_airspeed_filter_process_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_noise_std_dev = std_dev;};
+
 	void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;};
 	void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; };
 
@@ -532,12 +619,11 @@ public:
 
 	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
-	void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; }
+	void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; }
 
 	void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; }
 	void set_vertical_accel_limit(float limit) { _reference_param.vert_accel_limit = limit; _control_param.vert_accel_limit = limit; };
 
-	void set_speed_comp_filter_omega(float omega) { _airspeed_param.airspeed_estimate_freq = omega; };
 	void set_speed_weight(float weight) { _control_param.pitch_speed_weight = weight; };
 	void set_airspeed_error_time_constant(float time_const) { _control_param.airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); };
 
@@ -547,7 +633,6 @@ public:
 	void set_roll_throttle_compensation(float compensation) { _control_param.load_factor_correction = compensation; };
 	void set_load_factor(float load_factor) { _control_param.load_factor = load_factor; };
 
-	void set_speed_derivative_time_constant(float time_const) { _airspeed_param.speed_derivative_time_const = time_const; };
 	void set_ste_rate_time_const(float time_const) { _control_param.ste_rate_time_const = time_const; };
 
 	void set_seb_rate_ff_gain(float ff_gain) { _control_param.seb_rate_ff = ff_gain; };
@@ -574,8 +659,6 @@ public:
 	uint64_t timestamp() { return _update_timestamp; }
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
 
-	static constexpr float DT_DEFAULT = 0.02f;
-
 private:
 	TECSControl 		_control;				///< Control submodule.
 	TECSAirspeedFilter 	_airspeed_filter;			///< Airspeed filter submodule.
@@ -583,38 +666,32 @@ private:
 
 	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};		///< Current activated mode.
 
-	uint64_t _update_timestamp{0};					///< last timestamp of the update function call.
+	hrt_abstime _update_timestamp{0};				///< last timestamp of the update function call.
 
 	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
 	float _equivalent_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
-	float _equivalent_airspeed_trim{15.0f};				///< equivalent cruise airspeed for airspeed less mode (m/sec)
 
 	// controller mode logic
 	bool _uncommanded_descent_recovery{false};			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
-	bool _airspeed_enabled{false};					///< true when airspeed use has been enabled
-	bool _detect_underspeed_enabled{false};				///< true when underspeed detection is enabled
 
 	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
 	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)
 
-	static constexpr float _jerk_max = 1000.0f;			///< Magnitude of the maximum jerk allowed [m/s³].
-	static constexpr float _tas_error_percentage =
-		0.15f;		///< [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
-
 	DebugOutput _debug_status{};
 
 	// Params
 	/// Airspeed filter parameters.
-	TECSAirspeedFilter::Param _airspeed_param{
-		.equivalent_airspeed_trim = 0.0f,
-		.airspeed_estimate_freq = 0.0f,
-		.speed_derivative_time_const = 0.01f,
+	TECSAirspeedFilter::Param _airspeed_filter_param{
+		.equivalent_airspeed_trim = 15.0f,
+		.airspeed_measurement_std_dev = 0.2f,
+		.airspeed_rate_measurement_std_dev = 0.05f,
+		.airspeed_rate_noise_std_dev = 0.02f
 	};
 	/// Reference model parameters.
 	TECSReferenceModel::Param _reference_param{
 		.target_climbrate = 2.0f,
 		.target_sinkrate = 2.0f,
-		.jerk_max = _jerk_max,
+		.jerk_max = 1000.0f,
 		.vert_accel_limit = 0.0f,
 		.max_climb_rate = 2.0f,
 		.max_sink_rate = 2.0f,
@@ -633,7 +710,7 @@ private:
 		.throttle_min = 0.1f,
 		.altitude_error_gain = 0.2f,
 		.altitude_setpoint_gain_ff = 0.0f,
-		.tas_error_percentage = _tas_error_percentage,
+		.tas_error_percentage = 0.15f,
 		.airspeed_error_gain = 0.1f,
 		.ste_rate_time_const = 0.1f,
 		.seb_rate_ff = 1.0f,
@@ -647,6 +724,12 @@ private:
 		.load_factor = 1.0f,
 	};
 
+	TECSControl::Flag _control_flag{
+		.airspeed_enabled = false,
+		.climbout_mode_active = false,
+		.detect_underspeed_enabled = false,
+	};
+
 	/**
 	 * Update the desired airspeed
 	 */

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -134,15 +134,16 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
 	_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
 	_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
-	_tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
 	_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
 	_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
 	_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
 	_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
 	_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.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());
 	_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
+	_tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get());
+	_tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get());
+	_tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get());
 
 	int check_ret = PX4_OK;
 
@@ -457,11 +458,12 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 }
 
 void
-FixedwingPositionControl::tecs_status_publish()
+FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
+		float true_airspeed_derivative_raw, float throttle_trim)
 {
 	tecs_status_s t{};
 
-	TECS::DebugOutput debug_output{_tecs.getStatus()};
+	const TECS::DebugOutput &debug_output{_tecs.getStatus()};
 
 	switch (_tecs.tecs_mode()) {
 	case TECS::ECL_TECS_MODE_NORMAL:
@@ -481,25 +483,25 @@ FixedwingPositionControl::tecs_status_publish()
 		break;
 	}
 
+	t.altitude_sp = alt_sp;
 	t.altitude_filtered = debug_output.altitude_sp;
-
+	t.height_rate_setpoint = debug_output.control.altitude_rate_control;
+	t.height_rate = -_local_pos.vz;
+	t.equivalent_airspeed_sp = equivalent_airspeed_sp;
+	t.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp;
 	t.true_airspeed_filtered = debug_output.true_airspeed_filtered;
-
-	t.height_rate_setpoint = debug_output.altitude_rate_setpoint;
-	t.height_rate = debug_output.altitude_rate;
-
-	t.true_airspeed_derivative_sp = debug_output.true_airspeed_derivative_control;
+	t.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
 	t.true_airspeed_derivative = debug_output.true_airspeed_derivative;
-
-	t.total_energy_rate_error = debug_output.total_energy_rate_error;
-
-	t.energy_distribution_rate_error = debug_output.energy_balance_rate_error;
-
-	t.total_energy_rate_sp = debug_output.total_energy_rate_sp;
-	t.total_energy_balance_rate_sp = debug_output.energy_balance_rate_sp;
-
+	t.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
+	t.total_energy_rate = debug_output.control.total_energy_rate_estimate;
+	t.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
+	t.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
+	t.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
+	t.throttle_integ = debug_output.control.throttle_integrator;
+	t.pitch_integ = debug_output.control.pitch_integrator;
 	t.throttle_sp = _tecs.get_throttle_setpoint();
 	t.pitch_sp_rad = _tecs.get_pitch_setpoint();
+	t.throttle_trim = throttle_trim;
 
 	t.timestamp = hrt_absolute_time();
 
@@ -826,7 +828,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 		_was_in_air = true;
 		_time_went_in_air = now;
 
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 	}
 
 	/* reset flag when airplane landed */
@@ -834,7 +836,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 		_was_in_air = false;
 		_completed_manual_takeoff = false;
 
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 	}
 }
 
@@ -2314,7 +2316,7 @@ FixedwingPositionControl::Run()
 				_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 				_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
-				_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+				_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 
 				break;
 			}
@@ -2491,12 +2493,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	// We need an altitude lock to calculate the TECS control
-	if (!(_local_pos.timestamp > 0)) {
+	if (_local_pos.timestamp == 0) {
 		_reinitialize_tecs = true;
 	}
 
 	if (_reinitialize_tecs) {
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 		_reinitialize_tecs = false;
 	}
 
@@ -2504,7 +2506,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
 
 	if (_landed) {
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 	}
 
 	/* update TECS vehicle state estimates */
@@ -2530,7 +2532,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     -_local_pos.vz,
 		     hgt_rate_sp);
 
-	tecs_status_publish();
+	tecs_status_publish(alt_sp, airspeed_sp, -_local_pos.vz, throttle_trim_comp);
 }
 
 float

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -437,7 +437,8 @@ private:
 
 	void status_publish();
 	void landing_status_publish();
-	void tecs_status_publish();
+	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
+				 float throttle_trim);
 	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
 
 	/**
@@ -822,16 +823,17 @@ private:
 		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
 		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
 		(ParamFloat<px4::params::FW_T_SINK_MIN>) _param_fw_t_sink_min,
-		(ParamFloat<px4::params::FW_T_SPD_OMEGA>) _param_fw_t_spd_omega,
 		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
 		(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
 		(ParamFloat<px4::params::FW_T_THR_DAMP>) _param_fw_t_thr_damp,
 		(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_T_SEB_R_FF>) _param_seb_rate_ff,
 		(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
 		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
+		(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
+		(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
+		(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
 
 		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
 		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -709,22 +709,50 @@ PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f);
 PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
 
 /**
- * Complementary filter "omega" parameter for speed
+ * Airspeed measurement standard deviation for airspeed filter.
  *
- * This is the cross-over frequency (in radians/second) of the complementary
- * filter used to fuse longitudinal acceleration and airspeed to obtain an
- * improved airspeed estimate. Increasing this frequency weights the solution
- * more towards use of the airspeed sensor, whilst reducing it weights the
- * solution more towards use of the accelerometer data.
+ * This is the measurement standard deviation for the airspeed used in the airspeed filter in TECS.
  *
- * @unit rad/s
- * @min 1.0
+ * @unit m/s
+ * @min 0.01
  * @max 10.0
- * @decimal 1
- * @increment 0.5
+ * @decimal 2
+ * @increment 0.1
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
+PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.2f);
+
+/**
+ * Airspeed rate measurement standard deviation for airspeed filter.
+ *
+ * This is the measurement standard deviation for the airspeed rate used in the airspeed filter in TECS.
+ *
+ * @unit m/s^2
+ * @min 0.01
+ * @max 10.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.05f);
+
+/**
+ * Process noise standard deviation for the airspeed rate in the airspeed filter.
+ *
+ * This is the process noise standard deviation in the airspeed filter filter defining the noise in the
+ * airspeed rate for the constant airspeed rate model. This is used to define how much the airspeed and
+ * the airspeed rate are filtered. The smaller the value the more the measurements are smoothed with the
+ * drawback for delays.
+ *
+ * @unit m/s^2
+ * @min 0.01
+ * @max 10.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f);
+
 
 /**
  * Roll -> Throttle feedforward
@@ -856,21 +884,6 @@ PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2);
  */
 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.2f);
-
-
 /**
  * Specific total energy balance rate feedforward gain.
  *