Skip to content
Snippets Groups Projects
Commit 4c34a994 authored by C-Glick's avatar C-Glick
Browse files

Renamed student attitude and PID functions

parent 8944ab7c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 85 additions and 96 deletions
crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_attitude_controller.h
+ 8
− 13
View file @ 4c34a994
...@@ -35,14 +35,14 @@ ...@@ -35,14 +35,14 @@
* *
* @param updateDt expected delta time since last call for all PID loops * @param updateDt expected delta time since last call for all PID loops
*/ */
void attitudeControllerInit(const float updateDt); void studentAttitudeControllerInit(const float updateDt);
/** /**
* @brief Simple test to make sure controller is initialized * @brief Simple test to make sure controller is initialized
* *
* @return true/false * @return true/false
*/ */
bool attitudeControllerTest(void); bool studentAttitudeControllerTest(void);
/** /**
* Make the controller run an update of the attitude PID. The output is * Make the controller run an update of the attitude PID. The output is
...@@ -60,7 +60,7 @@ bool attitudeControllerTest(void); ...@@ -60,7 +60,7 @@ bool attitudeControllerTest(void);
* @param pitchRateDesired output * @param pitchRateDesired output
* @param yawRateDesired output * @param yawRateDesired output
*/ */
void attitudeControllerCorrectAttitudePID( void studentAttitudeControllerCorrectAttitudePID(
float eulerRollActual, float eulerPitchActual, float eulerYawActual, float eulerRollActual, float eulerPitchActual, float eulerYawActual,
float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired, float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired); float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired);
...@@ -78,7 +78,7 @@ void attitudeControllerCorrectAttitudePID( ...@@ -78,7 +78,7 @@ void attitudeControllerCorrectAttitudePID(
* @param pitchRateDesired input * @param pitchRateDesired input
* @param yawRateDesired input * @param yawRateDesired input
*/ */
void attitudeControllerCorrectRatePID( void studentAttitudeControllerCorrectRatePID(
float rollRateActual, float pitchRateActual, float yawRateActual, float rollRateActual, float pitchRateActual, float yawRateActual,
float rollRateDesired, float pitchRateDesired, float yawRateDesired, float rollRateDesired, float pitchRateDesired, float yawRateDesired,
int16_t* roll, int16_t* pitch, int16_t* yaw); int16_t* roll, int16_t* pitch, int16_t* yaw);
...@@ -86,27 +86,22 @@ void attitudeControllerCorrectRatePID( ...@@ -86,27 +86,22 @@ void attitudeControllerCorrectRatePID(
/** /**
* Reset controller roll attitude PID * Reset controller roll attitude PID
*/ */
void attitudeControllerResetRollAttitudePID(void); void studentAttitudeControllerResetRollAttitudePID(void);
/** /**
* Reset controller pitch attitude PID * Reset controller pitch attitude PID
*/ */
void attitudeControllerResetPitchAttitudePID(void); void studentAttitudeControllerResetPitchAttitudePID(void);
/** /**
* Reset controller yaw attitude PID * Reset controller yaw attitude PID
*/ */
void attitudeControllerResetYawAttitudePID(void); void studentAttitudeControllerResetYawAttitudePID(void);
/** /**
* Reset controller roll, pitch and yaw PID's. * Reset controller roll, pitch and yaw PID's.
*/ */
void attitudeControllerResetAllPID(void); void studentAttitudeControllerResetAllPID(void);
/**
* Get the actuator output.
*/
void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw);
#endif /* STUDENT_ATTITUDE_CONTROLLER_H_ */ #endif /* STUDENT_ATTITUDE_CONTROLLER_H_ */
crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/student_pid.h
+ 13
− 13
View file @ 4c34a994
...@@ -72,7 +72,7 @@ typedef struct ...@@ -72,7 +72,7 @@ typedef struct
* @param[in] cutoffFreq Frequency to set the low pass filter cutoff at * @param[in] cutoffFreq Frequency to set the low pass filter cutoff at
* @param[in] enableDFilter Enable setting for the D lowpass filter * @param[in] enableDFilter Enable setting for the D lowpass filter
*/ */
void pidInit(PidObject* pid, const float desired, const float kp, void studentPidInit(PidObject* pid, const float desired, const float kp,
const float ki, const float kd, const float dt, const float ki, const float kd, const float dt,
const float samplingRate, const float cutoffFreq, const float samplingRate, const float cutoffFreq,
bool enableDFilter); bool enableDFilter);
...@@ -83,7 +83,7 @@ typedef struct ...@@ -83,7 +83,7 @@ typedef struct
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] limit Pid integral swing limit. * @param[in] limit Pid integral swing limit.
*/ */
void pidSetIntegralLimit(PidObject* pid, const float limit); void studentPidSetIntegralLimit(PidObject* pid, const float limit);
/** /**
* Reset the PID error values * Reset the PID error values
...@@ -91,7 +91,7 @@ void pidSetIntegralLimit(PidObject* pid, const float limit); ...@@ -91,7 +91,7 @@ void pidSetIntegralLimit(PidObject* pid, const float limit);
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] limit Pid integral swing limit. * @param[in] limit Pid integral swing limit.
*/ */
void pidReset(PidObject* pid); void studentPidReset(PidObject* pid);
/** /**
* Update the PID parameters. * Update the PID parameters.
...@@ -99,10 +99,10 @@ void pidReset(PidObject* pid); ...@@ -99,10 +99,10 @@ void pidReset(PidObject* pid);
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] measured The measured value * @param[in] measured The measured value
* @param[in] updateError Set to TRUE if error should be calculated. * @param[in] updateError Set to TRUE if error should be calculated.
* Set to False if pidSetError() has been used. * Set to False if studentPidSetError() has been used.
* @return PID algorithm output * @return PID algorithm output
*/ */
float pidUpdate(PidObject* pid, const float measured, const bool updateError); float studentPidUpdate(PidObject* pid, const float measured, const bool updateError);
/** /**
* Set a new set point for the PID to track. * Set a new set point for the PID to track.
...@@ -110,19 +110,19 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError); ...@@ -110,19 +110,19 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError);
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] angle The new set point * @param[in] angle The new set point
*/ */
void pidSetDesired(PidObject* pid, const float desired); void studentPidSetDesired(PidObject* pid, const float desired);
/** /**
* Set a new set point for the PID to track. * Set a new set point for the PID to track.
* @return The set point * @return The set point
*/ */
float pidGetDesired(PidObject* pid); float studentPidGetDesired(PidObject* pid);
/** /**
* Find out if PID is active * Find out if PID is active
* @return TRUE if active, FALSE otherwise * @return TRUE if active, FALSE otherwise
*/ */
bool pidIsActive(PidObject* pid); bool studentPidIsActive(PidObject* pid);
/** /**
* Set a new error. Use if a special error calculation is needed. * Set a new error. Use if a special error calculation is needed.
...@@ -130,7 +130,7 @@ bool pidIsActive(PidObject* pid); ...@@ -130,7 +130,7 @@ bool pidIsActive(PidObject* pid);
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] error The new error * @param[in] error The new error
*/ */
void pidSetError(PidObject* pid, const float error); void studentPidSetError(PidObject* pid, const float error);
/** /**
* Set a new proportional gain for the PID. * Set a new proportional gain for the PID.
...@@ -138,7 +138,7 @@ void pidSetError(PidObject* pid, const float error); ...@@ -138,7 +138,7 @@ void pidSetError(PidObject* pid, const float error);
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] kp The new proportional gain * @param[in] kp The new proportional gain
*/ */
void pidSetKp(PidObject* pid, const float kp); void studentPidSetKp(PidObject* pid, const float kp);
/** /**
* Set a new integral gain for the PID. * Set a new integral gain for the PID.
...@@ -146,7 +146,7 @@ void pidSetKp(PidObject* pid, const float kp); ...@@ -146,7 +146,7 @@ void pidSetKp(PidObject* pid, const float kp);
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] ki The new integral gain * @param[in] ki The new integral gain
*/ */
void pidSetKi(PidObject* pid, const float ki); void studentPidSetKi(PidObject* pid, const float ki);
/** /**
* Set a new derivative gain for the PID. * Set a new derivative gain for the PID.
...@@ -154,7 +154,7 @@ void pidSetKi(PidObject* pid, const float ki); ...@@ -154,7 +154,7 @@ void pidSetKi(PidObject* pid, const float ki);
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] kd The derivative gain * @param[in] kd The derivative gain
*/ */
void pidSetKd(PidObject* pid, const float kd); void studentPidSetKd(PidObject* pid, const float kd);
/** /**
* Set a new dt gain for the PID. Defaults to IMU_UPDATE_DT upon construction * Set a new dt gain for the PID. Defaults to IMU_UPDATE_DT upon construction
...@@ -162,5 +162,5 @@ void pidSetKd(PidObject* pid, const float kd); ...@@ -162,5 +162,5 @@ void pidSetKd(PidObject* pid, const float kd);
* @param[in] pid A pointer to the pid object. * @param[in] pid A pointer to the pid object.
* @param[in] dt Delta time * @param[in] dt Delta time
*/ */
void pidSetDt(PidObject* pid, const float dt); void studentPidSetDt(PidObject* pid, const float dt);
#endif /* PID_H_ */ #endif /* PID_H_ */
\ No newline at end of file
crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
+ 8
− 14
View file @ 4c34a994
...@@ -34,14 +34,14 @@ static float accelz; ...@@ -34,14 +34,14 @@ static float accelz;
void controllerStudentInit(void) void controllerStudentInit(void)
{ {
attitudeControllerInit(STUDENT_UPDATE_DT); studentAttitudeControllerInit(STUDENT_UPDATE_DT);
} }
bool controllerStudentTest(void) bool controllerStudentTest(void)
{ {
bool pass = true; bool pass = true;
//controller passes check if attitude controller passes //controller passes check if attitude controller passes
pass &= attitudeControllerTest(); pass &= studentAttitudeControllerTest();
return pass; return pass;
} }
...@@ -114,7 +114,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat ...@@ -114,7 +114,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
// Run the attitude controller with the actual attitude and desired attitude // Run the attitude controller with the actual attitude and desired attitude
// outputs the desired attitude rates // outputs the desired attitude rates
attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw, studentAttitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw, attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
&rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw); &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
...@@ -124,30 +124,24 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat ...@@ -124,30 +124,24 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
// behavior if level mode is engaged later // behavior if level mode is engaged later
if (setpoint->mode.roll == modeVelocity) { if (setpoint->mode.roll == modeVelocity) {
rateDesired.roll = setpoint->attitudeRate.roll; rateDesired.roll = setpoint->attitudeRate.roll;
attitudeControllerResetRollAttitudePID(); studentAttitudeControllerResetRollAttitudePID();
} }
if (setpoint->mode.pitch == modeVelocity) { if (setpoint->mode.pitch == modeVelocity) {
rateDesired.pitch = setpoint->attitudeRate.pitch; rateDesired.pitch = setpoint->attitudeRate.pitch;
attitudeControllerResetPitchAttitudePID(); studentAttitudeControllerResetPitchAttitudePID();
} }
if(setpoint->mode.yaw == modeVelocity) { if(setpoint->mode.yaw == modeVelocity) {
rateDesired.yaw = setpoint->attitudeRate.yaw; rateDesired.yaw = setpoint->attitudeRate.yaw;
attitudeControllerResetYawAttitudePID(); studentAttitudeControllerResetYawAttitudePID();
} }
//update the attitude rate PID, given the current angular rate //update the attitude rate PID, given the current angular rate
//read by the gyro and the desired rate //read by the gyro and the desired rate
attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z, studentAttitudeControllerCorrectRatePID(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)); &(control->roll), &(control->pitch), &(control->yaw));
//get actuator outputs from the rate controller
//TODO MICROCART: remove this redundant call
attitudeControllerGetActuatorOutput(&control->roll,
&control->pitch,
&control->yaw);
//invert yaw control //invert yaw control
control->yaw = -control->yaw; control->yaw = -control->yaw;
} }
...@@ -173,7 +167,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat ...@@ -173,7 +167,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
control->yaw = 0; control->yaw = 0;
//reset all PID variables //reset all PID variables
attitudeControllerResetAllPID(); studentAttitudeControllerResetAllPID();
// Reset the YAW angle desired // Reset the YAW angle desired
attitudeDesired.yaw = state->attitude.yaw; attitudeDesired.yaw = state->attitude.yaw;
......
crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_attitude_controller.c
+ 44
− 44
View file @ 4c34a994
...@@ -63,7 +63,7 @@ PidObject pidPitch; ...@@ -63,7 +63,7 @@ PidObject pidPitch;
PidObject pidYaw; PidObject pidYaw;
//attitude controller outputs //attitude controller outputs
//No longer needed since attitudeControllerCorrectRatePID modifies values in place //No longer needed since studentAttitudeControllerCorrectRatePID modifies values in place
/* /*
static int16_t rollOutput; static int16_t rollOutput;
static int16_t pitchOutput; static int16_t pitchOutput;
...@@ -72,57 +72,57 @@ static int16_t yawOutput; ...@@ -72,57 +72,57 @@ static int16_t yawOutput;
static bool isInit; static bool isInit;
void attitudeControllerInit(const float updateDt) void studentAttitudeControllerInit(const float updateDt)
{ {
if(isInit) if(isInit)
return; return;
//TODO: get parameters from configuration manager instead //TODO: get parameters from configuration manager instead
//initialize all rate PID loops //initialize all rate PID loops
pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD, studentPidInit(&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); 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, studentPidInit(&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); 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, studentPidInit(&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); updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
//set integral limits for all rate PID loops //set integral limits for all rate PID loops
pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT); studentPidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT); studentPidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT); studentPidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT);
//initialize all attitude PID loops //initialize all attitude PID loops
pidInit(&pidRoll, 0, PID_ROLL_KP, PID_ROLL_KI, PID_ROLL_KD, updateDt, studentPidInit(&pidRoll, 0, PID_ROLL_KP, PID_ROLL_KI, PID_ROLL_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE); ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
pidInit(&pidPitch, 0, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, updateDt, studentPidInit(&pidPitch, 0, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE); ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
pidInit(&pidYaw, 0, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, updateDt, studentPidInit(&pidYaw, 0, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE); ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
//set integral limits for attitude PID loops //set integral limits for attitude PID loops
pidSetIntegralLimit(&pidRoll, PID_ROLL_INTEGRATION_LIMIT); studentPidSetIntegralLimit(&pidRoll, PID_ROLL_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT); studentPidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT); studentPidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT);
isInit = true; isInit = true;
} }
bool attitudeControllerTest() bool studentAttitudeControllerTest()
{ {
return isInit; return isInit;
} }
void attitudeControllerCorrectAttitudePID( void studentAttitudeControllerCorrectAttitudePID(
float eulerRollActual, float eulerPitchActual, float eulerYawActual, float eulerRollActual, float eulerPitchActual, float eulerYawActual,
float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired, float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired) float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired)
{ {
pidSetDesired(&pidRoll, eulerRollDesired); studentPidSetDesired(&pidRoll, eulerRollDesired);
*rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, true); *rollRateDesired = studentPidUpdate(&pidRoll, eulerRollActual, true);
// Update PID for pitch axis // Update PID for pitch axis
pidSetDesired(&pidPitch, eulerPitchDesired); studentPidSetDesired(&pidPitch, eulerPitchDesired);
*pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, true); *pitchRateDesired = studentPidUpdate(&pidPitch, eulerPitchActual, true);
// Update PID for yaw axis, handle error update here instead of in PID calculation // Update PID for yaw axis, handle error update here instead of in PID calculation
float yawError; float yawError;
...@@ -131,57 +131,57 @@ void attitudeControllerCorrectAttitudePID( ...@@ -131,57 +131,57 @@ void attitudeControllerCorrectAttitudePID(
yawError -= 360.0f; yawError -= 360.0f;
else if (yawError < -180.0f) else if (yawError < -180.0f)
yawError += 360.0f; yawError += 360.0f;
pidSetError(&pidYaw, yawError); studentPidSetError(&pidYaw, yawError);
*yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false); *yawRateDesired = studentPidUpdate(&pidYaw, eulerYawActual, false);
} }
void attitudeControllerCorrectRatePID( void studentAttitudeControllerCorrectRatePID(
float rollRateActual, float pitchRateActual, float yawRateActual, float rollRateActual, float pitchRateActual, float yawRateActual,
float rollRateDesired, float pitchRateDesired, float yawRateDesired, float rollRateDesired, float pitchRateDesired, float yawRateDesired,
int16_t* roll, int16_t* pitch, int16_t* yaw int16_t* roll, int16_t* pitch, int16_t* yaw
) )
{ {
pidSetDesired(&pidRollRate, rollRateDesired); studentPidSetDesired(&pidRollRate, rollRateDesired);
*roll = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, true)); *roll = saturateSignedInt16(studentPidUpdate(&pidRollRate, rollRateActual, true));
pidSetDesired(&pidPitchRate, pitchRateDesired); studentPidSetDesired(&pidPitchRate, pitchRateDesired);
*pitch = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, true)); *pitch = saturateSignedInt16(studentPidUpdate(&pidPitchRate, pitchRateActual, true));
pidSetDesired(&pidYawRate, yawRateDesired); studentPidSetDesired(&pidYawRate, yawRateDesired);
*yaw = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true)); *yaw = saturateSignedInt16(studentPidUpdate(&pidYawRate, yawRateActual, true));
//*roll = rollOutput; //*roll = rollOutput;
//*pitch = pitchOutput; //*pitch = pitchOutput;
//*yaw = yawOutput; //*yaw = yawOutput;
} }
void attitudeControllerResetRollAttitudePID(void) void studentAttitudeControllerResetRollAttitudePID(void)
{ {
pidReset(&pidRoll); studentPidReset(&pidRoll);
} }
void attitudeControllerResetYawAttitudePID(void) void studentAttitudeControllerResetYawAttitudePID(void)
{ {
pidReset(&pidYaw); studentPidReset(&pidYaw);
} }
void attitudeControllerResetPitchAttitudePID(void) void studentAttitudeControllerResetPitchAttitudePID(void)
{ {
pidReset(&pidPitch); studentPidReset(&pidPitch);
} }
void attitudeControllerResetAllPID(void) void studentAttitudeControllerResetAllPID(void)
{ {
pidReset(&pidRoll); studentPidReset(&pidRoll);
pidReset(&pidPitch); studentPidReset(&pidPitch);
pidReset(&pidYaw); studentPidReset(&pidYaw);
pidReset(&pidRollRate); studentPidReset(&pidRollRate);
pidReset(&pidPitchRate); studentPidReset(&pidPitchRate);
pidReset(&pidYawRate); studentPidReset(&pidYawRate);
} }
//no longer needed since attitudeControllerCorrectRatePID modifies the values in place //no longer needed since studentAttitudeControllerCorrectRatePID modifies the values in place
/*void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw) /*void studentAttitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw)
{ {
*roll = rollOutput; *roll = rollOutput;
*pitch = pitchOutput; *pitch = pitchOutput;
......
crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_pid.c
+ 12
− 12
View file @ 4c34a994
...@@ -5,7 +5,7 @@ ...@@ -5,7 +5,7 @@
//TODO MICROCART rewrite the PID module //TODO MICROCART rewrite the PID module
void pidInit(PidObject* pid, const float desired, const float kp, void studentPidInit(PidObject* pid, const float desired, const float kp,
const float ki, const float kd, const float dt, const float ki, const float kd, const float dt,
const float samplingRate, const float cutoffFreq, const float samplingRate, const float cutoffFreq,
bool enableDFilter) bool enableDFilter)
...@@ -28,7 +28,7 @@ void pidInit(PidObject* pid, const float desired, const float kp, ...@@ -28,7 +28,7 @@ void pidInit(PidObject* pid, const float desired, const float kp,
} }
} }
float pidUpdate(PidObject* pid, const float measured, const bool updateError) float studentPidUpdate(PidObject* pid, const float measured, const bool updateError)
{ {
float output = 0.0f; float output = 0.0f;
...@@ -76,12 +76,12 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError) ...@@ -76,12 +76,12 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError)
return output; return output;
} }
void pidSetIntegralLimit(PidObject* pid, const float limit) { void studentPidSetIntegralLimit(PidObject* pid, const float limit) {
pid->iLimit = limit; pid->iLimit = limit;
} }
void pidReset(PidObject* pid) void studentPidReset(PidObject* pid)
{ {
pid->error = 0; pid->error = 0;
pid->prevError = 0; pid->prevError = 0;
...@@ -89,22 +89,22 @@ void pidReset(PidObject* pid) ...@@ -89,22 +89,22 @@ void pidReset(PidObject* pid)
pid->deriv = 0; pid->deriv = 0;
} }
void pidSetError(PidObject* pid, const float error) void studentPidSetError(PidObject* pid, const float error)
{ {
pid->error = error; pid->error = error;
} }
void pidSetDesired(PidObject* pid, const float desired) void studentPidSetDesired(PidObject* pid, const float desired)
{ {
pid->desired = desired; pid->desired = desired;
} }
float pidGetDesired(PidObject* pid) float studentPidGetDesired(PidObject* pid)
{ {
return pid->desired; return pid->desired;
} }
bool pidIsActive(PidObject* pid) bool studentPidIsActive(PidObject* pid)
{ {
bool isActive = true; bool isActive = true;
...@@ -116,20 +116,20 @@ bool pidIsActive(PidObject* pid) ...@@ -116,20 +116,20 @@ bool pidIsActive(PidObject* pid)
return isActive; return isActive;
} }
void pidSetKp(PidObject* pid, const float kp) void studentPidSetKp(PidObject* pid, const float kp)
{ {
pid->kp = kp; pid->kp = kp;
} }
void pidSetKi(PidObject* pid, const float ki) void studentPidSetKi(PidObject* pid, const float ki)
{ {
pid->ki = ki; pid->ki = ki;
} }
void pidSetKd(PidObject* pid, const float kd) void studentPidSetKd(PidObject* pid, const float kd)
{ {
pid->kd = kd; pid->kd = kd;
} }
void pidSetDt(PidObject* pid, const float dt) { void studentPidSetDt(PidObject* pid, const float dt) {
pid->dt = dt; pid->dt = dt;
} }
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment