Merge branch 'ryan_pid' into 'master'

Student controller documentation and updates

See merge request !65

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

@@ -30,8 +30,18 @@
 #include <stdbool.h>
 #include "commander.h"
 
-
+/**
+ * @brief Initialize all PID data structures with PID coefficients defined in pid.h
+ * 
+ * @param updateDt expected delta time since last call for all PID loops
+ */
 void attitudeControllerInit(const float updateDt);
+
+/**
+ * @brief Simple test to make sure controller is initialized
+ * 
+ * @return true/false
+ */
 bool attitudeControllerTest(void);
 
 /**
@@ -39,6 +49,16 @@ bool attitudeControllerTest(void);
  * the desired rate which should be fed into a rate controller. The
  * attitude controller can be run in a slower update rate then the rate
  * controller.
+ * 
+ * @param eulerRollActual input
+ * @param eulerPitchActual input
+ * @param eulerYawActual input
+ * @param eulerRollDesired input
+ * @param eulerPitchDesired input
+ * @param eulerYawDesired input
+ * @param rollRateDesired output
+ * @param pitchRateDesired output
+ * @param yawRateDesired output
  */
 void attitudeControllerCorrectAttitudePID(
        float eulerRollActual, float eulerPitchActual, float eulerYawActual,
@@ -46,8 +66,17 @@ void attitudeControllerCorrectAttitudePID(
        float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired);
 
 /**
- * Make the controller run an update of the rate PID. The output is
- * the actuator force.
+ * Make the controller run an update of the rate PID. Input comes from the 
+ * correct attitude function. The output is the actuator force. 
+ * 
+ * TODO MICROCART: Add output to this function to better line up with the other controllers
+ * 
+ * @param rollRateActual input
+ * @param pitchRateActual input
+ * @param yawRateActual input
+ * @param rollRateDesired input
+ * @param pitchRateDesired input
+ * @param yawRateDesired input
  */
 void attitudeControllerCorrectRatePID(
        float rollRateActual, float pitchRateActual, float yawRateActual,

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

@@ -30,10 +30,32 @@
 
 // A position controller calculate the thrust, roll, pitch to approach
 // a 3D position setpoint
+
+/**
+ * @brief initialize all PID loops with the PID coefficents defined 
+ * from the parameter system
+ */
 void positionControllerInit();
 void positionControllerResetAllPID();
+/**
+ * @brief Update position PID loops, Output is velocity.
+ * 
+ * @param thrust output
+ * @param attitude output
+ * @param setpoint input
+ * @param state input
+ */
 void positionController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
                                                              const state_t *state);
+
+/**
+ * @brief Update velocity PID loops, Calculates roll and pitch (Excludes yaw control) and thrust
+ * 
+ * @param thrust output 
+ * @param attitude output
+ * @param setpoint input
+ * @param state input
+ */
 void velocityController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
                                                              const state_t *state);
 

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

@@ -37,7 +37,12 @@
 #define ATTITUDE_RATE_LPF_CUTOFF_FREQ 30.0f
 #define ATTITUDE_RATE_LPF_ENABLE false
 
-
+/**
+ * @brief Convert float to 16 bit integer
+ * 
+ * @param in float
+ * @return int16_t 
+ */
 static inline int16_t saturateSignedInt16(float in)
 {
   // don't use INT16_MIN, because later we may negate it, which won't work for that value.
@@ -49,6 +54,7 @@ static inline int16_t saturateSignedInt16(float in)
     return (int16_t)in;
 }
 
+//structs to hold PID data between executions for each axis
 PidObject pidRollRate;
 PidObject pidPitchRate;
 PidObject pidYawRate;
@@ -56,6 +62,7 @@ PidObject pidRoll;
 PidObject pidPitch;
 PidObject pidYaw;
 
+//attitude controller outputs
 static int16_t rollOutput;
 static int16_t pitchOutput;
 static int16_t yawOutput;
