tecs: propagate altitude setpoint based on target climb/sink rate

- avoids tecs always climbing and sinking and max rates and allows to fine tune these rates
- avoid numerical calculation of feedforward velocity using derivative, this
 was prone to jitter in dt

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

src/lib/tecs/TECS.cpp

@@ -174,51 +174,31 @@ void TECS::_update_speed_setpoint()
 
 }
 
-void TECS::_update_height_setpoint(float desired, float state)
+void TECS::updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s,
+				    float alt_amsl)
 {
-	// Detect first time through and initialize previous value to demand
+	target_climbrate_m_s = math::min(target_climbrate_m_s, _max_climb_rate);
-	if (PX4_ISFINITE(desired) && fabsf(_hgt_setpoint_in_prev) < 0.1f) {
+	target_sinkrate_m_s = math::min(target_sinkrate_m_s, _max_sink_rate);
-		_hgt_setpoint_in_prev = desired;
+
+	float feedforward_height_rate = 0.0f;
+
+	if (fabsf(alt_sp_amsl_m - _hgt_setpoint) < math::max(target_sinkrate_m_s, target_climbrate_m_s) * _dt) {
+		_hgt_setpoint = alt_sp_amsl_m;
+
+	} else if (alt_sp_amsl_m > _hgt_setpoint) {
+		_hgt_setpoint += target_climbrate_m_s * _dt;
+		feedforward_height_rate = target_climbrate_m_s;
+
+	} else if (alt_sp_amsl_m < _hgt_setpoint) {
+		_hgt_setpoint -= target_sinkrate_m_s * _dt;
+		feedforward_height_rate = -target_sinkrate_m_s;
 	}
 
-	// Apply a 2 point moving average to demanded height to reduce
-	// intersampling noise effects.
-	if (PX4_ISFINITE(desired)) {
-		_hgt_setpoint = 0.5f * (desired + _hgt_setpoint_in_prev);
-
-	} else {
-		_hgt_setpoint = _hgt_setpoint_in_prev;
-	}
-
-	_hgt_setpoint_in_prev = _hgt_setpoint;
-
-	// Apply a rate limit to respect vehicle performance limitations
-	if ((_hgt_setpoint - _hgt_setpoint_prev) > (_max_climb_rate * _dt)) {
-		_hgt_setpoint = _hgt_setpoint_prev + _max_climb_rate * _dt;
-
-	} else if ((_hgt_setpoint - _hgt_setpoint_prev) < (-_max_sink_rate * _dt)) {
-		_hgt_setpoint = _hgt_setpoint_prev - _max_sink_rate * _dt;
-	}
-
-	_hgt_setpoint_prev = _hgt_setpoint;
-
-	// Apply a first order noise filter
-	_hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev;
-
 	// 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_rate_setpoint = (_hgt_setpoint - alt_amsl) * _height_error_gain + _height_setpoint_gain_ff *
-			     (_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt;
+			     feedforward_height_rate;
-
+	_hgt_rate_setpoint = math::constrain(_hgt_rate_setpoint, -_max_sink_rate, _max_climb_rate);
-	_hgt_setpoint_adj_prev = _hgt_setpoint_adj;
-
-	// Limit the rate of change of height demand to respect vehicle performance limits
-	if (_hgt_rate_setpoint > _max_climb_rate) {
-		_hgt_rate_setpoint = _max_climb_rate;
-
-	} else if (_hgt_rate_setpoint < -_max_sink_rate) {
-		_hgt_rate_setpoint = -_max_sink_rate;
-	}
 }
 
 void TECS::_detect_underspeed()
@@ -229,7 +209,7 @@ void TECS::_detect_underspeed()
 	}
 
 	if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
-	    || ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) {
+	    || ((_vert_pos_state < _hgt_setpoint) && _underspeed_detected)) {
 
 		_underspeed_detected = true;
 
@@ -241,7 +221,7 @@ void TECS::_detect_underspeed()
 void TECS::_update_energy_estimates()
 {
 	// Calculate specific energy demands in units of (m**2/sec**2)
-	_SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy
+	_SPE_setpoint = _hgt_setpoint * CONSTANTS_ONE_G; // potential energy
 	_SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy
 
 	// Calculate total energy error
@@ -477,10 +457,7 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 		_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
 		_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
 		_pitch_setpoint_unc = _last_pitch_setpoint;
-		_hgt_setpoint_adj_prev = baro_altitude;
+		_hgt_setpoint = baro_altitude;
-		_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
-		_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
-		_hgt_setpoint_in_prev = _hgt_setpoint_adj_prev;
 		_TAS_setpoint_last = _EAS * EAS2TAS;
 		_TAS_setpoint_adj = _TAS_setpoint_last;
 		_underspeed_detected = false;
@@ -499,15 +476,12 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 		// throttle lower limit is set to a value that prevents throttle reduction
 		_throttle_setpoint_min  = _throttle_setpoint_max - 0.01f;
 
-		// height demand and associated states are set to track the measured height
-		_hgt_setpoint_adj_prev  = baro_altitude;
-		_hgt_setpoint_adj       = _hgt_setpoint_adj_prev;
-		_hgt_setpoint_prev      = _hgt_setpoint_adj_prev;
-
 		// airspeed demand states are set to track the measured airspeed
 		_TAS_setpoint_last      = _EAS * EAS2TAS;
 		_TAS_setpoint_adj       = _EAS * EAS2TAS;
 
+		_hgt_setpoint = baro_altitude;
+
 		// disable speed and decent error condition checks
 		_underspeed_detected = false;
 		_uncommanded_descent_recovery = false;
@@ -535,7 +509,8 @@ void TECS::_update_STE_rate_lim()
 
 void TECS::update_pitch_throttle(float pitch, float baro_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_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
+				 float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max,
+				 float target_climbrate, float target_sinkrate)
 {
 	// Calculate the time since last update (seconds)
 	uint64_t now = hrt_absolute_time();
@@ -573,8 +548,8 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	// Calculate the demanded true airspeed
 	_update_speed_setpoint();
 
-	// Calculate the demanded height
+	// calculate heigh rate setpoint based on altitude demand
-	_update_height_setpoint(hgt_setpoint, baro_altitude);
+	updateHeightRateSetpoint(hgt_setpoint, target_climbrate, target_sinkrate, baro_altitude);
 
 	// Calculate the specific energy values required by the control loop
 	_update_energy_estimates();

src/lib/tecs/TECS.hpp

@@ -84,7 +84,7 @@ public:
 	void update_pitch_throttle(float pitch, float baro_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_cruise,
-				   float pitch_limit_min, float pitch_limit_max);
+				   float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate);
 
 	float get_throttle_setpoint() { return _last_throttle_setpoint; }
 	float get_pitch_setpoint() { return _last_pitch_setpoint; }
@@ -138,7 +138,7 @@ public:
 	uint64_t timestamp() { return _pitch_update_timestamp; }
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
 
-	float hgt_setpoint_adj() { return _hgt_setpoint_adj; }
+	float hgt_setpoint() { return _hgt_setpoint; }
 	float vert_pos_state() { return _vert_pos_state; }
 
 	float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
@@ -185,11 +185,7 @@ public:
 	 */
 	void handle_alt_step(float delta_alt, float altitude)
 	{
-		// add height reset delta to all variables involved
+		_hgt_setpoint += delta_alt;
-		// in filtering the demanded height
-		_hgt_setpoint_in_prev += delta_alt;
-		_hgt_setpoint_prev += delta_alt;
-		_hgt_setpoint_adj_prev += delta_alt;
 
 		// reset height states
 		_vert_pos_state = altitude;
@@ -254,10 +250,6 @@ private:
 
 	// height demand calculations
 	float _hgt_setpoint{0.0f};					///< demanded height tracked by the TECS algorithm (m)
-	float _hgt_setpoint_in_prev{0.0f};				///< previous value of _hgt_setpoint after noise filtering (m)
-	float _hgt_setpoint_prev{0.0f};					///< previous value of _hgt_setpoint after noise filtering and rate limiting (m)
-	float _hgt_setpoint_adj{0.0f};					///< demanded height used by the control loops after all filtering has been applied (m)
-	float _hgt_setpoint_adj_prev{0.0f};				///< value of _hgt_setpoint_adj from previous frame (m)
 	float _hgt_rate_setpoint{0.0f};					///< demanded climb rate tracked by the TECS algorithm
 
 	// vehicle physical limits
@@ -313,9 +305,11 @@ private:
 	void _update_speed_setpoint();
 
 	/**
-	 * Update the desired height
+	 * Calculate desired height rate from altitude demand
 	 */
-	void _update_height_setpoint(float desired, float state);
+	void updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s,
+				      float alt_amsl);
+
 
 	/**
 	 * Detect if the system is not capable of maintaining airspeed

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -422,7 +422,7 @@ FixedwingPositionControl::tecs_status_publish()
 		break;
 	}
 
-	t.altitude_sp = _tecs.hgt_setpoint_adj();
+	t.altitude_sp = _tecs.hgt_setpoint();
 	t.altitude_filtered = _tecs.vert_pos_state();
 
 	t.true_airspeed_sp = _tecs.TAS_setpoint_adj();
@@ -2174,7 +2174,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 				    climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad - radians(_param_fw_psp_off.get()),
-				    pitch_max_rad - radians(_param_fw_psp_off.get()));
+				    pitch_max_rad - radians(_param_fw_psp_off.get()),
+				    _param_climbrate_target.get(), _param_sinkrate_target.get());
 
 	tecs_status_publish();
 }

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -423,6 +423,8 @@ private:
 		(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_CLIMB_R_SP>) _param_climbrate_target,
+		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
 
 		(ParamFloat<px4::params::FW_THR_ALT_SCL>) _param_fw_thr_alt_scl,
 		(ParamFloat<px4::params::FW_THR_CRUISE>) _param_fw_thr_cruise,