TECS: move to new control loop architecture

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

src/lib/tecs/TECS.cpp

@@ -174,9 +174,8 @@ void TECS::_update_speed_setpoint()
 
 	_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
 
-	// calculate the demanded rate of change of speed proportional to speed error
-	// and apply performance limits
-	_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _speed_error_gain, velRateMin, velRateMax);
+	// calculate the demanded true airspeed rate of change based on first order response of true airspeed error
+	_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax);
 
 }
 
@@ -211,10 +210,11 @@ void TECS::_update_height_setpoint(float desired, float state)
 	// Apply a first order noise filter
 	_hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev;
 
-	// Calculate the demanded climb rate proportional to height error plus a feedforward term to provide
-	// tight tracking during steady climb and descent manoeuvres.
+	// Use a first order system to calculate a height rate setpoint from the current height error.
+	// Additionally, allow to add feedforward from heigh setpoint change
 	_hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff *
 			     (_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt;
+
 	_hgt_setpoint_adj_prev = _hgt_setpoint_adj;
 
 	// Limit the rate of change of height demand to respect vehicle performance limits
@@ -233,7 +233,7 @@ void TECS::_detect_underspeed()
 		return;
 	}
 
-	if (((_tas_state < _TAS_min * 0.9f) && (_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
+	if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
 	    || ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) {
 
 		_underspeed_detected = true;
@@ -249,6 +249,21 @@ void TECS::_update_energy_estimates()
 	_SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy
 	_SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy
 
+	// Calculate total energy error
+	_STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate;
+
+	const float SKE_weighting = get_SKE_weighting();
+
+	// Calculate the weighting applied to control of specific potential energy error
+	const float SPE_weighting = 2.0f - SKE_weighting;
+
+	// Calculate the specific energy balance demand which specifies how the available total
+	// energy should be allocated to speed (kinetic energy) and height (potential energy)
+	float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting;
+
+	// Calculate the specific energy balance error
+	_SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting);
+
 	// Calculate specific energy rate demands in units of (m**2/sec**3)
 	_SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
 	_SKE_rate_setpoint = _tas_state * _TAS_rate_setpoint; // kinetic energy rate of change
@@ -264,28 +279,30 @@ void TECS::_update_energy_estimates()
 
 void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::Dcmf &rotMat)
 {
-	// Calculate total energy error
-	_STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate;
-
-	// Calculate demanded rate of change of total energy, respecting vehicle limits
-	float STE_rate_setpoint = constrain((_SPE_rate_setpoint + _SKE_rate_setpoint), _STE_rate_min, _STE_rate_max);
+	// Calculate demanded rate of change of total energy, respecting vehicle limits.
+	// We will constrain the value below.
+	float STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
 
 	// Calculate the total energy rate error, applying a first order IIR filter
 	// to reduce the effect of accelerometer noise
-	_STE_rate_error_filter.update(-_SPE_rate - _SKE_rate);
+	_STE_rate_error_filter.update(-_SPE_rate - _SKE_rate + _SPE_rate_setpoint + _SKE_rate_setpoint);
 	_STE_rate_error = _STE_rate_error_filter.getState();
 
+	float throttle_setpoint;
+
 	// Calculate the throttle demand
 	if (_underspeed_detected) {
 		// always use full throttle to recover from an underspeed condition
-		_throttle_setpoint = 1.0f;
+		throttle_setpoint = _throttle_setpoint_max;
 
 	} else {
 		// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
 		// Assume induced drag scales linearly with normal load factor.
 		// The additional normal load factor is given by (1/cos(bank angle) - 1)
-		float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
-		STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / constrain(cosPhi, 0.1f, 1.0f) - 1.0f);
+		const float cosPhi = constrain(sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))), 0.1f, 1.0f);
+		STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / cosPhi - 1.0f);
+
+		STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
 
 		// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
 		// as the starting point. Assume:
@@ -304,31 +321,22 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::
 
 		}
 
-		// Calculate gain scaler from specific energy error to throttle
-		float STE_to_throttle = 1.0f / (_throttle_time_constant * (_STE_rate_max - _STE_rate_min));
+		// Calculate gain scaler from specific energy rate error to throttle
+		const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min);
 
 		// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
