Fix heightrate feedforard for fixdwings

src/lib/tecs/TECS.cpp

@@ -148,6 +148,8 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
 	}
 
 	// Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference.
+	_alt_rate_ref = setpoint.alt_rate;
+
 	if (PX4_ISFINITE(setpoint.alt_rate)) {
 		_alt_rate_ref = setpoint.alt_rate;
 
@@ -317,9 +319,17 @@ float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const In
 float TECSControl::_calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param &param) const
 {
 	float altitude_rate_output;
-	altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
-			       param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate + setpoint.altitude_rate_setpoint;
-	altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
+
+	if (PX4_ISFINITE(input.altitude_rate_sp)) {
+		// Control only altitude rate if a valid setpoint is specified
+		altitude_rate_output = input.altitude_rate_sp;
+		altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
+
+	} else {
+		altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
+				       param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate + setpoint.altitude_rate_setpoint;
+		altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
+	}
 
 	return altitude_rate_output;
 }
@@ -650,6 +660,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 
 	const TECSControl::Input control_input{ .altitude = altitude,
 						.altitude_rate = altitude_rate,
+						.altitude_rate_sp = 0.0f,
 						.tas = eas_to_tas * equivalent_airspeed,
 						.tas_rate = 0.0f};
 
@@ -724,6 +735,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 
 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
+							.altitude_rate_sp = hgt_rate_sp,
 							.tas = eas_to_tas * eas.speed,
 							.tas_rate = eas_to_tas * eas.speed_rate};
 

src/lib/tecs/TECS.hpp

@@ -260,6 +260,7 @@ public:
 	struct Input {
 		float altitude;		///< Current altitude amsl of the UAS [m].
 		float altitude_rate;	///< Current altitude rate of the UAS [m/s].
+		float altitude_rate_sp; ///< Current altitude rate setpoint [m/s]
 		float tas;		///< Current true airspeed of the UAS [m/s].
 		float tas_rate;		///< Current true airspeed rate of the UAS [m/s²].
 	};

src/modules/fw_path_navigation/FixedwingPositionControl.cpp

@@ -1119,7 +1119,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+				   _param_climbrate_target.get(),
+				   false,
+				   pos_sp_curr.vz);
 }
 
 void