TECS: allow for fast descend up to maximum airspeed. Use pitch control loop to control max airspeed while giving minimal throttle.

src/lib/tecs/TECS.cpp

@@ -239,7 +239,7 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param
 
 	_detectUnderspeed(input, param, flag);
 
-	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
+	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)};
 	ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)};
 
 	_pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag);
@@ -381,7 +381,8 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param &param, cons
 			   math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
 }
 
-TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param &param, const Flag &flag)
+TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Input &input, const Param &param,
+		const Flag &flag)
 {
 
 	SpecificEnergyWeighting weight;
@@ -394,6 +395,10 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
 	} else if (!flag.airspeed_enabled) {
 		pitch_speed_weight = 0.0f;
 
+	} else if (flag.fast_descend) {
+		// pitch loop controls the airspeed to max
+		pitch_speed_weight = 2.0;
+
 	}
 
 	// don't allow any weight to be larger than one, as it has the same effect as reducing the control
@@ -405,10 +410,9 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
 }
 
 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)};
+	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)};
 	ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
 
 	_calcPitchControlUpdate(dt, input, seb_rate, param);
@@ -514,8 +518,12 @@ 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)};
+
+	if (flag.fast_descend) {
+		_throttle_setpoint = param.throttle_min;
+
+	} else {
 		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
 		float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};
 
@@ -527,6 +535,7 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec
 		}
 
 		_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
+	}
 
 	// Debug output
 	_debug_output.total_energy_rate_estimate = ste_rate.estimate;
@@ -651,6 +660,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 +715,10 @@ 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. */
+		const bool is_fast_descend{_checkFastDescend(hgt_setpoint, altitude)};
+
 		// Update airspeedfilter submodule
 		const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
 				.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
@@ -712,10 +726,19 @@ 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
+		if (is_fast_descend) {
+			// Reset the altitude reference model.
+			const TECSAltitudeReferenceModel::AltitudeReferenceState init_state{
+				.alt = altitude,
+				.alt_rate = hgt_rate};
+			_altitude_reference_model.initialize(init_state);
+
+		} 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();
@@ -727,6 +750,12 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 							.tas = eas_to_tas * _airspeed_filter.getState().speed,
 							.tas_rate = eas_to_tas * _airspeed_filter.getState().speed_rate};
 
+		if (is_fast_descend) {
+			control_setpoint.tas_setpoint = _control_param.tas_max;
+		}
+
+		_control_flag.fast_descend = is_fast_descend;
+
 		_control.update(dt, control_setpoint, control_input, _control_param, _control_flag);
 	}
 
@@ -740,3 +769,13 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	_update_timestamp = now;
 }
 
+bool TECS::_checkFastDescend(const float alt_setpoint, const float alt)
+{
+	bool ret_val{false};
+
+	if ((_fast_descend_alt_err > FLT_EPSILON) && ((alt_setpoint + _fast_descend_alt_err) < alt)) {
+		ret_val = true;
+	}
+
+	return ret_val;
+}

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]
@@ -278,6 +279,7 @@ public:
 	struct Flag {
 		bool airspeed_enabled;			///< Flag if the airspeed sensor is enabled.
 		bool detect_underspeed_enabled;		///< Flag if underspeed detection is enabled.
+		bool fast_descend; 			///< Flag if descending with higher airspeed is allowed.
 	};
 public:
 	TECSControl() = default;
@@ -393,7 +395,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 +406,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 +415,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.
 	 */
@@ -428,11 +430,12 @@ private:
 	/**
 	 * @brief Update specific energy balance weights.
 	 *
+	 * @param input is the current input measurement of the UAS.
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
 	 * @return Weights used for the specific energy balance.
 	 */
-	SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param &param, const Flag &flag);
+	SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Input &input, const Param &param, const Flag &flag);
 	/**
 	 * @brief Calculate pitch control.
 	 *
@@ -602,9 +605,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 +670,9 @@ 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].
 
 	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 +704,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,
@@ -722,5 +730,14 @@ private:
 		.airspeed_enabled = false,
 		.detect_underspeed_enabled = false,
 	};
+
+	/**
+	 * @brief Check if fast descend should be used
+	 *
+	 * @param alt_setpoint is the altitude setpoint
+	 * @param alt is the
+	 * @return true if fast descend should be enabled
+	 */
+	bool _checkFastDescend(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
  *