-		_throttle_setpoint = (_STE_error + _STE_rate_error * _throttle_damping_gain) * STE_to_throttle + throttle_predicted;
-		_throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
-
-		// Rate limit the throttle demand
-		if (fabsf(_throttle_slewrate) > 0.01f) {
-			float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate;
-			_throttle_setpoint = constrain(_throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit,
-						       _last_throttle_setpoint + throttle_increment_limit);
-		}
-
-		_last_throttle_setpoint = _throttle_setpoint;
+		throttle_setpoint = (_STE_rate_error * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
+		throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
 
 		if (_integrator_gain > 0.0f) {
 			// Calculate throttle integrator state upper and lower limits with allowance for
 			// 10% throttle saturation to accommodate noise on the demand.
-			float integ_state_max = _throttle_setpoint_max - _throttle_setpoint + 0.1f;
-			float integ_state_min = _throttle_setpoint_min - _throttle_setpoint - 0.1f;
+			float integ_state_max = _throttle_setpoint_max - throttle_setpoint + 0.1f;
+			float integ_state_min = _throttle_setpoint_min - throttle_setpoint - 0.1f;
 
 			// Calculate a throttle demand from the integrated total energy error
 			// This will be added to the total throttle demand to compensate for steady state errors
-			_throttle_integ_state = _throttle_integ_state + (_STE_error * _integrator_gain) * _dt * STE_to_throttle;
+			_throttle_integ_state = _throttle_integ_state + (_STE_rate_error * _integrator_gain) * _dt * STE_rate_to_throttle;
 
 			if (_climbout_mode_active) {
 				// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
@@ -346,16 +354,23 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::
 
 		if (airspeed_sensor_enabled()) {
 			// Add the integrator feedback during closed loop operation with an airspeed sensor
-			_throttle_setpoint = _throttle_setpoint + _throttle_integ_state;
+			throttle_setpoint += _throttle_integ_state;
 
 		} else {
 			// when flying without an airspeed sensor, use the predicted throttle only
-			_throttle_setpoint = throttle_predicted;
+			throttle_setpoint = throttle_predicted;
 
 		}
-
-		_throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
 	}
+
+	// Rate limit the throttle demand
+	if (fabsf(_throttle_slewrate) > 0.01f) {
+		const float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate;
+		throttle_setpoint = constrain(throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit,
+					      _last_throttle_setpoint + throttle_increment_limit);
+	}
+
+	_last_throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
 }
 
 void TECS::_detect_uncommanded_descent()
@@ -367,14 +382,15 @@ void TECS::_detect_uncommanded_descent()
 	*/
 
 	// Calculate rate of change of total specific energy
-	float STE_rate = _SPE_rate + _SKE_rate;
+	const float STE_rate = _SPE_rate + _SKE_rate;
 
 	// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
-	bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f) && (STE_rate < 0.0f)
-			  && (_throttle_setpoint >= _throttle_setpoint_max * 0.9f);
+	const bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f)
+				&& (STE_rate < 0.0f)
+				&& (_last_throttle_setpoint >= _throttle_setpoint_max * 0.9f);
 
 	// If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
-	bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f));
+	const bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f));
 
 	if (enter_mode) {
 		_uncommanded_descent_recovery = true;
@@ -397,39 +413,31 @@ void TECS::_update_pitch_setpoint()
 	 * rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
 	 * The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements.
 	*/
-
 	const float SKE_weighting = get_SKE_weighting();
 
 	// Calculate the weighting applied to control of specific potential energy error
 	const float SPE_weighting = 2.0f - SKE_weighting;
 
-	// Calculate the specific energy balance demand which specifies how the available total
-	// energy should be allocated to speed (kinetic energy) and height (potential energy)
-	float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting;
-
 	// Calculate the specific energy balance rate demand
-	float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting;
+	const float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting;
 
-	// Calculate the specific energy balance and balance rate error
-	_SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting);
+	// Calculate the specific energy balance rate error
 	_SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * SPE_weighting - _SKE_rate * SKE_weighting);
 
 	// Calculate derivative from change in climb angle to rate of change of specific energy balance
