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

Identical output to original Robomaster SDK

Browse Source
refactor
PgSocks 2 years ago
parent d47c817e0d
commit 71f01f93cf
4 changed files with 145 additions and 108 deletions
Show Stats Download Patch File Download Diff File

138
src/crc.c
Unescape Escape View File

@ -3,87 +3,66 @@
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
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 uint8_t CRC16_HIGH_TABLE[] =
static const uint16_t CRC16_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
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 = 0;
uint8_t crc = 0x77;
const uint8_t* bytes = (uint8_t*)block;
while(length--) crc = CRC8_TABLE[crc^*bytes++];
return crc;
@ -91,14 +70,9 @@ uint8_t crc8(const void* block, size_t length)
uint16_t crc16(const void* block, size_t length)
{
uint8_t crch = 0XFF, crcl = 0XFF;
uint16_t crc = 0x3692;
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);
crc = ((crc>>8)&0xFF)^CRC16_TABLE[((crc^*bytes++)&0xFF)];
return crc;
}

21
src/robomaster.c
Unescape Escape View File

@ -13,6 +13,8 @@ struct Session* session_new(void* buffer, size_t size) {
session->size = 0;
session->seq = RM_SDK_FIRST_SEQ_ID;
session->max_size = size;
// TODO: Make this configurable
session->hostbyte = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_CLIENT_INDEX);
return session;
}
@ -25,21 +27,25 @@ 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;
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 = htons(length & 0xff03 | 4);
message->header.length_l = length & 0xFF;
message->header.length_h = (length >> 8) & 0x3 | 4;
message->header.crc = crc8(message, 3);
message->header.seq_id = htons(session->seq++);
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;
memcpy((void*)message->body, body, length);
struct Footer* footer = (void*)message + length - sizeof(struct Footer);
footer->crc = htons(crc16(message, length - 2));
uint16_t crc = crc16(message, length - sizeof(struct Footer));
footer->crc = crc;
return message;
@ -49,10 +55,9 @@ message_new(struct Session* session, uint8_t cmdset, uint8_t cmdid, size_t lengt
const Message
set_sdk_connection_req_new (
Session session,
const struct SetSdkConnectionReq* msg ) {
struct SetSdkConnectionReq* msg ) {
msg->host = session->hostbyte;
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;
}

64
src/robomaster.h
Unescape Escape View File

@ -7,15 +7,29 @@
// Move to private header
uint8_t crc8(const void *block, size_t length);
uint8_t crc16(const void *block, size_t length);
uint16_t crc16(const void *block, size_t length);
struct Header {
#define PACKED __attribute__((__packed__))
struct PACKED Header {
uint8_t preamble;
union {
uint16_t length;
struct {
uint8_t length_l;
uint8_t length_h;
};
};
uint8_t crc;
uint8_t sender;
uint8_t receiver;
uint16_t seq_id;
union {
int16_t seq_id;
struct {
uint8_t seq_id_l;
uint8_t seq_id_h;
};
};
union {
uint8_t attribute;
struct {
@ -28,20 +42,26 @@ struct Header {
uint8_t cmdid;
};
struct Message {
struct PACKED Message {
struct Header header;
uint8_t body[];
};
struct Footer {
struct PACKED Footer {
uint16_t crc;
};
// TODO: Rename to Client
struct Session {
void* buffer;
size_t max_size;
size_t size;
uint16_t seq;
uint8_t hostbyte;
int16_t seq;
};
struct Message* message_new(struct Session* session, uint8_t cmdset, uint8_t cmdid, size_t length, const void* body);
@ -55,16 +75,32 @@ 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 int16_t RM_SDK_FIRST_SEQ_ID = 11239;
static const int16_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;
// 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;
struct SetSdkConnectionReq {
struct PACKED SetSdkConnectionReq {
uint8_t control;
uint8_t host;
uint8_t connection;
@ -73,20 +109,24 @@ struct SetSdkConnectionReq {
uint16_t port;
};
struct SetSdkConnectionResp {
struct PACKED SetSdkConnectionResp {
uint8_t retcode;
uint8_t state;
uint32_t config_ip;
};
const Message set_sdk_connection_req_new(Session session, const struct SetSdkConnectionReq* msg);
const Message set_sdk_connection_req_new(Session session, struct SetSdkConnectionReq* msg);
static const uint8_t GET_VERSION_CMDID = 0x01;
struct GetVersionResp {
// 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(Session session);

28
src/robomastersh.c
Unescape Escape View File

@ -2,6 +2,7 @@
#include <arpa/inet.h>
#include <stdio.h>
#include <assert.h>
uint8_t buffer[1024] = {0};
uint8_t recvbuff[1024] = {0};
@ -15,7 +16,8 @@ int main(int argc, char* argv[])
req.connection = CONNECTION_WIFI_AP;
req.protocol = 0;
req.ip_address = 0;
req.port = 10010;
//req.port = 10010;
req.port = 10242;
const Message msg = set_sdk_connection_req_new(session, &req);
int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
@ -26,12 +28,28 @@ int main(int argc, char* argv[])
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);
int recvb = recvfrom(sockfd, &recvbuff, sizeof(recvbuff), 0, (struct sockaddr*)&dest_addr, &addrlen);
struct Message* resp = (struct Message*)recvbuff;
addrlen = sizeof(dest_addr);
// check length
uint16_t resplen = (recvbuff[2] & 0x3) * 0xFF + recvbuff[1];
recvfrom(sockfd, resp->body, resplen - sizeof(struct Header), 0, (struct sockaddr*)&dest_addr, &addrlen);
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 = ntohs(*(uint16_t*)(recvbuff + recvb - 2));
if(actual_crc16 != expected_crc16)
fprintf(stderr, "expected crc16 %d got %d", actual_crc16, expected_crc16);
// check hostbyte
if(resp->header.receiver != session->hostbyte)
fprintf(stderr, "expected receiver %d got %d", session->hostbyte, resp->header.receiver);
return 0;
}

Write Preview
Loading…
Cancel
Save