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 fe25dc0529efbfe7c4b7af843374100f1266f69d..f27543b3ec43a5624b66c5899cdb807d9e02e210 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
@@ -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,
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/position_controller.h b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/position_controller.h
index b6655f8a16c43073c76233c2e26d470fceaba601..400d25fcc48edf620a97c8533377d3a52cc0ecc8 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/position_controller.h
+++ b/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);
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 b88001e842f5890aab40aa5949c3f628a93396ec..97f82f1df742b5ce4d32afd358ea6444fa718b0c 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
@@ -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);
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 efc7be6eecb95a94a5b205d78dee832596c3bed3..0981da331e77e166021efd67e2785d0b4dee02fb 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
@@ -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
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/pid.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/pid.c
index 4c160bfe10c68475729c747eb6032e04d3acdd0f..fd3b8c380e11788d362081c831bf14c03c21cd23 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/pid.c
+++ b/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,