-	float climb_angle_to_SEB_rate = _tas_state * _pitch_time_constant * CONSTANTS_ONE_G;
+	const float climb_angle_to_SEB_rate = _tas_state * CONSTANTS_ONE_G;
 
 	if (_integrator_gain > 0.0f) {
 		// Calculate pitch integrator input term
-		float pitch_integ_input = _SEB_error * _integrator_gain;
+		float pitch_integ_input = _SEB_rate_error * _integrator_gain;
 
 		// Prevent the integrator changing in a direction that will increase pitch demand saturation
-		// Decay the integrator at the control loop time constant if the pitch demand from the previous time step is saturated
+		// Decay the integrator using a 1 second time constant if the pitch demand from the previous time step is saturated
 		if (_pitch_setpoint_unc > _pitch_setpoint_max) {
-			pitch_integ_input = min(pitch_integ_input,
-						min((_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
+			pitch_integ_input = min(pitch_integ_input, (_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate);
 
 		} else if (_pitch_setpoint_unc < _pitch_setpoint_min) {
-			pitch_integ_input = max(pitch_integ_input,
-						max((_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
+			pitch_integ_input = max(pitch_integ_input, (_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate);
 		}
 
 		// Update the pitch integrator state.
@@ -440,7 +448,7 @@ void TECS::_update_pitch_setpoint()
 	}
 
 	// Calculate a specific energy correction that doesn't include the integrator contribution
-	float SEB_correction = _SEB_error + _SEB_rate_error * _pitch_damping_gain + SEB_rate_setpoint * _pitch_time_constant;
+	float SEB_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + SEB_rate_setpoint;
 
 	// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
 	// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
@@ -449,23 +457,16 @@ void TECS::_update_pitch_setpoint()
 		SEB_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate;
 	}
 
-	// Sum the correction terms and convert to a pitch angle demand. This calculation assumes:
+	// Convert the specific energy balance correction to a target pitch angle. This calculation assumes:
 	// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
 	// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
 	// pitch transients due to control action or turbulence.
-	_pitch_setpoint_unc = (SEB_correction + _pitch_integ_state) / climb_angle_to_SEB_rate;
+	_pitch_setpoint_unc = SEB_correction / climb_angle_to_SEB_rate;
 	_pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);
 
 	// Comply with the specified vertical acceleration limit by applying a pitch rate limit
-	float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
-
-	if ((_pitch_setpoint - _last_pitch_setpoint) > ptchRateIncr) {
-		_pitch_setpoint = _last_pitch_setpoint + ptchRateIncr;
-
-	} else if ((_pitch_setpoint - _last_pitch_setpoint) < -ptchRateIncr) {
-		_pitch_setpoint = _last_pitch_setpoint - ptchRateIncr;
-	}
-
+	const float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
+	_pitch_setpoint = constrain(_pitch_setpoint, _last_pitch_setpoint - ptchRateIncr, _last_pitch_setpoint + ptchRateIncr);
 	_last_pitch_setpoint = _pitch_setpoint;
 }
 

src/lib/tecs/TECS.hpp

@@ -86,7 +86,7 @@ public:
 				   float throttle_min, float throttle_setpoint_max, float throttle_cruise,
 				   float pitch_limit_min, float pitch_limit_max);
 
-	float get_throttle_setpoint() { return _throttle_setpoint; }
+	float get_throttle_setpoint() { return _last_throttle_setpoint; }
 	float get_pitch_setpoint() { return _pitch_setpoint; }
 	float get_speed_weight() { return _pitch_speed_weight; }
 
@@ -102,7 +102,6 @@ public:
 	void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
 
 	// setters for controller parameters
-	void set_time_const(float time_const) { _pitch_time_constant = time_const; }
 	void set_integrator_gain(float gain) { _integrator_gain = gain; }
 
 	void set_min_sink_rate(float rate) { _min_sink_rate = rate; }
@@ -110,7 +109,7 @@ public:
 	void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; }
 
 	void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
-	void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; }
+	void set_height_error_time_constant(float time_const) { _height_error_gain = 1.0f / math::max(time_const, 0.1f); }
 
 	void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; }
 	void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; }
@@ -120,9 +119,8 @@ public:
 
 	void set_speed_comp_filter_omega(float omega) { _tas_estimate_freq = omega; }
 	void set_speed_weight(float weight) { _pitch_speed_weight = weight; }
-	void set_speedrate_p(float speedrate_p) { _speed_error_gain = speedrate_p; }
+	void set_airspeed_error_time_constant(float time_const) { _airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); }
 
-	void set_time_const_throt(float time_const_throt) { _throttle_time_constant = time_const_throt; }
 	void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; }
 	void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; }
 
@@ -206,17 +204,15 @@ private:
 	float _max_climb_rate{2.0f};					///< climb rate produced by max allowed throttle (m/sec)
 	float _min_sink_rate{1.0f};					///< sink rate produced by min allowed throttle (m/sec)
 	float _max_sink_rate{2.0f};					///< maximum safe sink rate (m/sec)
