CMS Telemetry over CRSF w/ Lua Script (X9D, so far)

make/source.mk

@@ -170,6 +170,7 @@ FC_SRC = \
             io/displayport_oled.c \
             io/displayport_rcdevice.c \
             io/displayport_srxl.c \
+            io/displayport_crsf.c \
             io/rcdevice_cam.c \
             io/rcdevice.c \
             io/rcdevice_osd.c \

src/main/cms/cms.c

@@ -108,6 +108,19 @@ static displayPort_t *cmsDisplayPortSelectNext(void)
     return cmsDisplayPorts[cmsCurrentDevice];
 }
 
+bool cmsDisplayPortSelect(displayPort_t *instance)
+{
+    if (cmsDeviceCount == 0) {
+        return false;
+    }
+    for (int i = 0; i < cmsDeviceCount; i++) {
+        if (cmsDisplayPortSelectNext() == instance) {
+            return true;
+        }
+    }
+    return false;
+}
+
 #define CMS_UPDATE_INTERVAL_US  50000   // Interval of key scans (microsec)
 #define CMS_POLL_INTERVAL_US   100000   // Interval of polling dynamic values (microsec)
 
@@ -638,7 +651,7 @@ STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay)
     return 0;
 }
 
-STATIC_UNIT_TESTED void cmsMenuOpen(void)
+void cmsMenuOpen(void)
 {
     if (!cmsInMenu) {
         // New open

src/main/cms/cms.h

@@ -14,6 +14,8 @@ displayPort_t *pCurrentDisplay;
 void cmsInit(void);
 void cmsHandler(timeUs_t currentTimeUs);
 
+bool cmsDisplayPortSelect(displayPort_t *instance);
+void cmsMenuOpen(void);
 long cmsMenuChange(displayPort_t *pPort, const void *ptr);
 long cmsMenuExit(displayPort_t *pPort, const void *ptr);
 void cmsUpdate(uint32_t currentTimeUs);

src/main/drivers/display.h

@@ -58,6 +58,8 @@ typedef struct displayPortProfile_s {
     uint8_t whiteBrightness;
 } displayPortProfile_t;
 
+// Note: displayPortProfile_t used as a parameter group for CMS over CRSF (io/displayport_crsf)
+
 void displayGrab(displayPort_t *instance);
 void displayRelease(displayPort_t *instance);
 void displayReleaseAll(displayPort_t *instance);

src/main/fc/fc_init.c

@@ -106,6 +106,7 @@
 #include "io/displayport_max7456.h"
 #include "io/displayport_rcdevice.h"
 #include "io/displayport_srxl.h"
+#include "io/displayport_crsf.h"
 #include "io/serial.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
@@ -643,6 +644,10 @@ void init(void)
     cmsDisplayPortRegister(displayPortSrxlInit());
 #endif
 
+#if defined(USE_CMS) && defined(USE_CRSF_CMS_TELEMETRY) && defined(USE_TELEMETRY)
+    cmsDisplayPortRegister(displayPortCrsfInit());
+#endif
+
 #ifdef USE_GPS
     if (feature(FEATURE_GPS)) {
         gpsInit();

src/main/interface/crsf_protocol.h

@@ -31,9 +31,18 @@ typedef enum {
     // MSP commands
     CRSF_FRAMETYPE_MSP_REQ = 0x7A,   // response request using msp sequence as command
     CRSF_FRAMETYPE_MSP_RESP = 0x7B,  // reply with 58 byte chunked binary
-    CRSF_FRAMETYPE_MSP_WRITE = 0x7C  // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
+    CRSF_FRAMETYPE_MSP_WRITE = 0x7C,  // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
+    CRSF_FRAMETYPE_DISPLAYPORT_UPDATE = 0x7D, // transmit displayport buffer to remote
+    CRSF_FRAMETYPE_DISPLAYPORT_CLEAR = 0x7E, // clear remote
+    CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7F, // client request
 } crsfFrameType_e;
 
+enum {
+    CRSF_DISPLAYPORT_SUBCMD_OPEN = 0x01,  // client request to open cms menu
+    CRSF_DISPLAYPORT_SUBCMD_CLOSE = 0x02,  // client request to close cms menu
+    CRSF_DISPLAYPORT_SUBCMD_POLL = 0x03,  // client request to poll/refresh cms menu
+};
+
 enum {
     CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,

src/main/interface/settings.c

@@ -928,6 +928,12 @@ const clivalue_t valueTable[] = {
     { "displayport_msp_row_adjust", VAR_INT8    | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, rowAdjust) },
 #endif
 
+// PG_DISPLAY_PORT_CRSF_CONFIG
+#if defined(USE_CRSF_CMS_TELEMETRY)
+    { "displayport_crsf_col_adjust", VAR_INT8    | MASTER_VALUE, .config.minmax = { -8, 0 }, PG_DISPLAY_PORT_CRSF_CONFIG, offsetof(displayPortProfile_t, colAdjust) },
+    { "displayport_crsf_row_adjust", VAR_INT8    | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_CRSF_CONFIG, offsetof(displayPortProfile_t, rowAdjust) },
+#endif
+
 // PG_DISPLAY_PORT_MSP_CONFIG
 #ifdef USE_MAX7456
     { "displayport_max7456_col_adjust", VAR_INT8| MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MAX7456_CONFIG, offsetof(displayPortProfile_t, colAdjust) },

src/main/io/displayport_crsf.c

@@ -0,0 +1,214 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <string.h>
+
+#include "platform.h"
+
+#if defined(USE_CRSF_CMS_TELEMETRY)
+
+#include "cms/cms.h"
+#include "common/maths.h"
+#include "common/printf.h"
+#include "common/time.h"
+#include "drivers/display.h"
+#include "drivers/time.h"
+#include "io/displayport_crsf.h"
+#include "pg/pg_ids.h"
+
+#define CRSF_DISPLAY_PORT_OPEN_DELAY_MS     400
+#define CRSF_DISPLAY_PORT_CLEAR_DELAY_MS    38
+
+static crsfDisplayPortScreen_t crsfScreen;
+static timeMs_t delayTransportUntilMs = 0;
+
+PG_REGISTER(displayPortProfile_t, displayPortProfileCrsf, PG_DISPLAY_PORT_CRSF_CONFIG, 0);
+
+displayPort_t crsfDisplayPort;
+
+static int crsfGrab(displayPort_t *displayPort)
+{
+    return displayPort->grabCount = 1;
+}
+
+static int crsfClearScreen(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    crsfDisplayPortRow_t *screenRow;
+    for (int row=0; row<CRSF_DISPLAY_PORT_ROWS_MAX; row++) {
+        screenRow = &crsfScreen.rows[row];
+        screenRow->pendingTransport = false;
+        for (int col=0; col<CRSF_DISPLAY_PORT_COLS_MAX; col++) {
+            screenRow->data[col]=' ';
+        }
+    }
+    crsfScreen.reset = true;
+    delayTransportUntilMs = millis() + CRSF_DISPLAY_PORT_CLEAR_DELAY_MS;
+    return 0;
+}
+
+static int crsfRelease(displayPort_t *displayPort)
+{
+    displayPort->grabCount = 0;
+    return crsfClearScreen(displayPort);
+}
+
+static int crsfDrawScreen(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int crsfScreenSize(const displayPort_t *displayPort)
+{
+    return displayPort->rows * displayPort->cols;
+}
+
+
+static int crsfWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s)
+{
+    UNUSED(displayPort);
+    if (row >= CRSF_DISPLAY_PORT_ROWS_MAX || col >= CRSF_DISPLAY_PORT_COLS_MAX) {
+        return 0;
+    }
+    const size_t truncLen = MIN((int)strlen(s), CRSF_DISPLAY_PORT_COLS_MAX-col);  // truncate at CRSF_DISPLAY_PORT_COLS_MAX
+    crsfDisplayPortRow_t *screenRow = &crsfScreen.rows[row];
+    screenRow->pendingTransport = memcmp(&screenRow->data[col], s, truncLen);
+    if (screenRow->pendingTransport) {
+        memcpy(&screenRow->data[col], s, truncLen);
+    }
+    return 0;
+}
+
+static int crsfWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c)
+{
+    char s[1];
+    tfp_sprintf(s, "%c", c);
+    return crsfWriteString(displayPort, col, row, s);
+}
+
+static bool crsfIsTransferInProgress(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return false;
+}
+
+static bool crsfIsSynced(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return true;
+}
+
+static int crsfHeartbeat(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static void crsfResync(displayPort_t *displayPort)
+{
+    displayPort->rows = CRSF_DISPLAY_PORT_ROWS_MAX + displayPortProfileCrsf()->rowAdjust;
+    displayPort->cols = CRSF_DISPLAY_PORT_COLS_MAX + displayPortProfileCrsf()->colAdjust;
+    crsfClearScreen(displayPort);
+}
+
+static uint32_t crsfTxBytesFree(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return UINT32_MAX;
+}
+
+static const displayPortVTable_t crsfDisplayPortVTable = {
+    .grab = crsfGrab,
+    .release = crsfRelease,
+    .clearScreen = crsfClearScreen,
+    .drawScreen = crsfDrawScreen,
+    .screenSize = crsfScreenSize,
+    .writeString = crsfWriteString,
+    .writeChar = crsfWriteChar,
+    .isTransferInProgress = crsfIsTransferInProgress,
+    .heartbeat = crsfHeartbeat,
+    .resync = crsfResync,
+    .isSynced = crsfIsSynced,
+    .txBytesFree = crsfTxBytesFree
+};
+
+displayPort_t *displayPortCrsfInit()
+{
+    displayInit(&crsfDisplayPort, &crsfDisplayPortVTable);
+    crsfResync(&crsfDisplayPort);
+    return &crsfDisplayPort;
+}
+
+crsfDisplayPortScreen_t *crsfDisplayPortScreen(void)
+{
+    return &crsfScreen;
+}
+
+void crsfDisplayPortMenuOpen(void)
+{
+    if (cmsInMenu) {
+        return;
+    }
+    if (cmsDisplayPortSelect(&crsfDisplayPort)) {
+        cmsMenuOpen();
+        delayTransportUntilMs = millis() + CRSF_DISPLAY_PORT_OPEN_DELAY_MS;
+    }
+}
+
+void crsfDisplayPortMenuExit(void)
+{
+    if (!cmsInMenu) {
+        return;
+    }
+    uint8_t exitMenu = CMS_EXIT;
+    cmsMenuExit(&crsfDisplayPort, &exitMenu);
+}
+
+
+void crsfDisplayPortRefresh(void)
+{
+    if (!cmsInMenu) {
+        crsfDisplayPortMenuOpen();
+        return;
+    }
+    crsfDisplayPortRow_t *screenRow;
+    for (int row=0; row<CRSF_DISPLAY_PORT_ROWS_MAX; row++) {
+        screenRow = &crsfScreen.rows[row];
+        screenRow->pendingTransport = true;
+    }
+    crsfScreen.reset = true;
+}
+
+int crsfDisplayPortNextRow(void)
+{
+    const timeMs_t currentTimeMs = millis();
+    if (currentTimeMs < delayTransportUntilMs) {
+        return -1;
+    }
+    crsfDisplayPortRow_t *screenRow;
+    for(int i=0; i<CRSF_DISPLAY_PORT_ROWS_MAX; i++) {
+        screenRow = &crsfScreen.rows[i];
+        if (screenRow->pendingTransport) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+#endif

src/main/io/displayport_crsf.h

@@ -0,0 +1,44 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "pg/pg.h"
+#include "drivers/display.h"
+
+#define CRSF_DISPLAY_PORT_ROWS_MAX 8
+#define CRSF_DISPLAY_PORT_COLS_MAX 32
+
+typedef struct crsfDisplayPortRow_s {
+    char data[CRSF_DISPLAY_PORT_COLS_MAX];
+    bool pendingTransport;
+} crsfDisplayPortRow_t;
+
+typedef struct crsfDisplayPortScreen_s {
+    crsfDisplayPortRow_t rows[CRSF_DISPLAY_PORT_ROWS_MAX];
+    bool reset;
+} crsfDisplayPortScreen_t;
+
+PG_DECLARE(displayPortProfile_t, displayPortProfileCrsf);
+
+struct displayPort_s;
+struct displayPort_s *displayPortCrsfInit(void);
+crsfDisplayPortScreen_t *crsfDisplayPortScreen(void);
+void crsfDisplayPortMenuOpen(void);
+void crsfDisplayPortMenuExit(void);
+void crsfDisplayPortRefresh(void);
+int crsfDisplayPortNextRow(void);

src/main/pg/pg_ids.h

@@ -123,7 +123,8 @@
 #define PG_PINIOBOX_CONFIG 530
 #define PG_USB_CONFIG 531
 #define PG_SDIO_CONFIG 532
-#define PG_BETAFLIGHT_END 532
+#define PG_DISPLAY_PORT_CRSF_CONFIG 533
+#define PG_BETAFLIGHT_END 533
 
 
 // OSD configuration (subject to change)

src/main/rx/crsf.c

@@ -166,6 +166,12 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
                         case CRSF_FRAMETYPE_DEVICE_PING:
                             crsfScheduleDeviceInfoResponse();
                             break;
+#if defined(USE_CRSF_CMS_TELEMETRY)
+                        case CRSF_FRAMETYPE_DISPLAYPORT_CMD: ;
+                            uint8_t *cmd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
+                            crsfProcessDisplayPortCmd(*cmd);
+                            break;
+#endif
                         default:
                             break;
                     }

src/main/target/MIDELICF3/target.h

@@ -19,6 +19,9 @@
 
 #define TARGET_BOARD_IDENTIFIER "MIF3"
 
+// prevent flash overflow
+#undef USE_CRSF_CMS_TELEMETRY
+
 #define LED0_PIN                PB5
 
 #define USE_BEEPER

src/main/target/SPRACINGF3EVO/target.h

@@ -30,6 +30,7 @@
 #undef USE_COPY_PROFILE_CMS_MENU
 #undef USE_RX_MSP
 #undef USE_ESC_SENSOR_INFO
+#undef USE_CRSF_CMS_TELEMETRY
 
 
 #if !defined(AIORACERF3)

src/main/target/SPRACINGF3NEO/target.h

@@ -25,6 +25,7 @@
 #undef USE_COPY_PROFILE_CMS_MENU
 #undef USE_RX_MSP
 #undef USE_ESC_SENSOR_INFO
+#undef USE_CRSF_CMS_TELEMETRY
 
 
 #undef USE_COPY_PROFILE_CMS_MENU

src/main/target/common_fc_pre.h

@@ -171,6 +171,7 @@
 #define USE_GYRO_LPF2
 #define USE_ESC_SENSOR
 #define USE_ESC_SENSOR_INFO
+#define USE_CRSF_CMS_TELEMETRY
 
 #ifdef USE_SERIALRX_SPEKTRUM
 #define USE_SPEKTRUM_BIND

src/main/telemetry/crsf.c

@@ -47,6 +47,7 @@
 
 #include "interface/crsf_protocol.h"
 
+#include "io/displayport_crsf.h"
 #include "io/gps.h"
 #include "io/serial.h"
 
@@ -59,7 +60,6 @@
 #include "telemetry/crsf.h"
 #include "telemetry/msp_shared.h"
 
-
 #define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
 #define CRSF_DEVICEINFO_VERSION             0x01
 #define CRSF_DEVICEINFO_PARAMETER_COUNT     0
@@ -306,6 +306,34 @@ void crsfFrameDeviceInfo(sbuf_t *dst) {
     *lengthPtr = sbufPtr(dst) - lengthPtr;
 }
 
+#if defined(USE_CRSF_CMS_TELEMETRY)
+
+static void crsfFrameDisplayPortRow(sbuf_t *dst, uint8_t row, const char *str)
+{
+    uint8_t *lengthPtr = sbufPtr(dst);
+    const uint8_t bufLen = CRSF_DISPLAY_PORT_COLS_MAX + displayPortProfileCrsf()->colAdjust;
+    const uint8_t frameLength = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + bufLen;
+    sbufWriteU8(dst, frameLength);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_UPDATE);
+    sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+    sbufWriteU8(dst, row);
+    sbufWriteData(dst, str, bufLen);
+    *lengthPtr = sbufPtr(dst) - lengthPtr;
+}
+
+static void crsfFrameDisplayPortClear(sbuf_t *dst)
+{
+    uint8_t *lengthPtr = sbufPtr(dst);
+    sbufWriteU8(dst, CRSF_DISPLAY_PORT_COLS_MAX + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CLEAR);
+    sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+    *lengthPtr = sbufPtr(dst) - lengthPtr;
+}
+
+#endif
+
 #define BV(x)  (1 << (x)) // bit value
 
 // schedule array to decide how often each type of frame is sent
@@ -348,6 +376,7 @@ void crsfSendMspResponse(uint8_t *payload)
 static void processCrsf(void)
 {
     static uint8_t crsfScheduleIndex = 0;
+
     const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
 
     sbuf_t crsfPayloadBuf;
@@ -384,6 +413,7 @@ void crsfScheduleDeviceInfoResponse(void)
     deviceInfoReplyPending = true;
 }
 
+
 void initCrsfTelemetry(void)
 {
     // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
@@ -415,6 +445,28 @@ bool checkCrsfTelemetryState(void)
     return crsfTelemetryEnabled;
 }
 
+#if defined(USE_CRSF_CMS_TELEMETRY)
+void crsfProcessDisplayPortCmd(uint8_t cmd)
+{
+    switch (cmd) {
+    case CRSF_DISPLAYPORT_SUBCMD_OPEN:
+        crsfDisplayPortMenuOpen();
+        break;
+    case CRSF_DISPLAYPORT_SUBCMD_CLOSE:
+        crsfDisplayPortMenuExit();
+        break;
+    case CRSF_DISPLAYPORT_SUBCMD_POLL:
+        crsfDisplayPortRefresh();
+        crsfDisplayPortScreen()->reset = true;
+        break;
+    default:
+        break;
+    }
+
+}
+
+#endif
+
 /*
  * Called periodically by the scheduler
  */
@@ -450,6 +502,31 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
         return;
     }
 
+#if defined(USE_CRSF_CMS_TELEMETRY)
+    if (crsfDisplayPortScreen()->reset) {
+        crsfDisplayPortScreen()->reset = false;
+        sbuf_t crsfDisplayPortBuf;
+        sbuf_t *dst = &crsfDisplayPortBuf;
+        crsfInitializeFrame(dst);
+        crsfFrameDisplayPortClear(dst);
+        crsfFinalize(dst);
+        crsfLastCycleTime = currentTimeUs;
+        return;
+    }
+    const int nextRow = crsfDisplayPortNextRow();
+    if (nextRow >= 0) {
+        crsfDisplayPortRow_t *row = &crsfDisplayPortScreen()->rows[nextRow];
+        sbuf_t crsfDisplayPortBuf;
+        sbuf_t *dst = &crsfDisplayPortBuf;
+        crsfInitializeFrame(dst);
+        crsfFrameDisplayPortRow(dst, nextRow, row->data);
+        crsfFinalize(dst);
+        row->pendingTransport = false;
+        crsfLastCycleTime = currentTimeUs;
+        return;
+    }
+#endif
+
     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
     // Spread out scheduled frames evenly so each frame is sent at the same frequency.
     if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {

src/main/telemetry/crsf.h

@@ -26,13 +26,15 @@
 #define CRSF_MSP_RX_BUF_SIZE 128
 #define CRSF_MSP_TX_BUF_SIZE 128
 
-
 void initCrsfTelemetry(void);
 bool checkCrsfTelemetryState(void);
 void handleCrsfTelemetry(timeUs_t currentTimeUs);
 void crsfScheduleDeviceInfoResponse(void);
 void crsfScheduleMspResponse(void);
 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
+#if defined(USE_CRSF_CMS_TELEMETRY)
+void crsfProcessDisplayPortCmd(uint8_t cmd);
+#endif
 #if defined(USE_MSP_OVER_TELEMETRY)
 void initCrsfMspBuffer(void);
 bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);