Merge branch 'controller_consolidation' into 'master'

all student files created

See merge request !68

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

+/**
+ *    ||          ____  _ __  ______
+ * +------+      / __ )(_) /_/ ____/_________ _____  ___
+ * | 0xBC |     / __  / / __/ /    / ___/ __ `/_  / / _	\
+ * +------+    / /_/ / / /_/ /___ / /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\____//_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * attitude_controller.h: PID-based attitude controller
+ */
+
+#ifndef ATTITUDE_CONTROLLER_H_
+#define ATTITUDE_CONTROLLER_H_
+
+#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);
+
+/**
+ * Make the controller run an update of the attitude PID. The output is
+ * 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,
+       float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
+       float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired);
+
+/**
+ * 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,
+       float rollRateDesired, float pitchRateDesired, float yawRateDesired,
+       int16_t* roll, int16_t* pitch, int16_t* yaw);
+
+/**
+ * Reset controller roll attitude PID
+ */
+void attitudeControllerResetRollAttitudePID(void);
+
+/**
+ * Reset controller pitch attitude PID
+ */
+void attitudeControllerResetPitchAttitudePID(void);
+
+/**
+ * Reset controller roll, pitch and yaw PID's.
+ */
+void attitudeControllerResetAllPID(void);
+
+/**
+ * Get the actuator output.
+ */
+void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw);
+
+
+#endif /* ATTITUDE_CONTROLLER_H_ */

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

+#ifndef PID_H_
+#define PID_H_
+
+#include <stdbool.h>
+#include "filter.h"
+
+#define PID_ROLL_RATE_KP  250.0
+#define PID_ROLL_RATE_KI  500.0
+#define PID_ROLL_RATE_KD  2.5
+#define PID_ROLL_RATE_INTEGRATION_LIMIT    33.3
+
+#define PID_PITCH_RATE_KP  250.0
+#define PID_PITCH_RATE_KI  500.0
+#define PID_PITCH_RATE_KD  2.5
+#define PID_PITCH_RATE_INTEGRATION_LIMIT   33.3
+
+#define PID_YAW_RATE_KP  120.0
+#define PID_YAW_RATE_KI  16.7
+#define PID_YAW_RATE_KD  0.0
+#define PID_YAW_RATE_INTEGRATION_LIMIT     166.7
+
+#define PID_ROLL_KP  6.0
+#define PID_ROLL_KI  3.0
+#define PID_ROLL_KD  0.0
+#define PID_ROLL_INTEGRATION_LIMIT    20.0
+
+#define PID_PITCH_KP  6.0
+#define PID_PITCH_KI  3.0
+#define PID_PITCH_KD  0.0
+#define PID_PITCH_INTEGRATION_LIMIT   20.0
+
+#define PID_YAW_KP  6.0
+#define PID_YAW_KI  1.0
+#define PID_YAW_KD  0.35
+#define PID_YAW_INTEGRATION_LIMIT     360.0
+
+
+#define DEFAULT_PID_INTEGRATION_LIMIT 5000.0
+#define DEFAULT_PID_OUTPUT_LIMIT      0.0
+
+
+typedef struct
+{
+  float desired;      //< set point
+  float error;        //< error
+  float prevError;    //< previous error
+  float integ;        //< integral
+  float deriv;        //< derivative
+  float kp;           //< proportional gain
+  float ki;           //< integral gain
+  float kd;           //< derivative gain
+  float outP;         //< proportional output (debugging)
+  float outI;         //< integral output (debugging)
+  float outD;         //< derivative output (debugging)
+  float iLimit;       //< integral limit, absolute value. '0' means no limit.
+  float outputLimit;  //< total PID output limit, absolute value. '0' means no limit.
+  float dt;           //< delta-time dt
+  lpf2pData dFilter;  //< filter for D term
+  bool enableDFilter; //< filter for D term enable flag
+} PidObject;
+
+/**
+ * PID object initialization.
+ *
+ * @param[out] pid   A pointer to the pid object to initialize.
+ * @param[in] desired  The initial set point.
+ * @param[in] kp        The proportional gain
+ * @param[in] ki        The integral gain
+ * @param[in] kd        The derivative gain
+ * @param[in] dt        Delta time since the last call
+ * @param[in] samplingRate Frequency the update will be called
+ * @param[in] cutoffFreq   Frequency to set the low pass filter cutoff at
+ * @param[in] enableDFilter Enable setting for the D lowpass filter
+ */
+ 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,
+              bool enableDFilter);
+
+/**
+ * Set the integral limit for this PID in deg.
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] limit Pid integral swing limit.
+ */
+void pidSetIntegralLimit(PidObject* pid, const float limit);
+
+/**
+ * Reset the PID error values
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] limit Pid integral swing limit.
+ */
+void pidReset(PidObject* pid);
+
+/**
+ * Update the PID parameters.
+ *
+ * @param[in] pid         A pointer to the pid object.
+ * @param[in] measured    The measured value
+ * @param[in] updateError Set to TRUE if error should be calculated.
+ *                        Set to False if pidSetError() has been used.
+ * @return PID algorithm output
+ */
+float pidUpdate(PidObject* pid, const float measured, const bool updateError);
+
+/**
+ * Set a new set point for the PID to track.
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] angle The new set point
+ */
+void pidSetDesired(PidObject* pid, const float desired);
+
+/**
+ * Set a new set point for the PID to track.
+ * @return The set point
+ */
+float pidGetDesired(PidObject* pid);
+
+/**
+ * Find out if PID is active
+ * @return TRUE if active, FALSE otherwise
+ */
+bool pidIsActive(PidObject* pid);
+
+/**
+ * Set a new error. Use if a special error calculation is needed.
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] error The new error
+ */
+void pidSetError(PidObject* pid, const float error);
+
+/**
+ * Set a new proportional gain for the PID.
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] kp    The new proportional gain
+ */
+void pidSetKp(PidObject* pid, const float kp);
+
+/**
+ * Set a new integral gain for the PID.
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] ki    The new integral gain
+ */
+void pidSetKi(PidObject* pid, const float ki);
+
+/**
+ * Set a new derivative gain for the PID.
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] kd    The derivative gain
+ */
+void pidSetKd(PidObject* pid, const float kd);
+
+/**
+ * Set a new dt gain for the PID. Defaults to IMU_UPDATE_DT upon construction
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] dt    Delta time
+ */
+void pidSetDt(PidObject* pid, const float dt);
+#endif /* PID_H_ */
\ No newline at end of file

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

@@ -2,7 +2,7 @@
 #include "stabilizer.h"
 #include "stabilizer_types.h"
 
-#include "attitude_controller.h"
+#include "student_attitude_controller.h"
 #include "sensfusion6.h"
 #include "controller_student.h"
 
@@ -142,7 +142,8 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
     //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);
+                             rateDesired.roll, rateDesired.pitch, rateDesired.yaw,
+                             &(control->roll), &(control->pitch), &(control->yaw));
 
     //get actuator outputs from the rate controller 
     //TODO MICROCART: remove this redundant call

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

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie Firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * student_attitude_pid_controller.c: Attitude controller using PID correctors
+ */
+#include <stdbool.h>
+
+#include "FreeRTOS.h"
+
+#include "student_attitude_controller.h"
+#include "student_pid.h"
+#include "param.h"
+#include "log.h"
+
+#define ATTITUDE_LPF_CUTOFF_FREQ      15.0f
+#define ATTITUDE_LPF_ENABLE      false
+#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.
+  if (in > INT16_MAX)
+    return INT16_MAX;
+  else if (in < -INT16_MAX)
+    return -INT16_MAX;
+  else
+    return (int16_t)in;
+}
+
+//structs to hold PID data between executions for each axis
+PidObject pidRollRate;
+PidObject pidPitchRate;
+PidObject pidYawRate;
+PidObject pidRoll;
+PidObject pidPitch;
+PidObject pidYaw;
+
+//attitude controller outputs
+static int16_t rollOutput;
+static int16_t pitchOutput;
+static int16_t yawOutput;
+
+static bool isInit;
+
+void attitudeControllerInit(const float updateDt)
+{
+  if(isInit)
+    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,
+      updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
+  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,
+      ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
+  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);
+
+  isInit = true;
+}
+
+bool attitudeControllerTest()
+{
+  return isInit;
+}
+
+void attitudeControllerCorrectAttitudePID(
+       float eulerRollActual, float eulerPitchActual, float eulerYawActual,
+       float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
+       float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired)
+{
+  pidSetDesired(&pidRoll, eulerRollDesired);
+  *rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, true);
+
+  // Update PID for pitch axis
+  pidSetDesired(&pidPitch, eulerPitchDesired);
+  *pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, true);
+
+  // Update PID for yaw axis, handle error update here instead of in PID calculation
+  float yawError;
+  yawError = eulerYawDesired - eulerYawActual;
+  if (yawError > 180.0f)
+    yawError -= 360.0f;
+  else if (yawError < -180.0f)
+    yawError += 360.0f;
+  pidSetError(&pidYaw, yawError);
+  *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false);
+}
+
+void attitudeControllerCorrectRatePID(
+       float rollRateActual, float pitchRateActual, float yawRateActual,
+       float rollRateDesired, float pitchRateDesired, float yawRateDesired,
+       int16_t* roll, int16_t* pitch, int16_t* yaw
+       )
+{
+  pidSetDesired(&pidRollRate, rollRateDesired);
+  *roll = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, true));
+
+  pidSetDesired(&pidPitchRate, pitchRateDesired);
+  *pitch = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, true));
+
+  pidSetDesired(&pidYawRate, yawRateDesired);
+  *yaw = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true));
+
+  //*roll = rollOutput;
+  //*pitch = pitchOutput;
+  //*yaw = yawOutput;
+}
+
+void attitudeControllerResetRollAttitudePID(void)
+{
+    pidReset(&pidRoll);
+}
+
+void attitudeControllerResetPitchAttitudePID(void)
+{
+    pidReset(&pidPitch);
+}
+
+void attitudeControllerResetAllPID(void)
+{
+  pidReset(&pidRoll);
+  pidReset(&pidPitch);
+  pidReset(&pidYaw);
+  pidReset(&pidRollRate);
+  pidReset(&pidPitchRate);
+  pidReset(&pidYawRate);
+}
+
+/*void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw)
+{
+  *roll = rollOutput;
+  *pitch = pitchOutput;
+  *yaw = yawOutput;
+}*/
+
+/**
+ *  Log variables of attitude PID controller
+ */ 
+LOG_GROUP_START(pid_attitude)
+/**
+ * @brief Proportional output roll
+ */
+LOG_ADD(LOG_FLOAT, roll_outP, &pidRoll.outP)
+/**
+ * @brief Integral output roll
+ */
+LOG_ADD(LOG_FLOAT, roll_outI, &pidRoll.outI)
+/**
+ * @brief Derivative output roll
+ */
+LOG_ADD(LOG_FLOAT, roll_outD, &pidRoll.outD)
+/**
+ * @brief Proportional output pitch
+ */
+LOG_ADD(LOG_FLOAT, pitch_outP, &pidPitch.outP)
+/**
+ * @brief Integral output pitch
+ */
+LOG_ADD(LOG_FLOAT, pitch_outI, &pidPitch.outI)
+/**
+ * @brief Derivative output pitch
+ */
+LOG_ADD(LOG_FLOAT, pitch_outD, &pidPitch.outD)
+/**
+ * @brief Proportional output yaw
+ */
+LOG_ADD(LOG_FLOAT, yaw_outP, &pidYaw.outP)
+/**
+ * @brief Intergal output yaw
+ */
+LOG_ADD(LOG_FLOAT, yaw_outI, &pidYaw.outI)
+/**
+ * @brief Derivative output yaw
+ */
+LOG_ADD(LOG_FLOAT, yaw_outD, &pidYaw.outD)
+LOG_GROUP_STOP(pid_attitude)
+
+/**
+ *  Log variables of attitude rate PID controller
+ */
+LOG_GROUP_START(pid_rate)
+/**
+ * @brief Proportional output roll rate
+ */
+LOG_ADD(LOG_FLOAT, roll_outP, &pidRollRate.outP)
+/**
+ * @brief Integral output roll rate
+ */
+LOG_ADD(LOG_FLOAT, roll_outI, &pidRollRate.outI)
+/**
+ * @brief Derivative output roll rate
+ */
+LOG_ADD(LOG_FLOAT, roll_outD, &pidRollRate.outD)
+/**
+ * @brief Proportional output pitch rate
+ */
+LOG_ADD(LOG_FLOAT, pitch_outP, &pidPitchRate.outP)
+/**
+ * @brief Integral output pitch rate
+ */
+LOG_ADD(LOG_FLOAT, pitch_outI, &pidPitchRate.outI)
+/**
+ * @brief Derivative output pitch rate
+ */
+LOG_ADD(LOG_FLOAT, pitch_outD, &pidPitchRate.outD)
+/**
+ * @brief Proportional output yaw rate
+ */
+LOG_ADD(LOG_FLOAT, yaw_outP, &pidYawRate.outP)
+/**
+ * @brief Integral output yaw rate
+ */
+LOG_ADD(LOG_FLOAT, yaw_outI, &pidYawRate.outI)
+/**
+ * @brief Derivative output yaw rate
+ */
+LOG_ADD(LOG_FLOAT, yaw_outD, &pidYawRate.outD)
+LOG_GROUP_STOP(pid_rate)
+
+/**
+ * Tuning settings for the gains of the PID
+ * controller for the attitude of the Crazyflie which consists
+ * of the Yaw Pitch and Roll 
+ */
+PARAM_GROUP_START(pid_attitude)
+/**
+ * @brief Proportional gain for the PID roll controller
+ */
+PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRoll.kp)
+/**
+ * @brief Integral gain for the PID roll controller
+ */
+PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRoll.ki)
+/**
+ * @brief Derivative gain for the PID roll controller
+ */
+PARAM_ADD(PARAM_FLOAT, roll_kd, &pidRoll.kd)
+/**
+ * @brief Proportional gain for the PID pitch controller
+ */
+PARAM_ADD(PARAM_FLOAT, pitch_kp, &pidPitch.kp)
+/**
+ * @brief Integral gain for the PID pitch controller
+ */
+PARAM_ADD(PARAM_FLOAT, pitch_ki, &pidPitch.ki)
+/**
+ * @brief Derivative gain for the PID pitch controller
+ */
+PARAM_ADD(PARAM_FLOAT, pitch_kd, &pidPitch.kd)
+/**
+ * @brief Proportional gain for the PID yaw controller
+ */
+PARAM_ADD(PARAM_FLOAT, yaw_kp, &pidYaw.kp)
+/**
+ * @brief Integral gain for the PID yaw controller
+ */
+PARAM_ADD(PARAM_FLOAT, yaw_ki, &pidYaw.ki)
+/**
+ * @brief Derivative gain for the PID yaw controller
+ */
+PARAM_ADD(PARAM_FLOAT, yaw_kd, &pidYaw.kd)
+PARAM_GROUP_STOP(pid_attitude)
+
+/**
+ * Tuning settings for the gains of the PID controller for the rate angles of
+ * the Crazyflie, which consists of the yaw, pitch and roll rates 
+ */
+PARAM_GROUP_START(pid_rate)
+/**
+ * @brief Proportional gain for the PID roll rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRollRate.kp)
+/**
+ * @brief Integral gain for the PID roll rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRollRate.ki)
+/**
+ * @brief Derivative gain for the PID roll rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, roll_kd, &pidRollRate.kd)
+/**
+ * @brief Proportional gain for the PID pitch rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, pitch_kp, &pidPitchRate.kp)
+/**
+ * @brief Integral gain for the PID pitch rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, pitch_ki, &pidPitchRate.ki)
+/**
+ * @brief Derivative gain for the PID pitch rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, pitch_kd, &pidPitchRate.kd)
+/**
+ * @brief Proportional gain for the PID yaw rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, yaw_kp, &pidYawRate.kp)
+/**
+ * @brief Integral gain for the PID yaw rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, yaw_ki, &pidYawRate.ki)
+/**
+ * @brief Derivative gain for the PID yaw rate controller
+ */
+PARAM_ADD(PARAM_FLOAT, yaw_kd, &pidYawRate.kd)
+PARAM_GROUP_STOP(pid_rate)

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

