diff --git a/attitude_fw/ecl_wheel_controller.cpp b/attitude_fw/ecl_wheel_controller.cpp
index d6290cfe2c..527af899fe 100644
--- a/attitude_fw/ecl_wheel_controller.cpp
+++ b/attitude_fw/ecl_wheel_controller.cpp
@@ -119,9 +119,7 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da
 	}
 
 	/* Calculate the error */
-	float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
-	/* shortest angle (wrap around) */
-	yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
+	float yaw_error = _wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
 
 	/*  Apply P controller: rate setpoint from current error and time constant */
 	_rate_setpoint =  yaw_error / _tc;