You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
80 lines
2.2 KiB
C
80 lines
2.2 KiB
C
#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,
|
|
struct SetSdkConnectionReq* msg ) {
|
|
|
|
msg->host = session->hostbyte;
|
|
struct Message* message = message_new(session, 0x3F, SET_SDK_CONNECTION_CMDID, sizeof(struct SetSdkConnectionReq), msg);
|
|
return message;
|
|
}
|
|
|
|
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);
|
|
}
|