From 853900f2b341bb7c774c4c97cc9b794567f35c7e Mon Sep 17 00:00:00 2001
From: crglick <crglick@iastate.edu>
Date: Mon, 21 Mar 2022 20:28:04 -0500
Subject: [PATCH] Added custom mixed attitude setpoint packet
roll and pitch are absolute angles and yaw is an angle rate.
---
.../src/modules/src/crtp_commander_generic.c | 35 +++++++++++++++++++
1 file changed, 35 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 a4b1f55c4..f690d24e2 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
@@ -73,6 +73,7 @@ enum packet_type {
positionType = 7,
attitudeRateType = 8,
attitudeType = 9,
+ mixedAttitudeType = 10,
};
/* ---===== 2 - Decoding functions =====--- */
@@ -431,6 +432,39 @@ static void attitudeDecoder(setpoint_t *setpoint, uint8_t type, const void *data
setpoint->thrust = values->thrust;
}
+
+/*
+ * Custom mixed Attitude decoder, bypasses the normal attitude control path,
+ * packet contains the roll and pitch as attitude angles and the
+ * yaw as an attitude rate in addition to the thrust value
+ */
+struct mixedAttitudePacket_s{
+ float rollAngle; // deg
+ float pitchAngle; // deg
+ float yawAngleRate; // deg/s
+ float thrust; // thrust percentage 0 - 60,000
+} __attribute__((packed));
+static void mixedAttitudeDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen){
+
+ const struct mixedAttitudePacket_s *values = data;
+
+ ASSERT(datalen == sizeof(struct mixedAttitudePacket_s));
+
+ setpoint->mode.x = modeDisable;
+ setpoint->mode.y = modeDisable;
+ setpoint->mode.z = modeDisable;
+
+ setpoint->mode.roll = modeAbs;
+ setpoint->mode.pitch = modeAbs;
+ setpoint->mode.yaw = modeVelocity;
+
+ setpoint->attitude.roll = values->rollAngle;
+ setpoint->attitude.pitch = values->pitchAngle;
+ setpoint->attitudeRate.yaw = values->yawAngleRate;
+
+ setpoint->thrust = values->thrust;
+}
+
/* ---===== 3 - packetDecoders array =====--- */
const static packetDecoder_t packetDecoders[] = {
[stopType] = stopDecoder,
@@ -443,6 +477,7 @@ const static packetDecoder_t packetDecoders[] = {
[positionType] = positionDecoder,
[attitudeRateType] = attitudeRateDecoder,
[attitudeType] = attitudeDecoder,
+ [mixedAttitudeType] = mixedAttitudeDecoder,
};
/* Decoder switch */
--
GitLab