diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp
index aabece77b7c..e33250e2263 100644
--- a/src/lib/tecs/TECS.cpp
+++ b/src/lib/tecs/TECS.cpp
@@ -443,7 +443,7 @@ void TECS::_updateTrajectoryGenerationConstraints()
 {
 	_alt_control_traj_generator.setMaxJerk(_jerk_max);
 	_alt_control_traj_generator.setMaxAccel(_vert_accel_limit);
-	_alt_control_traj_generator.setMaxVel(max(_max_climb_rate, _max_sink_rate));
+	_alt_control_traj_generator.setMaxVel(fmax(_max_climb_rate, _max_sink_rate));
 
 	_velocity_control_traj_generator.setMaxJerk(_jerk_max);
 	_velocity_control_traj_generator.setMaxAccelUp(_vert_accel_limit);
diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp
index f362715e051..cfede513a5b 100644
--- a/src/lib/tecs/TECS.hpp
+++ b/src/lib/tecs/TECS.hpp
@@ -353,8 +353,6 @@ private:
 
 	void _updateTrajectoryGenerationConstraints();
 
-	void _updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint);
-
 	void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
 					  float target_sinkrate, float altitude_amsl);