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;