From e60aa4c2e3e620a57008e6b46581d5a97c187673 Mon Sep 17 00:00:00 2001 From: PgSocks Date: Tue, 27 Dec 2022 17:23:21 -0600 Subject: [PATCH] Refactor for public API --- CMakeLists.txt | 21 +++-- include/led.h | 39 ++++++++ include/message.h | 167 +++++++++++++++++++++++++++++++++ include/robomaster.h | 23 +++-- include/sdk_connection.h | 21 +++++ include/sdk_mode.h | 11 +++ include/wheel.h | 16 ++++ src/client.c | 16 ---- src/client.h | 16 ++-- src/connection.c | 78 --------------- src/connection.h | 68 +++++++++++++- src/crc.c | 78 --------------- src/crc.h | 86 ++++++++++++++++- src/led.h | 78 --------------- src/message.c | 16 ++++ src/message.h | 117 ----------------------- src/messages/led.c | 33 +++++++ src/messages/sdk_connection.c | 21 +++++ src/messages/sdk_mode.c | 13 +++ src/messages/set_wheel_speed.c | 19 ++++ src/robomaster.c | 65 +++++++++++++ src/robomastersh.c | 45 +-------- src/sdk_connection.h | 47 ---------- src/sdk_mode.h | 24 ----- src/set_wheel_speed.h | 24 ----- 25 files changed, 607 insertions(+), 535 deletions(-) create mode 100644 include/led.h create mode 100644 include/message.h create mode 100644 include/sdk_connection.h create mode 100644 include/sdk_mode.h create mode 100644 include/wheel.h delete mode 100644 src/client.c delete mode 100644 src/connection.c delete mode 100644 src/crc.c delete mode 100644 src/led.h create mode 100644 src/message.c delete mode 100644 src/message.h create mode 100644 src/messages/led.c create mode 100644 src/messages/sdk_connection.c create mode 100644 src/messages/sdk_mode.c create mode 100644 src/messages/set_wheel_speed.c create mode 100644 src/robomaster.c delete mode 100644 src/sdk_connection.h delete mode 100644 src/sdk_mode.h delete mode 100644 src/set_wheel_speed.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e4221d..463aec2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,8 +12,13 @@ set_property(GLOBAL PROPERTY CXX_STANDARD 17) add_compile_options(-Wall) add_library(robomaster + src/message.c + src/messages/sdk_connection.c + src/messages/led.c + src/messages/sdk_mode.c + src/messages/set_wheel_speed.c src/robomaster.c - src/crc.c + src/robomastersh.c ) target_include_directories(robomaster @@ -23,10 +28,10 @@ target_include_directories(robomaster 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/led.h b/include/led.h new file mode 100644 index 0000000..2c13e5f --- /dev/null +++ b/include/led.h @@ -0,0 +1,39 @@ +#pragma once + +#include "message.h" + +#include +#include + +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 +}; + +void set_system_led ( + 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/include/message.h b/include/message.h new file mode 100644 index 0000000..4d94dbb --- /dev/null +++ b/include/message.h @@ -0,0 +1,167 @@ +#pragma once + +#include +#include + +#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 + 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 + 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 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 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 SetSystemLedReq led; + struct SetWheelSpeedReq wheel; +}; +union Response { + struct Header header; + struct SetSdkConnectionResp sdkconn; + struct SetSdkModeReq sdkmode; + struct SetSystemLedResp led; + struct SetWheelSpeedResp wheel; +}; +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); diff --git a/include/robomaster.h b/include/robomaster.h index 96729e0..503ecea 100644 --- a/include/robomaster.h +++ b/include/robomaster.h @@ -1,17 +1,22 @@ #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); +#include "message.h" +#include "led.h" +#include "sdk_connection.h" +#include "sdk_mode.h" +#include "wheel.h" + +// SIMPLE +// Create a client +// Connect to a drone +// Send messages +// Poll for responses + +Client client_new(); void client_connect(Client client); -Message poll_message(Client client); +void poll_message(Client client, union Message* message); diff --git a/include/sdk_connection.h b/include/sdk_connection.h new file mode 100644 index 0000000..e755332 --- /dev/null +++ b/include/sdk_connection.h @@ -0,0 +1,21 @@ +#pragma once + +#include "message.h" + +#include +#include + +static const uint8_t SET_SDK_CONNECTION_CMDID = 0xD4; + +enum CONNECTION { + CONNECTION_WIFI_AP = 0, + CONNECTION_WIFI_STA = 1, + CONNECTION_USB_RNDIS = 2 +}; + +void +set_sdk_connection( + Client session, + enum CONNECTION connection_type, + uint32_t ip_address, + uint16_t port ); diff --git a/include/sdk_mode.h b/include/sdk_mode.h new file mode 100644 index 0000000..58fd44c --- /dev/null +++ b/include/sdk_mode.h @@ -0,0 +1,11 @@ +#pragma once + +#include "message.h" + +#include +#include + +static const uint8_t SET_SDK_MODE_CMDID = 0xd1; + +void +set_sdk_mode(Client session, bool enable); diff --git a/include/wheel.h b/include/wheel.h new file mode 100644 index 0000000..ff4caca --- /dev/null +++ b/include/wheel.h @@ -0,0 +1,16 @@ +#pragma once + +#include "message.h" + +#include +#include + +static const uint8_t SET_WHEEL_SPEED_CMDID = 0x20; + +void +set_wheel_speed ( + Client session, + int16_t w1, + int16_t w2, + int16_t w3, + int16_t w4 ); diff --git a/src/client.c b/src/client.c deleted file mode 100644 index 5c8f7ab..0000000 --- a/src/client.c +++ /dev/null @@ -1,16 +0,0 @@ -#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 index 5507151..6aba128 100644 --- a/src/client.h +++ b/src/client.h @@ -1,13 +1,9 @@ #pragma once -#include "connection.h" +#include struct Client { - void* buffer; - size_t max_size; - size_t size; - uint8_t hostbyte; int16_t seq; @@ -16,6 +12,11 @@ struct Client { }; +// Not sure what these are, 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; } @@ -25,8 +26,3 @@ static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) { *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/connection.c b/src/connection.c deleted file mode 100644 index ca75962..0000000 --- a/src/connection.c +++ /dev/null @@ -1,78 +0,0 @@ - -#include "robomaster.h" -#include "client.h" -#include "connection.h" -#include "message.h" - -static struct Connection* connection_new() -{ - struct Connection* conn = calloc(sizeof(struct Connection)); - - // Request a UDP socket - conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0); - - // Make the socket non-blocking - int flags = fcntl(conn->sockfd, F_GETFL); - fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK); - - // Set the address of the drone - conn->addrlen = sizeof(conn->remote_addr); - conn->dest_addr.sin_family = AF_INET; - conn->dest_addr.sin_port = htons(30030); - conn->dest_addr.sin_addr.s_addr = inet_addr("192.168.2.1"); - - return conn; -} - -void client_connect(Client client) { - client->connection = connection_new(); - - sendto(client->connection->sockfd, client->buffer, session_size(session), 0, (struct sockaddr*)&client->connection->dest_addr, client->connection->addrlen); -} - -// TODO: Use union to make all messages same size -void send_message(Client client, Message message, int length) { - sendto(client->connection->sockfd, message, length, 0, (struct sockaddr*)&client->connection->dest_addr, client->connection->addrlen); -} - -Message poll_message(Client client) { - - // Poll for messages - static const struct timeval timeout = {-1, 0}; - fd_set read_fds; - FD_ZERO(&read_fds); - FD_SET(client->connection->sockfd, &read_fds); - int result = select(client->connection->sockfd + 1, &read_fds, NULL, NULL, &timeout); - - // Check for socket polling errors - if(result < 0) { - perror("message polling failed"); - exit(EXIT_FAILURE); - } - - // Skip if nothing was received yet - // TODO: Make a static "empty" message or something - if (result == 0) { - return NULL; - } - - // Read a message from the socket - // TODO: Use union to make all messages same size - // NOTE: This is never freed, so it's a memory leak - void* buffer = malloc(1024); - int recvb = recvfrom(client->connection->sockfd, buffer, sizeof(client->max_size), 0, (struct sockaddr*)&client->connection->dest_addr, &client->connection->addrlen); - - // Check for socket read errors - if(recvb < 0) { - perror("reading socket failed"); - exit(EXIT_FAILURE); - } - - // Check for message errors - if(message_validate(buffer)) { - perror("invalid message"); - exit(EXIT_FAILURE); - } - - return (Message)buffer; -} diff --git a/src/connection.h b/src/connection.h index de824ba..795f627 100644 --- a/src/connection.h +++ b/src/connection.h @@ -1,8 +1,74 @@ #pragma once +#include "message.h" +#include "client.h" +#include "crc.h" + +#include +#include +#include +#include +#include +#include +#include + struct Connection { int sockfd; socklen_t addrlen; - sockaddr_in remote_addr; + struct sockaddr_in remote_addr; }; +inline +static +struct Connection* +connection_new() +{ + struct Connection* conn = malloc(sizeof(struct Connection)); + memset(conn, 0, sizeof(struct Connection)); + + // Request a UDP socket + conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0); + + // Make the socket non-blocking + int flags = fcntl(conn->sockfd, F_GETFL); + fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK); + + // Set the address of the drone + conn->addrlen = sizeof(conn->remote_addr); + conn->remote_addr.sin_family = AF_INET; + conn->remote_addr.sin_port = htons(30030); + conn->remote_addr.sin_addr.s_addr = inet_addr("192.168.2.1"); + + return conn; +} + +static +inline +void +req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, size_t length, union Request* req) { + + req->header.preamble = 0x55; + req->header.length_l = length & 0xFF; + req->header.length_h = (length >> 8) & 0x3 | 4; + req->header.crc = crc8(req, 3); + req->header.seq_id = client->seq++; + req->header.sender = client->hostbyte; + // TODO: Figure out what this is supposed to be + req->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX); + req->header.ack_needed = true; + req->header.cmdset = cmdset; + req->header.cmdid = cmdid; + + struct Footer* footer = (void*)req + length - sizeof(struct Footer); + uint16_t crc = crc16(req, length - sizeof(struct Footer)); + footer->crc = crc; + +} + +inline +static +void +req_send(struct Connection* conn, union Request* req, size_t length) { + sendto(conn->sockfd, req, length, 0, (struct sockaddr*)&conn->remote_addr, conn->addrlen); +} + diff --git a/src/crc.c b/src/crc.c deleted file mode 100644 index b6fc2b6..0000000 --- a/src/crc.c +++ /dev/null @@ -1,78 +0,0 @@ -#include -#include - -static const uint8_t CRC8_TABLE[] = -{ - 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, - 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, - 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1, - 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, - 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, - 0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, - 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39, - 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, - 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, - 0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, - 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e, - 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, - 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, - 0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, - 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0, - 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, - 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, - 0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, - 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b, - 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, - 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, - 0xd7, 0x89, 0x6b, 0x35 -}; - -static const uint16_t CRC16_TABLE[] = -{ - 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, - 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, - 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb, - 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, - 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, - 0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, - 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, - 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, - 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, - 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, - 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014, - 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, - 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, - 0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, - 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, - 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, - 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, - 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, - 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50, - 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, - 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, - 0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, - 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, - 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, - 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, - 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, - 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d, - 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, - 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 -}; - -uint8_t crc8(const void* block, size_t length) -{ - uint8_t crc = 0x77; - const uint8_t* bytes = (uint8_t*)block; - while(length--) crc = CRC8_TABLE[crc^*bytes++]; - return crc; -} - -uint16_t crc16(const void* block, size_t length) -{ - uint16_t crc = 0x3692; - const uint8_t* bytes = (uint8_t*)block; - while(length--) - crc = ((crc>>8)&0xFF)^CRC16_TABLE[((crc^*bytes++)&0xFF)]; - return crc; -} diff --git a/src/crc.h b/src/crc.h index a6adbd6..ebe62de 100644 --- a/src/crc.h +++ b/src/crc.h @@ -1,4 +1,86 @@ #pragma once -uint8_t crc8(const void *block, size_t length); -uint16_t crc16(const void *block, size_t length); +#include +#include + +static const uint8_t CRC8_TABLE[] = +{ + 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, + 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, + 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1, + 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, + 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, + 0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, + 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39, + 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, + 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, + 0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, + 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e, + 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, + 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, + 0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, + 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0, + 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, + 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, + 0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, + 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b, + 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, + 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, + 0xd7, 0x89, 0x6b, 0x35 +}; + +static const uint16_t CRC16_TABLE[] = +{ + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, + 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, + 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb, + 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, + 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, + 0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, + 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, + 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, + 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, + 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014, + 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, + 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, + 0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, + 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, + 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, + 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, + 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50, + 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, + 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, + 0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, + 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, + 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, + 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, + 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d, + 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, + 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; + +static +inline +uint8_t +crc8(const void* block, size_t length) +{ + uint8_t crc = 0x77; + const uint8_t* bytes = (uint8_t*)block; + while(length--) crc = CRC8_TABLE[crc^*bytes++]; + return crc; +} + +static +inline +uint16_t +crc16(const void* block, size_t length) +{ + uint16_t crc = 0x3692; + const uint8_t* bytes = (uint8_t*)block; + while(length--) + crc = ((crc>>8)&0xFF)^CRC16_TABLE[((crc^*bytes++)&0xFF)]; + return crc; +} diff --git a/src/led.h b/src/led.h deleted file mode 100644 index c5be64c..0000000 --- a/src/led.h +++ /dev/null @@ -1,78 +0,0 @@ -#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.c b/src/message.c new file mode 100644 index 0000000..0c9cb64 --- /dev/null +++ b/src/message.c @@ -0,0 +1,16 @@ +#include "message.h" +#include "crc.h" + +enum MESSAGEERR +message_validate(const union Message* message) { + + uint16_t length = (message->header.length_h & 0x3) * 0xFF + message->header.length_l; + + if(message->header.crc != crc8(message, 3)) + return MESSAGEERR_HEADERCRC; + + if(message->header.crc != crc16(message, length - sizeof(struct Footer))) + return MESSAGEERR_FOOTERCRC; + +} + diff --git a/src/message.h b/src/message.h deleted file mode 100644 index 612fb8f..0000000 --- a/src/message.h +++ /dev/null @@ -1,117 +0,0 @@ -#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; - -} - -enum MESSAGEERR { - MESSAGEERR_NONE, - MESSAGEERR_HEADERCRC, - MESSAGEERR_FOOTERCRC -} - -static -inline -enum MESSAGEERR -message_validate(const struct Message* message) { - - uint16_t length = (message->header->length_h[2] & 0x3) * 0xFF + message->header->length_l[1]; - - if(message->header.crc != crc8(message, 3)) - return MESSAGEERR_HEADERCRC; - - if(message->header.crc != crc16(message, length - sizeof(struct Footer))) - return MESSAGEERR_FOOTERCRC; - -} diff --git a/src/messages/led.c b/src/messages/led.c new file mode 100644 index 0000000..3d6cbdb --- /dev/null +++ b/src/messages/led.c @@ -0,0 +1,33 @@ +#include "message.h" +#include "connection.h" +#include "robomaster.h" + +void +set_system_led ( + 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 ) { + + union Request req = {0}; + + req.led.comp_mask = comp; + req.led.led_mask = led_mask; + req.led.effect_mode = effect; + req.led.control_mode = 7; + req.led.red = red; + req.led.green = green; + req.led.blue = blue; + req.led.loop = 0; + req.led.t1 = t1; + req.led.t2 = t2; + + req_finalize(session, 0x3F, SET_SYSTEM_LED_CMDID, sizeof(struct SetSystemLedReq), &req); + req_send(session->connection, &req, sizeof(struct SetSystemLedReq)); + +} diff --git a/src/messages/sdk_connection.c b/src/messages/sdk_connection.c new file mode 100644 index 0000000..5fc3b01 --- /dev/null +++ b/src/messages/sdk_connection.c @@ -0,0 +1,21 @@ +#include "message.h" +#include "connection.h" +#include "robomaster.h" + +void +set_sdk_connection( + Client session, + enum CONNECTION connection_type, + uint32_t ip_address, + uint16_t port ) { + + union Request req = {0}; + req.sdkconn.control = 0; + req.sdkconn.host = session->hostbyte; + req.sdkconn.connection = connection_type; + req.sdkconn.protocol = 0; + req.sdkconn.ip_address = ip_address; + req.sdkconn.port = port; + req_finalize(session, 0x3F, SET_SDK_CONNECTION_CMDID, sizeof(struct SetSdkConnectionReq), &req); + req_send(session->connection, &req, sizeof(struct SetSdkConnectionReq)); +} diff --git a/src/messages/sdk_mode.c b/src/messages/sdk_mode.c new file mode 100644 index 0000000..cef1fdc --- /dev/null +++ b/src/messages/sdk_mode.c @@ -0,0 +1,13 @@ +#include "message.h" +#include "connection.h" +#include "robomaster.h" + +void +set_sdk_mode( + Client session, + bool enable ) { + union Request req = {0}; + req.sdkmode.enable = enable; + req_finalize(session, 0x3F, SET_SDK_MODE_CMDID, sizeof(struct SetSdkModeReq), &req); + req_send(session->connection, &req, sizeof(struct SetSdkModeReq)); +} diff --git a/src/messages/set_wheel_speed.c b/src/messages/set_wheel_speed.c new file mode 100644 index 0000000..c601eff --- /dev/null +++ b/src/messages/set_wheel_speed.c @@ -0,0 +1,19 @@ +#include "message.h" +#include "connection.h" +#include "robomaster.h" + +void +set_wheel_speed ( + Client session, + int16_t w1, + int16_t w2, + int16_t w3, + int16_t w4 ) { + union Request req = {0}; + req.wheel.wheel_speed[0] = w1; + req.wheel.wheel_speed[1] = w2; + req.wheel.wheel_speed[2] = w3; + req.wheel.wheel_speed[3] = w4; + req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, sizeof(struct SetWheelSpeedReq), &req); + req_send(session->connection, &req, sizeof(struct SetWheelSpeedReq)); +} diff --git a/src/robomaster.c b/src/robomaster.c new file mode 100644 index 0000000..743921a --- /dev/null +++ b/src/robomaster.c @@ -0,0 +1,65 @@ +#include "robomaster.h" + +#include "client.h" +#include "message.h" +#include "connection.h" + +#include +#include +#include +#include +#include +#include + +Client client_new() { + struct Client* client = malloc(sizeof(struct Client)); + memset(client, 0, sizeof(struct Client)); + // TODO: Make this configurable + client->hostbyte = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_CLIENT_INDEX); + return client; +} + +void client_connect(Client client) { + client->connection = connection_new(); + + set_sdk_connection(client, CONNECTION_WIFI_AP, 0, 10010); +} + +void poll_message(Client client, union Message* resp) { + + // Poll for messages + static struct timeval timeout = {-1, 0}; + fd_set read_fds; + FD_ZERO(&read_fds); + FD_SET(client->connection->sockfd, &read_fds); + int result = select(client->connection->sockfd + 1, &read_fds, NULL, NULL, &timeout); + + // Check for socket polling errors + if(result < 0) { + perror("message polling failed"); + exit(EXIT_FAILURE); + } + + // Skip if nothing was received yet + // TODO: Make a static "empty" message or something + if (result == 0) { + return; + } + + // Read a message from the socket + // TODO: Use union to make all messages same size + int recvb = recvfrom(client->connection->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&client->connection->remote_addr, &client->connection->addrlen); + + // Check for socket read errors + if(recvb < 0) { + perror("reading socket failed"); + exit(EXIT_FAILURE); + } + + // Check for message errors + if(message_validate(resp)) { + perror("invalid message"); + exit(EXIT_FAILURE); + } + +} diff --git a/src/robomastersh.c b/src/robomastersh.c index 5c35d6f..593fbea 100644 --- a/src/robomastersh.c +++ b/src/robomastersh.c @@ -1,50 +1,9 @@ #include "robomaster.h" -#include -#include -#include - -uint8_t buffer[1024] = {0}; -uint8_t recvbuff[1024] = {0}; - int main(int argc, char* argv[]) { - Client session = session_new(buffer, sizeof(buffer)); - struct SetSdkConnectionReq req = {0}; - req.control = 0; - req.host = 0; - req.connection = CONNECTION_WIFI_AP; - req.protocol = 0; - req.ip_address = 0; - req.port = 10010; - const Message msg = set_sdk_connection_req_new(session, &req); - - int sockfd = socket(AF_INET, SOCK_DGRAM, 0); - struct sockaddr_in dest_addr = {0}; - socklen_t addrlen = sizeof(dest_addr); - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(30030); - dest_addr.sin_addr.s_addr = inet_addr("192.168.2.1"); - sendto(sockfd, buffer, session_size(session), 0, (struct sockaddr*)&dest_addr, addrlen); - - int recvb = recvfrom(sockfd, &recvbuff, sizeof(recvbuff), 0, (struct sockaddr*)&dest_addr, &addrlen); - struct Message* resp = (struct Message*)recvbuff; - - // check length - uint16_t resplen = (recvbuff[2] & 0x3) * 0xFF + recvbuff[1]; - if(recvb != resplen) - fprintf(stderr, "expected length %d got %d", recvb, resplen); - - // check header crc - uint8_t expected_crc8 = crc8(resp, 3); - if(resp->header.crc != expected_crc8) - fprintf(stderr, "expected crc8 %d got %d", resp->header.crc, expected_crc8); - - // check footer crc - uint16_t expected_crc16 = crc16(recvbuff, recvb - 2); - uint16_t actual_crc16 = *(uint16_t*)(recvbuff + recvb - 2); - if(actual_crc16 != expected_crc16) - fprintf(stderr, "expected crc16 %d got %d", actual_crc16, expected_crc16); + Client client = client_new(); + client_connect(client); return 0; } diff --git a/src/sdk_connection.h b/src/sdk_connection.h deleted file mode 100644 index 04b1401..0000000 --- a/src/sdk_connection.h +++ /dev/null @@ -1,47 +0,0 @@ -#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 deleted file mode 100644 index 6ddf932..0000000 --- a/src/sdk_mode.h +++ /dev/null @@ -1,24 +0,0 @@ -#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 deleted file mode 100644 index a106a70..0000000 --- a/src/set_wheel_speed.h +++ /dev/null @@ -1,24 +0,0 @@ -#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); -}