Skip to content

Commit 575ef03

Browse files
phobos-phobos-
committed
Implemented ExpressLrs support
1 parent c84450e commit 575ef03

1 file changed

Lines changed: 196 additions & 3 deletions

File tree

‎src/protocol/crsf_uart.c‎

Lines changed: 196 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,115 @@
2929
#define CRSF_CHANNELS 16
3030
#define CRSF_PACKET_SIZE 26
3131

32+
static u8 currentPktRate = 0;
33+
static u8 currentTlmRatio = 0;
34+
static u8 currentPower = 0;
35+
static u8 currentBind = 0;
36+
static u8 currentWiFi = 0;
37+
38+
static const char * const expresslrs_opts_24[] = {
39+
_tr_noop("Bad Pkts"), "0", "1000", "1", NULL,
40+
_tr_noop("Good Pkts"), "0", "1000", "1", NULL,
41+
// 0 1 3 5 6
42+
_tr_noop("Pkt. Rate"), "500Hz", "250Hz", "150Hz", "50Hz", "25Hz", NULL,
43+
// 0 1 2 3 4 5 6 7
44+
_tr_noop("TLM Ratio"), "OFF", "1:128", "1:64", "1:32", "1:16", "1:8", "1:4", "1:2", NULL,
45+
// 0 1 2 3 4 5 6 7
46+
_tr_noop("Power"), "10mW", "25mW", "50mW", "100mW", "250mW", "500mW", "1000mW", "2000mW", NULL,
47+
// 0 1 2 3 4 5 6
48+
_tr_noop("Rf Freq"), "?", "915 AU", "915 FCC", "868 EU", "433 AU", "433 EU", "2.4G ISM", NULL,
49+
_tr_noop("Bind"), "OFF", "ON", NULL,
50+
_tr_noop("WiFi Update"), "OFF", "ON", NULL,
51+
NULL
52+
};
53+
54+
static const char * const expresslrs_opts_900[] = {
55+
_tr_noop("Bad Pkts"), "0", "1000", "1", NULL,
56+
_tr_noop("Good Pkts"), "0", "1000", "1", NULL,
57+
// 2 4 5 6
58+
_tr_noop("Pkt. Rate"), "200Hz", "100Hz", "50Hz", "25Hz", NULL,
59+
// 0 1 2 3 4 5 6 7
60+
_tr_noop("TLM Ratio"), "OFF", "1:128", "1:64", "1:32", "1:16", "1:8", "1:4", "1:2", NULL,
61+
// 0 1 2 3 4 5 6 7
62+
_tr_noop("Power"), "10mW", "25mW", "50mW", "100mW", "250mW", "500mW", "1000mW", "2000mW", NULL,
63+
// 0 1 2 3 4 5 6
64+
_tr_noop("Rf Freq"), "?", "915 AU", "915 FCC", "868 EU", "433 AU", "433 EU", "2.4G ISM", NULL,
65+
_tr_noop("Bind"), "OFF", "ON", NULL,
66+
_tr_noop("WiFi Update"), "OFF", "ON", NULL,
67+
NULL
68+
};
69+
70+
static u8 convertPktRateToElrs(u8 rfFreq, u8 rate)
71+
{
72+
switch (rate) {
73+
case 0:
74+
if (rfFreq == 6) return 0;
75+
return 2;
76+
case 1:
77+
if (rfFreq == 6) return 1;
78+
return 4;
79+
case 2:
80+
if (rfFreq == 6) return 3;
81+
return 5;
82+
case 3:
83+
if (rfFreq == 6) return 5;
84+
return 6;
85+
case 4: return 6;
86+
}
87+
return 6;
88+
}
89+
90+
static u8 convertElrsToPktRate(u8 rfFreq, u8 elrsRate)
91+
{
92+
switch (elrsRate) {
93+
case 0: return 0; // 2.4
94+
case 1: return 1; // 2.4
95+
case 2: return 0; // 900
96+
case 3: return 2; // 2.4
97+
case 4: return 1; // 900
98+
case 5:
99+
if (rfFreq == 6) return 3;
100+
return 2;
101+
case 6:
102+
if (rfFreq == 6) return 4;
103+
return 3;
104+
}
105+
return 0;
106+
}
107+
108+
static u16 convertPktRateToPeriod(u8 rfFreq, u8 rate)
109+
{
110+
if (rfFreq == 0) return CRSF_FRAME_PERIOD;
111+
switch (rate) {
112+
case 0:
113+
if (rfFreq == 6) return 2000;
114+
return 5000;
115+
case 1:
116+
if (rfFreq == 6) return 4000;
117+
return 10000;
118+
case 2:
119+
if (rfFreq == 6) return 6666;
120+
return 20000;
121+
case 3:
122+
if (rfFreq == 6) return 20000;
123+
return 40000;
124+
case 4: return 40000;
125+
}
126+
return CRSF_FRAME_PERIOD;
127+
}
128+
129+
enum {
130+
PROTO_OPTS_BAD_PKTS,
131+
PROTO_OPTS_GOOD_PKTS,
132+
PROTO_OPTS_PKT_RATE,
133+
PROTO_OPTS_TLM_RATIO,
134+
PROTO_OPTS_POWER,
135+
PROTO_OPTS_RF_FREQ,
136+
PROTO_OPTS_BIND,
137+
PROTO_OPTS_WIFI_UPDATE,
138+
LAST_PROTO_OPT,
139+
};
140+
ctassert(LAST_PROTO_OPT <= NUM_PROTO_OPTS, too_many_protocol_opts);
32141

33142
// crc implementation from CRSF protocol document rev7
34143
static u8 crsf_crc8tab[256] = {
@@ -163,6 +272,35 @@ static void processCrossfireTelemetryFrame()
163272
}
164273
}
165274

275+
static void processElrsParamsData()
276+
{
277+
if (telemetryRxBuffer[2] == TYPE_SETTINGS_WRITE && telemetryRxBuffer[3] == ADDR_RADIO && telemetryRxBuffer[4] == ADDR_MODULE && telemetryRxBuffer[5] == 0xFF) {
278+
if (telemetryRxBuffer[6] == 0x01) {
279+
currentBind = 1;
280+
} else {
281+
currentBind = 0;
282+
}
283+
284+
if (telemetryRxBuffer[6] == 0x02) {
285+
currentWiFi = 1;
286+
} else {
287+
currentWiFi = 0;
288+
}
289+
290+
currentPktRate = convertElrsToPktRate(telemetryRxBuffer[10], telemetryRxBuffer[7]);
291+
currentTlmRatio = telemetryRxBuffer[8];
292+
currentPower = telemetryRxBuffer[9];
293+
Model.proto_opts[PROTO_OPTS_BIND] = currentBind;
294+
Model.proto_opts[PROTO_OPTS_WIFI_UPDATE] = currentWiFi;
295+
Model.proto_opts[PROTO_OPTS_PKT_RATE] = currentPktRate;
296+
Model.proto_opts[PROTO_OPTS_TLM_RATIO] = currentTlmRatio;
297+
Model.proto_opts[PROTO_OPTS_POWER] = currentPower;
298+
Model.proto_opts[PROTO_OPTS_RF_FREQ] = telemetryRxBuffer[10];
299+
Model.proto_opts[PROTO_OPTS_BAD_PKTS] = telemetryRxBuffer[11];
300+
Model.proto_opts[PROTO_OPTS_GOOD_PKTS] = telemetryRxBuffer[12] * 256 + telemetryRxBuffer[13];
301+
}
302+
}
303+
166304
// serial data receive ISR callback
167305
static void processCrossfireTelemetryData(u8 data, u8 status) {
168306
(void)status;
@@ -188,6 +326,8 @@ static void processCrossfireTelemetryData(u8 data, u8 status) {
188326
if (telemetryRxBuffer[2] < TYPE_PING_DEVICES) {
189327
processCrossfireTelemetryFrame(); // Broadcast frame
190328
#if SUPPORT_CRSF_CONFIG
329+
} else if (telemetryRxBuffer[2] == TYPE_SETTINGS_WRITE) {
330+
processElrsParamsData();
191331
} else {
192332
CRSF_serial_rcv(telemetryRxBuffer+2, telemetryRxBuffer[1]-1); // Extended frame
193333
#endif
@@ -200,7 +340,48 @@ static void processCrossfireTelemetryData(u8 data, u8 status) {
200340

201341
static u8 packet[CRSF_PACKET_SIZE];
202342

343+
#define ELRS_ADDRESS 0xEE
344+
#define ELRS_BIND_COMMAND 0xFF
345+
#define ELRS_WIFI_COMMAND 0xFE
346+
#define ELRS_PKT_RATE_COMMAND 1
347+
#define ELRS_TLM_RATIO_COMMAND 2
348+
#define ELRS_POWER_COMMAND 3
203349

350+
static u8 buildElrsPacket(u8 command, u8 value)
351+
{
352+
packet[0] = ADDR_MODULE;
353+
packet[1] = 6;
354+
packet[2] = TYPE_SETTINGS_WRITE;
355+
packet[3] = ELRS_ADDRESS;
356+
packet[4] = ADDR_RADIO;
357+
packet[5] = command;
358+
packet[6] = value;
359+
packet[7] = crsf_crc8(&packet[2], packet[1]-1);
360+
361+
return 8;
362+
}
363+
364+
static u8 setElrsOptions()
365+
{
366+
if (Model.proto_opts[PROTO_OPTS_BIND] == 1 && currentBind == 0) {
367+
currentBind = Model.proto_opts[PROTO_OPTS_BIND];
368+
return buildElrsPacket(ELRS_BIND_COMMAND, 0x01);
369+
}
370+
if (Model.proto_opts[PROTO_OPTS_WIFI_UPDATE] == 1 && currentWiFi == 0) {
371+
currentWiFi = Model.proto_opts[PROTO_OPTS_WIFI_UPDATE];
372+
return buildElrsPacket(ELRS_WIFI_COMMAND, 0x01);
373+
}
374+
if (Model.proto_opts[PROTO_OPTS_PKT_RATE] != currentPktRate) {
375+
return buildElrsPacket(ELRS_PKT_RATE_COMMAND, convertPktRateToElrs(Model.proto_opts[PROTO_OPTS_RF_FREQ], Model.proto_opts[PROTO_OPTS_PKT_RATE]));
376+
}
377+
if (Model.proto_opts[PROTO_OPTS_TLM_RATIO] != currentTlmRatio) {
378+
return buildElrsPacket(ELRS_TLM_RATIO_COMMAND, Model.proto_opts[PROTO_OPTS_TLM_RATIO]);
379+
}
380+
if (Model.proto_opts[PROTO_OPTS_POWER] != currentPower) {
381+
return buildElrsPacket(ELRS_POWER_COMMAND, Model.proto_opts[PROTO_OPTS_POWER]);
382+
}
383+
return 0;
384+
}
204385

205386
/* from CRSF document
206387
Center (1500us) = 992
@@ -276,7 +457,11 @@ static u16 serial_cb()
276457
case ST_DATA2:
277458
if (mixer_sync != MIX_DONE && mixer_runtime < 2000) mixer_runtime += 50;
278459
#if SUPPORT_CRSF_CONFIG
279-
length = CRSF_serial_txd(packet, sizeof packet);
460+
if (Model.proto_opts[PROTO_OPTS_RF_FREQ] == 0) {
461+
length = CRSF_serial_txd(packet, sizeof packet);
462+
} else {
463+
length = setElrsOptions();
464+
}
280465
if (length == 0) {
281466
length = build_rcdata_pkt();
282467
}
@@ -286,7 +471,7 @@ static u16 serial_cb()
286471
UART_Send(packet, length);
287472
state = ST_DATA1;
288473

289-
return CRSF_FRAME_PERIOD - mixer_runtime;
474+
return convertPktRateToPeriod(Model.proto_opts[PROTO_OPTS_RF_FREQ], currentPktRate) - mixer_runtime;
290475
}
291476

292477
return CRSF_FRAME_PERIOD; // avoid compiler warning
@@ -328,7 +513,15 @@ uintptr_t CRSF_Cmds(enum ProtoCmds cmd)
328513
case PROTOCMD_DEFAULT_NUMCHAN: return 8;
329514
case PROTOCMD_CHANNELMAP: return UNCHG;
330515
#if SUPPORT_CRSF_CONFIG
331-
case PROTOCMD_OPTIONSPAGE: return PAGEID_CRSFCFG;
516+
case PROTOCMD_OPTIONSPAGE:
517+
if (Model.proto_opts[PROTO_OPTS_RF_FREQ] == 0) return PAGEID_CRSFCFG;
518+
break;
519+
case PROTOCMD_GETOPTIONS:
520+
if (Model.proto_opts[PROTO_OPTS_RF_FREQ] == 6)
521+
return (uintptr_t)expresslrs_opts_24;
522+
else if (Model.proto_opts[PROTO_OPTS_RF_FREQ] > 0)
523+
return (uintptr_t)expresslrs_opts_900;
524+
break;
332525
#endif // SUPPORT_CRSF_CONFIG
333526
#if HAS_EXTENDED_TELEMETRY
334527
case PROTOCMD_TELEMETRYSTATE:

0 commit comments

Comments
 (0)