TECS: enable direct height-rate control

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

src/lib/tecs/TECS.cpp

@@ -201,6 +201,13 @@ void TECS::updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_
 	_hgt_rate_setpoint = math::constrain(_hgt_rate_setpoint, -_max_sink_rate, _max_climb_rate);
 }
 
+void TECS::_update_height_rate_setpoint(float hgt_rate_sp)
+{
+	// Limit the rate of change of height demand to respect vehicle performance limits
+	_hgt_rate_setpoint = math::constrain(hgt_rate_sp, -_max_sink_rate, _max_climb_rate);
+	_hgt_setpoint = _vert_pos_state;
+}
+
 void TECS::_detect_underspeed()
 {
 	if (!_detect_underspeed_enabled) {
@@ -510,7 +517,7 @@ 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 target_climbrate, float target_sinkrate)
+				 float target_climbrate, float target_sinkrate, float hgt_rate_sp)
 {
 	// Calculate the time since last update (seconds)
 	uint64_t now = hrt_absolute_time();
@@ -548,8 +555,14 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	// Calculate the demanded true airspeed
 	_update_speed_setpoint();
 
-	// calculate heigh rate setpoint based on altitude demand
-	updateHeightRateSetpoint(hgt_setpoint, target_climbrate, target_sinkrate, baro_altitude);
+	if (PX4_ISFINITE(hgt_rate_sp)) {
+		// use the provided height rate setpoint instead of the height setpoint
+		_update_height_rate_setpoint(hgt_rate_sp);
+
+	} else {
+		// calculate heigh rate setpoint based on altitude demand
+		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 target_climbrate, float target_sinkrate);
+				   float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);
 
 	float get_throttle_setpoint() { return _last_throttle_setpoint; }
 	float get_pitch_setpoint() { return _last_pitch_setpoint; }
@@ -311,6 +311,11 @@ private:
 				      float alt_amsl);
 
 
+	/**
+	 * Update the desired height rate setpoint
+	 */
+	void _update_height_rate_setpoint(float hgt_rate_sp);
+
 	/**
 	 * Detect if the system is not capable of maintaining airspeed
 	 */

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -2085,7 +2085,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
 		bool climbout_mode, float climbout_pitch_min_rad,
-		uint8_t mode)
+		uint8_t mode, float hgt_rate_sp)
 {
 	const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f);
 	_last_tecs_update = now;
@@ -2186,7 +2186,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    pitch_max_rad - radians(_param_fw_psp_off.get()),
-				    _param_climbrate_target.get(), _param_sinkrate_target.get());
+				    _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp);
 
 	tecs_status_publish();
 }

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -359,7 +359,7 @@ private:
 					float pitch_min_rad, float pitch_max_rad,
 					float throttle_min, float throttle_max, float throttle_cruise,
 					bool climbout_mode, float climbout_pitch_min_rad,
-					uint8_t mode = tecs_status_s::TECS_MODE_NORMAL);
+					uint8_t mode = tecs_status_s::TECS_MODE_NORMAL, float hgt_rate_sp = NAN);
 
 	DEFINE_PARAMETERS(