312 lines
7.7 KiB
C++
312 lines
7.7 KiB
C++
#include "CRC16.h"
|
|
|
|
volatile uint8_t control_flags = 0xC9;
|
|
volatile uint32_t timer_end = 0x00000000;
|
|
volatile uint32_t length = 0x00000000;
|
|
volatile uint32_t last_timer_sent = 0x00000000;
|
|
CRC16 crc = CRC16(0x1021, 0xFFFF, 0x0000, false, false);
|
|
|
|
int read_line = 22;
|
|
|
|
int rx2 = 16;
|
|
int tx2 = 17;
|
|
|
|
uint32_t htonl(uint32_t val) {
|
|
return ((val & 0x000000FF) << 24) |
|
|
((val & 0x0000FF00) << 8) |
|
|
((val & 0x00FF0000) >> 8) |
|
|
((val & 0xFF000000) >> 24);
|
|
}
|
|
|
|
uint16_t htons(uint16_t val) {
|
|
return ((val & 0xFF) << 8) | ((val & 0xFF00) >> 8);
|
|
}
|
|
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t type;
|
|
uint8_t controller_version;
|
|
uint8_t unknown[12];
|
|
uint16_t crc;
|
|
} pkt_discovery_t;
|
|
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t type;
|
|
uint8_t unknown_1[7];
|
|
uint32_t serial;
|
|
uint8_t unknown_2[2];
|
|
uint16_t crc;
|
|
} pkt_discovery_resp_t;
|
|
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t type;
|
|
uint8_t unknown[13];
|
|
uint16_t crc;
|
|
} pkt_init_t;
|
|
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t type;
|
|
uint8_t ack_type;
|
|
uint8_t unknown_1;
|
|
uint8_t unknown_2[7];
|
|
uint8_t unknown_3;
|
|
uint8_t unknown_4[3];
|
|
uint16_t crc;
|
|
} pkt_ack_t;
|
|
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t type;
|
|
uint8_t unknown[29];
|
|
uint16_t crc;
|
|
} pkt_set_poll_rate_t;
|
|
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t type;
|
|
uint8_t unknown_1;
|
|
uint16_t system_status;
|
|
uint8_t unknown_2[2];
|
|
uint8_t field_status;
|
|
uint8_t unknown_3[11];
|
|
uint8_t name[10];
|
|
uint8_t version[2];
|
|
uint16_t crc;
|
|
} pkt_poll_t;
|
|
|
|
void parse_discovery(uint8_t* pkt, size_t len) {
|
|
pkt_discovery_t parsed;
|
|
parsed.controller_version = pkt[1];
|
|
memcpy(parsed.unknown, &pkt[2], 12);
|
|
parsed.crc = (pkt[14] << 8) + (pkt[15] << 0);
|
|
crc.restart();
|
|
crc.add(pkt, 14);
|
|
uint16_t calc_crc = crc.calc();
|
|
if (calc_crc != parsed.crc) {
|
|
Serial.printf("Got dicovery packet with bad crc(%04x != %04x)\n", calc_crc, parsed.crc);
|
|
} else {
|
|
Serial.printf("Got dicovery packet with matching crc(%04x)\n", calc_crc);
|
|
}
|
|
|
|
pkt_discovery_resp_t response;
|
|
response.type = 0x03;
|
|
response.unknown_1[0] = 0x01;
|
|
response.unknown_1[1] = 0x64;
|
|
response.unknown_1[2] = 0x00;
|
|
response.unknown_1[3] = 0x00;
|
|
response.unknown_1[4] = 0x00;
|
|
response.unknown_1[5] = 0x00;
|
|
response.unknown_1[6] = 0xBA;
|
|
response.serial = htonl(0x881C7C00);
|
|
response.unknown_2[0] = 0x00;
|
|
response.unknown_2[1] = 0x00;
|
|
|
|
crc.restart();
|
|
crc.add((uint8_t*)&response, 14);
|
|
response.crc = htons(crc.calc());
|
|
|
|
digitalWrite(read_line, HIGH);
|
|
Serial2.write((uint8_t*)&response, 16);
|
|
delayMicroseconds(100);
|
|
digitalWrite(read_line, LOW);
|
|
}
|
|
|
|
void parse_init(uint8_t* pkt, size_t len) {
|
|
pkt_init_t parsed;
|
|
memcpy(parsed.unknown, &pkt[1], 13);
|
|
parsed.crc = (pkt[14] << 8) + (pkt[15] << 0);
|
|
crc.restart();
|
|
crc.add(pkt, 14);
|
|
uint16_t calc_crc = crc.calc();
|
|
if (calc_crc != parsed.crc) {
|
|
Serial.printf("Got init packet with bad crc(%04x != %04x)\n", calc_crc, parsed.crc);
|
|
} else {
|
|
Serial.printf("Got init packet with matching crc(%04x)\n", calc_crc);
|
|
}
|
|
|
|
pkt_ack_t response;
|
|
response.type = 0x02;
|
|
response.ack_type = pkt[0];
|
|
response.unknown_1 = 0x01;
|
|
memset(response.unknown_2, 0, 7);
|
|
response.unknown_3 = 0x01;
|
|
memset(response.unknown_4, 0, 3);
|
|
|
|
crc.restart();
|
|
crc.add((uint8_t*)&response, 14);
|
|
response.crc = htons(crc.calc());
|
|
|
|
digitalWrite(read_line, HIGH);
|
|
Serial2.write((uint8_t*)&response, 16);
|
|
delayMicroseconds(100);
|
|
digitalWrite(read_line, LOW);
|
|
}
|
|
|
|
void parse_get_poll_rate(uint8_t* pkt, size_t len, uint8_t poll_ms) {
|
|
crc.restart();
|
|
crc.add(pkt, 30);
|
|
uint16_t parsed_crc = (pkt[30] << 8) + (pkt[31] << 0);
|
|
uint16_t calc_crc = crc.calc();
|
|
|
|
if (parsed_crc != calc_crc) {
|
|
Serial.printf("Got get_poll_rate packet with bad crc(%04x != %04x\n", calc_crc, parsed_crc);
|
|
} else {
|
|
Serial.printf("Got get_poll_rate packet with matching crc(%04x)\n", calc_crc);
|
|
}
|
|
|
|
pkt_set_poll_rate_t response;
|
|
response.type = 0x04;
|
|
memset(response.unknown, 0, 29);
|
|
response.unknown[1] = 0x14;
|
|
|
|
crc.restart();
|
|
crc.add((uint8_t*)&response, 30);
|
|
response.crc = htons(crc.calc());
|
|
|
|
digitalWrite(read_line, HIGH);
|
|
Serial2.write((uint8_t*)&response, 32);
|
|
delayMicroseconds(200);
|
|
digitalWrite(read_line, LOW);
|
|
}
|
|
|
|
void send_ack(uint8_t ack_type, uint8_t unknown) {
|
|
uint8_t response[32];
|
|
memset(response, 0, 32);
|
|
response[0] = 0x02;
|
|
response[1] = ack_type;
|
|
response[2] = unknown;
|
|
response[10] = unknown;
|
|
|
|
crc.restart();
|
|
crc.add(response, 30);
|
|
uint16_t ack_crc = crc.calc();
|
|
|
|
response[30] = (ack_crc >> 8 & 0xFF);
|
|
response[31] = (ack_crc >> 0 & 0xFF);
|
|
|
|
digitalWrite(read_line, HIGH);
|
|
Serial2.write((uint8_t*)&response, 32);
|
|
delayMicroseconds(200);
|
|
digitalWrite(read_line, LOW);
|
|
}
|
|
|
|
void send_state(uint8_t control_flags, uint32_t timer) {
|
|
uint8_t response[32];
|
|
memset(response, 0, 32);
|
|
response[0] = 0x53;
|
|
response[1] = control_flags;
|
|
response[2] = 0x00;
|
|
response[3] = 0x00;
|
|
response[4] = ((timer >> 0) & 0xFF);
|
|
response[5] = ((timer >> 8) & 0xFF);
|
|
response[6] = ((timer >> 16) & 0xFF);
|
|
response[7] = ((timer >> 24) & 0xFF);
|
|
response[8] = 0x03;
|
|
|
|
crc.restart();
|
|
crc.add(response, 30);
|
|
uint16_t ack_crc = crc.calc();
|
|
|
|
response[30] = (ack_crc >> 8 & 0xFF);
|
|
response[31] = (ack_crc >> 0 & 0xFF);
|
|
|
|
digitalWrite(read_line, HIGH);
|
|
Serial2.write((uint8_t*)&response, 32);
|
|
delayMicroseconds(200);
|
|
digitalWrite(read_line, LOW);
|
|
}
|
|
|
|
void parse_poll(uint8_t* pkt, size_t len) {
|
|
pkt_poll_t* parsed = (pkt_poll_t*)pkt;
|
|
crc.restart();
|
|
crc.add(pkt, 30);
|
|
uint16_t crc_calc = crc.calc();
|
|
|
|
if (htons(parsed->crc) != crc_calc) {
|
|
Serial.printf("parsed poll with invalid CRC 0x%04x, calculated to be 0x%04x\n", htons(parsed->crc), crc_calc);
|
|
} else {
|
|
//Serial.printf("parsed poll with valid CRC 0x%04x, system status 0x%04x, field status 0x%02x, team name %s\n", htons(parsed->crc), htons(parsed->system_status), parsed->field_status, parsed->name);
|
|
}
|
|
|
|
uint32_t now = millis();
|
|
uint32_t current_timer = 0;
|
|
if (now < timer_end) {
|
|
current_timer = (timer_end - millis()) & 0xFFFFFF00;
|
|
} else {
|
|
current_timer = length;
|
|
control_flags = 0xC9;
|
|
}
|
|
|
|
if ((parsed->field_status == control_flags) && (current_timer == last_timer_sent)) {
|
|
send_ack(0xA7, 0x01);
|
|
} else if (control_flags == 0xC9) {
|
|
send_state(0xC9, length);
|
|
} else {
|
|
last_timer_sent = current_timer;
|
|
send_state(control_flags, current_timer);
|
|
}
|
|
}
|
|
|
|
void on_rx(void) {
|
|
uint8_t pkt[12];
|
|
int n = Serial.available();
|
|
if (n != 12) {
|
|
Serial.printf("Wrong number of bytes on serial port %d/12\n", n);
|
|
for (int i = 0; i < n; i++) {
|
|
Serial.read();
|
|
}
|
|
return;
|
|
}
|
|
int read = Serial.readBytes(pkt, n);
|
|
sscanf((const char*)pkt, "0x%2hhx%8x", &control_flags, &length);
|
|
timer_end = millis() + length;
|
|
Serial.printf("Parsed new control flags: 0x%02x, length 0x%08x\n", control_flags, length);
|
|
}
|
|
|
|
void on_rx_2(void) {
|
|
uint8_t pkt[32];
|
|
int n = Serial2.available();
|
|
if (n < 16) {
|
|
return;
|
|
}
|
|
int read = Serial2.readBytes(pkt, n);
|
|
switch (pkt[0]) {
|
|
case 0xA7:
|
|
parse_poll(pkt, read);
|
|
break;
|
|
case 0x0A:
|
|
parse_discovery(pkt, read);
|
|
break;
|
|
case 0x13:
|
|
parse_init(pkt, read);
|
|
break;
|
|
case 0x11:
|
|
parse_get_poll_rate(pkt, read, 20);
|
|
break;
|
|
case 0x02:
|
|
switch (pkt[1]){
|
|
case 0x04:
|
|
send_ack(0x02, 0x00);
|
|
break;
|
|
default:
|
|
Serial.printf("Unknown ack 0x%x\n", pkt[1]);
|
|
}
|
|
break;
|
|
default:
|
|
Serial.printf("Unknown command 0x%x\n", pkt[0]);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
Serial2.begin(1562500, SERIAL_8N1, rx2, tx2);
|
|
pinMode(read_line, OUTPUT);
|
|
digitalWrite(read_line, LOW);
|
|
Serial2.onReceive(on_rx_2);
|
|
Serial.onReceive(on_rx);
|
|
Serial.println("Started");
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
}
|