Spektrum, CMS over Telemetry. Rebased and squashed.

make/source.mk

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

src/main/cms/cms.c

@@ -126,10 +126,18 @@ static displayPort_t *cmsDisplayPortSelectNext(void)
 // HoTT Telemetry Screen
 //   21 cols x 8 rows
 //
+// Spektrum SRXL Telemtry Textgenerator
+//   13 cols x 9 rows, top row printed as a Bold Heading
+//   Needs the "smallScreen" adaptions
 
-#define LEFT_MENU_COLUMN  1
+
-#define RIGHT_MENU_COLUMN(p) ((p)->cols - 8)
+
-#define MAX_MENU_ITEMS(p)    ((p)->rows - 2)
+#define NORMAL_SCREEN_MIN_COLS 18      // Less is a small screen
+static bool    smallScreen;
+static uint8_t leftMenuColumn;
+static uint8_t rightMenuColumn;
+static uint8_t maxMenuItems;
+static uint8_t linesPerMenuItem;
 
 bool cmsInMenu = false;
 
@@ -181,14 +189,15 @@ static CMS_Menu menuErr = {
 
 static void cmsUpdateMaxRow(displayPort_t *instance)
 {
+    UNUSED(instance);
     pageMaxRow = 0;
 
     for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) {
         pageMaxRow++;
     }
 
-    if (pageMaxRow >  MAX_MENU_ITEMS(instance)) {
+    if (pageMaxRow > maxMenuItems) {
-        pageMaxRow = MAX_MENU_ITEMS(instance);
+        pageMaxRow = maxMenuItems;
     }
 
     pageMaxRow--;
@@ -196,13 +205,14 @@ static void cmsUpdateMaxRow(displayPort_t *instance)
 
 static uint8_t cmsCursorAbsolute(displayPort_t *instance)
 {
-    return currentCtx.cursorRow + currentCtx.page * MAX_MENU_ITEMS(instance);
+    UNUSED(instance);
+    return currentCtx.cursorRow + currentCtx.page * maxMenuItems;
 }
 
 static void cmsPageSelect(displayPort_t *instance, int8_t newpage)
 {
     currentCtx.page = (newpage + pageCount) % pageCount;
-    pageTop = &currentCtx.menu->entries[currentCtx.page * MAX_MENU_ITEMS(instance)];
+    pageTop = &currentCtx.menu->entries[currentCtx.page * maxMenuItems];
     cmsUpdateMaxRow(instance);
     displayClearScreen(instance);
 }
@@ -244,7 +254,13 @@ static void cmsFormatFloat(int32_t value, char *floatString)
         floatString[0] = ' ';
 }
 
-static void cmsPadToSize(char *buf, int size)
+// CMS on OSD legacy was to use LEFT aligned values, not the RIGHT way ;-)
+#define CMS_OSD_RIGHT_ALIGNED_VALUES
+
+#ifndef CMS_OSD_RIGHT_ALIGNED_VALUES
+
+// Pad buffer to the left, i.e. align left
+static void cmsPadRightToSize(char *buf, int size)
 {
     int i;
 
@@ -259,17 +275,69 @@ static void cmsPadToSize(char *buf, int size)
 
     buf[size] = 0;
 }
+#endif
+
+// Pad buffer to the left, i.e. align right
+static void cmsPadLeftToSize(char *buf, int size)
+{
+    int i,j;
+    int len = strlen(buf);
+
+    for (i = size - 1, j = size - len ; i - j >= 0 ; i--) {
+      buf[i] = buf[i - j];
+    }
+
+    for ( ; i >= 0 ; i--) {
+      buf[i] = ' ';
+    }
+
+    buf[size] = 0;
+}
+
+static void cmsPadToSize(char *buf, int size)
+{
+    // Make absolutely sure the string terminated.
+    buf[size] = 0x00,
+
+#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
+    cmsPadLeftToSize(buf, size);
+#else
+    smallScreen ? cmsPadLeftToSize(buf, size) : cmsPadRightToSize(buf, size);
+#endif
+}
+
+static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row, uint8_t maxSize)
+{
+    int colpos;
+    int cnt;
+
+    cmsPadToSize(buff, maxSize);
+#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
+    colpos = rightMenuColumn - maxSize;
+#else
+    colpos = smallScreen ? rightMenuColumn - maxSize : rightMenuColumn;
+#endif
+    cnt = displayWrite(pDisplay, colpos, row, buff);
+    return cnt;
+}
 
 static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
 {
-    #define CMS_DRAW_BUFFER_LEN 10u
+    #define CMS_DRAW_BUFFER_LEN 12
-    char buff[CMS_DRAW_BUFFER_LEN];
+    #define CMS_NUM_FIELD_LEN 5
+
+    char buff[CMS_DRAW_BUFFER_LEN +1]; // Make room for null terminator.
     int cnt = 0;
 
+    if (smallScreen) {
+        row++;
+    }
+
     switch (p->type) {
     case OME_String:
         if (IS_PRINTVALUE(p) && p->data) {
-            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data);
+            strncpy(buff, p->data, CMS_DRAW_BUFFER_LEN);
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -278,19 +346,19 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
     case OME_Funcall:
         if (IS_PRINTVALUE(p))  {
 
-            int colPos = RIGHT_MENU_COLUMN(pDisplay);
+            buff[0]= 0x0;
 
             if ((p->type == OME_Submenu) && p->func && (p->flags & OPTSTRING)) {
 
                 // Special case of sub menu entry with optional value display.
 
                 char *str = ((CMSMenuOptFuncPtr)p->func)();
-                cnt = displayWrite(pDisplay, colPos, row, str);
+                strncpy( buff, str, CMS_DRAW_BUFFER_LEN);
-                colPos += strlen(str);
             }
+            strncat(buff, ">", CMS_DRAW_BUFFER_LEN);
 
-            cnt += displayWrite(pDisplay, colPos, row, ">");
+            row = smallScreen  ? row - 1  : row;
-
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, strlen(buff));
             CLR_PRINTVALUE(p);
         }
         break;
@@ -298,10 +366,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
     case OME_Bool:
         if (IS_PRINTVALUE(p) && p->data) {
             if (*((uint8_t *)(p->data))) {
-                cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
+              strcpy(buff, "YES");
             } else {
-                cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
+              strcpy(buff, "NO ");
             }
+
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -310,9 +380,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
         if (IS_PRINTVALUE(p)) {
             OSD_TAB_t *ptr = p->data;
             char * str = (char *)ptr->names[*ptr->val];
-            memcpy(buff, str, MAX(CMS_DRAW_BUFFER_LEN, strlen(str)));
+            strncpy(buff, str, CMS_DRAW_BUFFER_LEN);
-            cmsPadToSize(buff, CMS_DRAW_BUFFER_LEN);
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN);
-            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -323,10 +392,11 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             uint16_t *val = (uint16_t *)p->data;
 
             if (VISIBLE(*val)) {
-                cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
+                strcpy(buff, "YES");
             } else {
-                cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
+              strcpy(buff, "NO ");
             }
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -336,8 +406,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_UINT8_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
-            cmsPadToSize(buff, 5);
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
-            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -346,8 +415,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_INT8_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
-            cmsPadToSize(buff, 5);
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
-            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -356,8 +424,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_UINT16_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
-            cmsPadToSize(buff, 5);
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
-            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -366,8 +433,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_UINT16_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
-            cmsPadToSize(buff, 5);
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
-            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -376,8 +442,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_FLOAT_t *ptr = p->data;
             cmsFormatFloat(*ptr->val * ptr->multipler, buff);
-            cmsPadToSize(buff, 5);
+            cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
-            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ???
             CLR_PRINTVALUE(p);
         }
         break;
@@ -385,7 +450,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
     case OME_Label:
         if (IS_PRINTVALUE(p) && p->data) {
             // A label with optional string, immediately following text
-            cnt = displayWrite(pDisplay, LEFT_MENU_COLUMN + 2 + strlen(p->text), row, p->data);
+            cnt = displayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, p->data);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -399,8 +464,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
         // Fall through
     default:
 #ifdef CMS_MENU_DEBUG
-        // Shouldn't happen. Notify creator of this menu content.
+        // Shouldn't happen. Notify creator of this menu content
-        cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT");
+#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
+        cnt = displayWrite(pDisplay, rightMenuColumn - 6, row, "BADENT");
+#else.
+        cnt = displayWrite(pDisplay, rightMenuColumn, row, "BADENT");
+#endif
 #endif
         break;
     }
