Skip to content

Commit

Permalink
Merge pull request #1456 from bitcraze/rik/yawconvention
Browse files Browse the repository at this point in the history 
Revert " #338: Fix yawrate sign in commander_generic packets"
  • Loading branch information
@ToveRumar
ToveRumar authored Feb 19, 2025
2 parents a67821c + a29c74b commit 16c5630
Show file tree
Hide file tree
Showing 2 changed files with 113 additions and 29 deletions.
2 changes: 1 addition & 1 deletion src/modules/interface/crtp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
#include <stdint.h>
#include <stdbool.h>

#define CRTP_PROTOCOL_VERSION 8
#define CRTP_PROTOCOL_VERSION 9

#define CRTP_MAX_DATA_SIZE 30

Expand Down
140 changes: 112 additions & 28 deletions src/modules/src/crtp_commander_generic.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,14 +63,17 @@ typedef void (*packetDecoder_t)(setpoint_t *setpoint, uint8_t type, const void *

/* ---===== 1 - packetType_e enum =====--- */
enum packet_type {
stopType = 0,
velocityWorldType = 1,
zDistanceType = 2,
cppmEmuType = 3,
altHoldType = 4,
hoverType = 5,
fullStateType = 6,
positionType = 7,
stopType = 0,
legacyVelocityWorldType = 1,
legacyZDistanceType = 2,
cppmEmuType = 3,
altHoldType = 4,
legacyHoverType = 5,
fullStateType = 6,
positionType = 7,
velocityWorldType = 8,
zDistanceType = 9,
hoverType = 10,
};

/* ---===== 2 - Decoding functions =====--- */
Expand All @@ -86,15 +89,16 @@ static void stopDecoder(setpoint_t *setpoint, uint8_t type, const void *data, si
return;
}

/* velocityDecoder
* Set the Crazyflie velocity in the world coordinate system
*/
struct velocityPacket_s {
float vx; // m in the world frame of reference
float vy; // ...
float vz; // ...
float yawrate; // deg/s
} __attribute__((packed));

/* velocityDecoder
* Set the Crazyflie velocity in the world coordinate system
*/
static void velocityDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
{
const struct velocityPacket_s *values = data;
Expand All @@ -111,18 +115,41 @@ static void velocityDecoder(setpoint_t *setpoint, uint8_t type, const void *data

setpoint->mode.yaw = modeVelocity;

setpoint->attitudeRate.yaw = -values->yawrate;
setpoint->attitudeRate.yaw = values->yawrate;
}

/* zDistanceDecoder
* Set the Crazyflie absolute height and roll/pitch angles
/* legacyVelocityDecoder
* Set the Crazyflie velocity in the world coordinate system
*/
static void legacyVelocityDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
{
const struct velocityPacket_s *values = data;

ASSERT(datalen == sizeof(struct velocityPacket_s));

setpoint->mode.x = modeVelocity;
setpoint->mode.y = modeVelocity;
setpoint->mode.z = modeVelocity;

setpoint->velocity.x = values->vx;
setpoint->velocity.y = values->vy;
setpoint->velocity.z = values->vz;

setpoint->mode.yaw = modeVelocity;

setpoint->attitudeRate.yaw = -values->yawrate;
}

struct zDistancePacket_s {
float roll; // deg
float pitch; // ...
float yawrate; // deg/s
float zDistance; // m in the world frame of reference
} __attribute__((packed));

/* zDistanceDecoder
* Set the Crazyflie absolute height and roll/pitch angles
*/
static void zDistanceDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
{
const struct zDistancePacket_s *values = data;
Expand All @@ -135,6 +162,33 @@ static void zDistanceDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
setpoint->position.z = values->zDistance;


setpoint->mode.yaw = modeVelocity;

setpoint->attitudeRate.yaw = values->yawrate;


setpoint->mode.roll = modeAbs;
setpoint->mode.pitch = modeAbs;

setpoint->attitude.roll = values->roll;
setpoint->attitude.pitch = values->pitch;
}

/* legacyZDistanceDecoder
* Set the Crazyflie absolute height and roll/pitch angles
*/
static void legacyZDistanceDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
{
const struct zDistancePacket_s *values = data;


ASSERT(datalen == sizeof(struct zDistancePacket_s));

setpoint->mode.z = modeAbs;

setpoint->position.z = values->zDistance;


setpoint->mode.yaw = modeVelocity;

setpoint->attitudeRate.yaw = -values->yawrate;
Expand Down Expand Up @@ -286,7 +340,7 @@ static void altHoldDecoder(setpoint_t *setpoint, uint8_t type, const void *data,

setpoint->mode.yaw = modeVelocity;

setpoint->attitudeRate.yaw = -values->yawrate;
setpoint->attitudeRate.yaw = values->yawrate;


setpoint->mode.roll = modeAbs;
Expand All @@ -296,15 +350,17 @@ static void altHoldDecoder(setpoint_t *setpoint, uint8_t type, const void *data,
setpoint->attitude.pitch = values->pitch;
}

/* hoverDecoder
* Set the Crazyflie absolute height and velocity in the body coordinate system
*/
struct hoverPacket_s {
float vx; // m/s in the body frame of reference
float vy; // ...
float yawrate; // deg/s
float zDistance; // m in the world frame of reference
} __attribute__((packed));

/* hoverDecoder
* Set the Crazyflie absolute height and velocity in the body coordinate system
*/

static void hoverDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
{
const struct hoverPacket_s *values = data;
Expand All @@ -315,6 +371,31 @@ static void hoverDecoder(setpoint_t *setpoint, uint8_t type, const void *data, s
setpoint->position.z = values->zDistance;


setpoint->mode.yaw = modeVelocity;
setpoint->attitudeRate.yaw = values->yawrate;


setpoint->mode.x = modeVelocity;
setpoint->mode.y = modeVelocity;
setpoint->velocity.x = values->vx;
setpoint->velocity.y = values->vy;

setpoint->velocity_body = true;
}

/* legacyHoverDecoder
* Set the Crazyflie absolute height and velocity in the body coordinate system
*/
static void legacyHoverDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
{
const struct hoverPacket_s *values = data;

ASSERT(datalen == sizeof(struct hoverPacket_s));

setpoint->mode.z = modeAbs;
setpoint->position.z = values->zDistance;


setpoint->mode.yaw = modeVelocity;
setpoint->attitudeRate.yaw = -values->yawrate;

Expand Down Expand Up @@ -354,7 +435,7 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
setpoint->velocity.x = (values->v ## x) / 1000.0f; \
setpoint->acceleration.x = (values->a ## x) / 1000.0f; \
setpoint->jerk.x = 0.0f; \

UNPACK(x)
UNPACK(y)
UNPACK(z)
Expand Down Expand Up @@ -401,14 +482,17 @@ static void positionDecoder(setpoint_t *setpoint, uint8_t type, const void *data

/* ---===== 3 - packetDecoders array =====--- */
const static packetDecoder_t packetDecoders[] = {
[stopType] = stopDecoder,
[velocityWorldType] = velocityDecoder,
[zDistanceType] = zDistanceDecoder,
[cppmEmuType] = cppmEmuDecoder,
[altHoldType] = altHoldDecoder,
[hoverType] = hoverDecoder,
[fullStateType] = fullStateDecoder,
[positionType] = positionDecoder,
[stopType] = stopDecoder,
[legacyVelocityWorldType] = legacyVelocityDecoder,
[legacyZDistanceType] = legacyZDistanceDecoder,
[cppmEmuType] = cppmEmuDecoder,
[altHoldType] = altHoldDecoder,
[legacyHoverType] = legacyHoverDecoder,
[fullStateType] = fullStateDecoder,
[positionType] = positionDecoder,
[velocityWorldType] = velocityDecoder,
[zDistanceType] = zDistanceDecoder,
[hoverType] = hoverDecoder,
};

/* Decoder switch */
Expand Down Expand Up @@ -436,7 +520,7 @@ void crtpCommanderGenericDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk)
* configure the maximum angle/rate output given a maximum stick input
* for CRTP packets with emulated CPPM channels (e.g. RC transmitters connecting
* directly to the NRF radio, often with a 4-in-1 Multimodule), or for CPPM channels
* from an external receiver.
* from an external receiver.
*/
PARAM_GROUP_START(cppm)

Expand Down

0 comments on commit 16c5630

Please sign in to comment.