@@ -68,6 +75,7 @@ void attitudeControllerInit(const float updateDt)
     return;
 
   //TODO: get parameters from configuration manager instead
+  //initialize all rate PID loops
   pidInit(&pidRollRate,  0, PID_ROLL_RATE_KP,  PID_ROLL_RATE_KI,  PID_ROLL_RATE_KD,
       updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
   pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD,
@@ -75,10 +83,12 @@ void attitudeControllerInit(const float updateDt)
   pidInit(&pidYawRate,   0, PID_YAW_RATE_KP,   PID_YAW_RATE_KI,   PID_YAW_RATE_KD,
       updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
 
+  //set integral limits for all rate PID loops
   pidSetIntegralLimit(&pidRollRate,  PID_ROLL_RATE_INTEGRATION_LIMIT);
   pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
   pidSetIntegralLimit(&pidYawRate,   PID_YAW_RATE_INTEGRATION_LIMIT);
 
+  //initialize all attitude PID loops 
   pidInit(&pidRoll,  0, PID_ROLL_KP,  PID_ROLL_KI,  PID_ROLL_KD,  updateDt,
       ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
   pidInit(&pidPitch, 0, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, updateDt,
@@ -86,6 +96,7 @@ void attitudeControllerInit(const float updateDt)
   pidInit(&pidYaw,   0, PID_YAW_KP,   PID_YAW_KI,   PID_YAW_KD,   updateDt,
       ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
 
+  //set integral limits for attitude PID loops
   pidSetIntegralLimit(&pidRoll,  PID_ROLL_INTEGRATION_LIMIT);
   pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT);
   pidSetIntegralLimit(&pidYaw,   PID_YAW_INTEGRATION_LIMIT);
@@ -98,20 +109,6 @@ bool attitudeControllerTest()
   return isInit;
 }
 
-void attitudeControllerCorrectRatePID(
-       float rollRateActual, float pitchRateActual, float yawRateActual,
-       float rollRateDesired, float pitchRateDesired, float yawRateDesired)
-{
-  pidSetDesired(&pidRollRate, rollRateDesired);
-  rollOutput = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, true));
-
-  pidSetDesired(&pidPitchRate, pitchRateDesired);
-  pitchOutput = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, true));
-
-  pidSetDesired(&pidYawRate, yawRateDesired);
-  yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true));
-}
-
 void attitudeControllerCorrectAttitudePID(
        float eulerRollActual, float eulerPitchActual, float eulerYawActual,
        float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
@@ -124,7 +121,7 @@ void attitudeControllerCorrectAttitudePID(
   pidSetDesired(&pidPitch, eulerPitchDesired);
   *pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, true);
 
-  // Update PID for yaw axis
+  // Update PID for yaw axis, handle error update here instead of in PID calculation
   float yawError;
   yawError = eulerYawDesired - eulerYawActual;
   if (yawError > 180.0f)
@@ -135,6 +132,20 @@ void attitudeControllerCorrectAttitudePID(
   *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false);
 }
 
