Skip to content

Commit f295c8b

Browse files
Paul Kendallpkendall64
Paul Kendall
authored and
pkendall64
committed
Add an ESPNow testing backpack
1 parent 2c863af commit f295c8b

3 files changed

Lines changed: 251 additions & 10 deletions

File tree

‎src/Test_Main.cpp‎

Lines changed: 219 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,219 @@
1+
#include <Arduino.h>
2+
#include <WiFi.h>
3+
#include <esp_now.h>
4+
#include <esp_wifi.h>
5+
6+
#include "msp.h"
7+
#include "msptypes.h"
8+
#include "options.h"
9+
10+
namespace {
11+
12+
constexpr uint32_t SERIAL_BAUD = 460800;
13+
constexpr uint8_t ESPNOW_CHANNEL = 1;
14+
15+
const char *packetTypeName(mspPacketType_e type)
16+
{
17+
switch (type)
18+
{
19+
case MSP_PACKET_COMMAND:
20+
return "command";
21+
case MSP_PACKET_RESPONSE:
22+
return "response";
23+
default:
24+
return "unknown";
25+
}
26+
}
27+
28+
const char *functionName(uint16_t function)
29+
{
30+
switch (function)
31+
{
32+
case MSP_SET_VTX_CONFIG:
33+
return "MSP_SET_VTX_CONFIG";
34+
case MSP_ELRS_BIND:
35+
return "MSP_ELRS_BIND";
36+
case MSP_ELRS_REQU_VTX_PKT:
37+
return "MSP_ELRS_REQU_VTX_PKT";
38+
case MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE:
39+
return "MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE";
40+
case MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE:
41+
return "MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE";
42+
case MSP_ELRS_GET_BACKPACK_VERSION:
43+
return "MSP_ELRS_GET_BACKPACK_VERSION";
44+
case MSP_ELRS_BACKPACK_CRSF_TLM:
45+
return "MSP_ELRS_BACKPACK_CRSF_TLM";
46+
case MSP_ELRS_SET_OSD:
47+
return "MSP_ELRS_SET_OSD";
48+
case MSP_ELRS_BACKPACK_CONFIG:
49+
return "MSP_ELRS_BACKPACK_CONFIG";
50+
case MSP_ELRS_BACKPACK_SET_RECORDING_STATE:
51+
return "MSP_ELRS_BACKPACK_SET_RECORDING_STATE";
52+
case MSP_ELRS_BACKPACK_SET_OSD_ELEMENT:
53+
return "MSP_ELRS_BACKPACK_SET_OSD_ELEMENT";
54+
case MSP_ELRS_BACKPACK_SET_HEAD_TRACKING:
55+
return "MSP_ELRS_BACKPACK_SET_HEAD_TRACKING";
56+
case MSP_ELRS_BACKPACK_SET_RTC:
57+
return "MSP_ELRS_BACKPACK_SET_RTC";
58+
case MSP_ELRS_BACKPACK_SET_PTR:
59+
return "MSP_ELRS_BACKPACK_SET_PTR";
60+
default:
61+
return nullptr;
62+
}
63+
}
64+
65+
void printMac(const uint8_t *mac)
66+
{
67+
Serial.printf("%02X:%02X:%02X:%02X:%02X:%02X",
68+
mac[0],
69+
mac[1],
70+
mac[2],
71+
mac[3],
72+
mac[4],
73+
mac[5]);
74+
}
75+
76+
void printBytes(const uint8_t *data, size_t len)
77+
{
78+
for (size_t i = 0; i < len; ++i)
79+
{
80+
if (i != 0)
81+
{
82+
Serial.write(' ');
83+
}
84+
Serial.printf("%02X", data[i]);
85+
}
86+
}
87+
88+
void printPacket(const uint8_t *mac, const mspPacket_t *packet)
89+
{
90+
Serial.print("MSP ");
91+
Serial.print(packetTypeName(packet->type));
92+
Serial.print(" from ");
93+
printMac(mac);
94+
Serial.print(": function=");
95+
96+
const char *name = functionName(packet->function);
97+
if (name != nullptr)
98+
{
99+
Serial.print(name);
100+
Serial.print(" (");
101+
}
102+
103+
Serial.printf("0x%04X", packet->function);
104+
105+
if (name != nullptr)
106+
{
107+
Serial.print(')');
108+
}
109+
110+
Serial.printf(", payload=%u", packet->payloadSize);
111+
112+
if (packet->payloadSize != 0)
113+
{
114+
Serial.print(", bytes=");
115+
printBytes(packet->payload, packet->payloadSize);
116+
}
117+
118+
Serial.println();
119+
}
120+
121+
bool setSoftMacAddress()
122+
{
123+
if (!firmwareOptions.hasUID)
124+
{
125+
Serial.println("No flashed UID found. Reflash this target with a UID first.");
126+
return false;
127+
}
128+
129+
Serial.print("Flashed UID: ");
130+
printMac(firmwareOptions.uid);
131+
Serial.println();
132+
133+
// MAC address can only be set with unicast, so first byte must be even, not odd.
134+
firmwareOptions.uid[0] &= ~0x01;
135+
136+
WiFi.mode(WIFI_STA);
137+
WiFi.setTxPower(WIFI_POWER_19_5dBm);
138+
esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR);
139+
WiFi.begin("network-name", "pass-to-network", ESPNOW_CHANNEL);
140+
delay(10);
141+
WiFi.disconnect();
142+
143+
if (esp_wifi_set_mac(WIFI_IF_STA, firmwareOptions.uid) != ESP_OK)
144+
{
145+
Serial.println("Failed to set receiver soft MAC");
146+
return false;
147+
}
148+
149+
Serial.print("Receiver STA SoftMAC: ");
150+
Serial.println(WiFi.macAddress());
151+
return true;
152+
}
153+
154+
void setupEspNow()
155+
{
156+
if (esp_now_init() != ESP_OK)
157+
{
158+
Serial.println("ESP-NOW init failed");
159+
ESP.restart();
160+
}
161+
}
162+
163+
void onDataRecv(const uint8_t *mac, const uint8_t *data, int len)
164+
{
165+
MSP recvMsp;
166+
bool parsed = false;
167+
168+
Serial.print("RAW from ");
169+
printMac(mac);
170+
Serial.printf(" (%d bytes): ", len);
171+
printBytes(data, static_cast<size_t>(len));
172+
Serial.println();
173+
174+
for (int i = 0; i < len; ++i)
175+
{
176+
if (!recvMsp.processReceivedByte(data[i]))
177+
{
178+
continue;
179+
}
180+
181+
parsed = true;
182+
printPacket(mac, recvMsp.getReceivedPacket());
183+
recvMsp.markPacketReceived();
184+
}
185+
186+
if (!parsed)
187+
{
188+
Serial.println("No complete MSP packet decoded from frame.");
189+
}
190+
191+
Serial.println();
192+
}
193+
194+
} // namespace
195+
196+
void setup()
197+
{
198+
Serial.begin(SERIAL_BAUD);
199+
delay(250);
200+
Serial.println();
201+
Serial.printf("ESP-NOW RX debug at %lu baud", SERIAL_BAUD);
202+
Serial.println();
203+
204+
options_init();
205+
if (!setSoftMacAddress())
206+
{
207+
return;
208+
}
209+
210+
setupEspNow();
211+
esp_now_register_recv_cb(onDataRecv);
212+
Serial.printf("Listening on channel %u", ESPNOW_CHANNEL);
213+
Serial.println();
214+
}
215+
216+
void loop()
217+
{
218+
delay(1000);
219+
}

