Merge pull request #338 from USC-ACTLab/issue335

Manual flight: same yaw rate for PID and Mellinger
bitcraze · Jul 31, 2018 · 3e881b6 · 3e881b6
2 parents b943ff8 + f31693b
commit 3e881b6
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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;