From 8170b3aa90e71f7957da8ae26b21a08726d893cd Mon Sep 17 00:00:00 2001 From: PgSocks Date: Sun, 25 Dec 2022 14:21:37 -0600 Subject: [PATCH] Move messages to their own headers --- CMakeLists.txt | 18 ++-- include/robomaster.h | 15 +++ src/client.c | 16 +++ src/client.h | 28 +++++ src/crc.h | 4 + src/led.h | 78 ++++++++++++++ src/message.h | 97 ++++++++++++++++++ src/robomaster.c | 95 ----------------- src/robomaster.h | 231 ------------------------------------------ src/sdk_connection.h | 47 +++++++++ src/sdk_mode.h | 24 +++++ src/set_wheel_speed.h | 24 +++++ 12 files changed, 343 insertions(+), 334 deletions(-) create mode 100644 include/robomaster.h create mode 100644 src/client.c create mode 100644 src/client.h create mode 100644 src/crc.h create mode 100644 src/led.h create mode 100644 src/message.h delete mode 100644 src/robomaster.c delete mode 100644 src/robomaster.h create mode 100644 src/sdk_connection.h create mode 100644 src/sdk_mode.h create mode 100644 src/set_wheel_speed.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f5d2d4d..7e4221d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,14 +17,16 @@ add_library(robomaster ) target_include_directories(robomaster - PUBLIC + PRIVATE src + PUBLIC + include ) -add_executable(robomastersh - src/robomastersh.c -) - -target_link_libraries(robomastersh - robomaster -) +#add_executable(robomastersh +# src/robomastersh.c +#) +# +#target_link_libraries(robomastersh +# robomaster +#) diff --git a/include/robomaster.h b/include/robomaster.h new file mode 100644 index 0000000..369b234 --- /dev/null +++ b/include/robomaster.h @@ -0,0 +1,15 @@ +#pragma once + +#include +#include +#include +#include + +// Public stuff +struct Message; +struct Client; +typedef struct Client* Client; +typedef struct Message* Message; + +Client client_new(void* buffer, size_t size); + diff --git a/src/client.c b/src/client.c new file mode 100644 index 0000000..5c8f7ab --- /dev/null +++ b/src/client.c @@ -0,0 +1,16 @@ +#include "robomaster.h" +#include "client.h" + +Client session_new(void* buffer, size_t size) { + struct Client* session = malloc(sizeof(struct Client)); + if(!buffer) + session->buffer = malloc(sizeof(struct Header)); + else + session->buffer = buffer; + session->size = 0; + session->seq = 0; + session->max_size = size; + // TODO: Make this configurable + session->hostbyte = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_CLIENT_INDEX); + return session; +} diff --git a/src/client.h b/src/client.h new file mode 100644 index 0000000..47008e2 --- /dev/null +++ b/src/client.h @@ -0,0 +1,28 @@ +#pragma once + +struct Client { + + void* buffer; + size_t max_size; + size_t size; + + uint8_t hostbyte; + + int16_t seq; + +}; + +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; +} + +// Not sure what these are for, but they are used for the hostbyte +static const uint8_t DEFAULT_CLIENT_HOST = 9; +static const uint8_t DEFAULT_CLIENT_INDEX = 6; +static const uint8_t DEFAULT_ROBOT_INDEX = 0; + diff --git a/src/crc.h b/src/crc.h new file mode 100644 index 0000000..a6adbd6 --- /dev/null +++ b/src/crc.h @@ -0,0 +1,4 @@ +#pragma once + +uint8_t crc8(const void *block, size_t length); +uint16_t crc16(const void *block, size_t length); diff --git a/src/led.h b/src/led.h new file mode 100644 index 0000000..c5be64c --- /dev/null +++ b/src/led.h @@ -0,0 +1,78 @@ +#pragma once + +#include "robomaster.h" +#include "message.h" + +static const uint8_t SET_SYSTEM_LED_CMDID = 0x33; + +enum LEDCOMP { + LEDCOMP_BOTTOM_BACK = 0x1, + LEDCOMP_BOTTOM_FRONT = 0x2, + LEDCOMP_BOTTOM_LEFT = 0x4, + LEDCOMP_BOTTOM_RIGHT = 0x8, + LEDCOMP_BOTTOM_ALL = 0xf, + LEDCOMP_TOP_LEFT = 0x10, + LEDCOMP_TOP_RIGHT = 0x20, + LEDCOMP_TOP_ALL = 0x30, + LEDCOMP_ALL = 0x3f +}; + +enum LEDEFFECT { + LEDEFFECT_OFF = 0, + LEDEFFECT_ON = 1, + LEDEFFECT_BREATH = 2, + LEDEFFECT_FLASH = 3, + LEDEFFECT_SCROLLING = 4 +}; + +struct PACKED SetSystemLedReq { + + // 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 PACKED SetSystemLedResp { + uint8_t retcode; +}; + +static +inline +const Message +set_system_led_new ( + Client session, + uint8_t red, + uint8_t green, + uint8_t blue, + enum LEDCOMP comp, + uint16_t led_mask, + enum LEDEFFECT effect, + uint16_t t1, + uint16_t t2 ) { + const struct SetSystemLedReq msg = { + comp, led_mask, + { effect, 7 }, + red, green, blue, + 0, t1, t2 + }; + return message_new(session, 0x3F, SET_SYSTEM_LED_CMDID, sizeof(msg), &msg); +} diff --git a/src/message.h b/src/message.h new file mode 100644 index 0000000..e1a4f08 --- /dev/null +++ b/src/message.h @@ -0,0 +1,97 @@ +#pragma once + +#include "client.h" + +#define PACKED __attribute__((__packed__)) + +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 + union { + uint16_t length; + struct { + uint8_t length_l; + uint8_t length_h; + }; + }; + + // 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 + union { + int16_t seq_id; + struct { + uint8_t seq_id_l; + uint8_t seq_id_h; + }; + }; + + // 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 + uint8_t cmdset; + uint8_t cmdid; + +}; + +struct PACKED Message { + struct Header header; + uint8_t body[]; +}; + +struct PACKED Footer { + uint16_t crc; +}; + +static +inline +struct Message* +message_new(struct Client* session, uint8_t cmdset, uint8_t cmdid, size_t length, const void* body) { + + struct Message* message = session->buffer + session->size; + memcpy((void*)message->body, body, length); + length += sizeof(struct Header) + sizeof(struct Footer); + session->size += length; + int16_t seq = session->seq++; + + message->header.preamble = 0x55; + message->header.length_l = length & 0xFF; + message->header.length_h = (length >> 8) & 0x3 | 4; + message->header.crc = crc8(message, 3); + message->header.seq_id = seq; + message->header.sender = session->hostbyte; + message->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX); + message->header.ack_needed = true; + message->header.cmdset = cmdset; + message->header.cmdid = cmdid; + + struct Footer* footer = (void*)message + length - sizeof(struct Footer); + uint16_t crc = crc16(message, length - sizeof(struct Footer)); + footer->crc = crc; + + return message; + +} + diff --git a/src/robomaster.c b/src/robomaster.c deleted file mode 100644 index 49179a1..0000000 --- a/src/robomaster.c +++ /dev/null @@ -1,95 +0,0 @@ -#include -#include -#include - -#include "robomaster.h" - -struct Client* session_new(void* buffer, size_t size) { - struct Client* session = malloc(sizeof(struct Client*)); - if(!buffer) - session->buffer = malloc(sizeof(struct Header)); - else - session->buffer = buffer; - session->size = 0; - session->seq = 0; - session->max_size = size; - // TODO: Make this configurable - session->hostbyte = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_CLIENT_INDEX); - return session; -} - -// private -struct Message* -message_new(struct Client* session, uint8_t cmdset, uint8_t cmdid, size_t length, const void* body) { - - struct Message* message = session->buffer + session->size; - memcpy((void*)message->body, body, length); - length += sizeof(struct Header) + sizeof(struct Footer); - session->size += length; - int16_t seq = session->seq++; - - message->header.preamble = 0x55; - message->header.length_l = length & 0xFF; - message->header.length_h = (length >> 8) & 0x3 | 4; - message->header.crc = crc8(message, 3); - message->header.seq_id = seq; - message->header.sender = session->hostbyte; - message->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX); - message->header.ack_needed = true; - message->header.cmdset = cmdset; - message->header.cmdid = cmdid; - - struct Footer* footer = (void*)message + length - sizeof(struct Footer); - uint16_t crc = crc16(message, length - sizeof(struct Footer)); - footer->crc = crc; - - return message; - -} - -// public -const Message -set_sdk_connection_req_new( - Client session, - enum CONNECTION connection_type, - uint32_t ip_address, - uint16_t port ) { - - const struct SetSdkConnectionReq msg = { - 0, - session->hostbyte, - connection_type, - 0, - ip_address, - port - }; - return message_new(session, 0x3F, SET_SDK_CONNECTION_CMDID, sizeof(msg), &msg); -} - -const Message -set_sdk_mode_req_new( - Client session, - bool enable ) { - const struct SetSdkModeReq msg = { enable }; - return message_new(session, 0x3F, SET_SDK_MODE_CMDID, sizeof(msg), &msg); -} - -const Message -set_system_led_req_new ( - Client session, - uint8_t red, - uint8_t green, - uint8_t blue, - enum LEDCOMP comp, - uint16_t led_mask, - enum LEDEFFECT effect, - uint16_t t1, - uint16_t t2 ) { - const struct SetSystemLedReq msg = { - comp, led_mask, - { effect, 7 }, - red, green, blue, - 0, t1, t2 - }; - return message_new(session, 0x3F, SET_SYSTEM_LED_CMDID, sizeof(msg), &msg); -} diff --git a/src/robomaster.h b/src/robomaster.h deleted file mode 100644 index 55a553b..0000000 --- a/src/robomaster.h +++ /dev/null @@ -1,231 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -// Move to private header -uint8_t crc8(const void *block, size_t length); -uint16_t crc16(const void *block, size_t length); - -#define PACKED __attribute__((__packed__)) - -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 - union { - uint16_t length; - struct { - uint8_t length_l; - uint8_t length_h; - }; - }; - - // 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 - union { - int16_t seq_id; - struct { - uint8_t seq_id_l; - uint8_t seq_id_h; - }; - }; - - // 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 - uint8_t cmdset; - uint8_t cmdid; - -}; - -struct PACKED Message { - struct Header header; - uint8_t body[]; -}; - -struct PACKED Footer { - uint16_t crc; -}; - -// TODO: Rename to Client -struct Client { - - void* buffer; - size_t max_size; - size_t size; - - uint8_t hostbyte; - - int16_t seq; - -}; - -struct Message* message_new(struct Client* session, uint8_t cmdset, uint8_t cmdid, size_t length, const void* body); - -// Public stuff -typedef struct Client* Client; -typedef struct Message* Message; - -Client client_new(void* buffer, size_t size); - -// Not sure what these are for, but they are used for the hostbyte -static const uint8_t DEFAULT_CLIENT_HOST = 9; -static const uint8_t DEFAULT_CLIENT_INDEX = 6; -static const uint8_t DEFAULT_ROBOT_INDEX = 0; - -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 const uint8_t SET_SDK_CONNECTION_CMDID = 0xD4; - -enum CONNECTION { - CONNECTION_WIFI_AP = 0, - CONNECTION_WIFI_STA = 1, - CONNECTION_USB_RNDIS = 2 -}; - -struct PACKED SetSdkConnectionReq { - uint8_t control; - uint8_t host; - uint8_t connection; - uint8_t protocol; - uint32_t ip_address; - uint16_t port; -}; - -struct PACKED SetSdkConnectionResp { - uint8_t retcode; - uint8_t state; - uint32_t config_ip; -}; - -const Message set_sdk_connection_req_new( - Client session, - enum CONNECTION connection_type, - uint32_t ip_address, - uint16_t port ); - -static const uint8_t SET_SDK_MODE_CMDID = 0xd1; - -struct PACKED SetSdkModeReq { - uint8_t enable; -}; - -struct PACKED SetSdkModeResp { - uint8_t retcode; -}; - -const Message set_sdk_mode_req_new( - Client session, - bool enable ); - -static const uint8_t GET_VERSION_CMDID = 0x01; - -// NOTE: No GetVersion request body - -struct PACKED GetVersionResp { - uint8_t retcode; - uint8_t aa; - uint8_t bb; - uint8_t cc; - uint8_t dd; -}; - -const Message get_version_req_new(Client session); - -static const uint8_t SET_SYSTEM_LED_CMDID = 0x33; - -enum LEDCOMP { - LEDCOMP_BOTTOM_BACK = 0x1, - LEDCOMP_BOTTOM_FRONT = 0x2, - LEDCOMP_BOTTOM_LEFT = 0x4, - LEDCOMP_BOTTOM_RIGHT = 0x8, - LEDCOMP_BOTTOM_ALL = 0xf, - LEDCOMP_TOP_LEFT = 0x10, - LEDCOMP_TOP_RIGHT = 0x20, - LEDCOMP_TOP_ALL = 0x30, - LEDCOMP_ALL = 0x3f -}; - -enum LEDEFFECT { - LEDEFFECT_OFF = 0, - LEDEFFECT_ON = 1, - LEDEFFECT_BREATH = 2, - LEDEFFECT_FLASH = 3, - LEDEFFECT_SCROLLING = 4 -}; - -struct PACKED SetSystemLedReq { - - // 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 PACKED SetSystemLedResp { - uint8_t retcode; -}; - -const Message -set_system_led_req_new ( - Client session, - uint8_t red, - uint8_t green, - uint8_t blue, - enum LEDCOMP comp, - uint16_t led_mask, - enum LEDEFFECT effect, - uint16_t t1, - uint16_t t2 ); diff --git a/src/sdk_connection.h b/src/sdk_connection.h new file mode 100644 index 0000000..04b1401 --- /dev/null +++ b/src/sdk_connection.h @@ -0,0 +1,47 @@ +#pragma once + +#include "robomaster.h" +#include "message.h" + +static const uint8_t SET_SDK_CONNECTION_CMDID = 0xD4; + +enum CONNECTION { + CONNECTION_WIFI_AP = 0, + CONNECTION_WIFI_STA = 1, + CONNECTION_USB_RNDIS = 2 +}; + +struct PACKED SetSdkConnectionReq { + uint8_t control; + uint8_t host; + uint8_t connection; + uint8_t protocol; + uint32_t ip_address; + uint16_t port; +}; + +struct PACKED SetSdkConnectionResp { + uint8_t retcode; + uint8_t state; + uint32_t config_ip; +}; + +static +inline +const Message +set_sdk_connection_new( + Client session, + enum CONNECTION connection_type, + uint32_t ip_address, + uint16_t port ) { + + const struct SetSdkConnectionReq msg = { + 0, + session->hostbyte, + connection_type, + 0, + ip_address, + port + }; + return message_new(session, 0x3F, SET_SDK_CONNECTION_CMDID, sizeof(msg), &msg); +} diff --git a/src/sdk_mode.h b/src/sdk_mode.h new file mode 100644 index 0000000..6ddf932 --- /dev/null +++ b/src/sdk_mode.h @@ -0,0 +1,24 @@ +#pragma once + +#include "robomaster.h" +#include "message.h" + +static const uint8_t SET_SDK_MODE_CMDID = 0xd1; + +struct PACKED SetSdkModeReq { + uint8_t enable; +}; + +struct PACKED SetSdkModeResp { + uint8_t retcode; +}; + +static +inline +const Message +set_sdk_mode_new( + Client session, + bool enable ) { + const struct SetSdkModeReq msg = { enable }; + return message_new(session, 0x3F, SET_SDK_MODE_CMDID, sizeof(msg), &msg); +} diff --git a/src/set_wheel_speed.h b/src/set_wheel_speed.h new file mode 100644 index 0000000..a106a70 --- /dev/null +++ b/src/set_wheel_speed.h @@ -0,0 +1,24 @@ +#pragma once + +#include "robomaster.h" +#include "message.h" + +static const uint8_t SET_WHEEL_SPEED_CMDID = 0x20; + +struct PACKED SetWheelSpeedReq +{ + int16_t wheel_speed[4]; +}; + +static +inline +const Message +set_wheel_speed_new ( + Client session, + int16_t w1, + int16_t w2, + int16_t w3, + int16_t w4 ) { + const struct SetWheelSpeedReq msg = { {w1, w2, w3, w4} }; + return message_new(session, 0x3F, SET_WHEEL_SPEED_CMDID, sizeof(msg), &msg); +}