@@ -415,7 +484,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
 
     uint8_t i;
     OSD_Entry *p;
-    uint8_t top = (pDisplay->rows - pageMaxRow) / 2 - 1;
+    uint8_t top = smallScreen ? 1 : (pDisplay->rows - pageMaxRow)/2;
 
     // Polled (dynamic) value display denominator.
 
@@ -450,14 +519,14 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
     cmsPageDebug();
 
     if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) {
-        room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, "  ");
+        room -= displayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, " ");
     }
 
     if (room < 30)
         return;
 
     if (pDisplay->cursorRow != currentCtx.cursorRow) {
-        room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCtx.cursorRow + top, " >");
+        room -= displayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, ">");
         pDisplay->cursorRow = currentCtx.cursorRow;
     }
 
@@ -465,25 +534,23 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
         return;
 
     // Print text labels
-    for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
+    for (i = 0, p = pageTop; i < maxMenuItems && p->type != OME_END; i++, p++) {
         if (IS_PRINTLABEL(p)) {
-            uint8_t coloff = LEFT_MENU_COLUMN;
+            uint8_t coloff = leftMenuColumn;
-            coloff += (p->type == OME_Label) ? 1 : 2;
+            coloff += (p->type == OME_Label) ? 0 : 1;
-            room -= displayWrite(pDisplay, coloff, i + top, p->text);
+            room -= displayWrite(pDisplay, coloff, top + i * linesPerMenuItem, p->text);
             CLR_PRINTLABEL(p);
             if (room < 30)
                 return;
         }
-    }
 
     // Print values
 
     // XXX Polled values at latter positions in the list may not be
     // XXX printed if not enough room in the middle of the list.
 
