Merge attitude_rate_control into master,

close !73

crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_attitude_controller.h

@@ -93,6 +93,11 @@ void attitudeControllerResetRollAttitudePID(void);
  */
 void attitudeControllerResetPitchAttitudePID(void);
 
+/**
+ * Reset controller yaw attitude PID  
+ */
+void attitudeControllerResetYawAttitudePID(void);
+
 /**
  * Reset controller roll, pitch and yaw PID's.
  */

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

@@ -54,7 +54,7 @@ bool controllerStudentTest(void)
  * @return float 
  */
 static float capAngle(float angle) {
-  //TODO MICROCART: Student written
+  //TODO MICROCART: remove
   
   float result = angle;
 
@@ -102,19 +102,10 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
       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 +129,10 @@ 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 

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

@@ -71,6 +71,8 @@ enum packet_type {
   hoverType         = 5,
   fullStateType     = 6,
   positionType      = 7,
+  attitudeRateType  = 8,
+
 };
 
 /* ---===== 2 - Decoding functions =====--- */
@@ -367,6 +369,37 @@ static void positionDecoder(setpoint_t *setpoint, uint8_t type, const void *data
   setpoint->attitude.yaw = values->yaw;
 }
 
+/*
+ * Attitude rate decoder,
+ * packet contains the desired attitude rate in addition to the thrust value
+ */
+struct attitudeRatePacket_s{
+  float rollRate;   // deg/s
+  float pitchRate;  // deg/s
+  float yawRate;    // deg/s
+  float thrust;     // thrust percentage 0 - 60,000 
+} __attribute__((packed));
+static void attitudeRateDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen){
+  
+  const struct attitudeRatePacket_s *values = data;
+
+  ASSERT(datalen == sizeof(struct attitudeRatePacket_s));
+
+  setpoint->mode.x = modeDisable;
+  setpoint->mode.y = modeDisable;
+  setpoint->mode.z = modeDisable;
+
+  setpoint->mode.roll = modeVelocity;
+  setpoint->mode.pitch = modeVelocity;
+  setpoint->mode.yaw = modeVelocity;
+  
+  setpoint->attitudeRate.roll = values->rollRate;
+  setpoint->attitudeRate.pitch = values->pitchRate;
+  setpoint->attitudeRate.yaw = values->yawRate;
+  
+  setpoint->thrust = values->thrust;
+}
+
  /* ---===== 3 - packetDecoders array =====--- */
 const static packetDecoder_t packetDecoders[] = {
   [stopType]          = stopDecoder,
@@ -377,6 +410,7 @@ const static packetDecoder_t packetDecoders[] = {
   [hoverType]         = hoverDecoder,
   [fullStateType]     = fullStateDecoder,
   [positionType]      = positionDecoder,
+  [attitudeRateType]  = attitudeRateDecoder,
 };
 
 /* Decoder switch */

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

@@ -157,6 +157,11 @@ void attitudeControllerResetRollAttitudePID(void)
     pidReset(&pidRoll);
 }
 
+void attitudeControllerResetYawAttitudePID(void)
+{
+    pidReset(&pidYaw);
+}
+
 void attitudeControllerResetPitchAttitudePID(void)
 {
     pidReset(&pidPitch);