#pragma once #include #include #define PACKED __attribute__((__packed__)) 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 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; }; 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; }; 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; }; 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); static inline uint8_t host2byte(uint8_t host, uint8_t index) { return index * 32 + host; } static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) { *host = (b & 0x1F); *index = b >> 5; } static inline size_t message_length(uint16_t cmd) { switch(cmd) { case SET_SDK_CONNECTION_CMD: return sizeof(struct SetSdkConnectionReq); case SDK_HEARTBEAT_CMD: return sizeof(struct SdkHeartbeatReq); case SET_SDK_MODE_CMD: return sizeof(struct SetSdkModeReq); case SET_SYSTEM_LED_CMD: return sizeof(struct SetSystemLedReq); case SET_ROBOT_MODE_CMD: return sizeof(struct SetRobotModeReq); case SUBNODE_RESET_CMD: return sizeof(struct SubNodeResetReq); case SUBSCRIBE_ADD_NODE_CMD: return sizeof(struct SubscribeAddNodeReq); case SET_WHEEL_SPEED_CMD: return sizeof(struct SetWheelSpeedReq); case CHASSIS_SPEED_MODE_CMD: return sizeof(struct ChassisSpeedModeReq); default: return 0; } } static inline uint8_t message_module(uint16_t cmd) { switch(cmd) { case SET_SDK_CONNECTION_CMD: case SDK_HEARTBEAT_CMD: case SET_SDK_MODE_CMD: case SET_SYSTEM_LED_CMD: case SET_ROBOT_MODE_CMD: case SUBNODE_RESET_CMD: case SUBSCRIBE_ADD_NODE_CMD: return host2byte(SDK_HOST, SDK_INDEX); case SET_WHEEL_SPEED_CMD: case CHASSIS_SPEED_MODE_CMD: return host2byte(CHASSIS_HOST, CHASSIS_INDEX); default: return 0; } }