-	float _pitch_time_constant{5.0f};				///< control time constant used by the pitch demand calculation (sec)
-	float _throttle_time_constant{8.0f};				///< control time constant used by the throttle demand calculation (sec)
 	float _pitch_damping_gain{0.0f};				///< damping gain of the pitch demand calculation (sec)
 	float _throttle_damping_gain{0.0f};				///< damping gain of the throttle demand calculation (sec)
 	float _integrator_gain{0.0f};					///< integrator gain used by the throttle and pitch demand calculation
 	float _vert_accel_limit{0.0f};					///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
 	float _load_factor_correction{0.0f};				///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
 	float _pitch_speed_weight{1.0f};				///< speed control weighting used by pitch demand calculation
-	float _height_error_gain{0.0f};					///< gain from height error to demanded climb rate (1/sec)
+	float _height_error_gain{0.2f};					///< height error inverse time constant [1/s]
 	float _height_setpoint_gain_ff{0.0f};				///< gain from height demand derivative to demanded climb rate
-	float _speed_error_gain{0.0f};					///< gain from speed error to demanded speed rate (1/sec)
+	float _airspeed_error_gain{0.1f};							///< airspeed error inverse time constant [1/s]
 	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)
@@ -224,7 +220,6 @@ private:
 	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)
 	float _pitch_setpoint{0.0f};					///< pitch angle demand (radians)
 
 	// complimentary filter states

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -98,8 +98,6 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 	_tecs.set_indicated_airspeed_min(_param_fw_airspd_min.get());
 	_tecs.set_indicated_airspeed_max(_param_fw_airspd_max.get());
-	_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
-	_tecs.set_time_const(_param_fw_t_time_const.get());
 	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
 	_tecs.set_integrator_gain(_param_fw_t_integ_gain.get());
@@ -108,9 +106,9 @@ FixedwingPositionControl::parameters_update()
 	_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_heightrate_p(_param_fw_t_hrate_p.get());
+	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
 	_tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get());
-	_tecs.set_speedrate_p(_param_fw_t_srate_p.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());
 
@@ -639,7 +637,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 
 		/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
 		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-		_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
+		//_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
 
 		Vector2f curr_wp{0.0f, 0.0f};
 		Vector2f prev_wp{0.0f, 0.0f};
@@ -845,7 +843,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 				// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
 				// landing airspeed and potentially tighter throttle control) already such that we don't
 				// have to do this switch (which can cause significant altitude errors) close to the ground.
-				_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
+				//_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
 				mission_airspeed = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
 				_att_sp.apply_flaps = true;
 			}
@@ -865,7 +863,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 					_att_sp.roll_body = 0.0f;
 				}
 
-				_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
+				//_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
 			}
 
 			tecs_update_pitch_throttle(now, alt_sp,
@@ -1331,7 +1329,7 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
 
 	// Enable tighter throttle control for landings
-	_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
+	//_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
 
 	// save time at which we started landing and reset abort_landing
 	if (_time_started_landing == 0) {

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -395,7 +395,7 @@ private:
 
 		(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_max,
 		(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
-		(ParamFloat<px4::params::FW_T_HRATE_P>) _param_fw_t_hrate_p,
+		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
 		(ParamFloat<px4::params::FW_T_INTEG_GAIN>) _param_fw_t_integ_gain,
 		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
 		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
@@ -403,10 +403,8 @@ private:
 		(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_SRATE_P>) _param_fw_t_srate_p,
+		(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_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,

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -520,38 +520,6 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
  */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 
-/**
- * TECS time constant
- *
- * This is the time constant of the TECS control algorithm (in seconds).
- * Smaller values make it faster to respond, larger values make it slower
- * to respond.
- *
- * @unit s
- * @min 1.0
- * @max 10.0
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
-
-/**
- * TECS Throttle time constant
- *
- * This is the time constant of the TECS throttle control algorithm (in seconds).
- * Smaller values make it faster to respond, larger values make it slower
- * to respond.
- *
- * @unit s
- * @min 1.0
- * @max 10.0
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
-
 /**
  * Throttle damping factor
  *
@@ -678,15 +646,14 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
 PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 
 /**
- * Height rate proportional factor
+ * Altitude error time constant.
  *
- * @min 0.0
- * @max 1.0
+ * @min 2.0
  * @decimal 2
- * @increment 0.05
+ * @increment 0.5
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
+PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f);
 
 /**
  * Height rate feed forward
@@ -700,15 +667,14 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
 PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.8f);
 
 /**
- * Speed rate P factor
+ * True airspeed error time constant.
  *
- * @min 0.0
- * @max 2.0
+ * @min 2.0
  * @decimal 2
- * @increment 0.01
+ * @increment 0.5
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f);
+PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 10.0f);
 
 /**
  * Minimum groundspeed
@@ -737,7 +703,7 @@ 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.
  *