diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/attitude_controller.h b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/attitude_controller.h
index f27543b3ec43a5624b66c5899cdb807d9e02e210..e6ed0cffcc4836e68b04f748d8269808551b6751 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/attitude_controller.h
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/attitude_controller.h
@@ -92,6 +92,12 @@ void attitudeControllerResetRollAttitudePID(void);
  */
 void attitudeControllerResetPitchAttitudePID(void);
 
+/**
+ * Reset controller yaw attitude PID  
+ */
+void attitudeControllerResetYawAttitudePID(void);
+
+
 /**
  * Reset controller roll, pitch and yaw PID's.
  */
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/attitude_pid_controller.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/attitude_pid_controller.c
index 97f82f1df742b5ce4d32afd358ea6444fa718b0c..928ebd8de77f0db3142bbd3caa0b3eed0d2e338e 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/attitude_pid_controller.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/attitude_pid_controller.c
@@ -156,6 +156,11 @@ void attitudeControllerResetPitchAttitudePID(void)
     pidReset(&pidPitch);
 }
 
+void attitudeControllerResetYawAttitudePID(void)
+{
+    pidReset(&pidYaw);
+}
+
 void attitudeControllerResetAllPID(void)
 {
   pidReset(&pidRoll);
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
index 9f3f8b572fb8020f15872a6696c22f97758a3636..7f04e02635a090c8d9d3bfd9ca7c1e2273d9359e 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
@@ -52,9 +52,9 @@ bool controllerStudentTest(void)
  * 
  * @param angle 
  * @return float 
- */
+ *
 static float capAngle(float angle) {
-  //TODO MICROCART: Student written
+  //TODO MICROCART: remove
   
   float result = angle;
 
@@ -68,6 +68,7 @@ static float capAngle(float angle) {
 
   return result;
 }
+*/
 
 /**
  * This function is called periodically to update the PID loop,
@@ -101,20 +102,11 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
       control->yaw = 0;
       return;
     }
-
-    // update desired yaw from setpoint
-    // Rate-controlled YAW is moving YAW angle setpoint
-    if (setpoint->mode.yaw == modeVelocity) {
-       attitudeDesired.yaw += setpoint->attitudeRate.yaw * STUDENT_UPDATE_DT;
-    // absolute controlled yaw 
-    } else {
-      attitudeDesired.yaw = setpoint->attitude.yaw;
-    }
-    attitudeDesired.yaw = capAngle(attitudeDesired.yaw);
     
-    //set desired roll and pitch
+    //set desired roll and pitch and yaw angles
     attitudeDesired.roll = setpoint->attitude.roll;
     attitudeDesired.pitch = setpoint->attitude.pitch;
+    attitudeDesired.yaw = setpoint->attitude.yaw;
 
     //set desired thrust
     thrustDesired = setpoint->thrust;
@@ -138,6 +130,11 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
       rateDesired.pitch = setpoint->attitudeRate.pitch;
       attitudeControllerResetPitchAttitudePID();
     }
+    if(setpoint->mode.yaw == modeVelocity) {
+      rateDesired.yaw = setpoint->attitudeRate.yaw;
+      attitudeControllerResetYawAttitudePID();
+    }
+    
 
     //update the attitude rate PID, given the current angular rate 
     //read by the gyro and the desired rate