fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 13:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Wheel controller: use library function to wrap pi

Browse Source
This commit is contained in:
Andreas Antener
2017-02-10 14:05:53 +01:00
committed by Lorenz Meier
parent 674ab3f81b
commit 5363e8d307
1 changed files with 1 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
attitude_fw/ecl_wheel_controller.cpp
+1 -3
View File
@@ -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;