From 4ff88e29fd1f18315f190c328c5674afc81f500b Mon Sep 17 00:00:00 2001 From: Noah Metz Date: Thu, 14 Dec 2023 17:08:12 -0700 Subject: [PATCH] Moved fc into fc and fc_wifi arduino projects --- fc/fc.ino | 300 ++++++++++++++++++++++++++++++++++ fc.ino => fc_wifi/fc_wifi.ino | 0 2 files changed, 300 insertions(+) create mode 100644 fc/fc.ino rename fc.ino => fc_wifi/fc_wifi.ino (100%) diff --git a/fc/fc.ino b/fc/fc.ino new file mode 100644 index 0000000..7d2f6ff --- /dev/null +++ b/fc/fc.ino @@ -0,0 +1,300 @@ +/* + DigitalReadSerial + + Reads a digital input on pin 2, prints the result to the Serial Monitor + + This example code is in the public domain. + + https://www.arduino.cc/en/Tutorial/BuiltInExamples/DigitalReadSerial +*/ +#include +#include + +#include "CRC16.h" + +int read_line = 22; + +int rx2 = 16; +int tx2 = 17; + +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_init_t; + +typedef struct __attribute__((packed)) { + uint8_t type; + uint8_t unknown[13]; + uint16_t crc; +} pkt_init_resp_t; + +typedef struct __attribute__((packed)) { + uint8_t type; + uint8_t unknown[13]; + uint16_t crc; +} pkt_init_2_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_init_3_resp_t; + +CRC16 crc = CRC16(0x1021, 0xFFFF, 0x0000, false, false); + +void parse_init(uint8_t* pkt, size_t len) { + pkt_init_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 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_init_resp_t response; + response.type = 0x03; + response.unknown[0] = 0x01; + response.unknown[1] = 0x64; + response.unknown[2] = 0x00; + response.unknown[3] = 0x00; + response.unknown[4] = 0x00; + response.unknown[5] = 0x00; + response.unknown[6] = 0xBA; + response.unknown[7] = 0x00; + response.unknown[8] = 0x7C; + response.unknown[9] = 0x1C; + response.unknown[10] = 0x88; + response.unknown[11] = 0x00; + response.unknown[12] = 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_2(uint8_t* pkt, size_t len) { + pkt_init_2_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_2 packet with bad crc(%04x != %04x)\n", calc_crc, parsed.crc); + } else { + Serial.printf("Got init_2 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_init_3(uint8_t* pkt, size_t len) { + 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 init_3 packet with bad crc(%04x != %04x\n", calc_crc, parsed_crc); + } else { + Serial.printf("Got init_3 packet with matching crc(%04x)\n", calc_crc); + } + + pkt_init_3_resp_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 >> 24 & 0xFF); + response[5] = (timer >> 16 & 0xFF); + response[6] = (timer >> 8 & 0xFF); + response[7] = (timer >> 0 & 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); +} + +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; + +volatile uint8_t control_flags = 0xC9; +volatile uint32_t timer = 0x00002000; +#define POLL_COUNT_MAX 5 + +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); + } + + if (parsed->field_status == control_flags) { + send_ack(0xA7, 0x01); + } { + send_state(control_flags, timer); + } +} + +void on_rx(void) { + uint8_t pkt[4]; + int n = Serial.available(); + if (n < 4) { + return; + } + int read = Serial.readBytes(pkt, n); + sscanf((const char*)pkt, "0x%2hhx", &control_flags); + Serial.printf("Parsed new control flags: 0x%02x\n", control_flags); +} + +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_init(pkt, read); + break; + case 0x13: + parse_init_2(pkt, read); + break; + case 0x11: + parse_init_3(pkt, read); + 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() +{ +} diff --git a/fc.ino b/fc_wifi/fc_wifi.ino similarity index 100% rename from fc.ino rename to fc_wifi/fc_wifi.ino