Initial Commit

refactor
PgSocks 2 years ago
commit d47c817e0d

@ -0,0 +1,30 @@
cmake_minimum_required(VERSION 3.15)
project (
RoboMasterSDK
VERSION 1.0.0
LANGUAGES C
)
set_property(GLOBAL PROPERTY C_STANDARD 17)
set_property(GLOBAL PROPERTY CXX_STANDARD 17)
add_compile_options(-Wall)
add_library(robomaster
src/robomaster.c
src/crc.c
)
target_include_directories(robomaster
PUBLIC
src
)
add_executable(robomastersh
src/robomastersh.c
)
target_link_libraries(robomastersh
robomaster
)

@ -0,0 +1,122 @@
# Ports
* `ROBOT_SDK_PORT_MIN = 10100`
* `ROBOT_SDK_PORT_MAX = 10500`
* `ROBOT_DEVICE_PORT = 20020`
* `ROBOT_PROXY_PORT = 30030`
* `ROBOT_BROADCAST_PORT = 40927`
* `ROBOT_DEFAULT_WIFI_ADDR = 192.168.2.1`
# Direct WIFI connection
Send a `SetSdkConnection` message to `192.168.2.1:30030` containing local IP
address (`0.0.0.0` is fine) and a random port between `10100` and `10500`. The
Robomaster responds with a state, which should be 2, and the IP address
`192.168.2.24` that the client should use. The client will then send further
messages to `192.168.2.1:20020` via `192.168.2.24:<random port>`.
# Commands
* UTF-8 encoded
* command
* version
* quit
[1]: https://github.com/dji-sdk/RoboMaster-SDK/blob/v0.1.2/sample_code/RoboMasterEP/connection/network/robot_connection.py
[2]: https://github.com/SMerrony/tello/blob/master/messages.go
# Message Format
All big-endian unless otherwise noted.
## Header
The preamble `0x55` indicates the start of every message and is part of the
message header along with the length and a checksum of the header. A header is
always 4 bytes long.
| bytes | description |
|-------|-------------|
| 1 | `0x55` preamble |
| 2 | length of message |
| 1 | 8-bit CRC |
The CRC is a checksum of the preamble and message length together. The length
in bytes is of the entire message including the header.
## Body
The data is always at least one byte.
| bytes | description |
|-------|-------------|
| 1 | sender |
| 1 | receiver |
| 2 | seq ID |
| 1 | attri |
| 1 | cmdset |
| 1 | cmdid |
| >1 | data |
| 2 | 16-bit CRC |
The last two bits of the attri byte starting from the LSB represent if the
message is an ack and if an ack is needed. The other bits in the byte are
unused.
| bit | set | unset |
|-----|-------------|-------|
| 8 | is ack | enc |
| 7 | ack needed | no ack needed |
### TODO
* What is a seq ID?
* What is the format of sender and receiver?
* may not actually matter
* index always seems to be 0
* `index * 32 + host`
* sender is `_client.hostbyte`
* What is the difference between cmdid and cmdset?
# Messages
| name | cmdset | cmdid | bytes |
|------|--------|--------|-------|
| led | `0x3f` | `0x33` | 15 |
| set sdk connection | `0x3f` | `0xd4` | 10 |
## SetSdkConnection
| bytes | description |
|-------|-------------|
| 1 | control |
| 1 | host |
| 1 | connection |
| 1 | protocol |
| 4 | IP address |
Response message.
| bytes | description |
|-------|-------------|
| 1 | Return code |
| 1 | state |
| 4 | IP address |
What is `config_ip`? What is `state`?
## LED
| bytes | description |
|-------|-------------|
| 4 | comp mask |
| 2 | LED mask |
| 1 | ctrl & effect mode |
| 1 | red |
| 1 | green |
| 1 | blue |
| 1 | loop |
| 2 | t1 |
| 2 | t2 |
Control and effect modes share one byte, in MSB and LSB respectively.

@ -0,0 +1,104 @@
#include <inttypes.h>
#include <stdlib.h>
static const uint8_t CRC8_TABLE[] =
{
0 ,94 ,188,226,97 ,63 ,221,131,194,156,126,32 ,163,253,31 ,65 ,
157,195,33 ,127,252,162,64 ,30 ,95 ,1 ,227,189,62 ,96 ,130,220,
35 ,125,159,193,66 ,28 ,254,160,225,191,93 ,3 ,128,222,60 ,98 ,
190,224,2 ,92 ,223,129,99 ,61 ,124,34 ,192,158,29 ,67 ,161,255,
70 ,24 ,250,164,39 ,121,155,197,132,218,56 ,102,229,187,89 ,7 ,
219,133,103,57 ,186,228,6 ,88 ,25 ,71 ,165,251,120,38 ,196,154,
101,59 ,217,135,4 ,90 ,184,230,167,249,27 ,69 ,198,152,122,36 ,
248,166,68 ,26 ,153,199,37 ,123,58 ,100,134,216,91 ,5 ,231,185,
140,210,48 ,110,237,179,81 ,15 ,78 ,16 ,242,172,47 ,113,147,205,
17 ,79 ,173,243,112,46 ,204,146,211,141,111,49 ,178,236,14 ,80 ,
175,241,19 ,77 ,206,144,114,44 ,109,51 ,209,143,12 ,82 ,176,238,
50 ,108,142,208,83 ,13 ,239,177,240,174,76 ,18 ,145,207,45 ,115,
202,148,118,40 ,171,245,23 ,73 ,8 ,86 ,180,234,105,55 ,213,139,
87 ,9 ,235,181,54 ,104,138,212,149,203,41 ,119,244,170,72 ,22 ,
233,183,85 ,11 ,136,214,52 ,106,43 ,117,151,201,74 ,20 ,246,168,
116,42 ,200,150,21 ,75 ,169,247,182,232,10 ,84 ,215,137,107,53
};
static const uint8_t CRC16_HIGH_TABLE[] =
{
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};
static const int8_t CRC16_LOW_TABLE[] =
{
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};
uint8_t crc8(const void* block, size_t length)
{
uint8_t crc = 0;
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)
{
uint8_t crch = 0XFF, crcl = 0XFF;
const uint8_t* bytes = (uint8_t*)block;
size_t i;
while(length--)
{
i = crcl ^ *bytes++;
crcl = crch ^ CRC16_HIGH_TABLE[i];
crch = CRC16_LOW_TABLE[i];
}
return (((uint16_t)(crch<<8))|crcl);
}

