robomaster-sdk/include/message.h

#pragma once

#include <stdbool.h>
#include <stdint.h>

#if defined(_MSC_VER)  // Check if compiling with Microsoft Visual Studio
#define PACKED(__DECL__) __pragma(pack(push, 1)) __DECL__ __pragma(pack(pop))
#elif defined(__GNUC__)  // Check if compiling with GCC or any compiler defining __GNUC__, including Clang
#define PACKED(__DECL__) __DECL__ __attribute__((packed))
#else
#error "Unsupported compiler"
#endif

static const uint8_t CLIENT_HOST = 9;
static const uint8_t CLIENT_INDEX = 6;

static const uint8_t PREAMBLE = 0x55;

PACKED( struct Header {

    // The preamble marks the start of a message and is always 0x55
    uint8_t preamble;

    // The length of the message includes the preamble and CRC16 at the end
    uint16_t length;

    // This is a CRC8 checksum for the preamble and length together
    uint8_t crc;

    // hostbyte of the message sender
    uint8_t sender;

    // hostbyte of the message receiver
    uint8_t receiver;

    // Each message has a sequence ID
    // The Robomaster will respond with the same ID for each request
    // The value of the sequence ID doesn't matter to the Robomaster
    // Repeating sequence IDs are acceptable
    int16_t seq_id;

    // The message attribute flags designate if a response is needed or if the
    // message is a response.
    union {
        uint8_t attribute;
        struct {
            uint8_t reserved : 6;
            bool ack_needed : 1;
            bool is_ack : 1;
        };
    };

    // Each command has a cmdset and cmdid that together make a cmd key
    union {
        uint16_t cmd;
        struct {
            uint8_t cmdset;
            uint8_t cmdid;
        };
    };

});

PACKED( struct Footer {
    uint16_t crc;
});

PACKED( struct SetSystemLedReq {

    struct Header header;

    // Which LEDs on which component to control
    uint32_t comp_mask;
    uint16_t led_mask;

    struct {
        // off, on, flashing, etc.
        uint8_t effect_mode : 4;
	// Always 7
        uint8_t control_mode : 4;
    };

    // RGB values for the LED color
    uint8_t red;
    uint8_t green;
    uint8_t blue;

    // Always 0
    uint8_t loop;

    // These time intervals have different meaning depending on effect
    uint16_t t1;
    uint16_t t2;

    struct Footer footer;

});

PACKED( struct SetSystemLedResp {
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
});

PACKED( struct SubscribeAddNodeReq
{
    struct Header header;
    uint8_t hostbyte;
    uint32_t sub_vision; // always 0x03000000
    struct Footer footer;
});

PACKED( struct SubscribeAddNodeResp
{
    struct Header header;
    uint8_t retcode; // 0x00 or 0x50 are acceptable here
    uint8_t hostbyte;
    struct Footer footer;
});

PACKED( struct SubNodeResetReq
{
    struct Header header;
    uint8_t hostbyte;
    struct Footer footer;
});

PACKED( struct SubNodeResetResp
{
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
});

PACKED( struct SetRobotModeReq
{
    struct Header header;
    uint8_t mode;
    struct Footer footer;
});

PACKED( struct SetRobotModeResp
{
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
});

PACKED( struct ChassisSpeedModeReq
{
    struct Header header;
    float speed[3];
    struct Footer footer;
});

PACKED( struct ChassisSpeedModeResp
{
    struct Header header;
    int8_t retcode;
    struct Footer footer;
});

PACKED( struct SetChassisWheelSpeedReq
{
    struct Header header;
    int8_t wheel_speed[4];
    struct Footer footer;
});

PACKED( struct SetChassisWheelSpeedResp
{
    struct Header header;
    int8_t retcode;
    struct Footer footer;
});

PACKED( struct GimbalCtrlSpeedReq
{
    struct Header header;
    // Values between -360 and 360
    union {
        int16_t yrp[3];
        struct {
            int16_t yaw;
            int16_t roll;
            int16_t pitch;
        };
    };
    // Always 0xDC
    uint8_t ctrl;
    struct Footer footer;
});

PACKED( struct GimbalCtrlSpeedResp
{
    struct Header header;
    int8_t retcode;
    struct Footer footer;
});

PACKED( struct SetWheelSpeedReq
{
    struct Header header;
    int16_t wheel_speed[4];
    struct Footer footer;
});

