rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #9746 from codecae/crsf_cms_compression 

CMS over CRSF compression
This commit is contained in:
Michael Keller 2020-06-22 00:31:21 +12:00 committed by GitHub
parent 312787e163 236f6706a3
commit 944eb989d5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 106 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/main/cms/cms.c
View File

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

22
src/main/io/displayport_crsf.c
View File

@ -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<CRSF_DISPLAY_PORT_ROWS_MAX; i++) {
if (crsfScreen.pendingTransport[i]) {
return i;
}
}
return -1;
const bool delayExpired = (currentTimeMs > delayTransportUntilMs);
const bool cmsReady = (cmsInMenu && (pCurrentDisplay = &crsfDisplayPort));
return (bool)(delayExpired && cmsReady);
}
displayPort_t *displayPortCrsfInit()

4
src/main/io/displayport_crsf.h
View File

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

95
src/main/telemetry/crsf.c
View File

@ -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;
}