TECS: keep _pitch_integ_state in radians

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/lib/tecs/TECS.cpp

@@ -394,7 +394,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific
 	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
 	ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
 
-	_calcPitchControlUpdate(dt, seb_rate, param);
+	_calcPitchControlUpdate(dt, input, seb_rate, param);
 	const float pitch_setpoint{_calcPitchControlOutput(input, seb_rate, param, flag)};
 
 	// Comply with the specified vertical acceleration limit by applying a pitch rate limit
@@ -433,11 +433,16 @@ TECSControl::ControlValues TECSControl::_calcPitchControlSebRate(const SpecificE
 	return seb_rate;
 }
 
-void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param &param)
+void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const ControlValues &seb_rate,
+		const Param &param)
 {
 	if (param.integrator_gain_pitch > FLT_EPSILON) {
+
+		// Calculate derivative from change in climb angle to rate of change of specific energy balance
+		const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
+
 		// Calculate pitch integrator input term
-		float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch;
+		float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch / climb_angle_to_SEB_rate;
 
 		// Prevent the integrator changing in a direction that will increase pitch demand saturation
 		if (_pitch_setpoint >= param.pitch_max) {
@@ -458,19 +463,19 @@ void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rat
 float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
 		const Flag &flag) const
 {
-	// Calculate a specific energy correction that doesn't include the integrator contribution
-	float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain + _pitch_integ_state +
-				    param.seb_rate_ff *
-				    seb_rate.setpoint;
-
 	// Calculate derivative from change in climb angle to rate of change of specific energy balance
 	const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
 
+	// Calculate a specific energy correction that doesn't include the integrator contribution
+	float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain +
+				    param.seb_rate_ff *
+				    seb_rate.setpoint;
+
 	// Convert the specific energy balance rate 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.
-	const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate;
+	const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate + _pitch_integ_state;
 
 	return constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max);
 }

src/lib/tecs/TECS.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -257,7 +257,7 @@ public:
 	 *
 	 */
 	struct Input {
-		float altitude;		///< Current altitude of the UAS [m].
+		float altitude;		///< Current altitude amsl of the UAS [m].
 		float altitude_rate;	///< Current altitude rate of the UAS [m/s].
 		float tas;		///< Current true airspeed of the UAS [m/s].
 		float tas_rate;		///< Current true airspeed rate of the UAS [m/s²].
@@ -456,13 +456,14 @@ private:
 
 	/**
 	 * @brief Calculate the pitch control update function.
-	 * Update the states of the pitch control
+	 * Update the states of the pitch control (pitch integrator).
 	 *
 	 * @param dt is the update time intervall in [s].
+	 * @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.
 	 */
-	void _calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param &param);
+	void _calcPitchControlUpdate(float dt, const Input &input, const ControlValues &seb_rate, const Param &param);
 
 	/**
 	 * @brief Calculate the pitch control output function.
@@ -499,7 +500,7 @@ private:
 
 	/**
 	 * @brief Calculate the throttle control update function.
-	 * Update the throttle control states.
+	 * Update the throttle control states (throttle integrator).
 	 *
 	 * @param dt is the update time intervall in [s].
 	 * @param limit is the specific total energy rate limits in [m²/s³].
@@ -526,8 +527,7 @@ private:
 	// State
 	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.
-
+	float _throttle_integ_state{0.0f};			///< Throttle integrator state [-].
 
 	// Output
 	DebugOutput _debug_output;				///< Debug output.
@@ -602,7 +602,7 @@ public:
 
 	void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; };
 
-	// // setters for parameters
+	// 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;};
@@ -654,7 +654,6 @@ public:
 	float get_pitch_setpoint() {return _control.getPitchSetpoint();}
 	float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
 
-	// // TECS status
 	uint64_t timestamp() { return _update_timestamp; }
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }