diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_attitude_controller.h b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_attitude_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..99ae38a69ac43f9ef08ac52d40f6630e76ea9278
--- /dev/null
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_attitude_controller.h
@@ -0,0 +1,107 @@
+/**
+ * || ____ _ __ ______
+ * +------+ / __ )(_) /_/ ____/_________ _____ ___
+ * | 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_ */
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_pid.h b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_pid.h
new file mode 100644
index 0000000000000000000000000000000000000000..480fe4dc864f8d491cd8f72f8372a90f6f2bd5a2
--- /dev/null
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_pid.h
@@ -0,0 +1,166 @@
+#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
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..cd8ed7b6a5853a68c87e484ec21a8c1a3cfff4dc 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
@@ -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
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_attitude_controller.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_attitude_controller.c
new file mode 100644
index 0000000000000000000000000000000000000000..221e3c93a840978d0995deedc24685f17d9749ee
--- /dev/null
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_attitude_controller.c
@@ -0,0 +1,351 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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)
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_pid.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_pid.c
new file mode 100644
index 0000000000000000000000000000000000000000..42aadfea708fcf2cfc8f93a69fbe5cf795c24f11
--- /dev/null
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_pid.c
@@ -0,0 +1,135 @@
+#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