Merge pull request #9746 from codecae/crsf_cms_compression

CMS over CRSF compression

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;

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);
+    crsfScreen.updated |= memcmp(rowStart, s, truncLen);
-    if (crsfScreen.pendingTransport[row]) {
+    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) {
+    const bool delayExpired = (currentTimeMs > delayTransportUntilMs);
-        return -1;
+    const bool cmsReady = (cmsInMenu && (pCurrentDisplay = &crsfDisplayPort));
-    }
+    return (bool)(delayExpired && cmsReady);
-    for(unsigned int i=0; i<CRSF_DISPLAY_PORT_ROWS_MAX; i++) {
-        if (crsfScreen.pendingTransport[i]) {
-            return i;
-        }
-    }
-    return -1;
 }
 
 displayPort_t *displayPortCrsfInit()

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);

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;
+    sbufWriteU8(dst, 0);
-    char *rowStart = &crsfDisplayPortScreen()->buffer[row * buflen];
-    const uint8_t frameLength = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + buflen;
-    sbufWriteU8(dst, frameLength);
     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);
+    uint8_t *metaPtr = sbufPtr(dst);
-    sbufWriteData(dst, rowStart, buflen);
+    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();
+    static uint8_t displayPortBatchId = 0;
-    if (nextRow >= 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);
+        displayPortBatchId = (displayPortBatchId  + 1) % CRSF_DISPLAYPORT_BATCH_MAX;
-        crsfFrameDisplayPortRow(dst, nextRow);
+        uint8_t i = 0;
-        crsfFinalize(dst);
+        while(sbufBytesRemaining(src)) {
-        crsfDisplayPortScreen()->pendingTransport[nextRow] = false;
+            crsfInitializeFrame(dst);
+            crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i);
+            crsfFinalize(dst);
+            crsfRxSendTelemetryData();
+            i++;
+        }
         crsfLastCycleTime = currentTimeUs;
         return;
     }