-    for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
         if (IS_PRINTVALUE(p)) {
-            room -= cmsDrawMenuEntry(pDisplay, p, top + i);
+            room -= cmsDrawMenuEntry(pDisplay, p, top + i * linesPerMenuItem);
             if (room < 30)
                 return;
         }
@@ -492,9 +559,10 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
 
 static void cmsMenuCountPage(displayPort_t *pDisplay)
 {
+    UNUSED(pDisplay);
     const OSD_Entry *p;
     for (p = currentCtx.menu->entries; p->type != OME_END; p++);
-    pageCount = (p - currentCtx.menu->entries - 1) / MAX_MENU_ITEMS(pDisplay) + 1;
+    pageCount = (p - currentCtx.menu->entries - 1) / maxMenuItems + 1;
 }
 
 STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay); // Forward; will be resolved after merging
@@ -538,9 +606,9 @@ long cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
         // currentCtx.cursorRow has been saved as absolute; convert it back to page + relative
 
         int8_t cursorAbs = currentCtx.cursorRow;
-        currentCtx.cursorRow = cursorAbs % MAX_MENU_ITEMS(pDisplay);
+        currentCtx.cursorRow = cursorAbs % maxMenuItems;
         cmsMenuCountPage(pDisplay);
-        cmsPageSelect(pDisplay, cursorAbs / MAX_MENU_ITEMS(pDisplay));
+        cmsPageSelect(pDisplay, cursorAbs / maxMenuItems);
     }
 
     cmsPageDebug();
@@ -594,6 +662,25 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
         }
     }
     displayGrab(pCurrentDisplay); // grab the display for use by the CMS
