diff --git a/make/source.mk b/make/source.mk
index f2b5396fb1..8b46bf6d41 100644
--- a/make/source.mk
+++ b/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 \
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 3a5687e013..a409e07cdc 100644
--- a/src/main/cms/cms.c
+++ b/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)) {
- pageMaxRow = MAX_MENU_ITEMS(instance);
+ if (pageMaxRow > maxMenuItems) {
+ 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 = ¤tCtx.menu->entries[currentCtx.page * MAX_MENU_ITEMS(instance)];
+ pageTop = ¤tCtx.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
- char buff[CMS_DRAW_BUFFER_LEN];
+ #define CMS_DRAW_BUFFER_LEN 12
+ #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);
- colPos += strlen(str);
+ strncpy( buff, str, CMS_DRAW_BUFFER_LEN);
}
+ 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)));
- cmsPadToSize(buff, CMS_DRAW_BUFFER_LEN);
- cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+ strncpy(buff, str, CMS_DRAW_BUFFER_LEN);
+ cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN);
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 = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+ cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
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 = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+ cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
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 = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+ cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
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 = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+ cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
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 = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ???
+ cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
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.
- cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT");
+ // Shouldn't happen. Notify creator of this menu content
+#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;
- coloff += (p->type == OME_Label) ? 1 : 2;
- room -= displayWrite(pDisplay, coloff, i + top, p->text);
+ uint8_t coloff = leftMenuColumn;
+ coloff += (p->type == OME_Label) ? 0 : 1;
+ 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);
}
diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h
index 80f37b68ed..bf952a4cb3 100644
--- a/src/main/cms/cms.h
+++ b/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"
diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c
index f1634807c4..40984cb551 100644
--- a/src/main/cms/cms_menu_builtin.c
+++ b/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;
}
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index eaad12a8d1..3521a5ea84 100644
--- a/src/main/fc/fc_init.c
+++ b/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)) {
diff --git a/src/main/io/displayport_srxl.c b/src/main/io/displayport_srxl.c
new file mode 100644
index 0000000000..684e80df8c
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include
+#include
+
+#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
diff --git a/src/main/io/displayport_srxl.h b/src/main/io/displayport_srxl.h
new file mode 100644
index 0000000000..72019e0f13
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+displayPort_t *displayPortSrxlInit();
diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c
index 94d1f980ad..6ca6fe53d9 100644
--- a/src/main/rx/spektrum.c
+++ b/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;
}
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 013c842105..fea43db40c 100644
--- a/src/main/target/common_fc_pre.h
+++ b/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
diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c
index 44597c5f04..052b682ca0 100644
--- a/src/main/telemetry/srxl.c
+++ b/src/main/telemetry/srxl.c
@@ -50,8 +50,7 @@
#include "telemetry/telemetry.h"
#include "telemetry/srxl.h"
-
-#define SRXL_CYCLETIME_US 100000 // 100ms, 10 Hz
+#define SRXL_CYCLETIME_US 33000 // 33ms, 30 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);
- sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
- sbufWriteU16(dst, getAmperage() / 10);
- sbufWriteU16(dst, getMAhDrawn());
- sbufWriteU16(dst, 0x7fff); // temp A
- sbufWriteU16(dst, 0xffff);
- sbufWriteU16(dst, 0xffff);
- sbufWriteU16(dst, 0x7fff); // temp B
- sbufWriteU16(dst, 0xffff);
+ uint16_t amps = getAmperage() / 10;
+ uint16_t mah = getMAhDrawn();
+ static uint16_t sentAmps;
+ static uint16_t sentMah;
+ static timeUs_t lastTimeSentFPmAh = 0;
+
+ timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh;
+
+ 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
-#define SRXL_SCHEDULE_COUNT_MAX 3
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
-typedef void (*srxlScheduleFnPtr)(sbuf_t *dst);
-const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = {
+// Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display.
+
+#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);
- srxlFinalize(dst);
+ if (srxlFnPtr(dst, currentTimeUs)) {
+ 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
diff --git a/src/main/telemetry/srxl.h b/src/main/telemetry/srxl.h
index 33fc329e69..957e50704a 100644
--- a/src/main/telemetry/srxl.h
+++ b/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);
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index ec19ad19d8..7c828ff07c 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/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");