+#include "student_pid.h"
+#include "num.h"
+#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,
+             bool enableDFilter)
+{
+  pid->error         = 0;
+  pid->prevError     = 0;
+  pid->integ         = 0;
+  pid->deriv         = 0;
+  pid->desired       = desired;
+  pid->kp            = kp;
+  pid->ki            = ki;
+  pid->kd            = kd;
+  pid->iLimit        = DEFAULT_PID_INTEGRATION_LIMIT;
+  pid->outputLimit   = DEFAULT_PID_OUTPUT_LIMIT;
+  pid->dt            = dt;
+  pid->enableDFilter = enableDFilter;
+  if (pid->enableDFilter)
+  {
+    lpf2pInit(&pid->dFilter, samplingRate, cutoffFreq);
+  }
+}
+
+float pidUpdate(PidObject* pid, const float measured, const bool updateError)
+{
+    float output = 0.0f;
+
+    if (updateError)
+    {
+        pid->error = pid->desired - measured;
+    }
+
+    pid->outP = pid->kp * pid->error;
+    output += pid->outP;
+
+    float deriv = (pid->error - pid->prevError) / pid->dt;
+    if (pid->enableDFilter)
+    {
+      pid->deriv = lpf2pApply(&pid->dFilter, deriv);
+    } else {
+      pid->deriv = deriv;
+    }
+    if (isnan(pid->deriv)) {
+      pid->deriv = 0;
+    }
+    pid->outD = pid->kd * pid->deriv;
+    output += pid->outD;
+
+    pid->integ += pid->error * pid->dt;
+
+    // Constrain the integral (unless the iLimit is zero)
+    if(pid->iLimit != 0)
+    {
+    	pid->integ = constrain(pid->integ, -pid->iLimit, pid->iLimit);
+    }
+
+    pid->outI = pid->ki * pid->integ;
+    output += pid->outI;
+
+    // Constrain the total PID output (unless the outputLimit is zero)
+    if(pid->outputLimit != 0)
+    {
+      output = constrain(output, -pid->outputLimit, pid->outputLimit);
+    }
+
+
+    pid->prevError = pid->error;
+
+    return output;
+}
+
+void pidSetIntegralLimit(PidObject* pid, const float limit) {
+    pid->iLimit = limit;
+}
+
+
+void pidReset(PidObject* pid)
+{
+  pid->error     = 0;
+  pid->prevError = 0;
+  pid->integ     = 0;
+  pid->deriv     = 0;
+}
+
+void pidSetError(PidObject* pid, const float error)
+{
+  pid->error = error;
+}
+
+void pidSetDesired(PidObject* pid, const float desired)
+{
+  pid->desired = desired;
+}
+
+float pidGetDesired(PidObject* pid)
+{
+  return pid->desired;
+}
+
+bool pidIsActive(PidObject* pid)
+{
+  bool isActive = true;
+
+  if (pid->kp < 0.0001f && pid->ki < 0.0001f && pid->kd < 0.0001f)
+  {
+    isActive = false;
+  }
+
+  return isActive;
+}
+
+void pidSetKp(PidObject* pid, const float kp)
+{
+  pid->kp = kp;
+}
+
+void pidSetKi(PidObject* pid, const float ki)
+{
+  pid->ki = ki;
+}
+
+void pidSetKd(PidObject* pid, const float kd)
+{
+  pid->kd = kd;
+}
+void pidSetDt(PidObject* pid, const float dt) {
+    pid->dt = dt;
+}
\ No newline at end of file