+
+    if ( pCurrentDisplay->cols < NORMAL_SCREEN_MIN_COLS) {
+      smallScreen       = true;
+      linesPerMenuItem  = 2;
+      leftMenuColumn    = 0;
+      rightMenuColumn   = pCurrentDisplay->cols;
+      maxMenuItems      = (pCurrentDisplay->rows) / linesPerMenuItem;
+    } else {
+      smallScreen       = false;
+      linesPerMenuItem  = 1;
+      leftMenuColumn    = 2;
+#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
+      rightMenuColumn   = pCurrentDisplay->cols - 2;
+#else
+      rightMenuColumn   = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
+#endif
+      maxMenuItems      = pCurrentDisplay->rows - 2;
+    }
+
     cmsMenuChange(pCurrentDisplay, currentCtx.menu);
 }
 

src/main/cms/cms.h

@@ -17,7 +17,7 @@ long cmsMenuChange(displayPort_t *pPort, const void *ptr);
 long cmsMenuExit(displayPort_t *pPort, const void *ptr);
 void cmsUpdate(uint32_t currentTimeUs);
 
-#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID"
+#define CMS_STARTUP_HELP_TEXT1 "MENU:THR MID"
 #define CMS_STARTUP_HELP_TEXT2     "+ YAW LEFT"
 #define CMS_STARTUP_HELP_TEXT3     "+ PITCH UP"
 

src/main/cms/cms_menu_builtin.c

@@ -53,20 +53,22 @@
 
 // Info
 
-static char infoGitRev[GIT_SHORT_REVISION_LENGTH];
+static char infoGitRev[GIT_SHORT_REVISION_LENGTH + 1];
 static char infoTargetName[] = __TARGET__;
 
 #include "interface/msp_protocol.h" // XXX for FC identification... not available elsewhere
 
 static long cmsx_InfoInit(void)
 {
-    for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
+    int i;
+    for ( i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
         if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f')
             infoGitRev[i] = shortGitRevision[i] - 'a' + 'A';
         else
             infoGitRev[i] = shortGitRevision[i];
     }
 
+    infoGitRev[i] = 0x0; // Terminate string
     return 0;
 }
 

src/main/fc/fc_init.c

@@ -107,6 +107,8 @@
 #include "io/vtx_smartaudio.h"
 #include "io/vtx_tramp.h"
 
+#include "io/displayport_srxl.h"
+
 #include "scheduler/scheduler.h"
 
 #include "sensors/acceleration.h"
@@ -597,6 +599,10 @@ void init(void)
     }
 #endif
 
