From cc1aeca427881a1b15872f24c644f751fda6342d Mon Sep 17 00:00:00 2001
From: C-Glick <colton.glick@gmail.com>
Date: Tue, 1 Feb 2022 19:58:01 -0600
Subject: [PATCH] Added attitude rate setpoint format and decoder
---
.../src/modules/src/crtp_commander_generic.c | 33 +++++++++++++++++++
1 file changed, 33 insertions(+)
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..9d038cc5c 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,7 @@ enum packet_type {
hoverType = 5,
fullStateType = 6,
positionType = 7,
+ attitudeRateType = 8,
};
/* ---===== 2 - Decoding functions =====--- */
@@ -367,6 +368,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 +409,7 @@ const static packetDecoder_t packetDecoders[] = {
[hoverType] = hoverDecoder,
[fullStateType] = fullStateDecoder,
[positionType] = positionDecoder,
+ [attitudeRateType] = attitudeRateDecoder,
};
/* Decoder switch */
--
GitLab