From 236f6706a30bf7c230947527eab61d64b0caff0d Mon Sep 17 00:00:00 2001 From: Curtis Bangert Date: Wed, 29 Apr 2020 19:41:16 -0400 Subject: [PATCH] CMS over CRSF compression --- src/main/cms/cms.c | 17 ++++-- src/main/io/displayport_crsf.c | 22 +++----- src/main/io/displayport_crsf.h | 4 +- src/main/telemetry/crsf.c | 95 +++++++++++++++++++++++++++++----- 4 files changed, 106 insertions(+), 32 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index da5ca4209..9db566bff 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -356,6 +356,17 @@ static void cmsPadToSize(char *buf, int size) #endif } +static int cmsDisplayWrite(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, const char *s) +{ + uint8_t *c = (uint8_t*)s; + const uint8_t *cEnd = c + strlen(s); + for (; c != cEnd; c++) { + *c = toupper(*c); // uppercase only + *c = (*c < 0x20 || *c > 0x5F) ? ' ' : *c; // limit to alphanumeric and punctuation + } + return displayWrite(instance, x, y, attr, s); +} + static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row, uint8_t maxSize) { int colpos; @@ -367,7 +378,7 @@ static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row #else colpos = smallScreen ? rightMenuColumn - maxSize : rightMenuColumn; #endif - cnt = displayWrite(pDisplay, colpos, row, DISPLAYPORT_ATTR_NONE, buff); + cnt = cmsDisplayWrite(pDisplay, colpos, row, DISPLAYPORT_ATTR_NONE, buff); return cnt; } @@ -516,7 +527,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t case OME_Label: if (IS_PRINTVALUE(*flags) && p->data) { // A label with optional string, immediately following text - cnt = displayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, DISPLAYPORT_ATTR_NONE, p->data); + cnt = cmsDisplayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, DISPLAYPORT_ATTR_NONE, p->data); CLR_PRINTVALUE(*flags); } break; @@ -672,7 +683,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) if (IS_PRINTLABEL(runtimeEntryFlags[i])) { uint8_t coloff = leftMenuColumn; coloff += (p->type == OME_Label) ? 0 : 1; - room -= displayWrite(pDisplay, coloff, top + i * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, p->text); + room -= cmsDisplayWrite(pDisplay, coloff, top + i * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, p->text); CLR_PRINTLABEL(runtimeEntryFlags[i]); if (room < 30) { return; diff --git a/src/main/io/displayport_crsf.c b/src/main/io/displayport_crsf.c index 3e26fbfea..7d405fc9a 100644 --- a/src/main/io/displayport_crsf.c +++ b/src/main/io/displayport_crsf.c @@ -50,7 +50,7 @@ static int crsfClearScreen(displayPort_t *displayPort) { UNUSED(displayPort); memset(crsfScreen.buffer, ' ', sizeof(crsfScreen.buffer)); - memset(crsfScreen.pendingTransport, 0, sizeof(crsfScreen.pendingTransport)); + crsfScreen.updated = false; crsfScreen.reset = true; delayTransportUntilMs = millis() + CRSF_DISPLAY_PORT_CLEAR_DELAY_MS; return 0; @@ -83,8 +83,8 @@ static int crsfWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, } const size_t truncLen = MIN((int)strlen(s), crsfScreen.cols-col); // truncate at colCount char *rowStart = &crsfScreen.buffer[row * crsfScreen.cols + col]; - crsfScreen.pendingTransport[row] = memcmp(rowStart, s, truncLen); - if (crsfScreen.pendingTransport[row]) { + crsfScreen.updated |= memcmp(rowStart, s, truncLen); + if (crsfScreen.updated) { memcpy(rowStart, s, truncLen); } return 0; @@ -183,23 +183,17 @@ void crsfDisplayPortRefresh(void) crsfDisplayPortMenuOpen(); return; } - memset(crsfScreen.pendingTransport, 1, crsfScreen.rows); + crsfScreen.updated = true; crsfScreen.reset = true; delayTransportUntilMs = millis() + CRSF_DISPLAY_PORT_CLEAR_DELAY_MS; } -int crsfDisplayPortNextRow(void) +bool crsfDisplayPortIsReady(void) { const timeMs_t currentTimeMs = millis(); - if (currentTimeMs < delayTransportUntilMs) { - return -1; - } - for(unsigned int i=0; i delayTransportUntilMs); + const bool cmsReady = (cmsInMenu && (pCurrentDisplay = &crsfDisplayPort)); + return (bool)(delayExpired && cmsReady); } displayPort_t *displayPortCrsfInit() diff --git a/src/main/io/displayport_crsf.h b/src/main/io/displayport_crsf.h index 9e2535f93..78cdab50e 100644 --- a/src/main/io/displayport_crsf.h +++ b/src/main/io/displayport_crsf.h @@ -28,7 +28,7 @@ typedef struct crsfDisplayPortScreen_s { char buffer[CRSF_DISPLAY_PORT_MAX_BUFFER_SIZE]; - bool pendingTransport[CRSF_DISPLAY_PORT_ROWS_MAX]; + bool updated; uint8_t rows; uint8_t cols; bool reset; @@ -39,5 +39,5 @@ crsfDisplayPortScreen_t *crsfDisplayPortScreen(void); void crsfDisplayPortMenuOpen(void); void crsfDisplayPortMenuExit(void); void crsfDisplayPortRefresh(void); -int crsfDisplayPortNextRow(void); +bool crsfDisplayPortIsReady(void); void crsfDisplayPortSetDimensions(uint8_t rows, uint8_t cols); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 9f21fad17..5d50dc3d4 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -337,20 +337,78 @@ void crsfFrameDeviceInfo(sbuf_t *dst) { } #if defined(USE_CRSF_CMS_TELEMETRY) +#define CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH 50 +#define CRSF_DISPLAYPORT_BATCH_MAX 0x3F +#define CRSF_DISPLAYPORT_FIRST_CHUNK_MASK 0x80 +#define CRSF_DISPLAYPORT_LAST_CHUNK_MASK 0x40 +#define CRSF_DISPLAYPORT_SANITIZE_MASK 0x60 +#define CRSF_RLE_CHAR_REPEATED_MASK 0x80 +#define CRSF_RLE_MAX_RUN_LENGTH 256 +#define CRSF_RLE_BATCH_SIZE 2 -static void crsfFrameDisplayPortRow(sbuf_t *dst, uint8_t row) +static uint16_t getRunLength(const void *start, const void *end) +{ + uint8_t *cursor = (uint8_t*)start; + uint8_t c = *cursor; + size_t runLength = 0; + for (; cursor != end; cursor++) { + if (*cursor == c) { + runLength++; + } else { + break; + } + } + return runLength; +} + +static void cRleEncodeStream(sbuf_t *source, sbuf_t *dest, uint8_t maxDestLen) +{ + const uint8_t *destEnd = sbufPtr(dest) + maxDestLen; + while (sbufBytesRemaining(source) && (sbufPtr(dest) < destEnd)) { + const uint8_t destRemaining = destEnd - sbufPtr(dest); + const uint8_t *srcPtr = sbufPtr(source); + const uint16_t runLength = getRunLength(srcPtr, source->end); + uint8_t c = *srcPtr; + if (runLength > 1) { + c |= CRSF_RLE_CHAR_REPEATED_MASK; + const uint8_t fullBatches = (runLength / CRSF_RLE_MAX_RUN_LENGTH); + const uint8_t remainder = (runLength % CRSF_RLE_MAX_RUN_LENGTH); + const uint8_t totalBatches = fullBatches + (remainder) ? 1 : 0; + if (destRemaining >= totalBatches * CRSF_RLE_BATCH_SIZE) { + for (unsigned int i=1; i<=totalBatches; i++) { + const uint8_t batchLength = (i < totalBatches) ? CRSF_RLE_MAX_RUN_LENGTH : remainder; + sbufWriteU8(dest, c); + sbufWriteU8(dest, batchLength); + } + sbufAdvance(source, runLength); + } else { + break; + } + } else if (destRemaining >= runLength) { + sbufWriteU8(dest, c); + sbufAdvance(source, runLength); + } + } +} + +static void crsfFrameDisplayPortChunk(sbuf_t *dst, sbuf_t *src, uint8_t batchId, uint8_t idx) { uint8_t *lengthPtr = sbufPtr(dst); - uint8_t buflen = crsfDisplayPortScreen()->cols; - char *rowStart = &crsfDisplayPortScreen()->buffer[row * buflen]; - const uint8_t frameLength = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + buflen; - sbufWriteU8(dst, frameLength); + sbufWriteU8(dst, 0); sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD); sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_UPDATE); - sbufWriteU8(dst, row); - sbufWriteData(dst, rowStart, buflen); + uint8_t *metaPtr = sbufPtr(dst); + sbufWriteU8(dst, batchId); + sbufWriteU8(dst, idx); + cRleEncodeStream(src, dst, CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH); + if (idx == 0) { + *metaPtr |= CRSF_DISPLAYPORT_FIRST_CHUNK_MASK; + } + if (!sbufBytesRemaining(src)) { + *metaPtr |= CRSF_DISPLAYPORT_LAST_CHUNK_MASK; + } *lengthPtr = sbufPtr(dst) - lengthPtr; } @@ -556,14 +614,25 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) crsfLastCycleTime = currentTimeUs; return; } - const int nextRow = crsfDisplayPortNextRow(); - if (nextRow >= 0) { + 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); sbuf_t crsfDisplayPortBuf; sbuf_t *dst = &crsfDisplayPortBuf; - crsfInitializeFrame(dst); - crsfFrameDisplayPortRow(dst, nextRow); - crsfFinalize(dst); - crsfDisplayPortScreen()->pendingTransport[nextRow] = false; + displayPortBatchId = (displayPortBatchId + 1) % CRSF_DISPLAYPORT_BATCH_MAX; + uint8_t i = 0; + while(sbufBytesRemaining(src)) { + crsfInitializeFrame(dst); + crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i); + crsfFinalize(dst); + crsfRxSendTelemetryData(); + i++; + } crsfLastCycleTime = currentTimeUs; return; }