+void attitudeControllerCorrectRatePID(
+       float rollRateActual, float pitchRateActual, float yawRateActual,
+       float rollRateDesired, float pitchRateDesired, float yawRateDesired)
+{
+  pidSetDesired(&pidRollRate, rollRateDesired);
+  rollOutput = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, true));
+
+  pidSetDesired(&pidPitchRate, pitchRateDesired);
+  pitchOutput = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, true));
+
+  pidSetDesired(&pidYawRate, yawRateDesired);
+  yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true));
+}
+
 void attitudeControllerResetRollAttitudePID(void)
 {
     pidReset(&pidRoll);

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

@@ -8,17 +8,22 @@
 #include "controller_student.h"
 
 #include "log.h"
+#include "debug.h"
+
 #include "param.h"
 #include "math3d.h"
 
-#define ATTITUDE_UPDATE_DT    (float)(1.0f/ATTITUDE_RATE)
+#define STUDENT_UPDATE_DT    (float)(1.0f/ATTITUDE_RATE)
 
 static bool tiltCompensationEnabled = false;
+static bool test_param = false;
 
+//desired vehicle state as calculated be PID controllers
 static attitude_t attitudeDesired;
 static attitude_t rateDesired;
-static float actuatorThrust;
+static float thrustDesired;
 
+//variables used only for logging PID command outputs
 static float cmd_thrust;
 static float cmd_roll;
 static float cmd_pitch;
@@ -30,20 +35,29 @@ static float accelz;
 
 void controllerStudentInit(void)
 {
-  attitudeControllerInit(ATTITUDE_UPDATE_DT);
+  attitudeControllerInit(STUDENT_UPDATE_DT);
   positionControllerInit();
 }
 
 bool controllerStudentTest(void)
 {
   bool pass = true;
-
+  //controller passes check if attitude controller passes
   pass &= attitudeControllerTest();
 
   return pass;
 }
 
+
+/**
+ * Limit the input angle between -180 and 180
+ * 
+ * @param angle 
+ * @return float 
+ */
 static float capAngle(float angle) {
+  //TODO MICROCART: Student written
+  
   float result = angle;
 
   while (result > 180.0f) {
@@ -57,42 +71,66 @@ static float capAngle(float angle) {
   return result;
 }
 
-void controllerStudent(control_t *control, setpoint_t *setpoint,
-                                         const sensorData_t *sensors,
-                                         const state_t *state,
-                                         const uint32_t tick)
+/**
+ * This function is called periodically to update the PID loop,
+ * Reads state estimate and setpoint values and passes them
+ * to the functions that preform PID calculations, postion PID (including Velocity PID),
+ * and attitude PID (including attitude rate)
+ * 
+ * @param control Output, struct is modified as the ouput of the control loop
+ * @param setpoint Input, setpoints for thrust, position, velocity, and acceleration of quad
+ * @param sensors Input, Raw sensor values (typically want to use the state estimated instead) includes gyro, 
+ * accelerometer, barometer, magnatometer 
+ * @param state Input, A more robust way to measure the current state of the quad, allows for direct
+ * measurements of the orientation of the quad. Includes attitude, position, velocity,
+ * and acceleration
+ * @param tick Input, system clock
+ */
+void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick)
 {
-  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 * ATTITUDE_UPDATE_DT;
-    } else {
-      attitudeDesired.yaw = setpoint->attitude.yaw;
-    }
 
-    attitudeDesired.yaw = capAngle(attitudeDesired.yaw);
-  }
+  //TODO MICROCART: student written
 
+  //Check if time to update position/velocity controller with RATE_DO_EXECUTE
+  //then update the position controller to find the desired attitude
   if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
-    positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
+    //note does not calculate desired yaw or yaw rate
+    positionController(&thrustDesired, &attitudeDesired, setpoint, state);
   }
 
+  // check if time to update the attutide controller
   if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
-    // Switch between manual and automatic position control
+    // 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);
+    
+    // if z position control disabled, set the output thrust to be the same as the setpoint thrust
+    // disreguarding the results of the position controller
     if (setpoint->mode.z == modeDisable) {
-      actuatorThrust = setpoint->thrust;
+      thrustDesired = setpoint->thrust;
     }
+    // if x or y position control is disabled, set the desired 
+    // roll and pitch to equal their setpoints, disreguarding the results of the position controller
     if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
       attitudeDesired.roll = setpoint->attitude.roll;
       attitudeDesired.pitch = setpoint->attitude.pitch;
     }
 
+    // Run the attitude controller with the actual attitude and desired attitude
+    // outputs the desired attitude rates
     attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
                                 attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
                                 &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
 
