Changed CF student controller

crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c

@@ -102,6 +102,13 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
       control->yaw = 0;
       return;
     }
+
+    // Rate-controled YAW is moving YAW angle setpoint
+    if (setpoint->mode.yaw == modeVelocity) {
+       attitudeDesired.yaw += setpoint->attitudeRate.yaw * STUDENT_UPDATE_DT;
+    } else {
+      attitudeDesired.yaw = setpoint->attitude.yaw;
+    }
     
     //set desired roll and pitch and yaw angles
     attitudeDesired.roll = setpoint->attitude.roll;
@@ -130,10 +137,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
       rateDesired.pitch = setpoint->attitudeRate.pitch;
       studentAttitudeControllerResetPitchAttitudePID();
     }
-    if(setpoint->mode.yaw == modeVelocity) {
-      rateDesired.yaw = setpoint->attitudeRate.yaw;
-      studentAttitudeControllerResetYawAttitudePID();
-    }
+
     
 
     //update the attitude rate PID, given the current angular rate