From f31693b6e787644a54db89cf81a9231efb5c8e60 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 22 Jun 2018 18:47:25 -0700 Subject: [PATCH] Manual flight: same yaw rate for PID and Mellinger The PID and mellinger controllers had different behaviors (inverse yaw direction) in manual flight mode, see issue #335. The root cause is that the legacy yaw input is actually inverse (with respect to the CF coordinate system). This change changes the sign accordingly before processing and fixes some other math issues related to yaw control. Tested with manual flight using PID and Mellinger controllers. --- src/modules/src/controller_mellinger.c | 4 ++-- src/modules/src/controller_pid.c | 2 +- src/modules/src/crtp_commander_rpyt.c | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/src/controller_mellinger.c b/src/modules/src/controller_mellinger.c index a60eb34044..d1aab541ca 100644 --- a/src/modules/src/controller_mellinger.c +++ b/src/modules/src/controller_mellinger.c @@ -179,9 +179,9 @@ void controllerMellinger(control_t *control, setpoint_t *setpoint, } } - // Rate-controled YAW is moving YAW angle setpoint + // Rate-controlled YAW is moving YAW angle setpoint if (setpoint->mode.yaw == modeVelocity) { - desiredYaw = state->attitude.yaw - setpoint->attitudeRate.yaw * dt; + desiredYaw = state->attitude.yaw + setpoint->attitudeRate.yaw * dt; } else if (setpoint->mode.yaw == modeAbs) { desiredYaw = setpoint->attitude.yaw; } else if (setpoint->mode.quat == modeAbs) { diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c index 3f06187e38..22117fd0c5 100644 --- a/src/modules/src/controller_pid.c +++ b/src/modules/src/controller_pid.c @@ -41,7 +41,7 @@ void controllerPid(control_t *control, setpoint_t *setpoint, if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { // Rate-controled YAW is moving YAW angle setpoint if (setpoint->mode.yaw == modeVelocity) { - attitudeDesired.yaw -= setpoint->attitudeRate.yaw/500.0f; + attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT; while (attitudeDesired.yaw > 180.0f) attitudeDesired.yaw -= 360.0f; while (attitudeDesired.yaw < -180.0f) diff --git a/src/modules/src/crtp_commander_rpyt.c b/src/modules/src/crtp_commander_rpyt.c index 9499864245..7a5b0f8695 100644 --- a/src/modules/src/crtp_commander_rpyt.c +++ b/src/modules/src/crtp_commander_rpyt.c @@ -202,7 +202,8 @@ void crtpCommanderRpytDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk) // Yaw if (!posSetMode) { - setpoint->attitudeRate.yaw = values->yaw; + // legacy rate input is inverted + setpoint->attitudeRate.yaw = -values->yaw; yawModeUpdate(setpoint); setpoint->mode.yaw = modeVelocity;