From a7e4bf41eb0cc2ab6be0ebd8ec583a38f63787ac Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Tue, 21 Feb 2023 16:10:56 +0000 Subject: [PATCH] Add CMS support on TX for ELRS (#12308) * Never block use of SWD pins * Throttle CMS displayport traffic on CRSF to prevent ELRS overrun --- src/main/rx/crsf.c | 3 + src/main/rx/crsf_protocol.h | 1 + src/main/telemetry/crsf.c | 107 +++++++++++++++++++++++++++++++----- src/main/telemetry/crsf.h | 1 + 4 files changed, 97 insertions(+), 15 deletions(-) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 36aaf43fc5..66b4525e7a 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -411,6 +411,9 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) case CRSF_FRAMETYPE_DEVICE_PING: crsfScheduleDeviceInfoResponse(); break; + case CRSF_FRAMETYPE_DEVICE_INFO: + crsfHandleDeviceInfoResponse(crsfFrame.frame.payload); + break; case CRSF_FRAMETYPE_DISPLAYPORT_CMD: { uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; crsfProcessDisplayPortCmd(frameStart); diff --git a/src/main/rx/crsf_protocol.h b/src/main/rx/crsf_protocol.h index 99fef0d1a1..71a6bf50d3 100644 --- a/src/main/rx/crsf_protocol.h +++ b/src/main/rx/crsf_protocol.h @@ -89,6 +89,7 @@ enum { CRSF_FRAME_LINK_STATISTICS_TX_PAYLOAD_SIZE = 6, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, + CRSF_FRAME_DEVICE_PING_PAYLOAD_SIZE = 2, }; enum { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 88868ce9c9..c69644056a 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -93,6 +93,20 @@ static mspBuffer_t mspRxBuffer; #define CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US 20000 // 20ms +#if defined(USE_CRSF_CMS_TELEMETRY) +#define CRSF_LINK_TYPE_CHECK_US 250000 // 250 ms +#define CRSF_ELRS_DISLAYPORT_CHUNK_INTERVAL_US 75000 // 75 ms + +typedef enum { + CRSF_LINK_UNKNOWN, + CRSF_LINK_ELRS, + CRSF_LINK_NOT_ELRS +} crsfLinkType_t; + +static crsfLinkType_t crsfLinkType = CRSF_LINK_UNKNOWN; +static timeDelta_t crsfDisplayPortChunkIntervalUs = 0; +#endif + static bool isCrsfV3Running = false; typedef struct { uint8_t hasPendingReply:1; @@ -282,6 +296,20 @@ void crsfFrameHeartbeat(sbuf_t *dst) sbufWriteU16BigEndian(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); } +/* +0x28 Ping +Payload: +int8_t destination_add ( Destination Device address ) +int8_t origin_add ( Origin Device address ) +*/ +void crsfFramePing(sbuf_t *dst) +{ + sbufWriteU8(dst, CRSF_FRAME_DEVICE_PING_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_PING); + sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); + sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); +} + typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -482,6 +510,23 @@ void speedNegotiationProcess(timeUs_t currentTimeUs) crsfRxSendTelemetryData(); // prevent overwriting previous data crsfFinalize(dst); crsfRxSendTelemetryData(); +#if defined(USE_CRSF_CMS_TELEMETRY) + } else if (crsfLinkType == CRSF_LINK_UNKNOWN) { + static timeUs_t lastPing; + + if ((cmpTimeUs(currentTimeUs, lastPing) > CRSF_LINK_TYPE_CHECK_US)) { + // Send a ping, the response to which will be a device info response giving the rx serial number + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + crsfInitializeFrame(dst); + crsfFramePing(dst); + crsfRxSendTelemetryData(); // prevent overwriting previous data + crsfFinalize(dst); + crsfRxSendTelemetryData(); + + lastPing = currentTimeUs; + } +#endif } } #endif @@ -669,6 +714,25 @@ void crsfScheduleDeviceInfoResponse(void) deviceInfoReplyPending = true; } +#if defined(USE_CRSF_CMS_TELEMETRY) +void crsfHandleDeviceInfoResponse(uint8_t *payload) +{ + // Skip over dst/src address bytes + payload += 2; + + // Skip over first string which is the rx model/part number + while (*payload++ != '\0'); + + // Check the serial number + if (memcmp(payload, "ELRS", 4) == 0) { + crsfLinkType = CRSF_LINK_ELRS; + crsfDisplayPortChunkIntervalUs = CRSF_ELRS_DISLAYPORT_CHUNK_INTERVAL_US; + } else { + crsfLinkType = CRSF_LINK_NOT_ELRS; + } +} +#endif + void initCrsfTelemetry(void) { // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) @@ -821,27 +885,40 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) crsfLastCycleTime = currentTimeUs; return; } - static uint8_t displayPortBatchId = 0; - if (crsfDisplayPortIsReady() && crsfDisplayPortScreen()->updated) { - crsfDisplayPortScreen()->updated = false; - uint16_t screenSize = crsfDisplayPortScreen()->rows * crsfDisplayPortScreen()->cols; - uint8_t *srcStart = (uint8_t*)crsfDisplayPortScreen()->buffer; - uint8_t *srcEnd = (uint8_t*)(crsfDisplayPortScreen()->buffer + screenSize); - sbuf_t displayPortSbuf; - sbuf_t *src = sbufInit(&displayPortSbuf, srcStart, srcEnd); + + if (crsfDisplayPortIsReady()) { + static uint8_t displayPortBatchId = 0; + static sbuf_t displayPortSbuf; + static sbuf_t *src = NULL; + static uint8_t batchIndex; + static timeUs_t batchLastTimeUs; sbuf_t crsfDisplayPortBuf; sbuf_t *dst = &crsfDisplayPortBuf; - displayPortBatchId = (displayPortBatchId + 1) % CRSF_DISPLAYPORT_BATCH_MAX; - uint8_t i = 0; - while (sbufBytesRemaining(src)) { + + if (crsfDisplayPortScreen()->updated) { + crsfDisplayPortScreen()->updated = false; + uint16_t screenSize = crsfDisplayPortScreen()->rows * crsfDisplayPortScreen()->cols; + uint8_t *srcStart = (uint8_t*)crsfDisplayPortScreen()->buffer; + uint8_t *srcEnd = (uint8_t*)(crsfDisplayPortScreen()->buffer + screenSize); + src = sbufInit(&displayPortSbuf, srcStart, srcEnd); + displayPortBatchId = (displayPortBatchId + 1) % CRSF_DISPLAYPORT_BATCH_MAX; + batchIndex = 0; + } + + // Wait between successive chunks of displayport data for CMS menu display to prevent ELRS buffer over-run if necessary + if (src && sbufBytesRemaining(src) && + (cmpTimeUs(currentTimeUs, batchLastTimeUs) > crsfDisplayPortChunkIntervalUs)) { crsfInitializeFrame(dst); - crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i); + crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, batchIndex); crsfFinalize(dst); crsfRxSendTelemetryData(); - i++; + batchIndex++; + batchLastTimeUs = currentTimeUs; + + crsfLastCycleTime = currentTimeUs; + + return; } - crsfLastCycleTime = currentTimeUs; - return; } #endif diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index bd33bee3f5..a026269111 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -35,6 +35,7 @@ bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); void crsfScheduleDeviceInfoResponse(void); void crsfScheduleMspResponse(uint8_t requestOriginID); +void crsfHandleDeviceInfoResponse(uint8_t *payload); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); void crsfProcessCommand(uint8_t *frameStart); #if defined(USE_CRSF_CMS_TELEMETRY)