From 31830dc10ca25a2e5071de163f14f15192322dcd Mon Sep 17 00:00:00 2001 From: Andi Kanzler Date: Thu, 21 Jun 2018 16:11:28 +0200 Subject: [PATCH 1/4] Bring the CMS to the HoTT-Textmode. --- make/source.mk | 1 + src/main/cms/cms.c | 156 +++++++++++++------------- src/main/cms/cms.h | 9 ++ src/main/fc/fc_init.c | 5 + src/main/io/displayport_hott.c | 187 ++++++++++++++++++++++++++++++++ src/main/io/displayport_hott.h | 25 +++++ src/main/target/common_fc_pre.h | 1 + src/main/telemetry/hott.c | 118 +++++++++++++++++++- src/main/telemetry/hott.h | 43 +++++--- 9 files changed, 451 insertions(+), 94 deletions(-) create mode 100644 src/main/io/displayport_hott.c create mode 100644 src/main/io/displayport_hott.h 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); From 1b98b1e2aa64b8d6e1e665ae0dbf876f0d9d36b5 Mon Sep 17 00:00:00 2001 From: Andi Kanzler Date: Wed, 27 Jun 2018 17:43:58 +0200 Subject: [PATCH 2/4] Improvements from review. --- src/main/cms/cms.c | 10 +++++----- src/main/cms/cms.h | 16 +++++++++------- src/main/io/displayport_hott.c | 31 +++++++++++++------------------ src/main/telemetry/hott.c | 29 ++++++++++++++++------------- src/main/telemetry/hott.h | 4 ++-- 5 files changed, 45 insertions(+), 45 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index daa0849a8b..2cb14f2e57 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -160,7 +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; +static cms_key_e externKey = CMS_KEY_NONE; bool cmsInMenu = false; @@ -768,7 +768,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) #define BUTTON_TIME 250 // msec #define BUTTON_PAUSE 500 // msec -STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) +STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, cms_key_e key) { uint16_t res = BUTTON_TIME; OSD_Entry *p; @@ -974,13 +974,13 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) return res; } -void cmsSetExternKey(uint8_t extKey) +void cmsSetExternKey(cms_key_e extKey) { if (externKey == CMS_KEY_NONE) externKey = extKey; } -uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, int repeatCount) +uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, cms_key_e key, int repeatCount) { uint16_t ret = 0; @@ -1025,7 +1025,7 @@ void cmsUpdate(uint32_t currentTimeUs) // Scan 'key' first // - uint8_t key = CMS_KEY_NONE; + cms_key_e key = CMS_KEY_NONE; if (externKey != CMS_KEY_NONE) { rcDelayMs = cmsHandleKey(pCurrentDisplay, externKey); diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index 83b2b7f072..afcf616eb0 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -24,13 +24,15 @@ #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 +typedef enum { + CMS_KEY_NONE, + CMS_KEY_UP, + CMS_KEY_DOWN, + CMS_KEY_LEFT, + CMS_KEY_RIGHT, + CMS_KEY_ESC, + CMS_KEY_MENU +} cms_key_e; extern bool cmsInMenu; diff --git a/src/main/io/displayport_hott.c b/src/main/io/displayport_hott.c index a79f093b54..a1967e5714 100644 --- a/src/main/io/displayport_hott.c +++ b/src/main/io/displayport_hott.c @@ -49,20 +49,12 @@ static int hottWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, u { 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; + if (row < ROW_CORRECTION || row >= HOTT_TEXTMODE_DISPLAY_ROWS + ROW_CORRECTION + || col < COL_CORRECTION || col >= HOTT_TEXTMODE_DISPLAY_COLUMNS + COL_CORRECTION) { + return 0; + } + row -= ROW_CORRECTION; + col -= COL_CORRECTION; hottTextmodeWriteChar(col, row, c); return 0; @@ -94,8 +86,9 @@ static bool hottIsTransferInProgress(const displayPort_t *displayPort) static int hottHeartbeat(displayPort_t *displayPort) { - if (!hottTextmodeIsAlive()) + if (!hottTextmodeIsAlive()) { cmsMenuExit(displayPort, (void*)CMS_EXIT_SAVE); + } return 0; } @@ -156,7 +149,7 @@ void hottCmsOpen() } } -void hottSetCmsKey(uint8_t hottKey, bool esc) +void hottSetCmsKey(uint8_t hottKey, bool keepCmsOpen) { switch (hottKey) { case HOTTV4_BUTTON_DEC: @@ -166,8 +159,9 @@ void hottSetCmsKey(uint8_t hottKey, bool esc) cmsSetExternKey(CMS_KEY_DOWN); break; case HOTTV4_BUTTON_SET: - if (cmsInMenu) + if (cmsInMenu) { cmsMenuExit(pCurrentDisplay, (void*)CMS_EXIT_SAVE); + } cmsSetExternKey(CMS_KEY_NONE); break; case HOTTV4_BUTTON_NEXT: @@ -175,8 +169,9 @@ void hottSetCmsKey(uint8_t hottKey, bool esc) break; case HOTTV4_BUTTON_PREV: cmsSetExternKey(CMS_KEY_LEFT); - if (esc) + if (keepCmsOpen) { // Make sure CMS is open until textmode is closed. cmsMenuOpen(); + } break; default: cmsSetExternKey(CMS_KEY_NONE); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 6bd2a47176..72c3d046f2 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -92,6 +92,7 @@ #include "scheduler/scheduler.h" #include "io/displayport_hott.h" +#define HOTT_TEXTMODE_TASK_PERIOD 1000 #define HOTT_TEXTMODE_RX_SCHEDULE 5000 #define HOTT_TEXTMODE_TX_DELAY_US 1000 #endif @@ -131,11 +132,11 @@ 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 hottTextModeMsg_t hottTextModeMessage; static bool textmodeIsAlive = false; -static int32_t telemetryPeriod = 0; +static int32_t telemetryTaskPeriod = 0; -static void initialiseTextmodeMessage(HOTT_TEXTMODE_MSG_t *msg) +static void initialiseTextmodeMessage(hottTextModeMsg_t *msg) { msg->start = HOTT_TEXTMODE_START; msg->esc = HOTT_EAM_SENSOR_TEXT_ID; @@ -450,8 +451,8 @@ static void hottTextmodeStart() // Increase menu speed cfTaskInfo_t taskInfo; getTaskInfo(TASK_TELEMETRY, &taskInfo); - telemetryPeriod = taskInfo.desiredPeriod; - rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(1000)); + telemetryTaskPeriod = taskInfo.desiredPeriod; + rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(HOTT_TEXTMODE_TASK_PERIOD)); rxSchedule = HOTT_TEXTMODE_RX_SCHEDULE; txDelayUs = HOTT_TEXTMODE_TX_DELAY_US; @@ -460,8 +461,9 @@ static void hottTextmodeStart() static void hottTextmodeStop() { // Set back to avoid slow down of the FC - if (telemetryPeriod > 0) - rescheduleTask(TASK_TELEMETRY, telemetryPeriod); + if (telemetryTaskPeriod > 0) { + rescheduleTask(TASK_TELEMETRY, telemetryTaskPeriod); + } rxSchedule = HOTT_RX_SCHEDULE; txDelayUs = HOTT_TX_DELAY_US; @@ -494,24 +496,25 @@ static void processHottTextModeRequest(const uint8_t cmd) { static bool setEscBack = false; - if (!textmodeIsAlive) - { + if (!textmodeIsAlive) { hottTextmodeStart(); textmodeIsAlive = true; } - if ((cmd & 0xF0) != HOTT_EAM_SENSOR_TEXT_ID) + 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) + if (hottTextModeMessage.esc != HOTT_TEXTMODE_ESC) { hottCmsOpen(); - else + } else { setEscBack = true; + } hottSetCmsKey(cmd & 0x0f, hottTextModeMessage.esc == HOTT_TEXTMODE_ESC); hottSendResponse((uint8_t *)&hottTextModeMessage, sizeof(hottTextModeMessage)); @@ -605,7 +608,7 @@ static void hottCheckSerialData(uint32_t currentMicros) processBinaryModeRequest(address); } #if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) - else if (requestId == HOTTV4_TEXT_MODE_REQUEST_ID){ + else if (requestId == HOTTV4_TEXT_MODE_REQUEST_ID) { processHottTextModeRequest(address); } #endif diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index c1f3eefedd..e3be8aa7c2 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -112,7 +112,7 @@ typedef enum { #define HOTT_TEXTMODE_STOP 0x7D #define HOTT_TEXTMODE_ESC 0x01 //Text mode msgs type -typedef struct HOTT_TEXTMODE_MSG_s { +typedef struct hottTextModeMsg_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 ... @@ -120,7 +120,7 @@ typedef struct HOTT_TEXTMODE_MSG_s { // Bit 7 = 1 -> Inverse character display // Display 21x8 uint8_t stop; //#171 constant value 0x7d -} HOTT_TEXTMODE_MSG_t; +} hottTextModeMsg_t; typedef struct HOTT_GAM_MSG_s { uint8_t start_byte; //#01 start uint8_t constant value 0x7c From a63beb5239ff26fa25b23451db395bbfe518a2a4 Mon Sep 17 00:00:00 2001 From: Andi Kanzler Date: Wed, 27 Jun 2018 18:49:07 +0200 Subject: [PATCH 3/4] Oops. Forget that extKey is now cms_key_e. --- src/main/cms/cms.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index afcf616eb0..18efcec23a 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -49,7 +49,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); +void cmsSetExternKey(cms_key_e extKey); #define CMS_STARTUP_HELP_TEXT1 "MENU:THR MID" #define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" From 0b01718869f63b15d0dd94daa22f3aae805b2c7d Mon Sep 17 00:00:00 2001 From: Andi Kanzler Date: Fri, 6 Jul 2018 15:59:20 +0200 Subject: [PATCH 4/4] Better initialization, according to #6286 --- src/main/fc/fc_init.c | 5 ----- src/main/io/displayport_hott.c | 5 +++++ src/main/io/displayport_hott.h | 1 + src/main/telemetry/hott.c | 4 ++++ 4 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index dc20da89ec..ee8da47916 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -112,7 +112,6 @@ #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" @@ -608,10 +607,6 @@ 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 index a1967e5714..21078ad0cb 100644 --- a/src/main/io/displayport_hott.c +++ b/src/main/io/displayport_hott.c @@ -141,6 +141,11 @@ displayPort_t *displayPortHottInit() return &hottDisplayPort; } +void hottDisplayportRegister() +{ + cmsDisplayPortRegister(displayPortHottInit()); +} + void hottCmsOpen() { if (!cmsInMenu) { diff --git a/src/main/io/displayport_hott.h b/src/main/io/displayport_hott.h index f399cf9fd4..43df02061e 100644 --- a/src/main/io/displayport_hott.h +++ b/src/main/io/displayport_hott.h @@ -21,5 +21,6 @@ displayPort_t *displayPortHottInit(); displayPort_t hottDisplayPort; +void hottDisplayportRegister(); void hottCmsOpen(); void hottSetCmsKey(uint8_t hottKey, bool esc); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 72c3d046f2..962f9883f9 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -342,6 +342,10 @@ void initHoTTTelemetry(void) portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT); hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT); +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + hottDisplayportRegister(); +#endif + initialiseMessages(); }