From 7db4244cb2f657c8097e3b1c21a966f273020b8c Mon Sep 17 00:00:00 2001 From: Noah Metz Date: Thu, 14 Dec 2023 18:28:01 -0700 Subject: [PATCH] Reorganized, and cleaned up code with new names --- fc_wifi/fc_wifi.ino | 453 ------------------------------ notes.txt | 9 +- fc/fc.ino => v5_field_control.ino | 70 ++--- 3 files changed, 43 insertions(+), 489 deletions(-) delete mode 100644 fc_wifi/fc_wifi.ino rename fc/fc.ino => v5_field_control.ino (85%) diff --git a/fc_wifi/fc_wifi.ino b/fc_wifi/fc_wifi.ino deleted file mode 100644 index 7ebe4b2..0000000 --- a/fc_wifi/fc_wifi.ino +++ /dev/null @@ -1,453 +0,0 @@ -/* - 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 - -#define _ASYNC_MQTT_LOGLEVEL_ 1 - -#define WIFI_SSID {WIFI_SSID} -#define WIFI_PASSWORD {WIFI_PKEY} -#define MQTT_HOST {MQTT_HOST} -#define MQTT_PORT {MQTT_PORT} -const char *SubTopic = "arena/1"; - -#include - -extern "C" -{ -#include "freertos/FreeRTOS.h" -#include "freertos/timers.h" -} -#include - -#include "CRC16.h" - -int read_line = 22; - -int rx2 = 16; -int tx2 = 17; - -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; - } -} - -AsyncMqttClient mqttClient; -TimerHandle_t mqttReconnectTimer; -TimerHandle_t wifiReconnectTimer; - -void connectToWifi() -{ - WiFi.begin(WIFI_SSID, WIFI_PASSWORD); -} - -void connectToMqtt() -{ - mqttClient.connect(); -} - -void WiFiEvent(WiFiEvent_t event) -{ - switch (event) - { -#if USING_CORE_ESP32_CORE_V200_PLUS - - case ARDUINO_EVENT_WIFI_READY: - Serial.println("WiFi ready"); - break; - - case ARDUINO_EVENT_WIFI_STA_START: - Serial.println("WiFi STA starting"); - break; - - case ARDUINO_EVENT_WIFI_STA_CONNECTED: - Serial.println("WiFi STA connected"); - break; - - case ARDUINO_EVENT_WIFI_STA_GOT_IP6: - case ARDUINO_EVENT_WIFI_STA_GOT_IP: - Serial.println("WiFi connected"); - Serial.print("IP address: "); - Serial.println(WiFi.localIP()); - connectToMqtt(); - break; - - case ARDUINO_EVENT_WIFI_STA_LOST_IP: - Serial.println("WiFi lost IP"); - break; - - case ARDUINO_EVENT_WIFI_STA_DISCONNECTED: - Serial.println("WiFi lost connection"); - xTimerStop(mqttReconnectTimer, 0); // ensure we don't reconnect to MQTT while reconnecting to Wi-Fi - xTimerStart(wifiReconnectTimer, 0); - break; -#else - - case SYSTEM_EVENT_STA_GOT_IP: - Serial.println("WiFi connected"); - Serial.println("IP address: "); - Serial.println(WiFi.localIP()); - connectToMqtt(); - break; - - case SYSTEM_EVENT_STA_DISCONNECTED: - Serial.println("WiFi lost connection"); - xTimerStop(mqttReconnectTimer, 0); // ensure we don't reconnect to MQTT while reconnecting to Wi-Fi - xTimerStart(wifiReconnectTimer, 0); - break; -#endif - - default: - break; - } -} - -void printSeparationLine() -{ - Serial.println("************************************************"); -} - -void onMqttConnect(bool sessionPresent) -{ - Serial.println("mqtt connected"); - mqttClient.subscribe(SubTopic, 2); -} - -void onMqttDisconnect(AsyncMqttClientDisconnectReason reason) -{ - (void) reason; - - if (WiFi.isConnected()) - { - xTimerStart(mqttReconnectTimer, 0); - } -} - -void onMqttSubscribe(const uint16_t& packetId, const uint8_t& qos) -{ - Serial.println("subscribed to topic"); -} - -void onMqttUnsubscribe(const uint16_t& packetId) -{ -} - -void onMqttMessage(char* topic, char* payload, const AsyncMqttClientMessageProperties& properties, - const size_t& len, const size_t& index, const size_t& total) -{ - if(len < 4) { - Serial.printf("not enough data: %d", len); - } - sscanf((const char*)payload, "0x%2hhx", &control_flags); - Serial.printf("Parsed new control flags: 0x%02x\n", control_flags); -} - -void onMqttPublish(const uint16_t& packetId) -{ - Serial.println("Publish acknowledged."); - Serial.print(" packetId: "); - Serial.println(packetId); -} - -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"); - - while (!Serial && millis() < 5000); - - delay(500); - - mqttReconnectTimer = xTimerCreate("mqttTimer", pdMS_TO_TICKS(2000), pdFALSE, (void*)0, - reinterpret_cast(connectToMqtt)); - wifiReconnectTimer = xTimerCreate("wifiTimer", pdMS_TO_TICKS(2000), pdFALSE, (void*)0, - reinterpret_cast(connectToWifi)); - - WiFi.onEvent(WiFiEvent); - - mqttClient.onConnect(onMqttConnect); - mqttClient.onDisconnect(onMqttDisconnect); - mqttClient.onSubscribe(onMqttSubscribe); - mqttClient.onUnsubscribe(onMqttUnsubscribe); - mqttClient.onMessage(onMqttMessage); - mqttClient.onPublish(onMqttPublish); - - mqttClient.setServer(MQTT_HOST, MQTT_PORT); - - connectToWifi(); -} - -void loop() -{ -} diff --git a/notes.txt b/notes.txt index 9582e7e..522241b 100644 --- a/notes.txt +++ b/notes.txt @@ -12,14 +12,15 @@ Connection Sequence: -> FC responds with 0x02 ACK - From now on, all packets are 32 bytes long -<- Controller sends "start something idk" packet 0x11 - - All bits are set to zero in the packets I've seen --> FC responds with 0x04 with byte 2 set to 0x14 +<- Controller sends "get poll rate" packet 0x11 +-> FC responds with "set poll rate" 0x04 with byte 2 set to the polling rate in ms <- Controller ACKs 0x02 -> FC ACKs the ACK 0x02 +- I just realized that the 0x14 byte corresponds to 20 in decimal, which is the interval that the controller is sending packets. Changing this to another value changes the controller poll interval so it looks like this sequence configures the controller polling interval. + Control Sequence: -- After this startup sequence is completed(actually just after the first 16 byte sequence is completed, not sure what the 0x11 sequence is for), the controller starts a sequence of sending 0xA7 packets every 20ms +- After this startup sequence is completed, the controller starts a sequence of sending 0xA7 packets every 20ms <- Controller sends 0xA7 with the robot name, control status flags, system status flags, and timer. - At this point the FC can respond in two ways: 1) -> FC sends 0x02 to ACK the 0xA7, controller continues operation and sends 0xA7 again in 20ms with the same info diff --git a/fc/fc.ino b/v5_field_control.ino similarity index 85% rename from fc/fc.ino rename to v5_field_control.ino index 7d2f6ff..0286bc4 100644 --- a/fc/fc.ino +++ b/v5_field_control.ino @@ -17,6 +17,13 @@ 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); } @@ -26,19 +33,21 @@ typedef struct __attribute__((packed)) { uint8_t controller_version; uint8_t unknown[12]; uint16_t crc; -} pkt_init_t; +} pkt_discovery_t; typedef struct __attribute__((packed)) { uint8_t type; - uint8_t unknown[13]; + uint8_t unknown_1[7]; + uint32_t serial; + uint8_t unknown_2[2]; uint16_t crc; -} pkt_init_resp_t; +} pkt_discovery_resp_t; typedef struct __attribute__((packed)) { uint8_t type; uint8_t unknown[13]; uint16_t crc; -} pkt_init_2_t; +} pkt_init_t; typedef struct __attribute__((packed)) { uint8_t type; @@ -54,12 +63,12 @@ typedef struct __attribute__((packed)) { uint8_t type; uint8_t unknown[29]; uint16_t crc; -} pkt_init_3_resp_t; +} pkt_set_poll_rate_t; CRC16 crc = CRC16(0x1021, 0xFFFF, 0x0000, false, false); -void parse_init(uint8_t* pkt, size_t len) { - pkt_init_t parsed; +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); @@ -67,26 +76,23 @@ void parse_init(uint8_t* pkt, size_t len) { 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); + Serial.printf("Got dicovery packet with bad crc(%04x != %04x)\n", calc_crc, parsed.crc); } else { - Serial.printf("Got init packet with matching crc(%04x)\n", calc_crc); + Serial.printf("Got dicovery packet with matching crc(%04x)\n", calc_crc); } - pkt_init_resp_t response; + pkt_discovery_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; + 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); @@ -98,17 +104,17 @@ void parse_init(uint8_t* pkt, size_t len) { digitalWrite(read_line, LOW); } -void parse_init_2(uint8_t* pkt, size_t len) { - pkt_init_2_t parsed; +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_2 packet with bad crc(%04x != %04x)\n", calc_crc, parsed.crc); + Serial.printf("Got init 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); + Serial.printf("Got init packet with matching crc(%04x)\n", calc_crc); } pkt_ack_t response; @@ -129,7 +135,7 @@ void parse_init_2(uint8_t* pkt, size_t len) { digitalWrite(read_line, LOW); } -void parse_init_3(uint8_t* pkt, size_t len) { +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); @@ -141,7 +147,7 @@ void parse_init_3(uint8_t* pkt, size_t len) { Serial.printf("Got init_3 packet with matching crc(%04x)\n", calc_crc); } - pkt_init_3_resp_t response; + pkt_set_poll_rate_t response; response.type = 0x04; memset(response.unknown, 0, 29); response.unknown[1] = 0x14; @@ -261,13 +267,13 @@ void on_rx_2(void) { parse_poll(pkt, read); break; case 0x0A: - parse_init(pkt, read); + parse_discovery(pkt, read); break; case 0x13: - parse_init_2(pkt, read); + parse_init(pkt, read); break; case 0x11: - parse_init_3(pkt, read); + parse_get_poll_rate(pkt, read, 50); break; case 0x02: switch (pkt[1]){