From a9c65d7bf6e392c6a1b26fabcfb40aa461969ddb Mon Sep 17 00:00:00 2001
From: C-Glick <colton.glick@gmail.com>
Date: Thu, 3 Feb 2022 20:56:42 -0600
Subject: [PATCH] Updated attitude rate control in student controller
---
.../modules/interface/attitude_controller.h | 6 +++++
.../src/modules/src/attitude_pid_controller.c | 5 ++++
.../src/modules/src/controller_student.c | 23 ++++++++-----------
3 files changed, 21 insertions(+), 13 deletions(-)
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 f27543b3e..e6ed0cffc 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
@@ -92,6 +92,12 @@ void attitudeControllerResetRollAttitudePID(void);
*/
void attitudeControllerResetPitchAttitudePID(void);
+/**
+ * Reset controller yaw attitude PID
+ */
+void attitudeControllerResetYawAttitudePID(void);
+
+
/**
* Reset controller roll, pitch and yaw PID's.
*/
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 97f82f1df..928ebd8de 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
@@ -156,6 +156,11 @@ void attitudeControllerResetPitchAttitudePID(void)
pidReset(&pidPitch);
}
+void attitudeControllerResetYawAttitudePID(void)
+{
+ pidReset(&pidYaw);
+}
+
void attitudeControllerResetAllPID(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 9f3f8b572..7f04e0263 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
@@ -52,9 +52,9 @@ bool controllerStudentTest(void)
*
* @param angle
* @return float
- */
+ *
static float capAngle(float angle) {
- //TODO MICROCART: Student written
+ //TODO MICROCART: remove
float result = angle;
@@ -68,6 +68,7 @@ static float capAngle(float angle) {
return result;
}
+*/
/**
* This function is called periodically to update the PID loop,
@@ -101,20 +102,11 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
control->yaw = 0;
return;
}
-
- // 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);
- //set desired roll and pitch
+ //set desired roll and pitch and yaw angles
attitudeDesired.roll = setpoint->attitude.roll;
attitudeDesired.pitch = setpoint->attitude.pitch;
+ attitudeDesired.yaw = setpoint->attitude.yaw;
//set desired thrust
thrustDesired = setpoint->thrust;
@@ -138,6 +130,11 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
rateDesired.pitch = setpoint->attitudeRate.pitch;
attitudeControllerResetPitchAttitudePID();
}
+ if(setpoint->mode.yaw == modeVelocity) {
+ rateDesired.yaw = setpoint->attitudeRate.yaw;
+ attitudeControllerResetYawAttitudePID();
+ }
+
//update the attitude rate PID, given the current angular rate
//read by the gyro and the desired rate
--
GitLab