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 bitcraze#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

@@ -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) {

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)

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;