TECS:use tas_setpoint instead of measured tas for specific kinetic energy calculation

src/lib/tecs/TECS.cpp

@@ -231,6 +231,8 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param
 
 	AltitudePitchControl control_setpoint;
 
+	control_setpoint.tas_setpoint = setpoint.tas_setpoint;
+
 	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
 
 	control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
@@ -274,6 +276,7 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &
 
 	AltitudePitchControl control_setpoint;
 
+	control_setpoint.tas_setpoint = setpoint.tas_setpoint;
 	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
 
 	if (PX4_ISFINITE(setpoint.altitude_rate_setpoint_direct)) {
@@ -320,9 +323,11 @@ float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const In
 	// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
 	if (flag.airspeed_enabled) {
 		// Calculate limits for the demanded rate of change of speed based on physical performance limits
-		// with a 50% margin to allow the total energy controller to correct for errors.
+		// with a 50% margin to allow the total energy controller to correct for errors. Increase it in case of fast descend
-		const float max_tas_rate_sp = 0.5f * limit.STE_rate_max / math::max(input.tas, FLT_EPSILON);
+		const float max_tas_rate_sp = (param.fast_descend * 0.5f + 0.5f) * limit.STE_rate_max / math::max(input.tas,
-		const float min_tas_rate_sp = 0.5f * limit.STE_rate_min / math::max(input.tas, FLT_EPSILON);
+					      FLT_EPSILON);
+		const float min_tas_rate_sp = (param.fast_descend * 0.5f + 0.5f) * limit.STE_rate_min / math::max(input.tas,
+					      FLT_EPSILON);
 		airspeed_rate_output = constrain((setpoint.tas_setpoint - input.tas) * param.airspeed_error_gain, min_tas_rate_sp,
 						 max_tas_rate_sp);
 	}
@@ -348,7 +353,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt
 	// Calculate specific energy rate demands in units of (m**2/sec**3)
 	specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint *
 			CONSTANTS_ONE_G; // potential energy rate of change
-	specific_energy_rates.ske_rate.setpoint = input.tas *
+	specific_energy_rates.ske_rate.setpoint = control_setpoint.tas_setpoint *
 			control_setpoint.tas_rate_setpoint; // kinetic energy rate of change
 
 	// Calculate specific energy rates in units of (m**2/sec**3)
@@ -394,19 +399,20 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
 	} else if (!flag.airspeed_enabled) {
 		pitch_speed_weight = 0.0f;
 
+	} else if (param.fast_descend > FLT_EPSILON) {
+		// pitch loop controls the airspeed to max
+		pitch_speed_weight = 1.f + param.fast_descend;
+
 	}
 
-	// don't allow any weight to be larger than one, as it has the same effect as reducing the control
+	weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 2.f);
-	// loop time constant and therefore can lead to a destabilization of that control loop
+	weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 2.f);
-	weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f);
-	weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 1.f);
 
 	return weight;
 }
 
 void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates,
-				    const Param &param,
+				    const Param &param, const Flag &flag)
-				    const Flag &flag)
 {
 	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
 	ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
@@ -514,20 +520,26 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec
 	const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate;
 	_ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const);
 	_ste_rate_estimate_filter.update(STE_rate_estimate_raw);
-
 	ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
-	_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
-	float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};
 
-	// Rate limit the throttle demand
+	if (1.f - param.fast_descend < FLT_EPSILON) {
-	if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
+		// During fast descend, we control airspeed over the pitch control loop and give minimal thrust.
-		const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
+		_throttle_setpoint = param.throttle_min;
-		throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
+
-					      _throttle_setpoint + throttle_increment_limit);
+	} else {
+		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
+		float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};
+
+		// Rate limit the throttle demand
+		if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
+			const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
+			throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
+						      _throttle_setpoint + throttle_increment_limit);
+		}
+
+		_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
 	}
 
-	_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
-
 	// Debug output
 	_debug_output.total_energy_rate_estimate = ste_rate.estimate;
 	_debug_output.total_energy_rate_sp = ste_rate.setpoint;
@@ -651,6 +663,7 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa
 	_reference_param.target_sinkrate = target_sinkrate;
 	// Control
 	_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
+	_control_param.tas_max = eas_to_tas * _equivalent_airspeed_max;
 	_control_param.pitch_max = pitch_limit_max;
 	_control_param.pitch_min = pitch_limit_min;
 	_control_param.throttle_trim = throttle_trim;
@@ -705,6 +718,11 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas);
 
 	} else {
+		/* Check if we want to fast descend. On fast descend, we set the throttle to min, and use the altitude control
+		loop to control the speed to the maximum airspeed. */
+		_setFastDescend(hgt_setpoint, altitude);
+		_control_param.fast_descend = _fast_descend;
+
 		// Update airspeedfilter submodule
 		const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
 				.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
@@ -712,15 +730,25 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		_airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled);
 
 		// Update Reference model submodule
-		const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
+		if (1.f - _fast_descend < FLT_EPSILON) {
-				.alt_rate = hgt_rate_sp};
+			// Reset the altitude reference model, while we are in fast descend.
+			const TECSAltitudeReferenceModel::AltitudeReferenceState init_state{
+				.alt = altitude,
+				.alt_rate = hgt_rate};
+			_altitude_reference_model.initialize(init_state);
 
