robomaster-sdk/include/message.h

#pragma once

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

#define PACKED __attribute__((__packed__))

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

static const uint8_t PREAMBLE = 0x55;

struct PACKED 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;
        };
    };

};

struct PACKED Footer {
    uint16_t crc;
};

struct PACKED 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;

};

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

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

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

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

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

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

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

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

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

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

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

struct PACKED 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;
};

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

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

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

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

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

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

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

struct PACKED 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;
};

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

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

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

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

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

struct PACKED VisionRect {
    union {
        struct {
            float x;
            float y;
            float w;
            float h;
            uint32_t info;
        } rect;
        struct {
            float x;
            float y;
            float theta;
            float c;
            uint32_t info;
        } line;
        struct {
            float x;
            float y;
            float w;
            float h;
            uint16_t info;
            uint16_t distance;
        } marker;
    };
};

struct PACKED VisionDetectInfo {
    struct Header header;
    uint8_t type;
    uint8_t status;
    uint16_t errcode;
    uint8_t count;
    struct VisionRect rects[];
};

struct PACKED GimbalActionPush {
    struct Header header;
    uint8_t id;
    uint8_t progress;
    uint8_t state;
    int16_t yaw;
    int16_t roll;
    int16_t pitch;
};

struct PACKED GimbalRotateReq {
    struct Header header;
    uint8_t id;

    //union {
        uint8_t control_bitfield;
    //    struct {
    //        // always 0 for start
    //        uint8_t control : 1;
    //        uint8_t frequency : 2;
    //    };
    //};

    //union {
        uint8_t coord_bitfield;
    //    struct {
    //        uint8_t coordinate : 3;
    //        // always true
    //        uint8_t pitch_valid : 1;
    //        // always false
    //        uint8_t roll_valid : 1;
    //        // always true
    //        uint8_t yaw_valid : 1;
    //    };
    //};

    union {
        int16_t yaw;
        struct {
            int8_t l_yaw;
            int8_t h_yaw;
        };
    };
    int16_t roll;
    union {
        int16_t pitch;
        struct {
            int8_t l_pitch;
            int8_t h_pitch;
        };
    };

    uint16_t error;
    uint16_t yaw_speed;
    uint16_t roll_speed;
    uint16_t pitch_speed;

    struct Footer footer;
};

struct PACKED GimbalRotateResp {
    struct Header header;
    uint8_t retcode;
    uint8_t accept;
    struct Footer footer;
};

struct PACKED VisionDetectEnableReq {
    struct Header header;
    uint16_t type;
    struct Footer footer;
};

struct PACKED VisionDetectEnableResp {
    struct Header header;
    uint8_t retcode;
    uint16_t error;
    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 VisionDetectEnableReq enablevision;
    struct GimbalRotateReq gimbalrot;
};
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 VisionDetectEnableResp enablevision;
    struct GimbalRotateResp gimbalrot;
};

union Push {
    struct GimbalActionPush gimbalaction;
    struct VisionDetectInfo vision;
};

union Message {
    struct Header header;
    union Request req;
    union Response resp;
    union Push push;
};

enum ACTIONSTATE {
    ACTION_RUNNING,
    ACTION_SUCCEEDED,
    ACTION_FAILED,
    ACTION_STARTED
};

enum MESSAGEERR {
    MESSAGEERR_NONE,
    MESSAGEERR_HEADERCRC,
    MESSAGEERR_FOOTERCRC
};

enum MESSAGEERR
message_validate(const union Message* message);