‎targets/common.ini‎

Lines changed: 20 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,8 @@ lib_deps =
8686
build_src_filter =
8787
${common_env_data.build_src_filter}
8888
; -<Tx_main.cpp>
89+
-<Timer_main.cpp>
90+
-<Test_main.cpp>
8991
-<Vrx_main.cpp>
9092
-<rapidfire.*>
9193
-<rx5808.*>
@@ -94,7 +96,6 @@ build_src_filter =
9496
-<hdzero.*>
9597
-<skyzone_msp.*>
9698
-<orqa.*>
97-
-<Timer_main.cpp>
9899
-<mfd_crossbow.*>
99100

100101
# ------------------------- COMMON RAPIDFIRE-BACKPACK DEFINITIONS -----------------
@@ -106,6 +107,8 @@ build_flags =
106107
build_src_filter =
107108
${common_env_data.build_src_filter}
108109
-<Tx_main.cpp>
110+
-<Timer_main.cpp>
111+
-<Test_main.cpp>
109112
; -<Vrx_main.cpp>
110113
; -<rapidfire.*>
111114
-<rx5808.*>
@@ -114,7 +117,6 @@ build_src_filter =
114117
-<hdzero.*>
115118
-<skyzone_msp.*>
116119
-<orqa.*>
117-
-<Timer_main.cpp>
118120
-<mfd_crossbow.*>
119121

120122
# ------------------------- COMMON RX5808-BACKPACK DEFINITIONS -----------------
@@ -126,6 +128,8 @@ build_flags =
126128
build_src_filter =
127129
${common_env_data.build_src_filter}
128130
-<Tx_main.cpp>
131+
-<Timer_main.cpp>
132+
-<Test_main.cpp>
129133
; -<Vrx_main.cpp>
130134
-<rapidfire.*>
131135
; -<rx5808.*>
@@ -134,7 +138,6 @@ build_src_filter =
134138
-<hdzero.*>
135139
-<skyzone_msp.*>
136140
-<orqa.*>
137-
-<Timer_main.cpp>
138141
-<mfd_crossbow.*>
139142

140143
# ------------------------- COMMON STEADYVIEW-BACKPACK DEFINITIONS -----------------
@@ -146,6 +149,8 @@ build_flags =
146149
build_src_filter =
147150
${common_env_data.build_src_filter}
148151
-<Tx_main.cpp>
152+
-<Timer_main.cpp>
153+
-<Test_main.cpp>
149154
; -<Vrx_main.cpp>
150155
-<rapidfire.*>
151156
-<rx5808.*>
@@ -154,7 +159,6 @@ build_src_filter =
154159
-<hdzero.*>
155160
-<skyzone_msp.*>
156161
-<orqa.*>
157-
-<Timer_main.cpp>
158162
-<mfd_crossbow.*>
159163

160164
# ------------------------- COMMON FUSION-BACKPACK DEFINITIONS -----------------
@@ -166,6 +170,8 @@ build_flags =
166170
build_src_filter =
167171
${common_env_data.build_src_filter}
168172
-<Tx_main.cpp>
173+
-<Timer_main.cpp>
174+
-<Test_main.cpp>
169175
; -<Vrx_main.cpp>
170176
-<rapidfire.*>
171177
-<rx5808.*>
@@ -174,7 +180,6 @@ build_src_filter =
174180
-<hdzero.*>
175181
-<skyzone_msp.*>
176182
-<orqa.*>
177-
-<Timer_main.cpp>
178183
-<mfd_crossbow.*>
179184

180185
# ------------------------- COMMON HDZERO-BACKPACK DEFINITIONS -----------------
@@ -186,6 +191,8 @@ build_flags =
186191
build_src_filter =
187192
${common_env_data.build_src_filter}
188193
-<Tx_main.cpp>
194+
-<Timer_main.cpp>
195+
-<Test_main.cpp>
189196
; -<Vrx_main.cpp>
190197
-<rapidfire.*>
191198
-<rx5808.*>
@@ -194,7 +201,6 @@ build_src_filter =
194201
; -<hdzero.*>
195202
-<skyzone_msp.*>
196203
-<orqa.*>
197-
-<Timer_main.cpp>
198204
-<mfd_crossbow.*>
199205

200206
# ------------------------- COMMON SKYZONE-MSP-BACKPACK DEFINITIONS -----------------
@@ -206,6 +212,8 @@ build_flags =
206212
build_src_filter =
207213
${common_env_data.build_src_filter}
208214
-<Tx_main.cpp>
215+
-<Timer_main.cpp>
216+
-<Test_main.cpp>
209217
; -<Vrx_main.cpp>
210218
-<rapidfire.*>
211219
-<rx5808.*>
@@ -214,7 +222,6 @@ build_src_filter =
214222
-<hdzero.*>
215223
; -<skyzone_msp.*>
216224
-<orqa.*>
217-
-<Timer_main.cpp>
218225
-<mfd_crossbow.*>
219226

220227
# ------------------------- COMMON ORQA-BACKPACK DEFINITIONS -------------------
@@ -226,6 +233,8 @@ build_flags =
226233
build_src_filter =
227234
${common_env_data.build_src_filter}
228235
-<Tx_main.cpp>
236+
-<Timer_main.cpp>
237+
-<Test_main.cpp>
229238
; -<Vrx_main.cpp>
230239
-<rapidfire.*>
231240
-<rx5808.*>
@@ -234,7 +243,6 @@ build_src_filter =
234243
-<hdzero.*>
235244
-<skyzone_msp.*>
236245
; -<orqa.*>
237-
-<Timer_main.cpp>
238246
-<mfd_crossbow.*>
239247

240248
# ------------------------- COMMON TIMER-BACKPACK DEFINITIONS -----------------
@@ -245,6 +253,8 @@ build_flags =
245253
build_src_filter =
246254
${common_env_data.build_src_filter}
247255
-<Tx_main.cpp>
256+
; -<Timer_main.cpp>
257+
-<Test_main.cpp>
248258
-<Vrx_main.cpp>
249259
-<rapidfire.*>
250260
-<rx5808.*>
@@ -253,7 +263,6 @@ build_src_filter =
253263
-<hdzero.*>
254264
-<skyzone_msp.*>
255265
-<orqa.*>
256-
; -<Timer_main.cpp>
257266
-<mfd_crossbow.*>
258267

259268
# ------------------------- COMMON MFD-CROSSBOW-BACKPACK DEFINITIONS -----------------
@@ -265,6 +274,8 @@ build_flags =
265274
build_src_filter =
266275
${common_env_data.build_src_filter}
267276
-<Tx_main.cpp>
277+
-<Timer_main.cpp>
278+
-<Test_main.cpp>
268279
; -<Vrx_main.cpp>
269280
-<rapidfire.*>
270281
-<rx5808.*>
@@ -273,7 +284,6 @@ build_src_filter =
273284
-<hdzero.*>
274285
-<skyzone_msp.*>
275286
-<orqa.*>
276-
-<Timer_main.cpp>
277287
; -<mfd_crossbow.*>
278288
lib_deps =
279289
${env.lib_deps}

0 commit comments

Comments
 (0)