+#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY)
+    // Register the srxl Textgen telemetry sensor as a displayport device
+    cmsDisplayPortRegister(displayPortSrxlInit());
+#endif
 
 #ifdef USE_GPS
     if (feature(FEATURE_GPS)) {

src/main/io/displayport_srxl.c

@@ -0,0 +1,135 @@
+/*
+ * 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 <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+
+#include "common/utils.h"
+
+#include "drivers/display.h"
+#include "cms/cms.h"
+
+#include "telemetry/srxl.h"
+
+static displayPort_t srxlDisplayPort;
+
+static int srxlDrawScreen(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int srxlScreenSize(const displayPort_t *displayPort)
+{
+    return displayPort->rows * displayPort->cols;
+}
+
+static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c)
+{
+    return (spektrumTmTextGenPutChar(col, row, c));
+    UNUSED(displayPort);
+}
+
+
+static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s)
+{
+    while (*s) {
+        srxlWriteChar(displayPort,  col++, row, *(s++));
+    }
+    return 0;
+}
+
+static int srxlClearScreen(displayPort_t *displayPort)
+{
+    for (int row = 0;  row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
+        for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) {
+            srxlWriteChar(displayPort, col, row, ' ');
+        }
+    }
+    srxlWriteString(displayPort, 1, 0,  "BETAFLIGHT");
+
+    if ( displayPort->grabCount == 0 ) {
+        srxlWriteString(displayPort, 0, 3,  CMS_STARTUP_HELP_TEXT1);
+        srxlWriteString(displayPort, 2, 4,  CMS_STARTUP_HELP_TEXT2);
+        srxlWriteString(displayPort, 2, 5,  CMS_STARTUP_HELP_TEXT3);
+    }
+    return 0;
+}
+
+static bool srxlIsTransferInProgress(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return false;
+}
+
+static int srxlHeartbeat(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static void srxlResync(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+}
+
+static uint32_t srxlTxBytesFree(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return UINT32_MAX;
+}
+
+static int srxlGrab(displayPort_t *displayPort)
+{
+    return displayPort->grabCount = 1;
+}
+
+static int srxlRelease(displayPort_t *displayPort)
+{
+    int cnt = displayPort->grabCount = 0;
+    srxlClearScreen(displayPort);
+    return cnt;
+}
+
+static const displayPortVTable_t srxlVTable = {
+    .grab = srxlGrab,
+    .release = srxlRelease,
+    .clearScreen = srxlClearScreen,
+    .drawScreen = srxlDrawScreen,
+    .screenSize = srxlScreenSize,
+    .writeString = srxlWriteString,
+    .writeChar = srxlWriteChar,
+    .isTransferInProgress = srxlIsTransferInProgress,
+    .heartbeat = srxlHeartbeat,
+    .resync = srxlResync,
+    .txBytesFree = srxlTxBytesFree
+};
+
+displayPort_t *displayPortSrxlInit()
+{
+    srxlDisplayPort.device = NULL;
+    displayInit(&srxlDisplayPort, &srxlVTable);
+    srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
+    srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
+    return &srxlDisplayPort;
+}
+
+#endif

src/main/io/displayport_srxl.h

@@ -0,0 +1,20 @@
+/*
+ * 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
+
+displayPort_t *displayPortSrxlInit();

src/main/rx/spektrum.c

@@ -56,6 +56,7 @@
 #define SPEKTRUM_1024_CHANNEL_COUNT     7
 
 #define SPEKTRUM_NEEDED_FRAME_INTERVAL  5000
+#define SPEKTRUM_TELEMETRY_FRAME_DELAY  1000   // Gap between received Rc frame and transmited TM frame, uS
 
 #define SPEKTRUM_BAUDRATE               115200
 
@@ -473,7 +474,7 @@ static uint8_t spektrumFrameStatus(void)
 
     /* only process if srxl enabled, some data in buffer AND servos in phase 0 */
     if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80)) {
-        dispatchAdd(&srxlTelemetryDispatch, 100);
+        dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY);
     }
     return RX_FRAME_COMPLETE;
 }

src/main/target/common_fc_pre.h

@@ -138,6 +138,7 @@
 #define USE_SPEKTRUM_FAKE_RSSI
 #define USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
 #define USE_SPEKTRUM_VTX_CONTROL
+#define USE_SPEKTRUM_CMS_TELEMETRY
 #endif
 #endif
 

src/main/telemetry/srxl.c

@@ -50,8 +50,7 @@
 #include "telemetry/telemetry.h"
 #include "telemetry/srxl.h"
 
-
+#define SRXL_CYCLETIME_US           33000 // 33ms, 30 Hz
-#define SRXL_CYCLETIME_US           100000 // 100ms, 10 Hz
 
 #define SRXL_ADDRESS_FIRST          0xA5
 #define SRXL_ADDRESS_SECOND         0x80
@@ -104,8 +103,10 @@ typedef struct
     UINT16 rxVoltage; // Volts, 0.01V increments
 } STRU_TELE_QOS;
 */
-void srxlFrameQos(sbuf_t *dst)
+bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs)
 {
+    UNUSED(currentTimeUs);
+
     sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS);
     sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
     sbufWriteU16BigEndian(dst, 0xFFFF); // A
@@ -115,6 +116,7 @@ void srxlFrameQos(sbuf_t *dst)
     sbufWriteU16BigEndian(dst, 0xFFFF); // F
     sbufWriteU16BigEndian(dst, 0xFFFF); // H
     sbufWriteU16BigEndian(dst, 0xFFFF); // rxVoltage
