From eefa1ae8609347e366c499201226f03b22a6d3b2 Mon Sep 17 00:00:00 2001
From: C-Glick <colton.glick@gmail.com>
Date: Tue, 15 Feb 2022 20:46:38 -0600
Subject: [PATCH] Merge attitude_rate_control into master, close !73
---
.../interface/student_attitude_controller.h | 5 +++
.../src/modules/src/controller_student.c | 19 ++++-------
.../src/modules/src/crtp_commander_generic.c | 34 +++++++++++++++++++
.../modules/src/student_attitude_controller.c | 5 +++
4 files changed, 51 insertions(+), 12 deletions(-)
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
index 99ae38a69..814c4c97e 100644
--- 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
@@ -93,6 +93,11 @@ 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/controller_student.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
index cd8ed7b6a..e9f1adb2f 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
@@ -54,7 +54,7 @@ bool controllerStudentTest(void)
* @return float
*/
static float capAngle(float angle) {
- //TODO MICROCART: Student written
+ //TODO MICROCART: remove
float result = angle;
@@ -102,19 +102,10 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
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 +129,10 @@ 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
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/crtp_commander_generic.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/crtp_commander_generic.c
index fd8745653..49aa8f3c6 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/crtp_commander_generic.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/crtp_commander_generic.c
@@ -71,6 +71,8 @@ enum packet_type {
hoverType = 5,
fullStateType = 6,
positionType = 7,
+ attitudeRateType = 8,
+
};
/* ---===== 2 - Decoding functions =====--- */
@@ -367,6 +369,37 @@ static void positionDecoder(setpoint_t *setpoint, uint8_t type, const void *data
setpoint->attitude.yaw = values->yaw;
}
+/*
+ * Attitude rate decoder,
+ * packet contains the desired attitude rate in addition to the thrust value
+ */
+struct attitudeRatePacket_s{
+ float rollRate; // deg/s
+ float pitchRate; // deg/s
+ float yawRate; // deg/s
+ float thrust; // thrust percentage 0 - 60,000
+} __attribute__((packed));
+static void attitudeRateDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen){
+
+ const struct attitudeRatePacket_s *values = data;
+
+ ASSERT(datalen == sizeof(struct attitudeRatePacket_s));
+
+ setpoint->mode.x = modeDisable;
+ setpoint->mode.y = modeDisable;
+ setpoint->mode.z = modeDisable;
+
+ setpoint->mode.roll = modeVelocity;
+ setpoint->mode.pitch = modeVelocity;
+ setpoint->mode.yaw = modeVelocity;
+
+ setpoint->attitudeRate.roll = values->rollRate;
+ setpoint->attitudeRate.pitch = values->pitchRate;
+ setpoint->attitudeRate.yaw = values->yawRate;
+
+ setpoint->thrust = values->thrust;
+}
+
/* ---===== 3 - packetDecoders array =====--- */
const static packetDecoder_t packetDecoders[] = {
[stopType] = stopDecoder,
@@ -377,6 +410,7 @@ const static packetDecoder_t packetDecoders[] = {
[hoverType] = hoverDecoder,
[fullStateType] = fullStateDecoder,
[positionType] = positionDecoder,
+ [attitudeRateType] = attitudeRateDecoder,
};
/* Decoder switch */
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
index 221e3c93a..9e2b45e4b 100644
--- 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
@@ -157,6 +157,11 @@ void attitudeControllerResetRollAttitudePID(void)
pidReset(&pidRoll);
}
+void attitudeControllerResetYawAttitudePID(void)
+{
+ pidReset(&pidYaw);
+}
+
void attitudeControllerResetPitchAttitudePID(void)
{
pidReset(&pidPitch);
--
GitLab