diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp
index 36985eccf5..e02ce6ab4b 100644
--- a/src/lib/tecs/TECS.cpp
+++ b/src/lib/tecs/TECS.cpp
@@ -181,10 +181,11 @@ void TECS::runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target
 	target_climbrate_m_s = math::min(target_climbrate_m_s, _max_climb_rate);
 	target_sinkrate_m_s = math::min(target_sinkrate_m_s, _max_sink_rate);
 
-	const float altitude_error_m = alt_sp_amsl_m - _alt_control_traj_generator.getCurrentPosition();
+	const float delta_trajectory_to_target_m = alt_sp_amsl_m - _alt_control_traj_generator.getCurrentPosition();
 
-	float height_rate_target = math::signNoZero<float>(altitude_error_m) * math::trajectory::computeMaxSpeedFromDistance(
-					   1000, _vert_accel_limit, fabsf(altitude_error_m), 0.0f);
+	float height_rate_target = math::signNoZero<float>(delta_trajectory_to_target_m) *
+				   math::trajectory::computeMaxSpeedFromDistance(
+					   _jerk_max, _vert_accel_limit, fabsf(delta_trajectory_to_target_m), 0.0f);
 
 	height_rate_target = math::constrain(height_rate_target, -target_sinkrate_m_s, target_climbrate_m_s);
 
@@ -440,11 +441,11 @@ void TECS::_update_pitch_setpoint()
 
 void TECS::_updateTrajectoryGenerationConstraints()
 {
-	_alt_control_traj_generator.setMaxJerk(1000.0f);	// we want infinite jerk, so just set a high value
+	_alt_control_traj_generator.setMaxJerk(_jerk_max);
 	_alt_control_traj_generator.setMaxAccel(_vert_accel_limit);
-	_alt_control_traj_generator.setMaxVel(_max_climb_rate);
+	_alt_control_traj_generator.setMaxVel(max(_max_climb_rate, _max_sink_rate));
 
-	_velocity_control_traj_generator.setMaxJerk(1000.0f);	// we want infinite jerk, so just set a high value
+	_velocity_control_traj_generator.setMaxJerk(_jerk_max);
 	_velocity_control_traj_generator.setMaxAccelUp(_vert_accel_limit);
 	_velocity_control_traj_generator.setMaxAccelDown(_vert_accel_limit);
 	_velocity_control_traj_generator.setMaxVelUp(_max_climb_rate);
@@ -457,6 +458,8 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
 	bool control_altitude = true;
 	const bool input_is_height_rate = PX4_ISFINITE(height_rate_sp);
 
+	_velocity_control_traj_generator.setVelSpFeedback(_hgt_rate_setpoint);
+
 	if (input_is_height_rate) {
 		_velocity_control_traj_generator.setCurrentPositionEstimate(altitude_amsl);
 		_velocity_control_traj_generator.update(_dt, height_rate_sp);
@@ -466,13 +469,11 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
 
 	} else {
 		_velocity_control_traj_generator.reset(0, _hgt_rate_setpoint, _hgt_setpoint);
-		_velocity_control_traj_generator.setVelSpFeedback(_hgt_rate_setpoint);
 	}
 
 
 	if (control_altitude) {
 		runAltitudeControllerSmoothVelocity(altitude_sp_amsl, target_climbrate, target_sinkrate, altitude_amsl);
-		_velocity_control_traj_generator.setVelSpFeedback(_hgt_rate_setpoint);
 
 	} else {
 		_alt_control_traj_generator.setCurrentVelocity(_hgt_rate_setpoint);
diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp
index 8d36e43d81..f04f3a7c55 100644
--- a/src/lib/tecs/TECS.hpp
+++ b/src/lib/tecs/TECS.hpp
@@ -203,6 +203,11 @@ public:
 
 private:
 
+	static constexpr float _jerk_max =
+		1000.0f;	// maximum jerk for creating height rate trajectories, we want infinite jerk so set a high value
+
+	uORB::Publication<tecs_status_s>	_tecs_status_pub{ORB_ID(tecs_status)};	///< TECS status publication
+
 	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};
 
 	// timestamps
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 156d2d2f23..b9eb352419 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -629,16 +629,6 @@ FixedwingPositionControl::in_takeoff_situation()
 	       && (_current_altitude <= _takeoff_ground_alt + _param_fw_clmbout_diff.get());
 }
 
-void
-FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min)
-{
-	/* demand "climbout_diff" m above ground if user switched into this mode during takeoff */
-	if (in_takeoff_situation()) {
-		*hold_altitude = _takeoff_ground_alt + _param_fw_clmbout_diff.get();
-		*pitch_limit_min = radians(10.0f);
-	}
-}
-
 void
 FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid)
 {
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index c09ab3e8e7..b960443f58 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -112,7 +112,6 @@ static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort l
 
 static constexpr float THROTTLE_THRESH =
 	0.05f;	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
-static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
 
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
@@ -312,13 +311,6 @@ private:
 	 */
 	bool 		in_takeoff_situation();
 
-	/**
-	 * Do takeoff help when in altitude controlled modes
-	 * @param hold_altitude altitude setpoint for controller
-	 * @param pitch_limit_min minimum pitch allowed
-	 */
-	void 		do_takeoff_help(float *hold_altitude, float *pitch_limit_min);
-
 	/**
 	 * Update desired altitude base on user pitch stick input
 	 *