+    return true;
 }
 
 /*
@@ -130,8 +132,10 @@ typedef struct
     // If only 1 antenna, set B = A
 } STRU_TELE_RPM;
 */
-void srxlFrameRpm(sbuf_t *dst)
+bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
 {
+    UNUSED(currentTimeUs);
+
     sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
     sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
     sbufWriteU16BigEndian(dst, 0xFFFF); // pulse leading edges
@@ -144,6 +148,7 @@ void srxlFrameRpm(sbuf_t *dst)
     sbufWriteU16BigEndian(dst, 0xFFFF);
     sbufWriteU16BigEndian(dst, 0xFFFF);
     sbufWriteU16BigEndian(dst, 0xFFFF);
+    return true;
 }
 
 /*
@@ -161,42 +166,156 @@ typedef struct
 } STRU_TELE_FP_MAH;
 */
 
-void srxlFrameFlightPackCurrent(sbuf_t *dst)
+#define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s
+
+bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs)
 {
-    sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH);
+    uint16_t amps = getAmperage() / 10;
-    sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+    uint16_t mah  = getMAhDrawn();
-    sbufWriteU16(dst, getAmperage() / 10);
+    static uint16_t sentAmps;
-    sbufWriteU16(dst, getMAhDrawn());
+    static uint16_t sentMah;
-    sbufWriteU16(dst, 0x7fff);            // temp A
+    static timeUs_t lastTimeSentFPmAh = 0;
-    sbufWriteU16(dst, 0xffff);
+
-    sbufWriteU16(dst, 0xffff);
+    timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh;
-    sbufWriteU16(dst, 0x7fff);            // temp B
+
-    sbufWriteU16(dst, 0xffff);
+    if ( (amps != sentAmps) || (mah != sentMah) ||
+         keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) {
+        sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH);
+        sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+        sbufWriteU16(dst, amps);
+        sbufWriteU16(dst, mah);
+        sbufWriteU16(dst, 0x7fff);            // temp A
+        sbufWriteU16(dst, 0xffff);
+        sbufWriteU16(dst, 0xffff);
+        sbufWriteU16(dst, 0x7fff);            // temp B
+        sbufWriteU16(dst, 0xffff);
+
+        sentAmps = amps;
+        sentMah = mah;
+        lastTimeSentFPmAh = currentTimeUs;
+        return true;
+    }
+    return false;
 }
 
-// schedule array to decide how often each type of frame is sent
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
-#define SRXL_SCHEDULE_COUNT_MAX     3
 