PACKED( struct SetWheelSpeedResp
{
    struct Header header;
    int8_t retcode;
    struct Footer footer;
});

PACKED( struct SdkHeartbeatReq {
    struct Header header;
    struct Footer footer;
});

PACKED( struct SdkHeartbeatResp {
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
});

PACKED( struct SetSdkModeReq {
    struct Header header;
    uint8_t enable;
    struct Footer footer;
});

PACKED( struct SetSdkModeResp {
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
});

PACKED( struct SetSdkConnectionReq {
    struct Header header;
    uint8_t control;
    uint8_t host;
    uint8_t connection;
    uint8_t protocol;
    uint32_t ip_address;
    uint16_t port;
    struct Footer footer;
});

PACKED( struct SetSdkConnectionResp {
    struct Header header;
    uint8_t retcode;
    uint8_t state;
    uint32_t config_ip;
    struct Footer footer;
});

PACKED( struct BlasterFireReq {
    struct Header header;
    struct {
        uint8_t type : 4;
        uint8_t times : 4;
    };
    struct Footer footer;
});

PACKED( struct BlasterFireResp {
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
});

PACKED( struct StreamCtrlReq {
    struct Header header;
    uint8_t ctrl;
    struct {
        uint8_t state : 4;
        uint8_t conn_type : 4;
    };
    uint8_t resolution;
    struct Footer footer;
});

PACKED( struct StreamCtrlResp {
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
});

PACKED( struct AddSubMsgReq {
    struct Header header;
    uint8_t node_id;
    uint8_t msg_id;
    uint8_t reserved;
    uint8_t sub_mode;
    uint8_t subject_count;
    uint64_t subject_uuid;
    uint16_t freq;
    struct Footer footer;
});

PACKED( struct AddSubMsgResp {
    struct Header header;
    uint8_t retcode;
    uint8_t pub_node_id;
    uint8_t ack_sub_mode;
    uint8_t ack_msg_id;
    uint16_t ack_err_uid_data;
    struct Footer footer;
});

PACKED( struct PushPeriodMsg {
    struct Header header;
    uint8_t sub_mode;
    uint8_t msg_id;
    union {
        struct {
            float x;
            float y;
            float z;
        } position;
        struct {
            uint16_t yaw_ground;
            uint16_t pitch_ground;
            uint16_t yaw;
            uint16_t pitch;
            struct {
                uint8_t option_mode : 2;
                uint8_t return_center : 1;
            };
        } gimbal;
    };
    struct Footer footer;
});

union Request {
    struct Header header;
    struct SetSdkConnectionReq sdkconn;
    struct SetSdkModeReq sdkmode;
    struct SdkHeartbeatReq heartbeat;
    struct SetSystemLedReq led;
    struct SetWheelSpeedReq wheel;
    struct SetChassisWheelSpeedReq chswheel;
    struct ChassisSpeedModeReq chsspeed;
    struct SetRobotModeReq mvmode;
    struct SubNodeResetReq subnodereset;
    struct SubscribeAddNodeReq subnodeadd;
    struct GimbalCtrlSpeedReq gimbspeed;
    struct BlasterFireReq blaster;
    struct StreamCtrlReq stream;
    struct AddSubMsgReq sub;
};
union Response {
    struct Header header;
    struct SetSdkConnectionResp sdkconn;
    struct SetSdkModeResp sdkmode;
    struct SdkHeartbeatResp heartbeat;
    struct SetSystemLedResp led;
    struct SetWheelSpeedResp wheel;
    struct SetChassisWheelSpeedResp chswheel;
    struct ChassisSpeedModeResp chsspeed;
    struct SetRobotModeResp mvmode;
    struct SubNodeResetResp subnodereset;
    struct SubscribeAddNodeResp subnodeadd;
    struct GimbalCtrlSpeedResp gimbspeed;
    struct BlasterFireResp blaster;
    struct StreamCtrlResp stream;
    struct AddSubMsgResp sub;
    struct PushPeriodMsg push;
};
union Message {
    struct Header header;
    union Request req;
    union Response resp;
};

enum MESSAGEERR {
    MESSAGEERR_NONE,
    MESSAGEERR_HEADERCRC,
    MESSAGEERR_FOOTERCRC
};

enum MESSAGEERR
message_validate(const union Message* message);