diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index 7416d1c963..7c157b0e70 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -67,13 +67,13 @@ void FlightTaskManualAltitude::scale_sticks()
 	/* map stick to velocity */
 	const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
 	_vel_sp_z = vel_max_z * _sticks_expo(2);
-	_yaw_rate_sp = _sticks(3) * _yaw_rate_scaling.get();
+	_yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get());
 
 }
 
 void FlightTaskManualAltitude::update_heading_setpoints()
 {
-	if (fabsf(_sticks(3) < FLT_EPSILON)) {
+	if (fabsf(_sticks(3)) < FLT_EPSILON) {
 		/* want to hold yaw */
 		_yaw_rate_sp = NAN;
 		_yaw_sp = _yaw_sp_predicted;
@@ -110,6 +110,7 @@ bool FlightTaskManualAltitude::update()
 	scale_sticks();
 	update_setpoints();
 
+
 	_setPositionSetpoint(Vector3f(NAN, NAN, _pos_sp_z));
 	_setVelocitySetpoint(Vector3f(NAN, NAN, _vel_sp_z));
 	_setYawSetpoint(_yaw_sp);
diff --git a/src/modules/mc_pos_control/TranslationControl.cpp b/src/modules/mc_pos_control/TranslationControl.cpp
index b3b8f778f7..d41b998911 100644
--- a/src/modules/mc_pos_control/TranslationControl.cpp
+++ b/src/modules/mc_pos_control/TranslationControl.cpp
@@ -87,7 +87,6 @@ void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s
 	_acc_sp = Data(&setpoint.acc_x);
 	_yaw_sp = setpoint.yaw; //integrate
 	_yawspeed_sp = setpoint.yawspeed;
-
 	_interfaceMapping();
 }
 
@@ -158,8 +157,6 @@ void TranslationControl::_positionController()
 	}
 
 	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
-
-	_velocityController(dt);
 }
 
 /* generates desired thrust vector */
@@ -278,7 +275,7 @@ void TranslationControl::_velocityController(const float &dt)
 
 void TranslationControl::_yawController(const float &dt)
 {
-	const float yaw_offset_max = _YawRateMax / _Pyaw;
+	const float yaw_offset_max = math::radians(_YawRateMax) / _Pyaw;
 	const float  yaw_target = _wrap_pi(_yaw_sp + _yawspeed_sp * dt);
 	const float yaw_offset = _wrap_pi(yaw_target - _yaw);