diff --git a/make/source.mk b/make/source.mk
index 142ab48576..f53514bd56 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -143,6 +143,7 @@ FC_SRC = \
io/displayport_oled.c \
io/displayport_srxl.c \
io/displayport_crsf.c \
+ io/displayport_hott.c \
io/rcdevice_cam.c \
io/rcdevice.c \
io/gps.c \
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 3fcfe78e20..daa0849a8b 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -160,6 +160,7 @@ static uint8_t leftMenuColumn;
static uint8_t rightMenuColumn;
static uint8_t maxMenuItems;
static uint8_t linesPerMenuItem;
+static uint8_t externKey = CMS_KEY_NONE;
bool cmsInMenu = false;
@@ -764,14 +765,6 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
#define IS_LO(X) (rcData[X] < 1250)
#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
-#define KEY_NONE 0
-#define KEY_UP 1
-#define KEY_DOWN 2
-#define KEY_LEFT 3
-#define KEY_RIGHT 4
-#define KEY_ESC 5
-#define KEY_MENU 6
-
#define BUTTON_TIME 250 // msec
#define BUTTON_PAUSE 500 // msec
@@ -783,17 +776,17 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
if (!currentCtx.menu)
return res;
- if (key == KEY_MENU) {
+ if (key == CMS_KEY_MENU) {
cmsMenuOpen();
return BUTTON_PAUSE;
}
- if (key == KEY_ESC) {
+ if (key == CMS_KEY_ESC) {
cmsMenuBack(pDisplay);
return BUTTON_PAUSE;
}
- if (key == KEY_DOWN) {
+ if (key == CMS_KEY_DOWN) {
if (currentCtx.cursorRow < pageMaxRow) {
currentCtx.cursorRow++;
} else {
@@ -802,7 +795,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
}
}
- if (key == KEY_UP) {
+ if (key == CMS_KEY_UP) {
currentCtx.cursorRow--;
// Skip non-title labels
@@ -816,14 +809,14 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
}
}
- if (key == KEY_DOWN || key == KEY_UP)
+ if (key == CMS_KEY_DOWN || key == CMS_KEY_UP)
return res;
p = pageTop + currentCtx.cursorRow;
switch (p->type) {
case OME_Submenu:
- if (key == KEY_RIGHT) {
+ if (key == CMS_KEY_RIGHT) {
cmsMenuChange(pDisplay, p->data);
res = BUTTON_PAUSE;
}
@@ -831,7 +824,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
case OME_Funcall:;
long retval;
- if (p->func && key == KEY_RIGHT) {
+ if (p->func && key == CMS_KEY_RIGHT) {
retval = p->func(pDisplay, p->data);
if (retval == MENU_CHAIN_BACK)
cmsMenuBack(pDisplay);
@@ -840,7 +833,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
break;
case OME_OSD_Exit:
- if (p->func && key == KEY_RIGHT) {
+ if (p->func && key == CMS_KEY_RIGHT) {
p->func(pDisplay, p->data);
res = BUTTON_PAUSE;
}
@@ -854,7 +847,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
case OME_Bool:
if (p->data) {
uint8_t *val = p->data;
- if (key == KEY_RIGHT)
+ if (key == CMS_KEY_RIGHT)
*val = 1;
else
*val = 0;
@@ -867,7 +860,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
if (p->data) {
uint16_t *val = (uint16_t *)p->data;
- if (key == KEY_RIGHT)
+ if (key == CMS_KEY_RIGHT)
*val |= VISIBLE_FLAG;
else
*val %= ~VISIBLE_FLAG;
@@ -880,7 +873,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
case OME_FLOAT:
if (p->data) {
OSD_UINT8_t *ptr = p->data;
- if (key == KEY_RIGHT) {
+ if (key == CMS_KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += ptr->step;
}
@@ -899,7 +892,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
if (p->type == OME_TAB) {
OSD_TAB_t *ptr = p->data;
- if (key == KEY_RIGHT) {
+ if (key == CMS_KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += 1;
}
@@ -916,7 +909,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
case OME_INT8:
if (p->data) {
OSD_INT8_t *ptr = p->data;
- if (key == KEY_RIGHT) {
+ if (key == CMS_KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += ptr->step;
}
@@ -934,7 +927,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
case OME_UINT16:
if (p->data) {
OSD_UINT16_t *ptr = p->data;
- if (key == KEY_RIGHT) {
+ if (key == CMS_KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += ptr->step;
}
@@ -952,7 +945,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
case OME_INT16:
if (p->data) {
OSD_INT16_t *ptr = p->data;
- if (key == KEY_RIGHT) {
+ if (key == CMS_KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += ptr->step;
}
@@ -981,6 +974,12 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
return res;
}
+void cmsSetExternKey(uint8_t extKey)
+{
+ if (externKey == CMS_KEY_NONE)
+ externKey = extKey;
+}
+
uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, int repeatCount)
{
uint16_t ret = 0;
@@ -1026,72 +1025,77 @@ void cmsUpdate(uint32_t currentTimeUs)
// Scan 'key' first
//
- uint8_t key = KEY_NONE;
+ uint8_t key = CMS_KEY_NONE;
- if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
- key = KEY_MENU;
- }
- else if (IS_HI(PITCH)) {
- key = KEY_UP;
- }
- else if (IS_LO(PITCH)) {
- key = KEY_DOWN;
- }
- else if (IS_LO(ROLL)) {
- key = KEY_LEFT;
- }
- else if (IS_HI(ROLL)) {
- key = KEY_RIGHT;
- }
- else if (IS_HI(YAW) || IS_LO(YAW))
- {
- key = KEY_ESC;
- }
-
- if (key == KEY_NONE) {
- // No 'key' pressed, reset repeat control
- holdCount = 1;
- repeatCount = 1;
- repeatBase = 0;
+ if (externKey != CMS_KEY_NONE) {
+ rcDelayMs = cmsHandleKey(pCurrentDisplay, externKey);
+ externKey = CMS_KEY_NONE;
} else {
- // The 'key' is being pressed; keep counting
- ++holdCount;
- }
+ if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
+ key = CMS_KEY_MENU;
+ }
+ else if (IS_HI(PITCH)) {
+ key = CMS_KEY_UP;
+ }
+ else if (IS_LO(PITCH)) {
+ key = CMS_KEY_DOWN;
+ }
+ else if (IS_LO(ROLL)) {
+ key = CMS_KEY_LEFT;
+ }
+ else if (IS_HI(ROLL)) {
+ key = CMS_KEY_RIGHT;
+ }
+ else if (IS_HI(YAW) || IS_LO(YAW))
+ {
+ key = CMS_KEY_ESC;
+ }
- if (rcDelayMs > 0) {
- rcDelayMs -= (currentTimeMs - lastCalledMs);
- } else if (key) {
- rcDelayMs = cmsHandleKeyWithRepeat(pCurrentDisplay, key, repeatCount);
+ if (key == CMS_KEY_NONE) {
+ // No 'key' pressed, reset repeat control
+ holdCount = 1;
+ repeatCount = 1;
+ repeatBase = 0;
+ } else {
+ // The 'key' is being pressed; keep counting
+ ++holdCount;
+ }
- // Key repeat effect is implemented in two phases.
- // First phldase is to decrease rcDelayMs reciprocal to hold time.
- // When rcDelayMs reached a certain limit (scheduling interval),
- // repeat rate will not raise anymore, so we call key handler
- // multiple times (repeatCount).
- //
- // XXX Caveat: Most constants are adjusted pragmatically.
- // XXX Rewrite this someday, so it uses actual hold time instead
- // of holdCount, which depends on the scheduling interval.
+ if (rcDelayMs > 0) {
+ rcDelayMs -= (currentTimeMs - lastCalledMs);
+ } else if (key) {
+ rcDelayMs = cmsHandleKeyWithRepeat(pCurrentDisplay, key, repeatCount);
- if (((key == KEY_LEFT) || (key == KEY_RIGHT)) && (holdCount > 20)) {
+ // Key repeat effect is implemented in two phases.
+ // First phldase is to decrease rcDelayMs reciprocal to hold time.
+ // When rcDelayMs reached a certain limit (scheduling interval),
+ // repeat rate will not raise anymore, so we call key handler
+ // multiple times (repeatCount).
+ //
+ // XXX Caveat: Most constants are adjusted pragmatically.
+ // XXX Rewrite this someday, so it uses actual hold time instead
+ // of holdCount, which depends on the scheduling interval.
- // Decrease rcDelayMs reciprocally
+ if (((key == CMS_KEY_LEFT) || (key == CMS_KEY_RIGHT)) && (holdCount > 20)) {
- rcDelayMs /= (holdCount - 20);
+ // Decrease rcDelayMs reciprocally
- // When we reach the scheduling limit,
+ rcDelayMs /= (holdCount - 20);
- if (rcDelayMs <= 50) {
+ // When we reach the scheduling limit,
- // start calling handler multiple times.
+ if (rcDelayMs <= 50) {
- if (repeatBase == 0)
- repeatBase = holdCount;
+ // start calling handler multiple times.
- repeatCount = repeatCount + (holdCount - repeatBase) / 5;
+ if (repeatBase == 0)
+ repeatBase = holdCount;
- if (repeatCount > 5) {
- repeatCount= 5;
+ repeatCount = repeatCount + (holdCount - repeatBase) / 5;
+
+ if (repeatCount > 5) {
+ repeatCount= 5;
+ }
}
}
}
diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h
index 0e46fe34f5..83b2b7f072 100644
--- a/src/main/cms/cms.h
+++ b/src/main/cms/cms.h
@@ -24,6 +24,14 @@
#include "common/time.h"
+#define CMS_KEY_NONE 0
+#define CMS_KEY_UP 1
+#define CMS_KEY_DOWN 2
+#define CMS_KEY_LEFT 3
+#define CMS_KEY_RIGHT 4
+#define CMS_KEY_ESC 5
+#define CMS_KEY_MENU 6
+
extern bool cmsInMenu;
// Device management
@@ -39,6 +47,7 @@ void cmsMenuOpen(void);
long cmsMenuChange(displayPort_t *pPort, const void *ptr);
long cmsMenuExit(displayPort_t *pPort, const void *ptr);
void cmsUpdate(uint32_t currentTimeUs);
+void cmsSetExternKey(uint8_t extKey);
#define CMS_STARTUP_HELP_TEXT1 "MENU:THR MID"
#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT"
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index ee8da47916..dc20da89ec 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -112,6 +112,7 @@
#include "io/beeper.h"
#include "io/displayport_max7456.h"
#include "io/displayport_srxl.h"
+#include "io/displayport_hott.h"
#include "io/displayport_crsf.h"
#include "io/serial.h"
#include "io/flashfs.h"
@@ -607,6 +608,10 @@ void init(void)
cmsDisplayPortRegister(displayPortCrsfInit());
#endif
+#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+ cmsDisplayPortRegister(displayPortHottInit());
+#endif
+
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
gpsInit();
diff --git a/src/main/io/displayport_hott.c b/src/main/io/displayport_hott.c
new file mode 100644
index 0000000000..a79f093b54
--- /dev/null
+++ b/src/main/io/displayport_hott.c
@@ -0,0 +1,187 @@
+/*
+ * 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_HOTT_TEXTMODE) && defined (USE_CMS)
+
+// Fixme:
+// Let the CMS believe we have a bigger display to avoid empty rows and columns
+// Better make this configurable in CMS
+#define ROW_CORRECTION 1
+#define COL_CORRECTION 2
+
+#include "common/utils.h"
+#include "cms/cms.h"
+#include "telemetry/hott.h"
+
+displayPort_t hottDisplayPort;
+
+static int hottDrawScreen(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static int hottScreenSize(const displayPort_t *displayPort)
+{
+ return displayPort->rows * displayPort->cols;
+}
+
+static int hottWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c)
+{
+ UNUSED(displayPort);
+
+
+ // Correct fake display dimensions
+ if (row <= ROW_CORRECTION - 1)
+ return 0;
+ else
+ row -= ROW_CORRECTION;
+
+ if (col <= COL_CORRECTION - 1)
+ return 0;
+ else
+ col -= COL_CORRECTION;
+
+ if (row >= HOTT_TEXTMODE_DISPLAY_ROWS || col >= HOTT_TEXTMODE_DISPLAY_COLUMNS)
+ return 0;
+
+ hottTextmodeWriteChar(col, row, c);
+ return 0;
+}
+
+static int hottWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s)
+{
+ while (*s) {
+ hottWriteChar(displayPort, col++, row, *(s++));
+ }
+ return 0;
+}
+
+static int hottClearScreen(displayPort_t *displayPort)
+{
+ for (int row = 0; row < displayPort->rows; row++) {
+ for (int col= 0; col < displayPort->cols; col++) {
+ hottWriteChar(displayPort, col, row, ' ');
+ }
+ }
+ return 0;
+}
+
+static bool hottIsTransferInProgress(const displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return false;
+}
+
+static int hottHeartbeat(displayPort_t *displayPort)
+{
+ if (!hottTextmodeIsAlive())
+ cmsMenuExit(displayPort, (void*)CMS_EXIT_SAVE);
+
+ return 0;
+}
+
+static void hottResync(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+}
+
+static uint32_t hottTxBytesFree(const displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return UINT32_MAX;
+}
+
+static int hottGrab(displayPort_t *displayPort)
+{
+ hottTextmodeGrab();
+ return displayPort->grabCount = 1;
+}
+
+static int hottRelease(displayPort_t *displayPort)
+{
+ int cnt = displayPort->grabCount = 0;
+ hottClearScreen(displayPort);
+ hottTextmodeExit();
+ return cnt;
+}
+
+static const displayPortVTable_t hottVTable = {
+ .grab = hottGrab,
+ .release = hottRelease,
+ .clearScreen = hottClearScreen,
+ .drawScreen = hottDrawScreen,
+ .screenSize = hottScreenSize,
+ .writeString = hottWriteString,
+ .writeChar = hottWriteChar,
+ .isTransferInProgress = hottIsTransferInProgress,
+ .heartbeat = hottHeartbeat,
+ .resync = hottResync,
+ .txBytesFree = hottTxBytesFree
+};
+
+displayPort_t *displayPortHottInit()
+{
+ hottDisplayPort.device = NULL;
+ displayInit(&hottDisplayPort, &hottVTable);
+ hottDisplayPort.rows = HOTT_TEXTMODE_DISPLAY_ROWS + ROW_CORRECTION * 2;
+ hottDisplayPort.cols = HOTT_TEXTMODE_DISPLAY_COLUMNS + COL_CORRECTION * 2;
+ return &hottDisplayPort;
+}
+
+void hottCmsOpen()
+{
+ if (!cmsInMenu) {
+ cmsDisplayPortSelect(&hottDisplayPort);
+ cmsMenuOpen();
+ }
+}
+
+void hottSetCmsKey(uint8_t hottKey, bool esc)
+{
+ switch (hottKey) {
+ case HOTTV4_BUTTON_DEC:
+ cmsSetExternKey(CMS_KEY_UP);
+ break;
+ case HOTTV4_BUTTON_INC:
+ cmsSetExternKey(CMS_KEY_DOWN);
+ break;
+ case HOTTV4_BUTTON_SET:
+ if (cmsInMenu)
+ cmsMenuExit(pCurrentDisplay, (void*)CMS_EXIT_SAVE);
+ cmsSetExternKey(CMS_KEY_NONE);
+ break;
+ case HOTTV4_BUTTON_NEXT:
+ cmsSetExternKey(CMS_KEY_RIGHT);
+ break;
+ case HOTTV4_BUTTON_PREV:
+ cmsSetExternKey(CMS_KEY_LEFT);
+ if (esc)
+ cmsMenuOpen();
+ break;
+ default:
+ cmsSetExternKey(CMS_KEY_NONE);
+ break;
+ }
+}
+
+#endif
diff --git a/src/main/io/displayport_hott.h b/src/main/io/displayport_hott.h
new file mode 100644
index 0000000000..f399cf9fd4
--- /dev/null
+++ b/src/main/io/displayport_hott.h
@@ -0,0 +1,25 @@
+/*
+ * 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
+#include "drivers/display.h"
+
+displayPort_t *displayPortHottInit();
+displayPort_t hottDisplayPort;
+
+void hottCmsOpen();
+void hottSetCmsKey(uint8_t hottKey, bool esc);
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 4ef7604f8d..4a8278870c 100644
--- a/src/main/target/common_fc_pre.h
+++ b/src/main/target/common_fc_pre.h
@@ -220,4 +220,5 @@
#define USE_UNCOMMON_MIXERS
#define USE_SIGNATURE
#define USE_ABSOLUTE_CONTROL
+#define USE_HOTT_TEXTMODE
#endif
diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c
index f756d3462b..6bd2a47176 100644
--- a/src/main/telemetry/hott.c
+++ b/src/main/telemetry/hott.c
@@ -26,6 +26,7 @@
* Carsten Giesen - cGiesen - Baseflight port
* Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering
* Adam Majerczyk - HoTT-for-ardupilot from which some information and ideas are borrowed.
+ * Scavanger & Ziege-One: CMS Textmode addon
*
* https://github.com/obayer/MultiWii-HoTT
* https://github.com/oBayer/MultiHoTT-Module
@@ -87,6 +88,14 @@
#include "telemetry/hott.h"
#include "telemetry/telemetry.h"
+#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+#include "scheduler/scheduler.h"
+#include "io/displayport_hott.h"
+
+#define HOTT_TEXTMODE_RX_SCHEDULE 5000
+#define HOTT_TEXTMODE_TX_DELAY_US 1000
+#endif
+
//#define HOTT_DEBUG
#define HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ ((1000 * 1000) / 5)
@@ -94,6 +103,9 @@
#define HOTT_TX_DELAY_US 3000
#define MILLISECONDS_IN_A_SECOND 1000
+static uint32_t rxSchedule = HOTT_RX_SCHEDULE;
+static uint32_t txDelayUs = HOTT_TX_DELAY_US;
+
static uint32_t lastHoTTRequestCheckAt = 0;
static uint32_t lastMessagesPreparedAt = 0;
static uint32_t lastHottAlarmSoundTime = 0;
@@ -118,6 +130,20 @@ static portSharing_e hottPortSharing;
static HOTT_GPS_MSG_t hottGPSMessage;
static HOTT_EAM_MSG_t hottEAMMessage;
+#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+static HOTT_TEXTMODE_MSG_t hottTextModeMessage;
+static bool textmodeIsAlive = false;
+static int32_t telemetryPeriod = 0;
+
+static void initialiseTextmodeMessage(HOTT_TEXTMODE_MSG_t *msg)
+{
+ msg->start = HOTT_TEXTMODE_START;
+ msg->esc = HOTT_EAM_SENSOR_TEXT_ID;
+ msg->warning = 0;
+ msg->stop = HOTT_TEXTMODE_STOP;
+}
+#endif
+
static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size)
{
memset(msg, 0, size);
@@ -151,6 +177,9 @@ static void initialiseMessages(void)
#ifdef USE_GPS
initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage));
#endif
+#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+ initialiseTextmodeMessage(&hottTextModeMessage);
+#endif
}
#ifdef USE_GPS
@@ -415,8 +444,88 @@ static void hottPrepareMessages(void) {
#endif
}
+#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+static void hottTextmodeStart()
+{
+ // Increase menu speed
+ cfTaskInfo_t taskInfo;
+ getTaskInfo(TASK_TELEMETRY, &taskInfo);
+ telemetryPeriod = taskInfo.desiredPeriod;
+ rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(1000));
+
+ rxSchedule = HOTT_TEXTMODE_RX_SCHEDULE;
+ txDelayUs = HOTT_TEXTMODE_TX_DELAY_US;
+}
+
+static void hottTextmodeStop()
+{
+ // Set back to avoid slow down of the FC
+ if (telemetryPeriod > 0)
+ rescheduleTask(TASK_TELEMETRY, telemetryPeriod);
+
+ rxSchedule = HOTT_RX_SCHEDULE;
+ txDelayUs = HOTT_TX_DELAY_US;
+}
+
+bool hottTextmodeIsAlive()
+{
+ return textmodeIsAlive;
+}
+
+void hottTextmodeGrab()
+{
+ hottTextModeMessage.esc = HOTT_EAM_SENSOR_TEXT_ID;
+}
+
+void hottTextmodeExit()
+{
+ hottTextModeMessage.esc = HOTT_TEXTMODE_ESC;
+}
+
+void hottTextmodeWriteChar(uint8_t column, uint8_t row, char c)
+{
+ if (column < HOTT_TEXTMODE_DISPLAY_COLUMNS && row < HOTT_TEXTMODE_DISPLAY_ROWS) {
+ if (hottTextModeMessage.txt[row][column] != c)
+ hottTextModeMessage.txt[row][column] = c;
+ }
+}
+
+static void processHottTextModeRequest(const uint8_t cmd)
+{
+ static bool setEscBack = false;
+
+ if (!textmodeIsAlive)
+ {
+ hottTextmodeStart();
+ textmodeIsAlive = true;
+ }
+
+ if ((cmd & 0xF0) != HOTT_EAM_SENSOR_TEXT_ID)
+ return;
+
+ if (setEscBack) {
+ hottTextModeMessage.esc = HOTT_EAM_SENSOR_TEXT_ID;
+ setEscBack = false;
+ }
+
+ if (hottTextModeMessage.esc != HOTT_TEXTMODE_ESC)
+ hottCmsOpen();
+ else
+ setEscBack = true;
+
+ hottSetCmsKey(cmd & 0x0f, hottTextModeMessage.esc == HOTT_TEXTMODE_ESC);
+ hottSendResponse((uint8_t *)&hottTextModeMessage, sizeof(hottTextModeMessage));
+}
+#endif
+
static void processBinaryModeRequest(uint8_t address)
{
+#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+ if (textmodeIsAlive) {
+ hottTextmodeStop();
+ textmodeIsAlive = false;
+ }
+#endif
#ifdef HOTT_DEBUG
static uint8_t hottBinaryRequests = 0;
@@ -474,7 +583,7 @@ static void hottCheckSerialData(uint32_t currentMicros)
lookingForRequest = false;
return;
} else {
- bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE;
+ bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= rxSchedule;
if (!enoughTimePassed) {
return;
@@ -495,6 +604,11 @@ static void hottCheckSerialData(uint32_t currentMicros)
*/
processBinaryModeRequest(address);
}
+#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+ else if (requestId == HOTTV4_TEXT_MODE_REQUEST_ID){
+ processHottTextModeRequest(address);
+ }
+#endif
}
static void hottSendTelemetryData(void) {
@@ -568,7 +682,7 @@ void handleHoTTTelemetry(timeUs_t currentTimeUs)
return;
if (hottIsSending) {
- if (currentTimeUs - serialTimer < HOTT_TX_DELAY_US) {
+ if (currentTimeUs - serialTimer < txDelayUs) {
return;
}
}
diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h
index ba5bd39a16..c1f3eefedd 100644
--- a/src/main/telemetry/hott.h
+++ b/src/main/telemetry/hott.h
@@ -23,7 +23,8 @@
* Dominic Clifton/Hydra
* Carsten Giesen
* Adam Majerczyk (majerczyk.adam@gmail.com)
- * Texmode add-on by Michi (mamaretti32@gmail.com)
+ * Textmode add-on by Michi (mamaretti32@gmail.com)
+ * Scavanger & Ziege-One: CMS Textmode addon
*/
#pragma once
@@ -35,12 +36,12 @@
#define HOTTV4_TEXT_MODE_REQUEST_ID 0x7f
#define HOTTV4_BINARY_MODE_REQUEST_ID 0x80
-#define HOTTV4_BUTTON_DEC 0xEB
-#define HOTTV4_BUTTON_INC 0xED
-#define HOTTV4_BUTTON_SET 0xE9
-#define HOTTV4_BUTTON_NIL 0x0F
-#define HOTTV4_BUTTON_NEXT 0xEE
-#define HOTTV4_BUTTON_PREV 0xE7
+#define HOTTV4_BUTTON_DEC 0xB
+#define HOTTV4_BUTTON_INC 0xD
+#define HOTTV4_BUTTON_SET 0x9
+#define HOTTV4_BUTTON_NIL 0xF
+#define HOTTV4_BUTTON_NEXT 0xE
+#define HOTTV4_BUTTON_PREV 0x7
#define HOTT_EAM_OFFSET_HEIGHT 500
#define HOTT_EAM_OFFSET_M2S 72
@@ -105,18 +106,21 @@ typedef enum {
#define HOTT_EAM_SENSOR_TEXT_ID 0xE0 // Electric Air Module ID
#define HOTT_GPS_SENSOR_TEXT_ID 0xA0 // GPS Module ID
-
-#define HOTT_TEXTMODE_MSG_TEXT_LEN 168
+#define HOTT_TEXTMODE_DISPLAY_ROWS 8
+#define HOTT_TEXTMODE_DISPLAY_COLUMNS 21
+#define HOTT_TEXTMODE_START 0x7B
+#define HOTT_TEXTMODE_STOP 0x7D
+#define HOTT_TEXTMODE_ESC 0x01
//Text mode msgs type
-struct HOTT_TEXTMODE_MSG {
- uint8_t start_byte; //#01 constant value 0x7b
- uint8_t fill1; //#02 constant value 0x00
- uint8_t warning_beeps;//#03 1=A 2=B ...
- uint8_t msg_txt[HOTT_TEXTMODE_MSG_TEXT_LEN]; //#04 ASCII text to display to
+typedef struct HOTT_TEXTMODE_MSG_s {
+ uint8_t start; //#01 constant value 0x7b
+ uint8_t esc; //#02 Low byte: Sensor ID or 0x01 for escape
+ uint8_t warning; //#03 1=A 2=B ...
+ uint8_t txt[HOTT_TEXTMODE_DISPLAY_ROWS][HOTT_TEXTMODE_DISPLAY_COLUMNS]; //#04 ASCII text to display to
// Bit 7 = 1 -> Inverse character display
// Display 21x8
- uint8_t stop_byte; //#171 constant value 0x7d
-};
+ uint8_t stop; //#171 constant value 0x7d
+} HOTT_TEXTMODE_MSG_t;
typedef struct HOTT_GAM_MSG_s {
uint8_t start_byte; //#01 start uint8_t constant value 0x7c
@@ -497,6 +501,13 @@ void initHoTTTelemetry(void);
void configureHoTTTelemetryPort(void);
void freeHoTTTelemetryPort(void);
+#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+bool hottTextmodeIsAlive();
+void hottTextmodeGrab();
+void hottTextmodeExit();
+void hottTextmodeWriteChar(uint8_t column, uint8_t row, char c);
+#endif
+
uint32_t getHoTTTelemetryProviderBaudRate(void);
void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage);