Skip to content

Commit 29db416

Browse files
xznhj8129xznhj8129
committed
Fix dual RX telemetry and link state isolation
1 parent c88d5a2 commit 29db416

12 files changed

Lines changed: 324 additions & 181 deletions

File tree

‎src/main/fc/fc_mavlink.c‎

Lines changed: 32 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -184,27 +184,32 @@ static uint16_t mavlinkDbmToMilliwatts(int8_t powerDbm)
184184
return powerDbm <= 0 ? 0 : lrintf(powf(10.0f, powerDbm / 10.0f));
185185
}
186186

187-
static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) {
187+
static void mavlinkParseRxStats(const mavlink_radio_status_t *msg)
188+
{
189+
rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY);
190+
188191
switch(mavActiveConfig->radio_type) {
189192
case MAVLINK_RADIO_SIK:
190-
rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127;
191-
rxLinkStatistics.uplinkSNR = msg->noise / 1.9;
192-
rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
193+
statistics->uplinkRSSI = (msg->rssi / 1.9) - 127;
194+
statistics->uplinkSNR = msg->noise / 1.9;
195+
statistics->uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
193196
break;
194197
case MAVLINK_RADIO_ELRS:
195-
rxLinkStatistics.uplinkRSSI = -msg->remrssi;
196-
rxLinkStatistics.uplinkSNR = msg->noise;
197-
rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100);
198+
statistics->uplinkRSSI = -msg->remrssi;
199+
statistics->uplinkSNR = msg->noise;
200+
statistics->uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100);
198201
break;
199202
case MAVLINK_RADIO_MLRS:
200203
break;
201204
case MAVLINK_RADIO_GENERIC:
202205
default:
203-
rxLinkStatistics.uplinkRSSI = msg->rssi;
204-
rxLinkStatistics.uplinkSNR = msg->noise;
205-
rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
206+
statistics->uplinkRSSI = msg->rssi;
207+
statistics->uplinkSNR = msg->noise;
208+
statistics->uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
206209
break;
207210
}
211+
212+
rxLinkStatisticsUpdated(RX_LINK_PRIMARY);
208213
}
209214

210215
static bool handleIncoming_RADIO_STATUS(void) {
@@ -269,11 +274,13 @@ static bool handleIncoming_MLRS_RADIO_LINK_STATS(uint8_t ingressPortIndex)
269274
return true;
270275
}
271276

272-
rxLinkStatistics.uplinkLQ = stats->rxLinkQualityRc;
273-
rxLinkStatistics.downlinkLQ = stats->rxLinkQualitySerial;
274-
rxLinkStatistics.uplinkRSSI = stats->rxRssi;
275-
rxLinkStatistics.uplinkSNR = stats->rxSnr;
276-
rxLinkStatistics.activeAntenna = stats->activeAntenna;
277+
rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY);
278+
statistics->uplinkLQ = stats->rxLinkQualityRc;
279+
statistics->downlinkLQ = stats->rxLinkQualitySerial;
280+
statistics->uplinkRSSI = stats->rxRssi;
281+
statistics->uplinkSNR = stats->rxSnr;
282+
statistics->activeAntenna = stats->activeAntenna;
283+
rxLinkStatisticsUpdated(RX_LINK_PRIMARY);
277284

278285
return true;
279286
}
@@ -310,14 +317,16 @@ static bool handleIncoming_MLRS_RADIO_LINK_INFORMATION(uint8_t ingressPortIndex)
310317
return true;
311318
}
312319

313-
rxLinkStatistics.uplinkTXPower = info->txPowerMw;
314-
rxLinkStatistics.downlinkTXPower = info->rxPowerMw;
315-
memset(rxLinkStatistics.band, 0, sizeof(rxLinkStatistics.band));
316-
memset(rxLinkStatistics.mode, 0, sizeof(rxLinkStatistics.mode));
317-
memcpy(rxLinkStatistics.band, info->bandStr, sizeof(rxLinkStatistics.band) - 1);
318-
memcpy(rxLinkStatistics.mode, info->modeStr, sizeof(rxLinkStatistics.mode) - 1);
319-
sl_toupperptr(rxLinkStatistics.band);
320-
sl_toupperptr(rxLinkStatistics.mode);
320+
rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY);
321+
statistics->uplinkTXPower = info->txPowerMw;
322+
statistics->downlinkTXPower = info->rxPowerMw;
323+
memset(statistics->band, 0, sizeof(statistics->band));
324+
memset(statistics->mode, 0, sizeof(statistics->mode));
325+
memcpy(statistics->band, info->bandStr, sizeof(statistics->band) - 1);
326+
memcpy(statistics->mode, info->modeStr, sizeof(statistics->mode) - 1);
327+
sl_toupperptr(statistics->band);
328+
sl_toupperptr(statistics->mode);
329+
rxLinkStatisticsUpdated(RX_LINK_PRIMARY);
321330

322331
return true;
323332
}

‎src/main/fc/fc_msp.c‎

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -3362,11 +3362,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
33623362
uint8_t sublinkID = sbufReadU8(src); // Sublink ID
33633363
sbufReadU8(src); // Valid link (Failsafe backup)
33643364
if (sublinkID == 0) {
3365+
rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY);
33653366
setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI %
3366-
rxLinkStatistics.uplinkRSSI = -sbufReadU8(src);
3367-
rxLinkStatistics.downlinkLQ = sbufReadU8(src);
3368-
rxLinkStatistics.uplinkLQ = sbufReadU8(src);
3369-
rxLinkStatistics.uplinkSNR = sbufReadI8(src);
3367+
statistics->uplinkRSSI = -sbufReadU8(src);
3368+
statistics->downlinkLQ = sbufReadU8(src);
3369+
statistics->uplinkLQ = sbufReadU8(src);
3370+
statistics->uplinkSNR = sbufReadI8(src);
3371+
rxLinkStatisticsUpdated(RX_LINK_PRIMARY);
33703372
}
33713373

33723374
return MSP_RESULT_NO_REPLY;
@@ -3380,20 +3382,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
33803382
uint8_t sublinkID = sbufReadU8(src);
33813383

33823384
if (sublinkID == 0) {
3383-
rxLinkStatistics.uplinkTXPower = sbufReadU16(src);
3384-
rxLinkStatistics.downlinkTXPower = sbufReadU16(src);
3385+
rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY);
3386+
statistics->uplinkTXPower = sbufReadU16(src);
3387+
statistics->downlinkTXPower = sbufReadU16(src);
33853388

33863389
for (int i = 0; i < 4; i++) {
3387-
rxLinkStatistics.band[i] = sbufReadU8(src);
3390+
statistics->band[i] = sbufReadU8(src);
33883391
}
33893392

3390-
sl_toupperptr(rxLinkStatistics.band);
3393+
sl_toupperptr(statistics->band);
33913394

33923395
for (int i = 0; i < 6; i++) {
3393-
rxLinkStatistics.mode[i] = sbufReadU8(src);
3396+
statistics->mode[i] = sbufReadU8(src);
33943397
}
33953398

3396-
sl_toupperptr(rxLinkStatistics.mode);
3399+
sl_toupperptr(statistics->mode);
3400+
rxLinkStatisticsUpdated(RX_LINK_PRIMARY);
33973401
}
33983402

33993403
return MSP_RESULT_NO_REPLY;

‎src/main/rx/crsf.c‎

Lines changed: 52 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -55,17 +55,17 @@ typedef struct crsfLinkState_s {
5555
uint32_t channelData[CRSF_MAX_CHANNEL];
5656
timeUs_t frameStartAtUs;
5757
uint8_t framePosition;
58+
serialPort_t *port;
59+
uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
60+
uint8_t telemetryBufLen;
61+
rxLink_e link;
5862
} crsfLinkState_t;
5963

6064
// Per-link parser and channel state so a primary and secondary CRSF receiver
6165
// don't share frame/channel buffers. Primary is index 0, so the single-RX path
6266
// is unchanged.
6367
static crsfLinkState_t crsfLinkStates[RX_LINK_COUNT];
6468

65-
static serialPort_t *serialPort;
66-
static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
67-
static uint8_t telemetryBufLen = 0;
68-
6969
const uint16_t crsfTxPowerStatesmW[CRSF_POWER_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50};
7070

7171
/*
@@ -186,8 +186,8 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
186186
case CRSF_FRAMETYPE_MSP_WRITE: {
187187
if (link->frame.frame.frameLength >= 4) {
188188
uint8_t *frameStart = (uint8_t *)&link->frame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
189-
if (bufferCrsfMspFrame(frameStart, link->frame.frame.frameLength - 4)) {
190-
crsfScheduleMspResponse(link->frame.frame.payload[1]);
189+
if (bufferCrsfMspFrame(link->link, frameStart, link->frame.frame.frameLength - 4)) {
190+
crsfScheduleMspResponse(link->link, link->frame.frame.payload[1]);
191191
}
192192
} else {
193193
link->frameDone = false;
@@ -295,33 +295,60 @@ STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConf
295295

296296
void crsfRxWriteTelemetryData(const void *data, int len)
297297
{
298-
len = MIN(len, (int)sizeof(telemetryBuf));
299-
memcpy(telemetryBuf, data, len);
300-
telemetryBufLen = len;
298+
for (rxLink_e link = RX_LINK_PRIMARY; link < RX_LINK_COUNT; link++) {
299+
if (crsfLinkStates[link].port) {
300+
crsfRxWriteTelemetryDataForLink(link, data, len);
301+
}
302+
}
301303
}
302304

303-
void crsfRxSendTelemetryData(void)
305+
void crsfRxWriteTelemetryDataForLink(rxLink_e linkIndex, const void *data, int len)
304306
{
305-
// if there is telemetry data to write
306-
if (telemetryBufLen > 0) {
307+
crsfLinkState_t *link = &crsfLinkStates[linkIndex];
308+
len = MIN(len, (int)sizeof(link->telemetryBuf));
309+
memcpy(link->telemetryBuf, data, len);
310+
link->telemetryBufLen = len;
311+
}
312+
313+
static void crsfRxSendTelemetryDataForLink(crsfLinkState_t *link)
314+
{
315+
if (link->telemetryBufLen > 0) {
307316
// check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame)
308317
// and that there is time to send the telemetry frame before the next RX frame arrives
309318
if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) {
310-
// Telemetry shares the primary link's port, so pace against its frame timing.
311-
const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), crsfLinkStates[RX_LINK_PRIMARY].frameStartAtUs);
319+
const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), link->frameStartAtUs);
312320
if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) ||
313321
(timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) {
314322
return;
315323
}
316324
}
317-
serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
318-
telemetryBufLen = 0; // reset telemetry buffer
325+
serialWriteBuf(link->port, link->telemetryBuf, link->telemetryBufLen);
326+
link->telemetryBufLen = 0;
319327
}
320328
}
321329

322-
bool crsfRxIsTelemetryBufEmpty(void)
330+
void crsfRxSendTelemetryData(void)
323331
{
324-
return telemetryBufLen == 0;
332+
for (rxLink_e link = RX_LINK_PRIMARY; link < RX_LINK_COUNT; link++) {
333+
if (crsfLinkStates[link].port) {
334+
crsfRxSendTelemetryDataForLink(&crsfLinkStates[link]);
335+
}
336+
}
337+
}
338+
339+
bool crsfRxIsTelemetryBufEmpty(rxLink_e link)
340+
{
341+
return crsfLinkStates[link].telemetryBufLen == 0;
342+
}
343+
344+
bool crsfRxAreTelemetryBufsEmpty(void)
345+
{
346+
for (rxLink_e link = RX_LINK_PRIMARY; link < RX_LINK_COUNT; link++) {
347+
if (crsfLinkStates[link].port && !crsfRxIsTelemetryBufEmpty(link)) {
348+
return false;
349+
}
350+
}
351+
return true;
325352
}
326353

327354
bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction)
@@ -331,6 +358,8 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig,
331358

332359
link->frameDone = false;
333360
link->framePosition = 0;
361+
link->telemetryBufLen = 0;
362+
link->link = linkIndex;
334363
for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
335364
link->channelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408;
336365
}
@@ -345,7 +374,7 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig,
345374
return false;
346375
}
347376

348-
serialPort_t *port = openSerialPort(portConfig->identifier,
377+
link->port = openSerialPort(portConfig->identifier,
349378
portFunction,
350379
crsfDataReceive,
351380
link,
@@ -354,23 +383,18 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig,
354383
CRSF_PORT_OPTIONS | (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
355384
);
356385

357-
// Telemetry replies go out the primary link's port.
358-
if (linkIndex == RX_LINK_PRIMARY) {
359-
serialPort = port;
360-
}
361-
362-
return port != NULL;
386+
return link->port != NULL;
363387
}
364388

365389
bool crsfRxIsActive(void)
366390
{
367-
return serialPort != NULL;
391+
return crsfLinkStates[RX_LINK_PRIMARY].port || crsfLinkStates[RX_LINK_SECONDARY].port;
368392
}
369393

370394

371395
void crsfBind(void)
372396
{
373-
if (serialPort != NULL) {
397+
if (crsfLinkStates[RX_LINK_PRIMARY].port) {
374398
uint8_t bindFrame[] = {
375399
CRSF_SYNC_BYTE,
376400
0x07, // frame length
@@ -382,7 +406,7 @@ void crsfBind(void)
382406
0x9E, // Command CRC8
383407
0xE8, // Packet CRC8
384408
};
385-
serialWriteBuf(serialPort, bindFrame, 9);
409+
serialWriteBuf(crsfLinkStates[RX_LINK_PRIMARY].port, bindFrame, 9);
386410
}
387411
}
388412

‎src/main/rx/crsf.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -135,8 +135,10 @@ typedef union crsfFrame_u {
135135

136136

137137
void crsfRxWriteTelemetryData(const void *data, int len);
138+
void crsfRxWriteTelemetryDataForLink(rxLink_e link, const void *data, int len);
138139
void crsfRxSendTelemetryData(void);
139-
bool crsfRxIsTelemetryBufEmpty(void);
140+
bool crsfRxIsTelemetryBufEmpty(rxLink_e link);
141+
bool crsfRxAreTelemetryBufsEmpty(void);
140142

141143
struct rxConfig_s;
142144
struct rxRuntimeConfig_s;

‎src/main/rx/rx.c‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -396,6 +396,10 @@ void rxInit(void)
396396
if (rxConfig()->receiverType == RX_TYPE_MSP || rxConfig()->receiverTypeSecondary == RX_TYPE_MSP) {
397397
dualRxEnabled = false;
398398
}
399+
if ((rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK) ||
400+
(rxConfig()->receiverTypeSecondary == RX_TYPE_SERIAL && rxConfig()->serialrx_provider_secondary == SERIALRX_MAVLINK)) {
401+
dualRxEnabled = false;
402+
}
399403
// Two links sharing a not-yet-instance-safe serial driver would alias its module
400404
// parser buffers and corrupt both. Refuse dual RX for that combination; links on
401405
// different drivers are unaffected.
@@ -554,6 +558,22 @@ bool rxIsPrimaryFailsafe(void)
554558
return !rxIsLinkReceivingSignal(RX_LINK_PRIMARY);
555559
}
556560

561+
rxLinkStatistics_t *rxGetLinkStatisticsMutable(rxLink_e link)
562+
{
563+
if ((unsigned)link >= RX_LINK_COUNT) {
564+
return NULL;
565+
}
566+
567+
return &rxLinks[link].statistics;
568+
}
569+
570+
void rxLinkStatisticsUpdated(rxLink_e link)
571+
{
572+
if (link == activeLink) {
573+
rxLinkStatistics = rxLinks[link].statistics;
574+
}
575+
}
576+
557577
static void rxApplyActiveLink(rxLink_e link)
558578
{
559579
activeLink = link;

‎src/main/rx/rx.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -251,6 +251,8 @@ rxLink_e rxGetActiveLink(void);
251251
void rxSetActiveLink(rxLink_e link);
252252
bool rxIsLinkReceivingSignal(rxLink_e link);
253253
bool rxIsPrimaryFailsafe(void);
254+
rxLinkStatistics_t *rxGetLinkStatisticsMutable(rxLink_e link);
255+
void rxLinkStatisticsUpdated(rxLink_e link);
254256
#ifdef UNIT_TEST
255257
rxRuntimeConfig_t *rxTestRuntimeConfig(rxLink_e link);
256258
#endif

0 commit comments

Comments
 (0)