Added test code and notes
parent
3fd7a3e3cc
commit
fe30042100
@ -0,0 +1,339 @@
|
|||||||
|
When disconnected, controller sends following message every 20ms:
|
||||||
|
|
||||||
|
0A 4B 00 00 09 00 00 40 1A D2 1C 00 61 01 41 46
|
||||||
|
|
||||||
|
When connected, the field control app responds with:
|
||||||
|
|
||||||
|
03 01 64 00 00 00 00 BA 00 7C 1C 88 00 00 8E 67
|
||||||
|
|
||||||
|
Next the controller sends:
|
||||||
|
|
||||||
|
13 20 00 00 1A D2 1C 00 00 00 00 00 00 00 96 F0
|
||||||
|
|
||||||
|
The field control app responds with:
|
||||||
|
|
||||||
|
02 13 01 00 00 00 00 00 00 00 01 00 00 00 F3 78
|
||||||
|
|
||||||
|
These four packets are sent quickly(times from end of sending packet to start of next packet [55us, 122us, 66us]), the next message is sent after 100ms
|
||||||
|
|
||||||
|
11 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 B2 AA
|
||||||
|
|
||||||
|
0xB2AA = CRC-16/CCITT-FALSE of 11 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00, so likely that's what the last two bytes are
|
||||||
|
|
||||||
|
Going back, 0xF378 is the CRC-16/CCITT-FALSE of 02 13 01 00 00 00 00 00 00 00 01 00 00 00, so it looks like every message is ended with the CRC-16/CCITT-FALSE of the preceeding bytes
|
||||||
|
|
||||||
|
Response:
|
||||||
|
|
||||||
|
04 00 14 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 F5 6B
|
||||||
|
|
||||||
|
Next:
|
||||||
|
|
||||||
|
02 04 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 CB 80
|
||||||
|
|
||||||
|
Response:
|
||||||
|
|
||||||
|
02 02 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 D8 B1
|
||||||
|
|
||||||
|
The next burst of 6 packets starts with:
|
||||||
|
|
||||||
|
A7 00 E1 11 00 00 C9 00 00 53 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00 29 93
|
||||||
|
|
||||||
|
Response:
|
||||||
|
|
||||||
|
54 09 00 70 C9 36 B8 50 58 39 00 EE FE 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 3C 9C
|
||||||
|
|
||||||
|
Next:
|
||||||
|
|
||||||
|
02 54 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 FA 50
|
||||||
|
|
||||||
|
Response:
|
||||||
|
|
||||||
|
02 02 01 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 D8 B1
|
||||||
|
|
||||||
|
Next:
|
||||||
|
|
||||||
|
A3 17 00 00 AA 5E 58 13 39 0C 00 04 00 06 00 02 40 01 40 00 40 00 40 30 30 FC 09 00 00 00 C9 DE
|
||||||
|
|
||||||
|
Response(after 3x the normal time):
|
||||||
|
|
||||||
|
02 A3 01 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 4B F7
|
||||||
|
|
||||||
|
|
||||||
|
Next burst of 4 packets starts with:
|
||||||
|
|
||||||
|
A7 00 E1 11 00 00 C9 00 00 56 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00 9B DE
|
||||||
|
|
||||||
|
Response:
|
||||||
|
|
||||||
|
54 09 00 71 C9 36 B8 50 56 20 00 4C 14 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 29 FA
|
||||||
|
|
||||||
|
Next:
|
||||||
|
|
||||||
|
02 54 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 FA 50
|
||||||
|
|
||||||
|
Response:
|
||||||
|
|
||||||
|
02 02 01 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 D8 B1
|
||||||
|
|
||||||
|
Then begins the pattern of the following send->response pattern every 20ms:
|
||||||
|
|
||||||
|
A7 00 E1 11 00 00 C9 00 00 53 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00 29 93
|
||||||
|
|
||||||
|
Response:
|
||||||
|
|
||||||
|
53 c9 00 00 00 00 00 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 34 8f
|
||||||
|
|
||||||
|
Since no additional control messages are sent, this polling response must contain the info of the current state + time
|
||||||
|
|
||||||
|
While in "driver", the pattern changes to:
|
||||||
|
|
||||||
|
A7 00 E1 31 00 00 E1 68 00 64 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00 BE 77
|
||||||
|
|
||||||
|
Response:
|
||||||
|
|
||||||
|
53 E1 00 00 12 99 01 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
|
||||||
|
|
||||||
|
While in "autonomous", the pattern changes to:
|
||||||
|
|
||||||
|
A7 00 E1 31 00 00 D1 0E 00 64 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00 91 AD
|
||||||
|
|
||||||
|
Repsonse:
|
||||||
|
|
||||||
|
53 D1 00 00 D9 37 00 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 A7 DA
|
||||||
|
|
||||||
|
Actually when auton starts this happens(without crc):
|
||||||
|
|
||||||
|
1) same pattern of
|
||||||
|
A7 00 E1 11 00 00 C9 00 00 64 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00
|
||||||
|
2) fc responds with
|
||||||
|
02 A7 01 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
|
||||||
|
3) controller sends exact same packet as before
|
||||||
|
A7 00 E1 11 00 00 C9 00 00 64 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00
|
||||||
|
4) fc responds with
|
||||||
|
02 A7 01 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
|
||||||
|
5) controller sends same packet
|
||||||
|
A7 00 E1 11 00 00 C9 00 00 64 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00
|
||||||
|
6) fc responds with
|
||||||
|
53 D1 00 00 7F 3E 00 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
|
||||||
|
7) controller sends
|
||||||
|
A7 00 E1 31 00 00 D1 0F 00 64 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00
|
||||||
|
8) fc responds with
|
||||||
|
53 D1 00 00 7D 3E 00 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
|
||||||
|
9) controller sends
|
||||||
|
A7 00 E1 31 00 00 D1 0F 00 64 00 00 00 00 00 01 00 00 32 31 30 5A 00 00 00 00 00 00 02 00
|
||||||
|
10) fc responds with
|
||||||
|
53 D1 00 00 64 3E 00 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
|
||||||
|
7-8(aka 9-10) continues to loop, looks like bytes 4 & 5 are the countdown
|
||||||
|
|
||||||
|
Notes:
|
||||||
|
|
||||||
|
1) It looks like the first byte is the type of the packet, these are the seen types:
|
||||||
|
- 0A sent while controller disconnected
|
||||||
|
- 03 sent as response to 0A
|
||||||
|
- 13 sent after 0A
|
||||||
|
- 02 sent with second byte as the type received, as an ack
|
||||||
|
- 11 first packet in connection sequence
|
||||||
|
- 04 second packet in connection sequence
|
||||||
|
- A7 part of second connection burst
|
||||||
|
- 54 response to A7 during connect
|
||||||
|
- A3 part of second connection burst
|
||||||
|
- 53 response to A7 normally
|
||||||
|
|
||||||
|
With these types, the connection sequence can be rewritten(with assumptions):
|
||||||
|
|
||||||
|
1 ) controller send "connect init" 0x0A
|
||||||
|
2 ) field control send "connect init response" 0x03
|
||||||
|
3 ) controller sends "begin setup" 0x13
|
||||||
|
4 ) field control sends "ack 0x13" 0x02
|
||||||
|
5 ) controller sensd "do the next thing" 0x11
|
||||||
|
6 ) field control responds "next thing done" 0x04
|
||||||
|
7 ) controller responds "ack 0x04" 0x02
|
||||||
|
8 ) fc responds "ack 0x02" 0x02
|
||||||
|
9 ) controller sends "poll state" 0xA7
|
||||||
|
10) fc responds "not normal response" 0x54
|
||||||
|
11) controller sends "ack 0x54" 0x02
|
||||||
|
12) fc responds "ack 0x02" 0x02
|
||||||
|
13) controller sends "some info probably" 0xA3
|
||||||
|
14) fc responds "ack 0xA3" 0x02
|
||||||
|
15) controller sends "poll state" 0xA7
|
||||||
|
16) fc responds "not normal response" 0x54
|
||||||
|
17) controller responds "ack 0x54" 0x02
|
||||||
|
18) fc responds "ack 0x02" 0x02
|
||||||
|
19) controller sends "poll state"
|
||||||
|
20) fc responds with "normal response" 0x53
|
||||||
|
|
||||||
|
The autonomous start sequence would be:
|
||||||
|
1) controller sends "poll state" 0xA7
|
||||||
|
2) fc sends "ack poll state" 0x02
|
||||||
|
3) controller sends "poll state"
|
||||||
|
4) fc sends "ack poll state" 0x02
|
||||||
|
5) controller sends "poll state" 0xA7
|
||||||
|
6) fc responds with "normal response" 0x53
|
||||||
|
7) controller sends "poll state" 0xA7
|
||||||
|
8) fc responds with "normal response" 0x53
|
||||||
|
|
||||||
|
Looking closer at the data, sometimes the FC responds to 0xA7 with "02A7"(aka ack 0xA7), and sometimes it responds with 0x53
|
||||||
|
|
||||||
|
In both cases the 0xA7 packet is the exact same. The 0x53 response seems to have 8 bits of data while the 0x02 has none
|
||||||
|
0x53 contains the timer and state
|
||||||
|
0x02 just acknowledges
|
||||||
|
|
||||||
|
I can think of two possibilities:
|
||||||
|
1) 0x02 is sent when the CPU doesn't have the data/time to send the full update
|
||||||
|
2) 0x02 is sent periodically since the protocol requires that every x messages is acknowledges with 0x02
|
||||||
|
|
||||||
|
Matching up some info with the UI on the field controller:
|
||||||
|
1) 11E1 is the "system status"
|
||||||
|
2) C9 is the "field status"
|
||||||
|
3) controller battery is 100%
|
||||||
|
4) controller version is 75(0x4B)
|
||||||
|
5) controller radio version is 48(0x30)
|
||||||
|
6) radio version is 49(0x31)
|
||||||
|
7) vex os version is 1.2.0
|
||||||
|
|
||||||
|
The 0x0A packet:
|
||||||
|
0A
|
||||||
|
4B - controller version
|
||||||
|
00
|
||||||
|
00
|
||||||
|
09
|
||||||
|
00
|
||||||
|
00
|
||||||
|
40
|
||||||
|
1A
|
||||||
|
D2
|
||||||
|
1C
|
||||||
|
00
|
||||||
|
61
|
||||||
|
01
|
||||||
|
41
|
||||||
|
46
|
||||||
|
|
||||||
|
The 0x03 packet:
|
||||||
|
03
|
||||||
|
01
|
||||||
|
64
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
BA
|
||||||
|
00
|
||||||
|
7C
|
||||||
|
1C
|
||||||
|
88
|
||||||
|
00
|
||||||
|
00
|
||||||
|
|
||||||
|
The 0x13 packet:
|
||||||
|
13
|
||||||
|
20
|
||||||
|
00
|
||||||
|
00
|
||||||
|
1A
|
||||||
|
D2
|
||||||
|
1C
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
|
||||||
|
The 0x02 packet:
|
||||||
|
02
|
||||||
|
13 - type being acknowledged
|
||||||
|
01 - ?
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
01 - ?
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
|
||||||
|
The 0xA7 packet(disabled):
|
||||||
|
A7 - type
|
||||||
|
00
|
||||||
|
E1 11 - system status
|
||||||
|
00
|
||||||
|
00
|
||||||
|
C9 - field status
|
||||||
|
00
|
||||||
|
00
|
||||||
|
53 - ?
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
01 - ?
|
||||||
|
00
|
||||||
|
00
|
||||||
|
32 31 30 5A - 210Z, team name
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
02 - ? probably part of vexos version
|
||||||
|
00
|
||||||
|
|
||||||
|
The 0xA7 packet(driver):
|
||||||
|
A7
|
||||||
|
00
|
||||||
|
E1 31 - system status
|
||||||
|
00
|
||||||
|
00
|
||||||
|
E1 - field status
|
||||||
|
68 - ?
|
||||||
|
00
|
||||||
|
64 - ? always 64 unless disabled
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
01 - ?
|
||||||
|
00
|
||||||
|
00
|
||||||
|
32 31 30 5A - 210Z, team name
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00
|
||||||
|
02 - ? probably part of vexos version
|
||||||
|
00
|
||||||
|
|
||||||
|
The 0x53 packet (auton)
|
||||||
|
53
|
||||||
|
D1 - control flags - 0b11010001
|
||||||
|
00
|
||||||
|
00
|
||||||
|
7D 3E 00 00 - 15997ms
|
||||||
|
03 - ?
|
||||||
|
|
||||||
|
The 0x53 packet(driver)
|
||||||
|
53
|
||||||
|
E1 - control flags - 0b11100001
|
||||||
|
00
|
||||||
|
00
|
||||||
|
12 99 01 00 - 104722ms
|
||||||
|
03 - ?
|
||||||
|
|
||||||
|
The 0x53 packet(disabled)
|
||||||
|
53
|
||||||
|
C9 - control flags - 0b11001001
|
||||||
|
00
|
||||||
|
00
|
||||||
|
00 00 00 00 - 0ms
|
||||||
|
03 - ?
|
||||||
|
|
||||||
@ -0,0 +1,453 @@
|
|||||||
|
/*
|
||||||
|
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 <SPI.h>
|
||||||
|
#include <Wire.h>
|
||||||
|
|
||||||
|
#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 <WiFi.h>
|
||||||
|
|
||||||
|
extern "C"
|
||||||
|
{
|
||||||
|
#include "freertos/FreeRTOS.h"
|
||||||
|
#include "freertos/timers.h"
|
||||||
|
}
|
||||||
|
#include <AsyncMQTT_ESP32.h>
|
||||||
|
|
||||||
|
#include "CRC16.h"
|
||||||
|
|
||||||
|
int read_line = 22;
|
||||||
|
|
||||||
|
int rx2 = 16;
|
||||||
|
int tx2 = 17;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint8_t type;
|
||||||
|
uint8_t controller_version;
|
||||||
|
uint8_t unknown[12];
|
||||||
|
uint16_t crc;
|
||||||
|
} pkt_init_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint8_t type;
|
||||||
|
uint8_t unknown[13];
|
||||||
|
uint16_t crc;
|
||||||
|
} pkt_init_resp_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint8_t type;
|
||||||
|
uint8_t unknown[13];
|
||||||
|
uint16_t crc;
|
||||||
|
} pkt_init_2_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
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 {
|
||||||
|
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<TimerCallbackFunction_t>(connectToMqtt));
|
||||||
|
wifiReconnectTimer = xTimerCreate("wifiTimer", pdMS_TO_TICKS(2000), pdFALSE, (void*)0,
|
||||||
|
reinterpret_cast<TimerCallbackFunction_t>(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()
|
||||||
|
{
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue