From 5363e8d30746b35fca0c415e870aa5bdf58f0247 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 10 Feb 2017 14:05:53 +0100 Subject: [PATCH] Wheel controller: use library function to wrap pi --- attitude_fw/ecl_wheel_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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;