From 0c875dd6d1091f6f49cea6dbc0a0b9513a378566 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 24 Sep 2015 20:56:42 +0200
Subject: [PATCH] calculate shortest yaw error

---
 src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp
index b0df2a95d6..12d2668f87 100644
--- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp
@@ -135,6 +135,9 @@ 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;
+    /*warnx("yaw_error: %.4f", (double)yaw_error);*/
 
     /*  Apply P controller: rate setpoint from current error and time constant */
     _rate_setpoint =  yaw_error / _tc;