diff --git a/attitude_fw/ecl_wheel_controller.cpp b/attitude_fw/ecl_wheel_controller.cpp
index 645ffe313b..c3c4a96388 100644
--- a/attitude_fw/ecl_wheel_controller.cpp
+++ b/attitude_fw/ecl_wheel_controller.cpp
@@ -102,13 +102,9 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	//xxx: until start detection is available: integral part in control signal is limited here
 	float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
 
-	/* Apply PI rate controller and store non-limited output */
-	_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
-		       _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler +
-		       integrator_constrained;
-	/*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
-	        (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/
-
+    /* Apply PI rate controller and store non-limited output */
+    _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
+             ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained);
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
@@ -122,11 +118,10 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da
 		return _rate_setpoint;
 	}
 
-	/* Calculate the error */
-	float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
-	/* shortest angle (wrap around) */
-	yaw_error = (float)fmodf((float)fmodf((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
-	/*warnx("yaw_error: %.4f", (double)yaw_error);*/
+    /* 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;
 
 	/*  Apply P controller: rate setpoint from current error and time constant */
 	_rate_setpoint =  yaw_error / _tc;