-typedef void (*srxlScheduleFnPtr)(sbuf_t *dst);
+// Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display.
-const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = {
+
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN (0x0C)     // Text Generator
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS (9)   // Text Generator ROWS
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS (13)  // Text Generator COLS
+
+/*
+typedef struct
+{
+    UINT8		identifier;
+    UINT8		sID;		       // Secondary ID
+    UINT8		lineNumber;	       // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen)
+    char		text[13];	       // 0-terminated text when < 13 chars
+} STRU_SPEKTRUM_SRXL_TEXTGEN;
+*/
+
+#if ( SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS > SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS )
+static char srxlTextBuff[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS][SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS];
+static bool lineSent[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS];
+#else
+static char srxlTextBuff[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS][SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS];
+static bool lineSent[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS];
+#endif
+
+//**************************************************************************
+// API Running in external client task context. E.g. in the CMS task 
+int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c)
+{
+    if (row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS && col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS) {
+      srxlTextBuff[row][col] = c;
+      lineSent[row] = false;
+    }
+    return 0;
+}
+//**************************************************************************
+
+bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    static uint8_t lineNo = 0;
+    int lineCount = 0;
+
+    // Skip already sent lines...
+    while (lineSent[lineNo] &&
+           lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) {
+        lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
+        lineCount++;
+    }
+
+    sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN);
+    sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+    sbufWriteU8(dst, lineNo);
+    sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS);
+
+    lineSent[lineNo] = true;
+    lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
+
+    // Always send something, Always one user frame after the two mandatory frames
+    // I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true
+    // too keep the "Waltz" sequence intact.
+    return true;
+}
+
+#endif
+
+// Schedule array to decide how often each type of frame is sent
+// The frames are scheduled in sets of 3 frames, 2 mandatory and 1 user frame.
+// The user frame type is cycled for each set.
+// Example. QOS, RPM,.CURRENT, QOS, RPM, TEXT. QOS, RPM, CURRENT, etc etc
+
+#define SRXL_SCHEDULE_MANDATORY_COUNT  2 // Mandatory QOS and RPM sensors
+
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+#define SRXL_SCHEDULE_CMS_COUNT  1
+#else
+#define SRXL_SCHEDULE_CMS_COUNT  0
+#endif
+
+#define SRXL_SCHEDULE_USER_COUNT (1 + SRXL_SCHEDULE_CMS_COUNT)
+
+#define SRXL_SCHEDULE_COUNT_MAX  (SRXL_SCHEDULE_MANDATORY_COUNT + 1)
+#define SRXL_TOTAL_COUNT         (SRXL_SCHEDULE_MANDATORY_COUNT + SRXL_SCHEDULE_USER_COUNT)
+
+typedef bool (*srxlScheduleFnPtr)(sbuf_t *dst, timeUs_t currentTimeUs);
+
+const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_TOTAL_COUNT] = {
     /* must send srxlFrameQos, Rpm and then alternating items of our own */
     srxlFrameQos,
     srxlFrameRpm,
-    srxlFrameFlightPackCurrent
+    srxlFrameFlightPackCurrent,
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+    srxlFrameText,
+#endif
 };
 
-static void processSrxl(void)
+static void processSrxl(timeUs_t currentTimeUs)
 {
     static uint8_t srxlScheduleIndex = 0;
+    static uint8_t srxlScheduleUserIndex = 0;
 
     sbuf_t srxlPayloadBuf;
     sbuf_t *dst = &srxlPayloadBuf;
+    srxlScheduleFnPtr srxlFnPtr;
+
+    if (srxlScheduleIndex < SRXL_SCHEDULE_MANDATORY_COUNT) {
+        srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex];
+    } else {
+        srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex + srxlScheduleUserIndex];
+        srxlScheduleUserIndex = (srxlScheduleUserIndex + 1) % SRXL_SCHEDULE_USER_COUNT;
+    }
 
-    srxlScheduleFnPtr srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex];
     if (srxlFnPtr) {
         srxlInitializeFrame(dst);
-        srxlFnPtr(dst);
+        if (srxlFnPtr(dst, currentTimeUs)) {
-        srxlFinalize(dst);
+            srxlFinalize(dst);
+        }
     }
     srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX;
 }
@@ -227,7 +346,7 @@ void handleSrxlTelemetry(timeUs_t currentTimeUs)
     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
     if (currentTimeUs >= srxlLastCycleTime + SRXL_CYCLETIME_US) {
         srxlLastCycleTime = currentTimeUs;
-        processSrxl();
+        processSrxl(currentTimeUs);
     }
 }
 #endif

src/main/telemetry/srxl.h

@@ -22,3 +22,10 @@
 void initSrxlTelemetry(void);
 bool checkSrxlTelemetryState(void);
 void handleSrxlTelemetry(timeUs_t currentTimeUs);
+
+#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS 9
+#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 12 // Airware 1.20
+//#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 13 // Airware 1.21
+#define SPEKTRUM_SRXL_TEXTGEN_CLEAR_SCREEN 255
+
+int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c);

src/test/unit/osd_unittest.cc

@@ -188,7 +188,7 @@ TEST(OsdTest, TestInit)
 
     // then
     // display buffer should contain splash screen
-    displayPortTestBufferSubstring(7, 8, "MENU: THR MID");
+    displayPortTestBufferSubstring(7, 8, "MENU:THR MID");
     displayPortTestBufferSubstring(11, 9, "+ YAW LEFT");
     displayPortTestBufferSubstring(11, 10, "+ PITCH UP");