@ -0,0 +1,58 @@
#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>
#include "robomaster.h"
struct Session* session_new(void* buffer, size_t size) {
struct Session* session = malloc(sizeof(struct Session*));
if(!buffer)
session->buffer = malloc(sizeof(struct Header));
else
session->buffer = buffer;
session->size = 0;
session->seq = RM_SDK_FIRST_SEQ_ID;
session->max_size = size;
return session;
}
size_t session_size(Session session) {
return session->size;
}
// private
struct Message*
message_new(struct Session* session, uint8_t cmdset, uint8_t cmdid, size_t length, const void* body) {
struct Message* message = session->buffer + session->size;
length += sizeof(struct Header) + sizeof(struct Footer);
session->size += length;
message->header.preamble = 0x55;
message->header.length = htons(length & 0xff03 | 4);
message->header.crc = crc8(message, 3);
message->header.seq_id = htons(session->seq++);
message->header.ack_needed = true;
message->header.cmdset = cmdset;
message->header.cmdid = cmdid;
memcpy((void*)message->body, body, length);
struct Footer* footer = (void*)message + length - sizeof(struct Footer);
footer->crc = htons(crc16(message, length - 2));
return message;
}
// public
const Message
set_sdk_connection_req_new (
Session session,
const struct SetSdkConnectionReq* msg ) {
struct Message* message = message_new(session, 0x3F, SET_SDK_CONNECTION_CMDID, sizeof(struct SetSdkConnectionReq), msg);
struct SetSdkConnectionReq* body = (struct SetSdkConnectionReq*)message->body;
body->port = htons(body->port);
return message;
}

@ -0,0 +1,92 @@
#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);
uint8_t crc16(const void *block, size_t length);
struct Header {
uint8_t preamble;
uint16_t length;
uint8_t crc;
uint8_t sender;
uint8_t receiver;
uint16_t seq_id;
union {
uint8_t attribute;
struct {
uint8_t reserved : 6;
bool ack_needed : 1;
bool is_ack : 1;
};
};
uint8_t cmdset;
uint8_t cmdid;
};
struct Message {
struct Header header;
uint8_t body[];
};
struct Footer {
uint16_t crc;
};
struct Session {
void* buffer;
size_t max_size;
size_t size;
uint16_t seq;
};
struct Message* message_new(struct Session* session, uint8_t cmdset, uint8_t cmdid, size_t length, const void* body);
// Public stuff
typedef struct Session* Session;
typedef struct Message* Message;
Session session_new(void* buffer, size_t size);
size_t session_size(Session session);
// SEQ ID will loop around
static const uint16_t RM_SDK_FIRST_SEQ_ID = 10000;
static const uint16_t RM_SDK_LAST_SEQ_ID = 20000;
static const uint8_t CONNECTION_WIFI_AP = 0;
static const uint8_t CONNECTION_WIFI_STA = 1;
static const uint8_t CONNECTION_USB_RNDIS = 2;
static const uint8_t SET_SDK_CONNECTION_CMDID = 0xD4;
struct SetSdkConnectionReq {
uint8_t control;
uint8_t host;
uint8_t connection;
uint8_t protocol;
uint32_t ip_address;
uint16_t port;
};
struct SetSdkConnectionResp {
uint8_t retcode;
uint8_t state;
uint32_t config_ip;
};
const Message set_sdk_connection_req_new(Session session, const struct SetSdkConnectionReq* msg);
static const uint8_t GET_VERSION_CMDID = 0x01;
struct GetVersionResp {
uint8_t retcode;
uint8_t aa;
uint8_t bb;
uint8_t cc;
uint8_t dd;
};

@ -0,0 +1,38 @@
#include "robomaster.h"
#include <arpa/inet.h>
#include <stdio.h>
uint8_t buffer[1024] = {0};
uint8_t recvbuff[1024] = {0};
int main(int argc, char* argv[])
{
Session 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);
addrlen = sizeof(dest_addr);
recvfrom(sockfd, &recvbuff, sizeof(struct Header), 0, (struct sockaddr*)&dest_addr, &addrlen);
struct Message* resp = (struct Message*)recvbuff;
addrlen = sizeof(dest_addr);
uint16_t resplen = (recvbuff[2] & 0x3) * 0xFF + recvbuff[1];
recvfrom(sockfd, resp->body, resplen - sizeof(struct Header), 0, (struct sockaddr*)&dest_addr, &addrlen);
return 0;
}
Loading…
Cancel
Save