-    // For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
-    // value. Also reset the PID to avoid error buildup, which can lead to unstable
+    // For roll and pitch, if velocity mode, overwrite rateDesired output 
+    // from the attitude controller with the setpoint value
+    // Also reset the PID to avoid error buildup, which can lead to unstable
     // behavior if level mode is engaged later
     if (setpoint->mode.roll == modeVelocity) {
       rateDesired.roll = setpoint->attitudeRate.roll;
@@ -103,16 +141,21 @@ void controllerStudent(control_t *control, setpoint_t *setpoint,
       attitudeControllerResetPitchAttitudePID();
     }
 
-    // TODO: Investigate possibility to subtract gyro drift.
+    //update the attitude rate PID, given the current angular rate 
+    //read by the gyro and the desired rate 
     attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
                              rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
 
+    //get actuator outputs from the rate controller 
+    //TODO MICROCART: remove this redundant call
     attitudeControllerGetActuatorOutput(&control->roll,
                                         &control->pitch,
                                         &control->yaw);
 
+    //invert yaw control
     control->yaw = -control->yaw;
 
+    //copy values for logging
     cmd_thrust = control->thrust;
     cmd_roll = control->roll;
     cmd_pitch = control->pitch;
@@ -123,15 +166,19 @@ void controllerStudent(control_t *control, setpoint_t *setpoint,
     accelz = sensors->acc.z;
   }
 
+  //tilt compensation, increases thrust when tilting more to maintain height
+  //copy working value of actuator thrust to control->thrust for output
   if (tiltCompensationEnabled)
   {
-    control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
+    control->thrust = thrustDesired / sensfusion6GetInvThrustCompensationForTilt();
   }
   else
   {
-    control->thrust = actuatorThrust;
+    control->thrust = thrustDesired;
   }
 
+
+  //if no thrust active, set all other outputs to 0 and reset PID variables
   if (control->thrust == 0)
   {
     control->thrust = 0;
@@ -144,19 +191,20 @@ void controllerStudent(control_t *control, setpoint_t *setpoint,
     cmd_pitch = control->pitch;
     cmd_yaw = control->yaw;
 
+    //reset all PID variables
     attitudeControllerResetAllPID();
     positionControllerResetAllPID();
 
-    // Reset the calculated YAW angle for rate control
+    // Reset the YAW angle for rate control
     attitudeDesired.yaw = state->attitude.yaw;
   }
 }
 
 /**
  * Logging variables for the command and reference signals for the
- * altitude PID controller
+ * student PID controller
  */
-LOG_GROUP_START(controller)
+LOG_GROUP_START(ctrlStdnt)
 /**
  * @brief Thrust command
  */
@@ -192,7 +240,7 @@ LOG_ADD(LOG_FLOAT, accelz, &accelz)
 /**
  * @brief Thrust command without (tilt)compensation
  */
-LOG_ADD(LOG_FLOAT, actuatorThrust, &actuatorThrust)
+LOG_ADD(LOG_FLOAT, thrustDesired, &thrustDesired)
 /**
  * @brief Desired roll setpoint
  */
@@ -217,15 +265,17 @@ LOG_ADD(LOG_FLOAT, pitchRate, &rateDesired.pitch)
  * @brief Desired yaw rate setpoint
  */
 LOG_ADD(LOG_FLOAT, yawRate,   &rateDesired.yaw)
-LOG_GROUP_STOP(controller)
+
+LOG_GROUP_STOP(ctrlStdnt)
 
 
 /**
  * Controller parameters
  */
-PARAM_GROUP_START(controller)
+PARAM_GROUP_START(ctrlStdnt)
 /**
  * @brief Nonzero for tilt compensation enabled (default: 0)
  */
 PARAM_ADD(PARAM_UINT8, tiltComp, &tiltCompensationEnabled)
-PARAM_GROUP_STOP(controller)
\ No newline at end of file
+PARAM_ADD(PARAM_UINT8, TEST_PARAM, &test_param)
+PARAM_GROUP_STOP(ctrlStdnt)
\ No newline at end of file

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

@@ -30,6 +30,8 @@
 #include <math.h>
 #include <float.h>
 
+//TODO MICROCART rewrite the PID module
+
 void pidInit(PidObject* pid, const float desired, const float kp,
              const float ki, const float kd, const float dt,
              const float samplingRate, const float cutoffFreq,