-		_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
+		} else {
+			const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
+					.alt_rate = hgt_rate_sp};
+
+			_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
+		}
 
 		TECSControl::Setpoint control_setpoint;
 		control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
 		control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect();
-		control_setpoint.tas_setpoint = eas_to_tas * EAS_setpoint;
+		control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas *
+						EAS_setpoint;
 
 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
@@ -740,3 +768,17 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	_update_timestamp = now;
 }
 
+void TECS::_setFastDescend(const float alt_setpoint, const float alt)
+{
+	if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON)
+	    && ((alt_setpoint + _fast_descend_alt_err) < alt)) {
+		_fast_descend = 1.f;
+
+	} else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) {
+		// Were in fast descend, scale it down. up until 5m above target altitude
+		_fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f);
+
+	} else {
+		_fast_descend = 0.f;
+	}
+}

src/lib/tecs/TECS.hpp

@@ -202,6 +202,7 @@ public:
 		float vert_accel_limit;			///< Magnitude of the maximum vertical acceleration allowed [m/s²].
 		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
 		float tas_min;				///< True airspeed demand lower limit [m/s].
+		float tas_max;				///< True airspeed demand upper limit [m/s].
 		float pitch_max;			///< Maximum pitch angle allowed in [rad].
 		float pitch_min;			///< Minimal pitch angle allowed in [rad].
 		float throttle_trim;		///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
@@ -233,6 +234,8 @@ public:
 
 		float load_factor_correction;				///< Gain from normal load factor increase to total energy rate demand [m²/s³].
 		float load_factor;					///< Additional normal load factor.
+
+		float fast_descend;
 	};
 
 	/**
@@ -363,6 +366,7 @@ private:
 	struct AltitudePitchControl {
 		float altitude_rate_setpoint;	///< Controlled altitude rate setpoint [m/s].
 		float tas_rate_setpoint;	///< Controlled true airspeed rate setpoint [m/s²].
+		float tas_setpoint; 		///< Controller true airspeed setpoint [m/s]
 	};
 
 	/**
@@ -393,7 +397,7 @@ private:
 	 * @brief calculate airspeed control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
-	 * @param input	is the current input measurment of the UAS.
+	 * @param input	is the current input measurement of the UAS.
 	 * @param param	is the control parameters.
 	 * @param flag	is the control flags.
 	 * @return controlled airspeed rate setpoint in [m/s²].
@@ -404,7 +408,7 @@ private:
 	 * @brief calculate altitude control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
-	 * @param input is the current input measurment of the UAS.
+	 * @param input is the current input measurement of the UAS.
 	 * @param param is the control parameters.
 	 * @return controlled altitude rate setpoint in [m/s].
 	 */
@@ -413,14 +417,14 @@ private:
 	 * @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.
+	 * @param input is the current input measurement of the UAS.
 	 * @return Specific energy rates in [m²/s³].
 	 */
 	SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const;
 	/**
 	 * @brief Detect underspeed.
 	 *
-	 * @param input is the current input measurment of the UAS.
+	 * @param input is the current input measurement of the UAS.
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
 	 */
@@ -602,9 +606,11 @@ public:
 	void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; };
 
 	void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; };
-	void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; };
+	void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f); };
+	void set_fast_descend_altitude_error(float altitude_error) { _fast_descend_alt_err = altitude_error; };
 
 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
+	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = 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; }
@@ -665,7 +671,10 @@ private:
 
 	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_min{10.0f};				///< equivalent airspeed demand lower limit (m/sec)
+	float _equivalent_airspeed_max{20.0f};				///< equivalent airspeed demand upper limit (m/sec)
+	float _fast_descend_alt_err{-1.f};	 				///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m].
+	float _fast_descend{0.f};					///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude.
 
 	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)
@@ -697,6 +706,7 @@ private:
 		.vert_accel_limit = 0.0f,
 		.equivalent_airspeed_trim = 15.0f,
 		.tas_min = 10.0f,
+		.tas_max = 20.0f,
 		.pitch_max = 0.5f,
 		.pitch_min = -0.5f,
 		.throttle_trim = 0.0f,
@@ -716,11 +726,20 @@ private:
 		.throttle_slewrate = 0.0f,
 		.load_factor_correction = 0.0f,
 		.load_factor = 1.0f,
+		.fast_descend = 0.f
 	};
 
 	TECSControl::Flag _control_flag{
 		.airspeed_enabled = false,
 		.detect_underspeed_enabled = false,
 	};
+
+	/**
+	 * @brief Set fast descend value
+	 *
+	 * @param alt_setpoint is the altitude setpoint
+	 * @param alt is the
+	 */
+	void _setFastDescend(float alt_setpoint, float alt);
 };
 

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -129,6 +129,7 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 	_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
 	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
+	_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
 	_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get());
 	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
@@ -137,6 +138,7 @@ FixedwingPositionControl::parameters_update()
 	_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_fast_descend_altitude_error(_param_fw_t_fast_alt_err.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());

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -957,6 +957,7 @@ private:
 
 		(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
 		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
+		(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
 		(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
 		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
 		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,

src/modules/fw_pos_control/fw_path_navigation_params.c

@@ -633,6 +633,15 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f);
  */
 PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f);
 
+/**
+ * Minimum altitude error needed to descend with max airspeed. A negative value disables fast descend.
+ *
+ * @min -1.0
+ * @decimal 0
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f);
+
 /**
  * Height rate feed forward
  *