pgsocks
/
robomaster-sdk
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Move messages to their own headers

Browse Source
refactor
PgSocks 2 years ago
parent dc1d5f77af
commit 8170b3aa90
12 changed files with 343 additions and 334 deletions
Show Stats Download Patch File Download Diff File

18
CMakeLists.txt
Unescape Escape View File

@ -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
#)

15
include/robomaster.h
Unescape Escape View File

@ -0,0 +1,15 @@
#pragma once
#include <stdlib.h>
#include <stddef.h>
#include <inttypes.h>
#include <stdbool.h>
// Public stuff
struct Message;
struct Client;
typedef struct Client* Client;
typedef struct Message* Message;
Client client_new(void* buffer, size_t size);

16
src/client.c
Unescape Escape View File

@ -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;
}

28
src/client.h
Unescape Escape View File

@ -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;

4
src/crc.h
Unescape Escape View File

@ -0,0 +1,4 @@
#pragma once
uint8_t crc8(const void *block, size_t length);
uint16_t crc16(const void *block, size_t length);

78
src/led.h
Unescape Escape View File

@ -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);
}

97
src/message.h
Unescape Escape View File

@ -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;
}

95
src/robomaster.c
Unescape Escape View File

@ -1,95 +0,0 @@
#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>
#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);
}

231
src/robomaster.h
Unescape Escape View File

@ -1,231 +0,0 @@
#pragma once
#include <stdlib.h>
#include <stddef.h>
#include <inttypes.h>
#include <stdbool.h>
// 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 );

47
src/sdk_connection.h
Unescape Escape View File

@ -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);
}

24
src/sdk_mode.h
Unescape Escape View File

@ -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);
}

24
src/set_wheel_speed.h
Unescape Escape View File

@ -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);
}
Write Preview
Loading…
Cancel
Save