diff --git a/Makefile b/Makefile index f20832a712..ba4fd13707 100644 --- a/Makefile +++ b/Makefile @@ -491,6 +491,7 @@ COMMON_SRC = \ drivers/bus_i2c_soft.c \ drivers/bus_spi.c \ drivers/bus_spi_soft.c \ + drivers/display.c \ drivers/exti.c \ drivers/gyro_sync.c \ drivers/io.c \ @@ -556,6 +557,14 @@ COMMON_SRC = \ HIGHEND_SRC = \ blackbox/blackbox.c \ blackbox/blackbox_io.c \ + cms/cms.c \ + cms/cms_menu_blackbox.c \ + cms/cms_menu_builtin.c \ + cms/cms_menu_imu.c \ + cms/cms_menu_ledstrip.c \ + cms/cms_menu_misc.c \ + cms/cms_menu_osd.c \ + cms/cms_menu_vtx.c \ common/colorconversion.c \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ @@ -565,9 +574,13 @@ HIGHEND_SRC = \ flight/gtune.c \ flight/navigation.c \ flight/gps_conversion.c \ + io/dashboard.c \ + io/displayport_max7456.c \ + io/displayport_msp.c \ + io/displayport_oled.c \ io/gps.c \ io/ledstrip.c \ - io/dashboard.c \ + io/osd.c \ sensors/sonar.c \ sensors/barometer.c \ telemetry/telemetry.c \ diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c new file mode 100644 index 0000000000..b8965ee9e3 --- /dev/null +++ b/src/main/cms/cms.c @@ -0,0 +1,871 @@ +/* + * 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 . + */ + +/* + Original OSD code created by Marcin Baliniak + OSD-CMS separation by jflyper + CMS-displayPort separation by jflyper and martinbudden + */ + +//#define CMS_MENU_DEBUG // For external menu content creators + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef CMS + +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" + +#include "cms/cms.h" +#include "cms/cms_menu_builtin.h" +#include "cms/cms_types.h" + +#include "common/typeconversion.h" + +#include "drivers/system.h" + +// For 'ARM' related +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +// For rcData, stopAllMotors, stopPwmAllMotors +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +// For VISIBLE* (Actually, included by config_master.h) +#include "io/osd.h" + +// DisplayPort management + +#ifndef CMS_MAX_DEVICE +#define CMS_MAX_DEVICE 4 +#endif + +static displayPort_t *pCurrentDisplay; + +static displayPort_t *cmsDisplayPorts[CMS_MAX_DEVICE]; +static int cmsDeviceCount; +static int cmsCurrentDevice = -1; + +bool cmsDisplayPortRegister(displayPort_t *pDisplay) +{ + if (cmsDeviceCount == CMS_MAX_DEVICE) + return false; + + cmsDisplayPorts[cmsDeviceCount++] = pDisplay; + + return true; +} + +static displayPort_t *cmsDisplayPortSelectCurrent(void) +{ + if (cmsDeviceCount == 0) + return NULL; + + if (cmsCurrentDevice < 0) + cmsCurrentDevice = 0; + + return cmsDisplayPorts[cmsCurrentDevice]; +} + +static displayPort_t *cmsDisplayPortSelectNext(void) +{ + if (cmsDeviceCount == 0) + return NULL; + + cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay + + return cmsDisplayPorts[cmsCurrentDevice]; +} + +#define CMS_UPDATE_INTERVAL_US 50000 // Interval of key scans (microsec) +#define CMS_POLL_INTERVAL_US 100000 // Interval of polling dynamic values (microsec) + +// XXX LEFT_MENU_COLUMN and RIGHT_MENU_COLUMN must be adjusted +// dynamically depending on size of the active output device, +// or statically to accomodate sizes of all supported devices. +// +// Device characteristics +// OLED +// 21 cols x 8 rows +// 128x64 with 5x7 (6x8) : 21 cols x 8 rows +// MAX7456 (PAL) +// 30 cols x 16 rows +// MAX7456 (NTSC) +// 30 cols x 13 rows +// HoTT Telemetry Screen +// 21 cols x 8 rows +// + +#define LEFT_MENU_COLUMN 1 +#define RIGHT_MENU_COLUMN(p) ((p)->cols - 8) +#define MAX_MENU_ITEMS(p) ((p)->rows - 2) + +static bool cmsInMenu = false; + +STATIC_UNIT_TESTED const CMS_Menu *currentMenu; // Points to top entry of the current page + +// XXX Does menu backing support backing into second page??? + +static const CMS_Menu *menuStack[10]; // Stack to save menu transition +static uint8_t menuStackHistory[10];// cursorRow in a stacked menu +static uint8_t menuStackIdx = 0; + +static OSD_Entry *pageTop; // Points to top entry of the current page +static OSD_Entry *pageTopAlt; // Only 2 pages are allowed (for now) +static uint8_t maxRow; // Max row in the current page + +static int8_t cursorRow; + +#ifdef CMS_MENU_DEBUG // For external menu content creators + +static char menuErrLabel[21 + 1] = "RANDOM DATA"; + +static OSD_Entry menuErrEntries[] = { + { "BROKEN MENU", OME_Label, NULL, NULL, 0 }, + { menuErrLabel, OME_Label, NULL, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu menuErr = { + "MENUERR", + OME_MENU, + NULL, + NULL, + NULL, + menuErrEntries, +}; +#endif + +// Stick/key detection + +#define IS_HI(X) (rcData[X] > 1750) +#define IS_LO(X) (rcData[X] < 1250) +#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) + +//key definiotion because API provide menu navigation over MSP/GUI app - not used NOW +#define KEY_ENTER 0 +#define KEY_UP 1 +#define KEY_DOWN 2 +#define KEY_LEFT 3 +#define KEY_RIGHT 4 +#define KEY_ESC 5 + +#define BUTTON_TIME 250 // msec +#define BUTTON_PAUSE 500 // msec + +static void cmsUpdateMaxRow(displayPort_t *instance) +{ + maxRow = 0; + + for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) { + maxRow++; + } + + if (maxRow > MAX_MENU_ITEMS(instance)) { + maxRow = MAX_MENU_ITEMS(instance); + } + + maxRow--; +} + +static void cmsFormatFloat(int32_t value, char *floatString) +{ + uint8_t k; + // np. 3450 + + itoa(100000 + value, floatString, 10); // Create string from abs of integer value + + // 103450 + + floatString[0] = floatString[1]; + floatString[1] = floatString[2]; + floatString[2] = '.'; + + // 03.450 + // usuwam koncowe zera i kropke + // Keep the first decimal place + for (k = 5; k > 3; k--) + if (floatString[k] == '0' || floatString[k] == '.') + floatString[k] = 0; + else + break; + + // oraz zero wiodonce + if (floatString[0] == '0') + floatString[0] = ' '; +} + +static void cmsPadToSize(char *buf, int size) +{ + int i; + + for (i = 0 ; i < size ; i++) { + if (buf[i] == 0) + break; + } + + for ( ; i < size ; i++) { + buf[i] = ' '; + } + + buf[size] = 0; +} + +static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) +{ + char buff[10]; + int cnt = 0; + + switch (p->type) { + case OME_String: + if (IS_PRINTVALUE(p) && p->data) { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data); + CLR_PRINTVALUE(p); + } + break; + case OME_Submenu: + case OME_Funcall: + if (IS_PRINTVALUE(p)) { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); + CLR_PRINTVALUE(p); + } + break; + case OME_Bool: + if (IS_PRINTVALUE(p) && p->data) { + if (*((uint8_t *)(p->data))) { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + } else { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + } + CLR_PRINTVALUE(p); + } + break; + case OME_TAB: { + if (IS_PRINTVALUE(p)) { + OSD_TAB_t *ptr = p->data; + //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, (char *)ptr->names[*ptr->val]); + CLR_PRINTVALUE(p); + } + break; + } +#ifdef OSD + case OME_VISIBLE: + if (IS_PRINTVALUE(p) && p->data) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + + if (VISIBLE(*val)) { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + } else { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + } + CLR_PRINTVALUE(p); + } + break; +#endif + case OME_UINT8: + 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); + CLR_PRINTVALUE(p); + } + break; + case OME_INT8: + 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); + CLR_PRINTVALUE(p); + } + break; + case OME_UINT16: + 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); + CLR_PRINTVALUE(p); + } + break; + case OME_INT16: + 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); + CLR_PRINTVALUE(p); + } + break; + case OME_FLOAT: + 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 ??? + CLR_PRINTVALUE(p); + } + break; + 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); + CLR_PRINTVALUE(p); + } + break; + case OME_OSD_Exit: + case OME_END: + case OME_Back: + break; + case OME_MENU: + // 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"); +#endif + break; + } + + return cnt; +} + +static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) +{ + if (!pageTop) + return; + + uint8_t i; + OSD_Entry *p; + uint8_t top = (pDisplay->rows - maxRow) / 2 - 1; + + // Polled (dynamic) value display denominator. + + bool drawPolled = false; + static uint32_t lastPolledUs = 0; + + if (currentTimeUs > lastPolledUs + CMS_POLL_INTERVAL_US) { + drawPolled = true; + lastPolledUs = currentTimeUs; + } + + uint32_t room = displayTxBytesFree(pDisplay); + + if (pDisplay->cleared) { + for (p = pageTop, i= 0; p->type != OME_END; p++, i++) { + SET_PRINTLABEL(p); + SET_PRINTVALUE(p); + } + + if (i > MAX_MENU_ITEMS(pDisplay)) // max per page + { + pageTopAlt = pageTop + MAX_MENU_ITEMS(pDisplay); + if (pageTopAlt->type == OME_END) + pageTopAlt = NULL; + } + + pDisplay->cleared = false; + } else if (drawPolled) { + for (p = pageTop ; p <= pageTop + maxRow ; p++) { + if (IS_DYNAMIC(p)) + SET_PRINTVALUE(p); + } + } + + // Cursor manipulation + + while ((pageTop + cursorRow)->type == OME_Label) // skip label + cursorRow++; + + if (pDisplay->cursorRow >= 0 && cursorRow != pDisplay->cursorRow) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, " "); + } + + if (room < 30) + return; + + if (pDisplay->cursorRow != cursorRow) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, cursorRow + top, " >"); + pDisplay->cursorRow = cursorRow; + } + + if (room < 30) + return; + + // Print text labels + for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && 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); + 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); + if (room < 30) + return; + } + } +} + +long cmsMenuChange(displayPort_t *pDisplay, const void *ptr) +{ + CMS_Menu *pMenu = (CMS_Menu *)ptr; + + if (pMenu) { +#ifdef CMS_MENU_DEBUG + if (pMenu->GUARD_type != OME_MENU) { + // ptr isn't pointing to a CMS_Menu. + if (pMenu->GUARD_type <= OME_MAX) { + strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1); + } else { + strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1); + } + pMenu = &menuErr; + } +#endif + + // Stack the current menu and move to a new menu. + // The (pMenu == curretMenu) case occurs when reopening for display sw + + if (pMenu != currentMenu) { + menuStack[menuStackIdx] = currentMenu; + cursorRow += pageTop - currentMenu->entries; // Convert cursorRow to absolute value + menuStackHistory[menuStackIdx] = cursorRow; + menuStackIdx++; + + currentMenu = pMenu; + cursorRow = 0; + + if (pMenu->onEnter) + pMenu->onEnter(); + } + + pageTop = currentMenu->entries; + pageTopAlt = NULL; + + displayClear(pDisplay); + cmsUpdateMaxRow(pDisplay); + } + + return 0; +} + +STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay) +{ + // Let onExit function decide whether to allow exit or not. + + if (currentMenu->onExit && currentMenu->onExit(pageTop + cursorRow) < 0) + return -1; + + if (menuStackIdx) { + displayClear(pDisplay); + menuStackIdx--; + currentMenu = menuStack[menuStackIdx]; + cursorRow = menuStackHistory[menuStackIdx]; + + // cursorRow is absolute offset of a focused entry when stacked. + // Convert it back to page and relative offset. + + pageTop = currentMenu->entries; // Temporary for cmsUpdateMaxRow() + cmsUpdateMaxRow(pDisplay); + + if (cursorRow > maxRow) { + // Cursor was in the second page. + pageTopAlt = currentMenu->entries; + pageTop = pageTopAlt + maxRow + 1; + cursorRow -= (maxRow + 1); + cmsUpdateMaxRow(pDisplay); // Update maxRow for the second page + } + } + + return 0; +} + +STATIC_UNIT_TESTED void cmsMenuOpen(void) +{ + if (!cmsInMenu) { + // New open + pCurrentDisplay = cmsDisplayPortSelectCurrent(); + if (!pCurrentDisplay) + return; + cmsInMenu = true; + currentMenu = &menuMain; + DISABLE_ARMING_FLAG(OK_TO_ARM); + } else { + // Switch display + displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); + if (pNextDisplay != pCurrentDisplay) { + displayRelease(pCurrentDisplay); + pCurrentDisplay = pNextDisplay; + } else { + return; + } + } + displayGrab(pCurrentDisplay); // grab the display for use by the CMS + cmsMenuChange(pCurrentDisplay, currentMenu); +} + +static void cmsTraverseGlobalExit(const CMS_Menu *pMenu) +{ + debug[0]++; + + for (const OSD_Entry *p = pMenu->entries; p->type != OME_END ; p++) { + if (p->type == OME_Submenu) { + cmsTraverseGlobalExit(p->data); + } + } + + if (pMenu->onGlobalExit) { + debug[1]++; + pMenu->onGlobalExit(); + } +} + +long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) +{ + if (ptr) { + displayClear(pDisplay); + + displayWrite(pDisplay, 5, 3, "REBOOTING..."); + displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? + + stopMotors(); + stopPwmAllMotors(); + delay(200); + + cmsTraverseGlobalExit(&menuMain); + + if (currentMenu->onExit) + currentMenu->onExit((OSD_Entry *)NULL); // Forced exit + + saveConfigAndNotify(); + } + + cmsInMenu = false; + + displayRelease(pDisplay); + currentMenu = NULL; + + if (ptr) + systemReset(); + + ENABLE_ARMING_FLAG(OK_TO_ARM); + + return 0; +} + +STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) +{ + uint16_t res = BUTTON_TIME; + OSD_Entry *p; + + if (!currentMenu) + return res; + + if (key == KEY_ESC) { + cmsMenuBack(pDisplay); + return BUTTON_PAUSE; + } + + if (key == KEY_DOWN) { + if (cursorRow < maxRow) { + cursorRow++; + } else { + if (pageTopAlt) { // we have another page + displayClear(pDisplay); + p = pageTopAlt; + pageTopAlt = pageTop; + pageTop = (OSD_Entry *)p; + cmsUpdateMaxRow(pDisplay); + } + cursorRow = 0; // Goto top in any case + } + } + + if (key == KEY_UP) { + cursorRow--; + + if ((pageTop + cursorRow)->type == OME_Label && cursorRow > 0) + cursorRow--; + + if (cursorRow == -1 || (pageTop + cursorRow)->type == OME_Label) { + if (pageTopAlt) { + displayClear(pDisplay); + p = pageTopAlt; + pageTopAlt = pageTop; + pageTop = (OSD_Entry *)p; + cmsUpdateMaxRow(pDisplay); + } + cursorRow = maxRow; // Goto bottom in any case + } + } + + if (key == KEY_DOWN || key == KEY_UP) + return res; + + p = pageTop + cursorRow; + + switch (p->type) { + case OME_Submenu: + case OME_Funcall: + case OME_OSD_Exit: + if (p->func && key == KEY_RIGHT) { + p->func(pDisplay, p->data); + res = BUTTON_PAUSE; + } + break; + case OME_Back: + cmsMenuBack(pDisplay); + res = BUTTON_PAUSE; + break; + case OME_Bool: + if (p->data) { + uint8_t *val = p->data; + if (key == KEY_RIGHT) + *val = 1; + else + *val = 0; + SET_PRINTVALUE(p); + } + break; +#ifdef OSD + case OME_VISIBLE: + if (p->data) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + + if (key == KEY_RIGHT) + *val |= VISIBLE_FLAG; + else + *val %= ~VISIBLE_FLAG; + SET_PRINTVALUE(p); + } + break; +#endif + case OME_UINT8: + case OME_FLOAT: + if (p->data) { + OSD_UINT8_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } + } + break; + case OME_TAB: + if (p->type == OME_TAB) { + OSD_TAB_t *ptr = p->data; + + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += 1; + } + else { + if (*ptr->val > 0) + *ptr->val -= 1; + } + if (p->func) + p->func(pDisplay, p->data); + SET_PRINTVALUE(p); + } + break; + case OME_INT8: + if (p->data) { + OSD_INT8_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } + } + break; + case OME_UINT16: + if (p->data) { + OSD_UINT16_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } + } + break; + case OME_INT16: + if (p->data) { + OSD_INT16_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } + } + break; + case OME_String: + break; + case OME_Label: + case OME_END: + break; + case OME_MENU: + // Shouldn't happen + break; + } + return res; +} + +static void cmsUpdate(uint32_t currentTimeUs) +{ + static int16_t rcDelayMs = BUTTON_TIME; + static uint32_t lastCalledMs = 0; + static uint32_t lastCmsHeartBeatMs = 0; + + const uint32_t currentTimeMs = currentTimeUs / 1000; + + if (!cmsInMenu) { + // Detect menu invocation + if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsMenuOpen(); + rcDelayMs = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME + } + } else { + uint8_t key = 0; + if (rcDelayMs > 0) { + rcDelayMs -= (currentTimeMs - lastCalledMs); + } + else if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + // Double enter = display switching + cmsMenuOpen(); + rcDelayMs = BUTTON_PAUSE; + } + else if (IS_HI(PITCH)) { + key = KEY_UP; + rcDelayMs = BUTTON_TIME; + } + else if (IS_LO(PITCH)) { + key = KEY_DOWN; + rcDelayMs = BUTTON_TIME; + } + else if (IS_LO(ROLL)) { + key = KEY_LEFT; + rcDelayMs = BUTTON_TIME; + } + else if (IS_HI(ROLL)) { + key = KEY_RIGHT; + rcDelayMs = BUTTON_TIME; + } + else if (IS_HI(YAW) || IS_LO(YAW)) + { + key = KEY_ESC; + rcDelayMs = BUTTON_TIME; + } + + //lastCalled = currentTime; + + if (key) { + rcDelayMs = cmsHandleKey(pCurrentDisplay, key); + return; + } + + cmsDrawMenu(pCurrentDisplay, currentTimeUs); + + if (currentTimeMs > lastCmsHeartBeatMs + 500) { + // Heart beat for external CMS display device @ 500msec + // (Timeout @ 1000msec) + displayHeartbeat(pCurrentDisplay); + lastCmsHeartBeatMs = currentTimeMs; + } + } + lastCalledMs = currentTimeMs; +} + +void cmsHandler(uint32_t currentTime) +{ + if (cmsDeviceCount < 0) + return; + + static uint32_t lastCalled = 0; + + if (currentTime >= lastCalled + CMS_UPDATE_INTERVAL_US) { + lastCalled = currentTime; + cmsUpdate(currentTime); + } +} + +// Is initializing with menuMain better? +// Can it be done with the current main()? +void cmsInit(void) +{ + cmsDeviceCount = 0; + cmsCurrentDevice = -1; +} + +#endif // CMS diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h new file mode 100644 index 0000000000..8e5110af22 --- /dev/null +++ b/src/main/cms/cms.h @@ -0,0 +1,17 @@ +#pragma once + +#include "drivers/display.h" + +// Device management +bool cmsDisplayPortRegister(displayPort_t *pDisplay); + +// For main.c and scheduler +void cmsInit(void); +void cmsHandler(uint32_t currentTime); + +long cmsMenuChange(displayPort_t *pPort, const void *ptr); +long cmsMenuExit(displayPort_t *pPort, const void *ptr); + +#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_blackbox.c b/src/main/cms/cms_menu_blackbox.c new file mode 100644 index 0000000000..0d65249fbc --- /dev/null +++ b/src/main/cms/cms_menu_blackbox.c @@ -0,0 +1,111 @@ +/* + * 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 . + */ + +// +// CMS things for blackbox and flashfs. +// + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_blackbox.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/flashfs.h" + +#ifdef USE_FLASHFS +static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) +{ + UNUSED(ptr); + + displayClear(pDisplay); + displayWrite(pDisplay, 5, 3, "ERASING FLASH..."); + displayResync(pDisplay); // Was max7456RefreshAll(); Why at this timing? + + flashfsEraseCompletely(); + while (!flashfsIsReady()) { + delay(100); + } + + displayClear(pDisplay); + displayResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? + + return 0; +} +#endif // USE_FLASHFS + +static bool featureRead = false; +static uint8_t cmsx_FeatureBlackbox; + +static long cmsx_Blackbox_FeatureRead(void) +{ + if (!featureRead) { + cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; + featureRead = true; + } + + return 0; +} + +static long cmsx_Blackbox_FeatureWriteback(void) +{ + if (cmsx_FeatureBlackbox) + featureSet(FEATURE_BLACKBOX); + else + featureClear(FEATURE_BLACKBOX); + + return 0; +} + +static OSD_Entry cmsx_menuBlackboxEntries[] = +{ + { "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, + { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, + { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &masterConfig.blackbox_rate_denom,1,32,1 }, 0 }, + +#ifdef USE_FLASHFS + { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, +#endif // USE_FLASHFS + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuBlackbox = { + .GUARD_text = "MENUBB", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Blackbox_FeatureRead, + .onExit = NULL, + .onGlobalExit = cmsx_Blackbox_FeatureWriteback, + .entries = cmsx_menuBlackboxEntries +}; +#endif diff --git a/src/main/cms/cms_menu_blackbox.h b/src/main/cms/cms_menu_blackbox.h new file mode 100644 index 0000000000..a35ac4211b --- /dev/null +++ b/src/main/cms/cms_menu_blackbox.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 + +extern CMS_Menu cmsx_menuBlackbox; diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c new file mode 100644 index 0000000000..50a85a4c08 --- /dev/null +++ b/src/main/cms/cms_menu_builtin.c @@ -0,0 +1,143 @@ +/* + * 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 . + */ + +// +// Built-in menu contents and support functions +// + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef CMS + +#include "build/version.h" + +#include "drivers/system.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_builtin.h" + +// Sub menus + +#include "cms/cms_menu_imu.h" +#include "cms/cms_menu_blackbox.h" +#include "cms/cms_menu_vtx.h" +#include "cms/cms_menu_osd.h" +#include "cms/cms_menu_ledstrip.h" +#include "cms/cms_menu_misc.h" + + +// Info + +static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; +static char infoTargetName[] = __TARGET__; + +#include "msp/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++) { + if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') + infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; + else + infoGitRev[i] = shortGitRevision[i]; + } + + return 0; +} + +static OSD_Entry menuInfoEntries[] = { + { "--- INFO ---", OME_Label, NULL, NULL, 0 }, + { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 }, + { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, + { "GITREV", OME_String, NULL, infoGitRev, 0 }, + { "TARGET", OME_String, NULL, infoTargetName, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu menuInfo = { + .GUARD_text = "MENUINFO", + .GUARD_type = OME_MENU, + .onEnter = cmsx_InfoInit, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuInfoEntries +}; + +// Features + +static OSD_Entry menuFeaturesEntries[] = +{ + {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, + {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0}, +#if defined(VTX) || defined(USE_RTC6705) + {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, +#endif // VTX || USE_RTC6705 +#ifdef LED_STRIP + {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, +#endif // LED_STRIP + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +static CMS_Menu menuFeatures = { + .GUARD_text = "MENUFEATURES", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuFeaturesEntries, +}; + +// Main + +static OSD_Entry menuMainEntries[] = +{ + {"-- MAIN --", OME_Label, NULL, NULL, 0}, + + {"PROFILE", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0}, + {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, +#ifdef OSD + {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, +#endif + {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, + {"MISC", OME_Submenu, cmsMenuChange, &cmsx_menuMisc, 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, +#ifdef CMS_MENU_DEBUG + {"ERR SAMPLE", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0}, +#endif + + {NULL,OME_END, NULL, NULL, 0} +}; + +CMS_Menu menuMain = { + .GUARD_text = "MENUMAIN", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuMainEntries, +}; +#endif diff --git a/src/main/cms/cms_menu_builtin.h b/src/main/cms/cms_menu_builtin.h new file mode 100644 index 0000000000..8bb33757aa --- /dev/null +++ b/src/main/cms/cms_menu_builtin.h @@ -0,0 +1,22 @@ +/* + * 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 "cms/cms_types.h" + +extern CMS_Menu menuMain; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c new file mode 100644 index 0000000000..24b988cb07 --- /dev/null +++ b/src/main/cms/cms_menu_imu.c @@ -0,0 +1,384 @@ +/* + * 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 . + */ + +// Menu contents for PID, RATES, RC preview, misc +// Should be part of the relevant .c file. + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef CMS + +#include "build/version.h" + +#include "drivers/system.h" + +//#include "common/typeconversion.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_imu.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/pid.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +// +// PID +// +static uint8_t tmpProfileIndex; +static uint8_t profileIndex; +static char profileIndexString[] = " p"; +static uint8_t tempPid[3][3]; + +static uint8_t tmpRateProfileIndex; +static uint8_t rateProfileIndex; +static char rateProfileIndexString[] = " p-r"; +static controlRateConfig_t rateProfile; + +static long cmsx_menuImu_onEnter(void) +{ + profileIndex = masterConfig.current_profile_index; + tmpProfileIndex = profileIndex + 1; + + rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile; + tmpRateProfileIndex = rateProfileIndex + 1; + + return 0; +} + +static long cmsx_menuImu_onExit(const OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.current_profile_index = profileIndex; + masterConfig.profile[profileIndex].activeRateProfile = rateProfileIndex; + + return 0; +} + +static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + profileIndex = tmpProfileIndex - 1; + + return 0; +} + +static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + rateProfileIndex = tmpRateProfileIndex - 1; + + return 0; +} + +static long cmsx_PidRead(void) +{ + + for (uint8_t i = 0; i < 3; i++) { + tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i]; + tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i]; + tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i]; + } + + return 0; +} + +static long cmsx_PidOnEnter(void) +{ + profileIndexString[1] = '0' + tmpProfileIndex; + cmsx_PidRead(); + + return 0; +} + +static long cmsx_PidWriteback(const OSD_Entry *self) +{ + UNUSED(self); + + for (uint8_t i = 0; i < 3; i++) { + masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0]; + masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1]; + masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2]; + } + + return 0; +} + +static OSD_Entry cmsx_menuPidEntries[] = +{ + { "-- PID --", OME_Label, NULL, profileIndexString, 0}, + + { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][0], 0, 200, 1 }, 0 }, + { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][1], 0, 200, 1 }, 0 }, + { "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][2], 0, 200, 1 }, 0 }, + + { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][0], 0, 200, 1 }, 0 }, + { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][1], 0, 200, 1 }, 0 }, + { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][2], 0, 200, 1 }, 0 }, + + { "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][0], 0, 200, 1 }, 0 }, + { "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][1], 0, 200, 1 }, 0 }, + { "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][2], 0, 200, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuPid = { + .GUARD_text = "XPID", + .GUARD_type = OME_MENU, + .onEnter = cmsx_PidOnEnter, + .onExit = cmsx_PidWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuPidEntries +}; + +// +// Rate & Expo +// + +static long cmsx_RateProfileRead(void) +{ + memcpy(&rateProfile, &masterConfig.profile[profileIndex].controlRateProfile[rateProfileIndex], sizeof(controlRateConfig_t)); + + return 0; +} + +static long cmsx_RateProfileWriteback(const OSD_Entry *self) +{ + UNUSED(self); + + memcpy(&masterConfig.profile[profileIndex].controlRateProfile[rateProfileIndex], &rateProfile, sizeof(controlRateConfig_t)); + + return 0; +} + +static long cmsx_RateProfileOnEnter(void) +{ + rateProfileIndexString[1] = '0' + tmpProfileIndex; + rateProfileIndexString[3] = '0' + tmpRateProfileIndex; + cmsx_RateProfileRead(); + + return 0; +} + +static OSD_Entry cmsx_menuRateProfileEntries[] = +{ + { "-- RATE --", OME_Label, NULL, rateProfileIndexString, 0 }, + + { "RC RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRate8, 0, 255, 1, 10 }, 0 }, + { "RC YAW RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawRate8, 0, 255, 1, 10 }, 0 }, + + { "ROLL SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[0], 0, 100, 1, 10 }, 0 }, + { "PITCH SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[1], 0, 100, 1, 10 }, 0 }, + { "YAW SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[2], 0, 100, 1, 10 }, 0 }, + + { "RC EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo8, 0, 100, 1, 10 }, 0 }, + { "RC YAW EXP", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawExpo8, 0, 100, 1, 10 }, 0 }, + + { "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID, 0, 100, 1, 10}, 0 }, + { "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10}, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuRateProfile = { + .GUARD_text = "MENURATE", + .GUARD_type = OME_MENU, + .onEnter = cmsx_RateProfileOnEnter, + .onExit = cmsx_RateProfileWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuRateProfileEntries +}; + +static uint8_t cmsx_dtermSetpointWeight; +static uint8_t cmsx_setpointRelaxRatio; +static uint8_t cmsx_angleStrength; +static uint8_t cmsx_horizonStrength; +static uint8_t cmsx_horizonTransition; + +static long cmsx_profileOtherOnEnter(void) +{ + profileIndexString[1] = '0' + tmpProfileIndex; + + cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight; + cmsx_setpointRelaxRatio = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio; + + cmsx_angleStrength = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL]; + cmsx_horizonStrength = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL]; + cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL]; + + return 0; +} + +static long cmsx_profileOtherOnExit(const OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight; + masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio; + + masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength; + masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength; + masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition; + + return 0; +} + +static OSD_Entry cmsx_menuProfileOtherEntries[] = { + { "-- OTHER PP --", OME_Label, NULL, profileIndexString, 0 }, + + { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, + { "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, + { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, + { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 }, + { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuProfileOther = { + .GUARD_text = "XPROFOTHER", + .GUARD_type = OME_MENU, + .onEnter = cmsx_profileOtherOnEnter, + .onExit = cmsx_profileOtherOnExit, + .onGlobalExit = NULL, + .entries = cmsx_menuProfileOtherEntries, +}; + +static OSD_Entry cmsx_menuFilterGlobalEntries[] = +{ + { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, + + { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, + { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, + { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, + { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, + { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuFilterGlobal = { + .GUARD_text = "XFLTGLB", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuFilterGlobalEntries, +}; + +static uint16_t cmsx_dterm_lpf_hz; +static uint16_t cmsx_dterm_notch_hz; +static uint16_t cmsx_dterm_notch_cutoff; +static uint16_t cmsx_yaw_lpf_hz; +static uint16_t cmsx_yaw_p_limit; + +static long cmsx_FilterPerProfileRead(void) +{ + cmsx_dterm_lpf_hz = masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz; + cmsx_dterm_notch_hz = masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz; + cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff; + cmsx_yaw_lpf_hz = masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz; + cmsx_yaw_p_limit = masterConfig.profile[profileIndex].pidProfile.yaw_p_limit; + + return 0; +} + +static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz; + masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz = cmsx_dterm_notch_hz; + masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff; + masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz; + masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit; + + return 0; +} + +static OSD_Entry cmsx_menuFilterPerProfileEntries[] = +{ + { "-- FILTER PP --", OME_Label, NULL, NULL, 0 }, + + { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf_hz, 0, 500, 1 }, 0 }, + { "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 }, + { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 }, + { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lpf_hz, 0, 500, 1 }, 0 }, + { "YAW P LIM", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_p_limit, 100, 500, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuFilterPerProfile = { + .GUARD_text = "XFLTPP", + .GUARD_type = OME_MENU, + .onEnter = cmsx_FilterPerProfileRead, + .onExit = cmsx_FilterPerProfileWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuFilterPerProfileEntries, +}; + +static OSD_Entry cmsx_menuImuEntries[] = +{ + { "-- IMU --", OME_Label, NULL, NULL, 0}, + + {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, + {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, + {"OTHER PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0}, + + {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, MAX_RATEPROFILES, 1}, 0}, + {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, + + {"FLT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, + + {"FLT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, + + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuImu = { + .GUARD_text = "XIMU", + .GUARD_type = OME_MENU, + .onEnter = cmsx_menuImu_onEnter, + .onExit = cmsx_menuImu_onExit, + .onGlobalExit = NULL, + .entries = cmsx_menuImuEntries, +}; +#endif // CMS diff --git a/src/main/cms/cms_menu_imu.h b/src/main/cms/cms_menu_imu.h new file mode 100644 index 0000000000..a5e05e3103 --- /dev/null +++ b/src/main/cms/cms_menu_imu.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 + +extern CMS_Menu cmsx_menuImu; diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c new file mode 100644 index 0000000000..bd7efe2cd2 --- /dev/null +++ b/src/main/cms/cms_menu_ledstrip.c @@ -0,0 +1,82 @@ +/* + * 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 + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_ledstrip.h" + +#ifdef LED_STRIP + +static bool featureRead = false; +static uint8_t cmsx_FeatureLedstrip; + +static long cmsx_Ledstrip_FeatureRead(void) +{ + if (!featureRead) { + cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; + featureRead = true; + } + + return 0; +} + +static long cmsx_Ledstrip_FeatureWriteback(void) +{ + if (cmsx_FeatureLedstrip) + featureSet(FEATURE_LED_STRIP); + else + featureClear(FEATURE_LED_STRIP); + + return 0; +} + +static OSD_Entry cmsx_menuLedstripEntries[] = +{ + { "-- LED STRIP --", OME_Label, NULL, NULL, 0 }, + { "ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuLedstrip = { + .GUARD_text = "MENULED", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Ledstrip_FeatureRead, + .onExit = NULL, + .onGlobalExit = cmsx_Ledstrip_FeatureWriteback, + .entries = cmsx_menuLedstripEntries +}; +#endif // LED_STRIP +#endif // CMS diff --git a/src/main/cms/cms_menu_ledstrip.h b/src/main/cms/cms_menu_ledstrip.h new file mode 100644 index 0000000000..f740b8911c --- /dev/null +++ b/src/main/cms/cms_menu_ledstrip.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 + +extern CMS_Menu cmsx_menuLedstrip; diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c new file mode 100644 index 0000000000..684fbf8b62 --- /dev/null +++ b/src/main/cms/cms_menu_misc.c @@ -0,0 +1,104 @@ +/* + * 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 + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_ledstrip.h" + +// +// Misc +// + +static long cmsx_menuRcConfirmBack(const OSD_Entry *self) +{ + if (self && self->type == OME_Back) + return 0; + else + return -1; +} + +// +// RC preview +// +static OSD_Entry cmsx_menuRcEntries[] = +{ + { "-- RC PREV --", OME_Label, NULL, NULL, 0}, + + { "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[ROLL], 1, 2500, 0 }, DYNAMIC }, + { "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[PITCH], 1, 2500, 0 }, DYNAMIC }, + { "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[THROTTLE], 1, 2500, 0 }, DYNAMIC }, + { "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[YAW], 1, 2500, 0 }, DYNAMIC }, + + { "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX1], 1, 2500, 0 }, DYNAMIC }, + { "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX2], 1, 2500, 0 }, DYNAMIC }, + { "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX3], 1, 2500, 0 }, DYNAMIC }, + { "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX4], 1, 2500, 0 }, DYNAMIC }, + + { "BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuRcPreview = { + .GUARD_text = "XRCPREV", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = cmsx_menuRcConfirmBack, + .onGlobalExit = NULL, + .entries = cmsx_menuRcEntries +}; + + +static OSD_Entry menuMiscEntries[]= +{ + { "-- MISC --", OME_Label, NULL, NULL, 0 }, + + { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1000, 2000, 1 }, 0 }, + { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 }, + { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 }, + { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, + + { "BACK", OME_Back, NULL, NULL, 0}, + { NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuMisc = { + .GUARD_text = "XMISC", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuMiscEntries +}; + +#endif // CMS diff --git a/src/main/cms/cms_menu_misc.h b/src/main/cms/cms_menu_misc.h new file mode 100644 index 0000000000..8b0ed9fc67 --- /dev/null +++ b/src/main/cms/cms_menu_misc.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 + +extern CMS_Menu cmsx_menuMisc; diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c new file mode 100644 index 0000000000..1ef32700df --- /dev/null +++ b/src/main/cms/cms_menu_osd.c @@ -0,0 +1,112 @@ +/* + * 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" + +#include "build/version.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_osd.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#if defined(OSD) && defined(CMS) + +OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; +OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; +OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1}; +OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1}; + +OSD_Entry cmsx_menuAlarmsEntries[] = +{ + {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0}, + {"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0}, + {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0}, + {"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuAlarms = { + .GUARD_text = "MENUALARMS", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuAlarmsEntries, +}; + +OSD_Entry menuOsdActiveElemsEntries[] = +{ + {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0}, + {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0}, + {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0}, + {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0}, + {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0}, + {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0}, + {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0}, +#ifdef VTX + {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, +#endif // VTX + {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0}, + {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0}, +#ifdef GPS + {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0}, + {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0}, +#endif // GPS + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu menuOsdActiveElems = { + .GUARD_text = "MENUOSDACT", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuOsdActiveElemsEntries +}; + +OSD_Entry cmsx_menuOsdLayoutEntries[] = +{ + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, + {"ACTIVE ELEM", OME_Submenu, cmsMenuChange, &menuOsdActiveElems, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuOsdLayout = { + .GUARD_text = "MENULAYOUT", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuOsdLayoutEntries +}; +#endif // CMS diff --git a/src/main/cms/cms_menu_osd.h b/src/main/cms/cms_menu_osd.h new file mode 100644 index 0000000000..9b0b001d28 --- /dev/null +++ b/src/main/cms/cms_menu_osd.h @@ -0,0 +1,21 @@ +/* + * 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 + +extern CMS_Menu cmsx_menuAlarms; +extern CMS_Menu cmsx_menuOsdLayout; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c new file mode 100644 index 0000000000..3b0cb021a4 --- /dev/null +++ b/src/main/cms/cms_menu_vtx.c @@ -0,0 +1,146 @@ +/* + * 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" + +#include "build/version.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_vtx.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#ifdef CMS + +#if defined(VTX) || defined(USE_RTC6705) + +static bool featureRead = false; +static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; + +static long cmsx_Vtx_FeatureRead(void) +{ + if (!featureRead) { + cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; + featureRead = true; + } + + return 0; +} + +static long cmsx_Vtx_FeatureWriteback(void) +{ + if (cmsx_featureVtx) + featureSet(FEATURE_VTX); + else + featureClear(FEATURE_VTX); + + return 0; +} + +static const char * const vtxBandNames[] = { + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +static OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; +static OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; + +static void cmsx_Vtx_ConfigRead(void) +{ +#ifdef VTX + cmsx_vtxBand = masterConfig.vtx_band; + cmsx_vtxChannel = masterConfig.vtx_channel + 1; +#endif // VTX + +#ifdef USE_RTC6705 + cmsx_vtxBand = masterConfig.vtx_channel / 8; + cmsx_vtxChannel = masterConfig.vtx_channel % 8 + 1; +#endif // USE_RTC6705 +} + +static void cmsx_Vtx_ConfigWriteback(void) +{ +#ifdef VTX + masterConfig.vtx_band = cmsx_vtxBand; + masterConfig.vtx_channel = cmsx_vtxChannel - 1; +#endif // VTX + +#ifdef USE_RTC6705 + masterConfig.vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1; +#endif // USE_RTC6705 +} + +static long cmsx_Vtx_onEnter(void) +{ + cmsx_Vtx_FeatureRead(); + cmsx_Vtx_ConfigRead(); + + return 0; +} + +static long cmsx_Vtx_onExit(const OSD_Entry *self) +{ + UNUSED(self); + + cmsx_Vtx_ConfigWriteback(); + + return 0; +} + +#ifdef VTX +static OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; +static OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; +#endif // VTX + +static OSD_Entry cmsx_menuVtxEntries[] = +{ + {"--- VTX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0}, +#ifdef VTX + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, +#endif // VTX + {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, +#ifdef USE_RTC6705 + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0}, +#endif // USE_RTC6705 + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuVtx = { + .GUARD_text = "MENUVTX", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Vtx_onEnter, + .onExit= cmsx_Vtx_onExit, + .onGlobalExit = cmsx_Vtx_FeatureWriteback, + .entries = cmsx_menuVtxEntries +}; + +#endif // VTX || USE_RTC6705 +#endif // CMS diff --git a/src/main/cms/cms_menu_vtx.h b/src/main/cms/cms_menu_vtx.h new file mode 100644 index 0000000000..2e15372cad --- /dev/null +++ b/src/main/cms/cms_menu_vtx.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 + +extern CMS_Menu cmsx_menuVtx; diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h new file mode 100644 index 0000000000..427e1d38e6 --- /dev/null +++ b/src/main/cms/cms_types.h @@ -0,0 +1,155 @@ +/* + * 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 . + */ + +// +// Menu element types +// XXX Upon separation, all OME would be renamed to CME_ or similar. +// + +#pragma once + +//type of elements + +typedef enum +{ + OME_Label, + OME_Back, + OME_OSD_Exit, + OME_Submenu, + OME_Funcall, + OME_Bool, + OME_INT8, + OME_UINT8, + OME_UINT16, + OME_INT16, + OME_String, + OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's + //wlasciwosci elementow +#ifdef OSD + OME_VISIBLE, +#endif + OME_TAB, + OME_END, + + // Debug aid + OME_MENU, + + OME_MAX = OME_MENU +} OSD_MenuElement; + +typedef long (*CMSEntryFuncPtr)(displayPort_t *displayPort, const void *ptr); + +typedef struct +{ + const char *text; + const OSD_MenuElement type; + const CMSEntryFuncPtr func; + void *data; + uint8_t flags; +} OSD_Entry; + +// Bits in flags +#define PRINT_VALUE 0x01 // Value has been changed, need to redraw +#define PRINT_LABEL 0x02 // Text label should be printed +#define DYNAMIC 0x04 // Value should be updated dynamically + +#define IS_PRINTVALUE(p) ((p)->flags & PRINT_VALUE) +#define SET_PRINTVALUE(p) { (p)->flags |= PRINT_VALUE; } +#define CLR_PRINTVALUE(p) { (p)->flags &= ~PRINT_VALUE; } + +#define IS_PRINTLABEL(p) ((p)->flags & PRINT_LABEL) +#define SET_PRINTLABEL(p) { (p)->flags |= PRINT_LABEL; } +#define CLR_PRINTLABEL(p) { (p)->flags &= ~PRINT_LABEL; } + +#define IS_DYNAMIC(p) ((p)->flags & DYNAMIC) + + +typedef long (*CMSMenuFuncPtr)(void); + +/* +onExit function is called with self: +(1) Pointer to an OSD_Entry when cmsMenuBack() was called. + Point to an OSD_Entry with type == OME_Back if BACK was selected. +(2) NULL if called from menu exit (forced exit at top level). +*/ + +typedef long (*CMSMenuOnExitPtr)(const OSD_Entry *self); + +typedef struct +{ + // These two are debug aids for menu content creators. + const char *GUARD_text; + const OSD_MenuElement GUARD_type; + + const CMSMenuFuncPtr onEnter; + const CMSMenuOnExitPtr onExit; + const CMSMenuFuncPtr onGlobalExit; + OSD_Entry *entries; +} CMS_Menu; + +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; +} OSD_UINT8_t; + +typedef struct +{ + int8_t *val; + int8_t min; + int8_t max; + int8_t step; +} OSD_INT8_t; + +typedef struct +{ + int16_t *val; + int16_t min; + int16_t max; + int16_t step; +} OSD_INT16_t; + +typedef struct +{ + uint16_t *val; + uint16_t min; + uint16_t max; + uint16_t step; +} OSD_UINT16_t; + +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; + uint16_t multipler; +} OSD_FLOAT_t; + +typedef struct +{ + uint8_t *val; + uint8_t max; + const char * const *names; +} OSD_TAB_t; + +typedef struct +{ + char *val; +} OSD_String_t; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index dc5912e80f..60a2f35548 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -21,6 +21,8 @@ #include "config/config_profile.h" +#include "cms/cms.h" + #include "drivers/pwm_rx.h" #include "drivers/sound_beeper.h" #include "drivers/sonar_hcsr04.h" diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c new file mode 100644 index 0000000000..6589a7e930 --- /dev/null +++ b/src/main/drivers/display.c @@ -0,0 +1,72 @@ +/* + * 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 "platform.h" + +#include "common/utils.h" + +#include "display.h" + +void displayClear(displayPort_t *instance) +{ + instance->vTable->clear(instance); + instance->cleared = true; + instance->cursorRow = -1; +} + +void displayGrab(displayPort_t *instance) +{ + instance->vTable->grab(instance); + instance->vTable->clear(instance); + instance->isGrabbed = true; +} + +void displayRelease(displayPort_t *instance) +{ + instance->vTable->release(instance); + instance->isGrabbed = false; +} + +bool displayIsGrabbed(const displayPort_t *instance) +{ + // can be called before initialised + return (instance && instance->isGrabbed); +} + +int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s) +{ + return instance->vTable->write(instance, x, y, s); +} + +void displayHeartbeat(displayPort_t *instance) +{ + instance->vTable->heartbeat(instance); +} + +void displayResync(displayPort_t *instance) +{ + instance->vTable->resync(instance); +} + +uint16_t displayTxBytesFree(const displayPort_t *instance) +{ + return instance->vTable->txBytesFree(instance); +} + diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h new file mode 100644 index 0000000000..1605237736 --- /dev/null +++ b/src/main/drivers/display.h @@ -0,0 +1,49 @@ +/* + * 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 + +struct displayPortVTable_s; +typedef struct displayPort_s { + const struct displayPortVTable_s *vTable; + uint8_t rows; + uint8_t cols; + + // CMS state + bool cleared; + int8_t cursorRow; + bool isGrabbed; +} displayPort_t; + +typedef struct displayPortVTable_s { + int (*grab)(displayPort_t *displayPort); + int (*release)(displayPort_t *displayPort); + int (*clear)(displayPort_t *displayPort); + int (*write)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text); + int (*heartbeat)(displayPort_t *displayPort); + void (*resync)(displayPort_t *displayPort); + uint32_t (*txBytesFree)(const displayPort_t *displayPort); +} displayPortVTable_t; + +void displayGrab(displayPort_t *instance); +void displayRelease(displayPort_t *instance); +bool displayIsGrabbed(const displayPort_t *instance); +void displayClear(displayPort_t *instance); +int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s); +void displayHeartbeat(displayPort_t *instance); +void displayResync(displayPort_t *instance); +uint16_t displayTxBytesFree(const displayPort_t *instance); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index a552673556..10cc071411 100755 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -294,7 +294,7 @@ void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c) screenBuffer[y*30+x] = c; } -void max7456Write(uint8_t x, uint8_t y, char *buff) +void max7456Write(uint8_t x, uint8_t y, const char *buff) { uint8_t i = 0; for (i = 0; *(buff+i); i++) @@ -387,7 +387,7 @@ void max7456RefreshAll(void) } } -void max7456WriteNvm(uint8_t char_address, uint8_t *font_data) +void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) { uint8_t x; diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 6663b12934..a4ebe0ab4e 100755 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -146,9 +146,9 @@ extern uint16_t maxScreenSize; void max7456Init(uint8_t system); void max7456DrawScreen(void); -void max7456WriteNvm(uint8_t char_address, uint8_t *font_data); +void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data); uint8_t max7456GetRowsCount(void); -void max7456Write(uint8_t x, uint8_t y, char *buff); +void max7456Write(uint8_t x, uint8_t y, const char *buff); void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c); void max7456ClearScreen(void); void max7456RefreshAll(void); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c5a24dde41..bfad9682e7 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -26,6 +26,8 @@ #include "blackbox/blackbox_io.h" +#include "cms/cms.h" + #include "common/color.h" #include "common/axis.h" #include "common/maths.h" @@ -496,7 +498,7 @@ void createDefaultConfig(master_t *config) #ifdef OSD intFeatureSet(FEATURE_OSD, config); - resetOsdConfig(&config->osdProfile); + osdResetConfig(&config->osdProfile); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 016c9ccaac..d5aedd7a15 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -62,9 +62,7 @@ #include "io/flashfs.h" #include "io/transponder_ir.h" #include "io/asyncfatfs/asyncfatfs.h" -#include "io/osd.h" #include "io/serial_4way.h" -#include "io/vtx.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -1012,7 +1010,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, masterConfig.osdProfile.time_alarm); sbufWriteU16(dst, masterConfig.osdProfile.alt_alarm); - for (i = 0; i < OSD_MAX_ITEMS; i++) { + for (i = 0; i < OSD_ITEM_COUNT; i++) { sbufWriteU16(dst, masterConfig.osdProfile.item_pos[i]); } #else diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 7466c1adac..2ffbdc91ec 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -21,6 +21,8 @@ #include +#include "cms/cms.h" + #include "common/axis.h" #include "common/color.h" #include "common/utils.h" @@ -264,7 +266,7 @@ static void taskTransponder(uint32_t currentTime) static void taskUpdateOsd(uint32_t currentTime) { if (feature(FEATURE_OSD)) { - updateOsd(currentTime); + osdUpdate(currentTime); } } #endif @@ -329,6 +331,13 @@ void fcTasksInit(void) #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif +#ifdef CMS +#ifdef USE_MSP_DISPLAYPORT + setTaskEnabled(TASK_CMS, true); +#else + setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); +#endif +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -488,4 +497,13 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif + +#ifdef CMS + [TASK_CMS] = { + .taskName = "CMS", + .taskFunc = cmsHandler, + .desiredPeriod = 1000000 / 60, // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif }; diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 972b14a835..932af2e859 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -23,14 +23,19 @@ #ifdef USE_DASHBOARD +#include "common/utils.h" + #include "build/version.h" #include "build/debug.h" #include "build/build_config.h" #include "drivers/system.h" +#include "drivers/display.h" #include "drivers/display_ug2864hsweg01.h" +#include "cms/cms.h" + #include "common/printf.h" #include "common/maths.h" #include "common/axis.h" @@ -50,6 +55,8 @@ #include "flight/imu.h" #include "flight/failsafe.h" +#include "io/displayport_oled.h" + #ifdef GPS #include "io/gps.h" #include "flight/navigation.h" @@ -77,6 +84,7 @@ static uint32_t nextDisplayUpdateAt = 0; static bool dashboardPresent = false; static rxConfig_t *rxConfig; +static displayPort_t *displayPort; #define PAGE_TITLE_LINE_COUNT 1 @@ -581,6 +589,12 @@ void dashboardUpdate(uint32_t currentTime) { static uint8_t previousArmedState = 0; +#ifdef CMS + if (displayIsGrabbed(displayPort)) { + return; + } +#endif + const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L; if (!updateNow) { return; @@ -692,6 +706,11 @@ void dashboardInit(rxConfig_t *rxConfigToUse) resetDisplay(); delay(200); + displayPort = displayPortOledInit(); +#if defined(CMS) + cmsDisplayPortRegister(displayPort); +#endif + rxConfig = rxConfigToUse; memset(&pageState, 0, sizeof(pageState)); @@ -728,5 +747,4 @@ void dashboardDisablePageCycling(void) { pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED; } - -#endif +#endif // USE_DASHBOARD diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c new file mode 100644 index 0000000000..0bfbb4231c --- /dev/null +++ b/src/main/io/displayport_max7456.c @@ -0,0 +1,107 @@ +/* + * 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 "platform.h" + +#ifdef OSD + +#include "common/utils.h" + +#include "config/config_master.h" + +#include "drivers/display.h" +#include "drivers/max7456.h" + +displayPort_t max7456DisplayPort; // Referenced from osd.c + +extern uint16_t refreshTimeout; + +static int grab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + osdResetAlarms(); + displayPort->isGrabbed = true; + refreshTimeout = 0; + + return 0; +} + +static int release(displayPort_t *displayPort) +{ + UNUSED(displayPort); + displayPort->isGrabbed = false; + + return 0; +} + +static int clearScreen(displayPort_t *displayPort) +{ + UNUSED(displayPort); + max7456ClearScreen(); + + return 0; +} + +static int write(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) +{ + UNUSED(displayPort); + max7456Write(x, y, s); + + return 0; +} + +static void resync(displayPort_t *displayPort) +{ + UNUSED(displayPort); + max7456RefreshAll(); + displayPort->rows = max7456GetRowsCount(); + displayPort->cols = 30; +} + +static int heartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static uint32_t txBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static displayPortVTable_t max7456VTable = { + .grab = grab, + .release = release, + .clear = clearScreen, + .write = write, + .heartbeat = heartbeat, + .resync = resync, + .txBytesFree = txBytesFree, +}; + +displayPort_t *max7456DisplayPortInit(void) +{ + max7456DisplayPort.vTable = &max7456VTable; + max7456DisplayPort.isGrabbed = false; + resync(&max7456DisplayPort); + return &max7456DisplayPort; +} +#endif // OSD diff --git a/src/main/io/displayport_max7456.h b/src/main/io/displayport_max7456.h new file mode 100644 index 0000000000..bbb4aced35 --- /dev/null +++ b/src/main/io/displayport_max7456.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 *max7456DisplayPortInit(void); diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c new file mode 100644 index 0000000000..ff16c04c8f --- /dev/null +++ b/src/main/io/displayport_msp.c @@ -0,0 +1,119 @@ +/* + * 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 + +#include "platform.h" + +#ifdef USE_MSP_DISPLAYPORT + +#include "common/utils.h" + +#include "drivers/display.h" +#include "drivers/system.h" + +#include "fc/fc_msp.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +static displayPort_t mspDisplayPort; + +static int output(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, int len) +{ + UNUSED(displayPort); + return mspSerialPush(cmd, buf, len); +} + +static int grab(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 0 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int heartbeat(displayPort_t *displayPort) +{ + return grab(displayPort); // ensure display is not released by MW OSD software +} + +static int release(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 1 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int clear(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 2 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int write(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string) +{ +#define MSP_OSD_MAX_STRING_LENGTH 30 + uint8_t buf[MSP_OSD_MAX_STRING_LENGTH + 4]; + + int len = strlen(string); + if (len >= MSP_OSD_MAX_STRING_LENGTH) { + len = MSP_OSD_MAX_STRING_LENGTH; + } + + buf[0] = 3; + buf[1] = row; + buf[2] = col; + buf[3] = 0; + memcpy(&buf[4], string, len); + + return output(displayPort, MSP_DISPLAYPORT, buf, len + 4); +} + +static void resync(displayPort_t *displayPort) +{ + displayPort->rows = 13; // XXX Will reflect NTSC/PAL in the future + displayPort->cols = 30; +} + +static uint32_t txBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return mspSerialTxBytesFree(); +} + +static const displayPortVTable_t mspDisplayPortVTable = { + .grab = grab, + .release = release, + .clear = clear, + .write = write, + .heartbeat = heartbeat, + .resync = resync, + .txBytesFree = txBytesFree +}; + +displayPort_t *displayPortMspInit(void) +{ + mspDisplayPort.vTable = &mspDisplayPortVTable; + mspDisplayPort.isGrabbed = false; + resync(&mspDisplayPort); + return &mspDisplayPort; +} +#endif // USE_MSP_DISPLAYPORT diff --git a/src/main/io/displayport_msp.h b/src/main/io/displayport_msp.h new file mode 100644 index 0000000000..955d0b852c --- /dev/null +++ b/src/main/io/displayport_msp.h @@ -0,0 +1,21 @@ +/* + * 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 + +struct displayPort_s; +struct displayPort_s *displayPortMspInit(void); diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c new file mode 100644 index 0000000000..8bb20329fc --- /dev/null +++ b/src/main/io/displayport_oled.c @@ -0,0 +1,91 @@ +/* + * 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 "platform.h" + +#include "common/utils.h" + +#include "drivers/display.h" +#include "drivers/display_ug2864hsweg01.h" + +static displayPort_t oledDisplayPort; + +static int oledGrab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int oledRelease(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int oledClear(displayPort_t *displayPort) +{ + UNUSED(displayPort); + i2c_OLED_clear_display_quick(); + return 0; +} + +static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) +{ + UNUSED(displayPort); + i2c_OLED_set_xy(x, y); + i2c_OLED_send_string(s); + return 0; +} + +static int oledHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static void oledResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t oledTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static const displayPortVTable_t oledVTable = { + .grab = oledGrab, + .release = oledRelease, + .clear = oledClear, + .write = oledWrite, + .heartbeat = oledHeartbeat, + .resync = oledResync, + .txBytesFree = oledTxBytesFree +}; + +displayPort_t *displayPortOledInit(void) +{ + oledDisplayPort.vTable = &oledVTable; + oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT; + oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT; + oledDisplayPort.isGrabbed = false; + return &oledDisplayPort; +} diff --git a/src/main/io/displayport_oled.h b/src/main/io/displayport_oled.h new file mode 100644 index 0000000000..4daa6de1cd --- /dev/null +++ b/src/main/io/displayport_oled.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 *displayPortOledInit(void); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 812d57a67f..9bef7f9cf6 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -62,8 +62,6 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/gps.h" -#include "io/osd.h" -#include "io/vtx.h" #include "flight/failsafe.h" #include "flight/mixer.h" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 56e027e361..9c677ccf20 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -18,6 +18,8 @@ /* Created by Marcin Baliniak some functions based on MinimOSD + + OSD-CMS separation by jflyper */ #include @@ -33,8 +35,14 @@ #include "common/utils.h" +#include "drivers/display.h" #include "drivers/system.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_osd.h" + +#include "io/displayport_max7456.h" #include "io/flashfs.h" #include "io/osd.h" @@ -42,8 +50,6 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/pid.h" - #include "config/config_profile.h" #include "config/config_master.h" #include "config/feature.h" @@ -55,582 +61,56 @@ #include "drivers/max7456.h" #include "drivers/max7456_symbols.h" -#ifdef USE_RTC6705 -#include "drivers/vtx_soft_spi_rtc6705.h" -#endif - #include "common/printf.h" +#include "build/debug.h" + +// Character coordinate and attributes + +#define OSD_POS(x,y) (x | (y << 5)) +#define OSD_X(x) (x & 0x001F) +#define OSD_Y(x) ((x >> 5) & 0x001F) + +// Things in both OSD and CMS + #define IS_HI(X) (rcData[X] > 1750) #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) -//key definiotion because API provide menu navigation over MSP/GUI app - not used NOW -#define KEY_ENTER 0 -#define KEY_UP 1 -#define KEY_DOWN 2 -#define KEY_LEFT 3 -#define KEY_RIGHT 4 -#define KEY_ESC 5 - -//osd current screen - to reduce long lines ;-) -#define OSD_cfg masterConfig.osdProfile -#define curr_profile masterConfig.profile[masterConfig.current_profile_index] - -uint16_t refreshTimeout = 0; - -#define VISIBLE_FLAG 0x0800 -#define BLINK_FLAG 0x0400 bool blinkState = true; -#define OSD_POS(x,y) (x | (y << 5)) -#define OSD_X(x) (x & 0x001F) -#define OSD_Y(x) ((x >> 5) & 0x001F) -#define VISIBLE(x) (x & VISIBLE_FLAG) -#define BLINK(x) ((x & BLINK_FLAG) && blinkState) -#define BLINK_OFF(x) (x & ~BLINK_FLAG) - -extern uint8_t RSSI; // TODO: not used? +//extern uint8_t RSSI; // TODO: not used? static uint16_t flyTime = 0; -uint8_t statRssi; +static uint8_t statRssi; -statistic_t stats; +typedef struct statistic_s { + int16_t max_speed; + int16_t min_voltage; // /10 + int16_t max_current; // /10 + int16_t min_rssi; + int16_t max_altitude; +} statistic_t; -#define BUTTON_TIME 2 -#define BUTTON_PAUSE 5 +static statistic_t stats; + +uint16_t refreshTimeout = 0; #define REFRESH_1S 12 -#define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN 23 -#define MAX_MENU_ITEMS (max7456GetRowsCount() - 2) +static uint8_t armState; -uint8_t osdRows; +static displayPort_t *osd7456DisplayPort; -//type of elements -typedef enum -{ - OME_Label, - OME_Back, - OME_OSD_Exit, - OME_Submenu, - OME_Bool, - OME_INT8, - OME_UINT8, - OME_UINT16, - OME_INT16, - OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's - //wlasciwosci elementow - OME_VISIBLE, - OME_POS, - OME_TAB, - OME_END, -} OSD_MenuElement; -//local variable to detect arm/disarm and show statistic etc -uint8_t armState; -uint8_t featureBlackbox = 0; -uint8_t featureLedstrip = 0; - -#if defined(VTX) || defined(USE_RTC6705) -uint8_t featureVtx = 0, vtxBand, vtxChannel; -#endif // VTX || USE_RTC6705 - -// We are in menu flag -bool inMenu = false; - -typedef void (* OSDMenuFuncPtr)(void *data); - -void osdUpdate(uint32_t currentTime); -char osdGetAltitudeSymbol(); -int32_t osdGetAltitude(int32_t alt); -void osdOpenMenu(void); -void osdExitMenu(void * ptr); -void osdMenuBack(void); -void osdEditElement(void *ptr); -void osdEraseFlash(void *ptr); -void osdUpdateMaxRows(void); -void osdChangeScreen(void * ptr); -void osdDrawElements(void); -void osdDrawSingleElement(uint8_t item); - -typedef struct -{ - char *text; - OSD_MenuElement type; - OSDMenuFuncPtr func; - void *data; -} OSD_Entry; - -typedef struct -{ - uint8_t *val; - uint8_t min; - uint8_t max; - uint8_t step; -} OSD_UINT8_t; - -typedef struct -{ - int8_t *val; - int8_t min; - int8_t max; - int8_t step; -} OSD_INT8_t; - -typedef struct -{ - int16_t *val; - int16_t min; - int16_t max; - int16_t step; -} OSD_INT16_t; - -typedef struct -{ - uint16_t *val; - uint16_t min; - uint16_t max; - uint16_t step; -} OSD_UINT16_t; - -typedef struct -{ - uint8_t *val; - uint8_t min; - uint8_t max; - uint8_t step; - uint16_t multipler; -} OSD_FLOAT_t; - -typedef struct -{ - uint8_t *val; - uint8_t max; - const char * const *names; -} OSD_TAB_t; - -OSD_Entry *menuStack[10]; //tab to save menu stack -uint8_t menuStackHistory[10]; //current position in menu stack -uint8_t menuStackIdx = 0; - -OSD_Entry *currentMenu; -OSD_Entry *nextPage = NULL; - -int8_t currentMenuPos = 0; -uint8_t currentMenuIdx = 0; -uint16_t *currentElement = NULL; - -OSD_Entry menuAlarms[]; -OSD_Entry menuOsdLayout[]; -OSD_Entry menuOsdActiveElems[]; -OSD_Entry menuOsdElemsPositions[]; -OSD_Entry menuFeatures[]; -OSD_Entry menuBlackbox[]; -#ifdef LED_STRIP -OSD_Entry menuLedstrip[]; -#endif // LED_STRIP -#if defined(VTX) || defined(USE_RTC6705) -OSD_Entry menu_vtx[]; -#endif // VTX || USE_RTC6705 -OSD_Entry menuImu[]; -OSD_Entry menuPid[]; -OSD_Entry menuRc[]; -OSD_Entry menuRateExpo[]; -OSD_Entry menuMisc[]; - -OSD_Entry menuMain[] = -{ - {"----MAIN MENU----", OME_Label, NULL, NULL}, - {"SCREEN LAYOUT", OME_Submenu, osdChangeScreen, &menuOsdLayout[0]}, - {"ALARMS", OME_Submenu, osdChangeScreen, &menuAlarms[0]}, - {"CFG. IMU", OME_Submenu, osdChangeScreen, &menuImu[0]}, - {"FEATURES", OME_Submenu, osdChangeScreen, &menuFeatures[0]}, - {"SAVE & EXIT", OME_OSD_Exit, osdExitMenu, (void*)1}, - {"EXIT", OME_OSD_Exit, osdExitMenu, (void*)0}, - {NULL,OME_END, NULL, NULL} -}; - -OSD_Entry menuFeatures[] = -{ - {"----- FEATURES -----", OME_Label, NULL, NULL}, - {"BLACKBOX", OME_Submenu, osdChangeScreen, &menuBlackbox[0]}, -#ifdef LED_STRIP - {"LED STRIP", OME_Submenu, osdChangeScreen, &menuLedstrip[0]}, -#endif // LED_STRIP -#if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, osdChangeScreen, &menu_vtx[0]}, -#endif // VTX || USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; - -OSD_Entry menuBlackbox[] = -{ - {"--- BLACKBOX ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureBlackbox}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom}, -#ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, osdEraseFlash, NULL}, -#endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -#ifdef LED_STRIP -//local variable to keep color value -uint8_t ledColor; - -static const char * const LED_COLOR_NAMES[] = { - " BLACK ", - " WHITE ", - " RED ", - " ORANGE ", - " YELLOW ", - " LIME GREEN", - " GREEN ", - " MINT GREEN", - " CYAN ", - " LIGHT BLUE", - " BLUE ", - "DARK VIOLET", - " MAGENTA ", - " DEEP PINK" -}; - -//find first led with color flag and restore color index -//after saving all leds with flags color will have color set in OSD -void getLedColor(void) -{ - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[ledIndex]; - - int fn = ledGetFunction(ledConfig); - - if (fn == LED_FUNCTION_COLOR) { - ledColor = ledGetColor(ledConfig); - break; - } - } -} - -//udate all leds with flag color -void applyLedColor(void * ptr) -{ - UNUSED(ptr); - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[ledIndex]; - if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) - *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); - } -} - -OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; - -OSD_Entry menuLedstrip[] = -{ - {"--- LED TRIP ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureLedstrip}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; -#endif // LED_STRIP - -#if defined(VTX) || defined(USE_RTC6705) -static const char * const vtxBandNames[] = { - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; - -OSD_TAB_t entryVtxBand = {&vtxBand,4,&vtxBandNames[0]}; -OSD_UINT8_t entryVtxChannel = {&vtxChannel, 1, 8, 1}; - -#ifdef VTX -OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; -OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; -#endif // VTX - -OSD_Entry menu_vtx[] = -{ - {"--- VTX ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureVtx}, -#ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz}, -#endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel}, -#ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power}, -#endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; -#endif // VTX || USE_RTC6705 - -OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; -OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; -OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5}; -OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5}; -OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5}; -OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; -OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; - -OSD_Entry menuMisc[]= -{ - {"----- MISC -----", OME_Label, NULL, NULL}, - {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz}, - {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf}, - {"YAW LPF", OME_UINT16, NULL, &entryYawLpf}, - {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit}, - {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle}, - {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale}, - {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; - -OSD_Entry menuImu[] = -{ - {"-----CFG. IMU-----", OME_Label, NULL, NULL}, - {"PID", OME_Submenu, osdChangeScreen, &menuPid[0]}, - {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile}, - {"RATE & RXPO", OME_Submenu, osdChangeScreen, &menuRateExpo[0]}, - {"RC PREVIEW", OME_Submenu, osdChangeScreen, &menuRc[0]}, - {"MISC", OME_Submenu, osdChangeScreen, &menuMisc[0]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -uint8_t tempPid[4][3]; - -static OSD_UINT8_t entryRollP = {&tempPid[PIDROLL][0], 10, 150, 1}; -static OSD_UINT8_t entryRollI = {&tempPid[PIDROLL][1], 1, 150, 1}; -static OSD_UINT8_t entryRollD = {&tempPid[PIDROLL][2], 0, 150, 1}; - -static OSD_UINT8_t entryPitchP = {&tempPid[PIDPITCH][0], 10, 150, 1}; -static OSD_UINT8_t entryPitchI = {&tempPid[PIDPITCH][1], 1, 150, 1}; -static OSD_UINT8_t entryPitchD = {&tempPid[PIDPITCH][2], 0, 150, 1}; - -static OSD_UINT8_t entryYawP = {&tempPid[PIDYAW][0], 10, 150, 1}; -static OSD_UINT8_t entryYawI = {&tempPid[PIDYAW][1], 1, 150, 1}; -static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1}; - -OSD_Entry menuPid[] = -{ - {"------- PID -------", OME_Label, NULL, NULL}, - {"ROLL P", OME_UINT8, NULL, &entryRollP}, - {"ROLL I", OME_UINT8, NULL, &entryRollI}, - {"ROLL D", OME_UINT8, NULL, &entryRollD}, - - {"PITCH P", OME_UINT8, NULL, &entryPitchP}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD}, - - {"YAW P", OME_UINT8, NULL, &entryYawP}, - {"YAW I", OME_UINT8, NULL, &entryYawI}, - {"YAW D", OME_UINT8, NULL, &entryYawD}, - - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -controlRateConfig_t rateProfile; - -static OSD_FLOAT_t entryRollRate = {&rateProfile.rates[0], 0, 250, 1, 10}; -static OSD_FLOAT_t entryPitchRate = {&rateProfile.rates[1], 0, 250, 1, 10}; -static OSD_FLOAT_t entryYawRate = {&rateProfile.rates[2], 0, 250, 1, 10}; -static OSD_FLOAT_t entryRcRate = {&rateProfile.rcRate8, 0, 200, 1, 10}; -static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10}; -static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10}; -static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10}; -static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10}; -static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10}; -static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10}; - -OSD_Entry menuRateExpo[] = -{ - {"----RATE & EXPO----", OME_Label, NULL, NULL}, - {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate}, - {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate}, - {"YAW RATE", OME_FLOAT, NULL, &entryYawRate}, - {"RC RATE", OME_FLOAT, NULL, &entryRcRate}, - {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo}, - {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw}, - {"THR. PID ATT.", OME_FLOAT, NULL, &extryTpaEntry}, - {"TPA BREAKPOINT", OME_UINT16, NULL, &entryTpaBreak}, - {"D SETPOINT", OME_FLOAT, NULL, &entryDSetpoint}, - {"D SETPOINT TRANSITION", OME_FLOAT, NULL, &entryPSetpoint}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; -static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0}; -static OSD_INT16_t entryRcThrottle = {&rcData[THROTTLE], 1, 2500, 0}; -static OSD_INT16_t entryRcYaw = {&rcData[YAW], 1, 2500, 0}; -static OSD_INT16_t entryRcAux1 = {&rcData[AUX1], 1, 2500, 0}; -static OSD_INT16_t entryRcAux2 = {&rcData[AUX2], 1, 2500, 0}; -static OSD_INT16_t entryRcAux3 = {&rcData[AUX3], 1, 2500, 0}; -static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0}; - -OSD_Entry menuRc[] = -{ - {"---- RC PREVIEW ----", OME_Label, NULL, NULL}, - {"ROLL", OME_INT16, NULL, &entryRcRoll}, - {"PITCH", OME_INT16, NULL, &entryRcPitch}, - {"THROTTLE", OME_INT16, NULL, &entryRcThrottle}, - {"YAW", OME_INT16, NULL, &entryRcYaw}, - {"AUX1", OME_INT16, NULL, &entryRcAux1}, - {"AUX2", OME_INT16, NULL, &entryRcAux2}, - {"AUX3", OME_INT16, NULL, &entryRcAux3}, - {"AUX4", OME_INT16, NULL, &entryRcAux4}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdLayout[] = -{ - {"---SCREEN LAYOUT---", OME_Label, NULL, NULL}, - {"ACTIVE ELEM.", OME_Submenu, osdChangeScreen, &menuOsdActiveElems[0]}, - {"POSITION", OME_Submenu, osdChangeScreen, &menuOsdElemsPositions[0]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryAlarmRssi = {&OSD_cfg.rssi_alarm, 5, 90, 5}; -OSD_UINT16_t entryAlarmCapacity = {&OSD_cfg.cap_alarm, 50, 30000, 50}; -OSD_UINT16_t enryAlarmFlyTime = {&OSD_cfg.time_alarm, 1, 200, 1}; -OSD_UINT16_t entryAlarmAltitude = {&OSD_cfg.alt_alarm, 1, 200, 1}; - -OSD_Entry menuAlarms[] = -{ - {"------ ALARMS ------", OME_Label, NULL, NULL}, - {"RSSI", OME_UINT8, NULL, &entryAlarmRssi}, - {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity}, - {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime}, - {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdElemsPositions[] = -{ - {"---POSITION---", OME_Label, NULL, NULL}, - {"RSSI", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]}, - {"MAIN BATTERY", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]}, - {"UPTIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ONTIME]}, - {"FLY TIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYTIME]}, - {"FLY MODE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYMODE]}, - {"NAME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]}, - {"THROTTLE POS", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]}, -#ifdef VTX - {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, -#endif // VTX - {"CURRENT (A)", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]}, - {"USED MAH", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]}, -#ifdef GPS - {"GPS SPEED", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED]}, - {"GPS SATS.", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS]}, -#endif // GPS - {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdActiveElems[] = -{ - {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL}, - {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]}, - {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]}, - {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]}, - {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS]}, - {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME]}, - {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME]}, - {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE]}, - {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]}, - {"THROTTLE POS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]}, -#ifdef VTX - {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, -#endif // VTX - {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]}, - {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]}, -#ifdef GPS - {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED]}, - {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS]}, -#endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -void resetOsdConfig(osd_profile_t *osdProfile) -{ - osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 11) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(22, 12) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(12, 11) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(12, 12); - osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 4); - osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6); - osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 3); - osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(15, 3); - osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); - osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); - osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); - - osdProfile->rssi_alarm = 20; - osdProfile->cap_alarm = 2200; - osdProfile->time_alarm = 10; // in minutes - osdProfile->alt_alarm = 100; // meters or feet depend on configuration - - osdProfile->video_system = 0; -} - -void osdInit(void) -{ - char x, string_buffer[30]; - - armState = ARMING_FLAG(ARMED); - - max7456Init(masterConfig.osdProfile.video_system); - - max7456ClearScreen(); - - // display logo and help - x = 160; - for (int i = 1; i < 5; i++) { - for (int j = 3; j < 27; j++) { - if (x != 255) - max7456WriteChar(j, i, x++); - } - } - - sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); - max7456Write(5, 6, string_buffer); - max7456Write(7, 7, "MENU: THRT MID"); - max7456Write(13, 8, "YAW RIGHT"); - max7456Write(13, 9, "PITCH UP"); - max7456RefreshAll(); - - refreshTimeout = 4 * REFRESH_1S; -} +#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees +#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees +#define AH_SIDEBAR_WIDTH_POS 7 +#define AH_SIDEBAR_HEIGHT_POS 3 /** * Gets the correct altitude symbol for the current unit system */ -char osdGetAltitudeSymbol() +static char osdGetAltitudeSymbol() { switch (masterConfig.osdProfile.units) { case OSD_UNIT_IMPERIAL: @@ -644,7 +124,7 @@ char osdGetAltitudeSymbol() * Converts altitude based on the current unit system. * @param alt Raw altitude (i.e. as taken from BaroAlt) */ -int32_t osdGetAltitude(int32_t alt) +static int32_t osdGetAltitude(int32_t alt) { switch (masterConfig.osdProfile.units) { case OSD_UNIT_IMPERIAL: @@ -654,831 +134,13 @@ int32_t osdGetAltitude(int32_t alt) } } -void osdUpdateAlarms(void) +static void osdDrawSingleElement(uint8_t item) { - int32_t alt = osdGetAltitude(BaroAlt) / 100; - statRssi = rssi * 100 / 1024; - - if (statRssi < OSD_cfg.rssi_alarm) - OSD_cfg.item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; - - if (vbat <= (batteryWarningVoltage - 1)) - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; - - if (STATE(GPS_FIX) == 0) - OSD_cfg.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; - - if (flyTime / 60 >= OSD_cfg.time_alarm && ARMING_FLAG(ARMED)) - OSD_cfg.item_pos[OSD_FLYTIME] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - - if (mAhDrawn >= OSD_cfg.cap_alarm) - OSD_cfg.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; - - if (alt >= OSD_cfg.alt_alarm) - OSD_cfg.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; -} - -void osdResetAlarms(void) -{ - OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; -} - -uint8_t osdHandleKey(uint8_t key) -{ - uint8_t res = BUTTON_TIME; - OSD_Entry *p; - - if (!currentMenu) - return res; - - if (key == KEY_ESC) { - osdMenuBack(); - return BUTTON_PAUSE; - } - - if (key == KEY_DOWN) { - if (currentMenuPos < currentMenuIdx) - currentMenuPos++; - else { - if (nextPage) // we have more pages - { - max7456ClearScreen(); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - osdUpdateMaxRows(); - } - currentMenuPos = 0; - } - } - - if (key == KEY_UP) { - currentMenuPos--; - - if ((currentMenu + currentMenuPos)->type == OME_Label && currentMenuPos > 0) - currentMenuPos--; - - if (currentMenuPos == -1 || (currentMenu + currentMenuPos)->type == OME_Label) { - if (nextPage) { - max7456ClearScreen(); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - osdUpdateMaxRows(); - } - currentMenuPos = currentMenuIdx; - } - } - - if (key == KEY_DOWN || key == KEY_UP) - return res; - - p = currentMenu + currentMenuPos; - - switch (p->type) { - case OME_POS: - if (key == KEY_RIGHT) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - if (!(*val & VISIBLE_FLAG)) // no submenu for hidden elements - break; - } - case OME_Submenu: - case OME_OSD_Exit: - if (p->func && key == KEY_RIGHT) { - p->func(p->data); - res = BUTTON_PAUSE; - } - break; - case OME_Back: - osdMenuBack(); - res = BUTTON_PAUSE; - break; - case OME_Bool: - if (p->data) { - uint8_t *val = p->data; - if (key == KEY_RIGHT) - *val = 1; - else - *val = 0; - } - break; - case OME_VISIBLE: - if (p->data) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - - if (key == KEY_RIGHT) - *val |= VISIBLE_FLAG; - else - *val %= ~VISIBLE_FLAG; - } - break; - case OME_UINT8: - case OME_FLOAT: - if (p->data) { - OSD_UINT8_t *ptr = p->data; - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += ptr->step; - } - else { - if (*ptr->val > ptr->min) - *ptr->val -= ptr->step; - } - } - break; - case OME_TAB: - if (p->type == OME_TAB) { - OSD_TAB_t *ptr = p->data; - - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += 1; - } - else { - if (*ptr->val > 0) - *ptr->val -= 1; - } - if (p->func) - p->func(p->data); - } - break; - case OME_INT8: - if (p->data) { - OSD_INT8_t *ptr = p->data; - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += ptr->step; - } - else { - if (*ptr->val > ptr->min) - *ptr->val -= ptr->step; - } - } - break; - case OME_UINT16: - if (p->data) { - OSD_UINT16_t *ptr = p->data; - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += ptr->step; - } - else { - if (*ptr->val > ptr->min) - *ptr->val -= ptr->step; - } - } - break; - case OME_INT16: - if (p->data) { - OSD_INT16_t *ptr = p->data; - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += ptr->step; - } - else { - if (*ptr->val > ptr->min) - *ptr->val -= ptr->step; - } - } - break; - case OME_Label: - case OME_END: - break; - } - return res; -} - -void osdUpdateMaxRows(void) -{ - OSD_Entry *ptr; - - currentMenuIdx = 0; - for (ptr = currentMenu; ptr->type != OME_END; ptr++) - currentMenuIdx++; - - if (currentMenuIdx > MAX_MENU_ITEMS) - currentMenuIdx = MAX_MENU_ITEMS; - - currentMenuIdx--; -} - -void osdMenuBack(void) -{ - uint8_t i; - - // becasue pids and rates meybe stored in profiles we need some thicks to manipulate it - // hack to save pid profile - if (currentMenu == &menuPid[0]) { - for (i = 0; i < 3; i++) { - curr_profile.pidProfile.P8[i] = tempPid[i][0]; - curr_profile.pidProfile.I8[i] = tempPid[i][1]; - curr_profile.pidProfile.D8[i] = tempPid[i][2]; - } - - curr_profile.pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - curr_profile.pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - curr_profile.pidProfile.D8[PIDLEVEL] = tempPid[3][2]; - } - - // hack - save rate config for current profile - if (currentMenu == &menuRateExpo[0]) - memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); - - if (menuStackIdx) { - max7456ClearScreen(); - menuStackIdx--; - nextPage = NULL; - currentMenu = menuStack[menuStackIdx]; - currentMenuPos = menuStackHistory[menuStackIdx]; - - osdUpdateMaxRows(); - } - else - osdOpenMenu(); -} - -void simple_ftoa(int32_t value, char *floatString) -{ - uint8_t k; - // np. 3450 - - itoa(100000 + value, floatString, 10); // Create string from abs of integer value - - // 103450 - - floatString[0] = floatString[1]; - floatString[1] = floatString[2]; - floatString[2] = '.'; - - // 03.450 - // usuwam koncowe zera i kropke - for (k = 5; k > 1; k--) - if (floatString[k] == '0' || floatString[k] == '.') - floatString[k] = 0; - else - break; - - // oraz zero wiodonce - if (floatString[0] == '0') - floatString[0] = ' '; -} - -void osdDrawMenu(void) -{ - uint8_t i = 0; - OSD_Entry *p; - char buff[10]; - uint8_t top = (osdRows - currentMenuIdx) / 2 - 1; - if (!currentMenu) + if (!VISIBLE(masterConfig.osdProfile.item_pos[item]) || BLINK(masterConfig.osdProfile.item_pos[item])) return; - if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label - currentMenuPos++; - - for (p = currentMenu; p->type != OME_END; p++) { - if (currentMenuPos == i) - max7456Write(LEFT_MENU_COLUMN, i + top, " >"); - else - max7456Write(LEFT_MENU_COLUMN, i + top, " "); - max7456Write(LEFT_MENU_COLUMN + 2, i + top, p->text); - - switch (p->type) { - case OME_POS: { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - if (!(*val & VISIBLE_FLAG)) - break; - } - case OME_Submenu: - max7456Write(RIGHT_MENU_COLUMN, i + top, ">"); - break; - case OME_Bool: - if (p->data) { - if (*((uint8_t *)(p->data))) - max7456Write(RIGHT_MENU_COLUMN, i + top, "YES"); - else - max7456Write(RIGHT_MENU_COLUMN, i + top, "NO "); - } - break; - case OME_TAB: { - OSD_TAB_t *ptr = p->data; - max7456Write(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); - break; - } - case OME_VISIBLE: - if (p->data) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - - if (VISIBLE(*val)) - max7456Write(RIGHT_MENU_COLUMN, i + top, "YES"); - else - max7456Write(RIGHT_MENU_COLUMN, i + top, "NO "); - } - break; - case OME_UINT8: - if (p->data) { - OSD_UINT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_INT8: - if (p->data) { - OSD_INT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_UINT16: - if (p->data) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_INT16: - if (p->data) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_FLOAT: - if (p->data) { - OSD_FLOAT_t *ptr = p->data; - simple_ftoa(*ptr->val * ptr->multipler, buff); - max7456Write(RIGHT_MENU_COLUMN - 1, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN - 1, i + top, buff); - } - break; - case OME_OSD_Exit: - case OME_Label: - case OME_END: - case OME_Back: - break; - } - i++; - - if (i == MAX_MENU_ITEMS) // max per page - { - nextPage = currentMenu + i; - if (nextPage->type == OME_END) - nextPage = NULL; - break; - } - } -} - -void osdResetStats(void) -{ - stats.max_current = 0; - stats.max_speed = 0; - stats.min_voltage = 500; - stats.max_current = 0; - stats.min_rssi = 99; - stats.max_altitude = 0; -} - -void osdUpdateStats(void) -{ - int16_t value; - - value = GPS_speed * 36 / 1000; - if (stats.max_speed < value) - stats.max_speed = value; - - if (stats.min_voltage > vbat) - stats.min_voltage = vbat; - - value = amperage / 100; - if (stats.max_current < value) - stats.max_current = value; - - if (stats.min_rssi > statRssi) - stats.min_rssi = statRssi; - - if (stats.max_altitude < BaroAlt) - stats.max_altitude = BaroAlt; -} - -void osdShowStats(void) -{ - uint8_t top = 2; - char buff[10]; - - max7456ClearScreen(); - max7456Write(2, top++, " --- STATS ---"); - - if (STATE(GPS_FIX)) { - max7456Write(2, top, "MAX SPEED :"); - itoa(stats.max_speed, buff, 10); - max7456Write(22, top++, buff); - } - - max7456Write(2, top, "MIN BATTERY :"); - sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10); - max7456Write(22, top++, buff); - - max7456Write(2, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - max7456Write(22, top++, buff); - - if (feature(FEATURE_CURRENT_METER)) { - max7456Write(2, top, "MAX CURRENT :"); - itoa(stats.max_current, buff, 10); - strcat(buff, "A"); - max7456Write(22, top++, buff); - - max7456Write(2, top, "USED MAH :"); - itoa(mAhDrawn, buff, 10); - strcat(buff, "\x07"); - max7456Write(22, top++, buff); - } - - max7456Write(2, top, "MAX ALTITUDE :"); - int32_t alt = osdGetAltitude(stats.max_altitude); - sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); - max7456Write(22, top++, buff); - - refreshTimeout = 60 * REFRESH_1S; -} - -// called when motors armed -void osdArmMotors(void) -{ - max7456ClearScreen(); - max7456Write(12, 7, "ARMED"); - refreshTimeout = REFRESH_1S / 2; - osdResetStats(); -} - -void updateOsd(uint32_t currentTime) -{ - static uint32_t counter; -#ifdef MAX7456_DMA_CHANNEL_TX - // don't touch buffers if DMA transaction is in progress - if (max7456DmaInProgres()) - return; -#endif // MAX7456_DMA_CHANNEL_TX - - // redraw values in buffer - if (counter++ % 5 == 0) - osdUpdate(currentTime); - else // rest of time redraw screen 10 chars per idle to don't lock the main idle - max7456DrawScreen(); - - // do not allow ARM if we are in menu - if (inMenu) - DISABLE_ARMING_FLAG(OK_TO_ARM); -} - -void osdUpdate(uint32_t currentTime) -{ - static uint8_t rcDelay = BUTTON_TIME; - static uint8_t lastSec = 0; - uint8_t key = 0, sec; - - // detect enter to menu - if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) - osdOpenMenu(); - - // detect arm/disarm - if (armState != ARMING_FLAG(ARMED)) { - if (ARMING_FLAG(ARMED)) - osdArmMotors(); // reset statistic etc - else - osdShowStats(); // show statistic - - armState = ARMING_FLAG(ARMED); - } - - osdUpdateStats(); - - sec = currentTime / 1000000; - - if (ARMING_FLAG(ARMED) && sec != lastSec) { - flyTime++; - lastSec = sec; - } - - if (refreshTimeout) { - if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics - refreshTimeout = 1; - refreshTimeout--; - if (!refreshTimeout) - max7456ClearScreen(); - return; - } - - blinkState = (millis() / 200) % 2; - - if (inMenu) { - if (rcDelay) { - rcDelay--; - } - else if (IS_HI(PITCH)) { - key = KEY_UP; - rcDelay = BUTTON_TIME; - } - else if (IS_LO(PITCH)) { - key = KEY_DOWN; - rcDelay = BUTTON_TIME; - } - else if (IS_LO(ROLL)) { - key = KEY_LEFT; - rcDelay = BUTTON_TIME; - } - else if (IS_HI(ROLL)) { - key = KEY_RIGHT; - rcDelay = BUTTON_TIME; - } - else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != menuRc) // this menu is used to check transmitter signals so can exit using YAW - { - key = KEY_ESC; - rcDelay = BUTTON_TIME; - } - - if (key && !currentElement) { - rcDelay = osdHandleKey(key); - return; - } - if (currentElement) // edit position of element - { - if (key) { - if (key == KEY_ESC) { - // exit - osdMenuBack(); - rcDelay = BUTTON_PAUSE; - *currentElement &= ~BLINK_FLAG; - currentElement = NULL; - return; - } - else { - uint8_t x, y; - x = OSD_X(*currentElement); - y = OSD_Y(*currentElement); - switch (key) { - case KEY_UP: - y--; - break; - case KEY_DOWN: - y++; - break; - case KEY_RIGHT: - x++; - break; - case KEY_LEFT: - x--; - break; - } - - *currentElement &= 0xFC00; - *currentElement |= OSD_POS(x, y); - max7456ClearScreen(); - } - } - osdDrawElements(); - } - else - osdDrawMenu(); - } - else { - osdUpdateAlarms(); - osdDrawElements(); - } -} - -void osdChangeScreen(void *ptr) -{ - uint8_t i; - if (ptr) { - max7456ClearScreen(); - // hack - save profile to temp - if (ptr == &menuPid[0]) { - for (i = 0; i < 3; i++) { - tempPid[i][0] = curr_profile.pidProfile.P8[i]; - tempPid[i][1] = curr_profile.pidProfile.I8[i]; - tempPid[i][2] = curr_profile.pidProfile.D8[i]; - } - tempPid[3][0] = curr_profile.pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = curr_profile.pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = curr_profile.pidProfile.D8[PIDLEVEL]; - } - - if (ptr == &menuRateExpo[0]) - memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); - - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; - - currentMenu = (OSD_Entry *)ptr; - currentMenuPos = 0; - osdUpdateMaxRows(); - } -} - -#ifdef USE_FLASHFS -void osdEraseFlash(void *ptr) -{ - UNUSED(ptr); - - max7456ClearScreen(); - max7456Write(5, 3, "ERASING FLASH..."); - max7456RefreshAll(); - - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); - } - max7456ClearScreen(); - max7456RefreshAll(); -} -#endif // USE_FLASHFS - -void osdEditElement(void *ptr) -{ - uint32_t address = (uint32_t)ptr; - - // zsave position on menu stack - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; - - currentElement = (uint16_t *)address; - - *currentElement |= BLINK_FLAG; - max7456ClearScreen(); -} - -void osdExitMenu(void *ptr) -{ - max7456ClearScreen(); - max7456Write(5, 3, "RESTARTING IMU..."); - max7456RefreshAll(); - stopMotors(); - stopPwmAllMotors(); - delay(200); - - if (ptr) { - // save local variables to configuration - if (featureBlackbox) - featureSet(FEATURE_BLACKBOX); - else - featureClear(FEATURE_BLACKBOX); - - if (featureLedstrip) - featureSet(FEATURE_LED_STRIP); - else - featureClear(FEATURE_LED_STRIP); -#if defined(VTX) || defined(USE_RTC6705) - if (featureVtx) - featureSet(FEATURE_VTX); - else - featureClear(FEATURE_VTX); -#endif // VTX || USE_RTC6705 - -#ifdef VTX - masterConfig.vtxBand = vtxBand; - masterConfig.vtx_channel = vtxChannel - 1; -#endif // VTX - -#ifdef USE_RTC6705 - masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1; -#endif // USE_RTC6705 - - saveConfigAndNotify(); - } - - systemReset(); -} - -void osdOpenMenu(void) -{ - if (inMenu) - return; - - if (feature(FEATURE_LED_STRIP)) - featureLedstrip = 1; - - if (feature(FEATURE_BLACKBOX)) - featureBlackbox = 1; - -#if defined(VTX) || defined(USE_RTC6705) - if (feature(FEATURE_VTX)) - featureVtx = 1; -#endif // VTX || USE_RTC6705 - -#ifdef VTX - vtxBand = masterConfig.vtxBand; - vtxChannel = masterConfig.vtx_channel + 1; -#endif // VTX - -#ifdef USE_RTC6705 - vtxBand = masterConfig.vtx_channel / 8; - vtxChannel = masterConfig.vtx_channel % 8 + 1; -#endif // USE_RTC6705 - - osdRows = max7456GetRowsCount(); - inMenu = true; - refreshTimeout = 0; - max7456ClearScreen(); - currentMenu = &menuMain[0]; - osdResetAlarms(); - osdChangeScreen(currentMenu); -#ifdef LED_STRIP - getLedColor(); -#endif // LED_STRIP -} - -void osdDrawElementPositioningHelp(void) -{ - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 1, "USE ROLL/PITCH"); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 2, "TO MOVE ELEM. "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 3, " "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); -} - -void osdDrawElements(void) -{ - max7456ClearScreen(); - - if (currentElement) - osdDrawElementPositioningHelp(); - else if (sensors(SENSOR_ACC) || inMenu) - { - osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); - osdDrawSingleElement(OSD_CROSSHAIRS); - } - - osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); - osdDrawSingleElement(OSD_RSSI_VALUE); - osdDrawSingleElement(OSD_FLYTIME); - osdDrawSingleElement(OSD_ONTIME); - osdDrawSingleElement(OSD_FLYMODE); - osdDrawSingleElement(OSD_THROTTLE_POS); - osdDrawSingleElement(OSD_VTX_CHANNEL); - osdDrawSingleElement(OSD_CURRENT_DRAW); - osdDrawSingleElement(OSD_MAH_DRAWN); - osdDrawSingleElement(OSD_CRAFT_NAME); - osdDrawSingleElement(OSD_ALTITUDE); - -#ifdef GPS - if (sensors(SENSOR_GPS) || inMenu) { - osdDrawSingleElement(OSD_GPS_SATS); - osdDrawSingleElement(OSD_GPS_SPEED); - } -#endif // GPS -} - -#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees -#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees -#define AH_SIDEBAR_WIDTH_POS 7 -#define AH_SIDEBAR_HEIGHT_POS 3 - - -void osdDrawSingleElement(uint8_t item) -{ - if (!VISIBLE(OSD_cfg.item_pos[item]) || BLINK(OSD_cfg.item_pos[item])) - return; - - uint8_t elemPosX = OSD_X(OSD_cfg.item_pos[item]); - uint8_t elemPosY = OSD_Y(OSD_cfg.item_pos[item]); + uint8_t elemPosX = OSD_X(masterConfig.osdProfile.item_pos[item]); + uint8_t elemPosY = OSD_Y(masterConfig.osdProfile.item_pos[item]); char buff[32]; switch(item) { @@ -1601,30 +263,26 @@ void osdDrawSingleElement(uint8_t item) #endif // VTX case OSD_CROSSHAIRS: - { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; - + elemPosX = 14 - 1; // Offset for 1 char to the left + elemPosY = 6; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; - - screenBuffer[position - 1] = (SYM_AH_CENTER_LINE); - screenBuffer[position + 1] = (SYM_AH_CENTER_LINE_RIGHT); - screenBuffer[position] = (SYM_AH_CENTER); - - return; - } + ++elemPosY; + buff[0] = SYM_AH_CENTER_LINE; + buff[1] = SYM_AH_CENTER; + buff[2] = SYM_AH_CENTER_LINE_RIGHT; + buff[3] = 0; + break; case OSD_ARTIFICIAL_HORIZON: { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; + elemPosX = 14; + elemPosY = 6 - 4; // Top center of the AH area int rollAngle = attitude.values.roll; int pitchAngle = attitude.values.pitch; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; + ++elemPosY; if (pitchAngle > AH_MAX_PITCH) pitchAngle = AH_MAX_PITCH; @@ -1635,13 +293,15 @@ void osdDrawSingleElement(uint8_t item) if (rollAngle < -AH_MAX_ROLL) rollAngle = -AH_MAX_ROLL; - for (uint8_t x = 0; x <= 8; x++) { - int y = (rollAngle * (4 - x)) / 64; - y -= pitchAngle / 8; - y += 41; + // Convert pitchAngle to y compensation value + pitchAngle = (pitchAngle / 8) - 41; // 41 = 4 * 9 + 5 + + for (int8_t x = -4; x <= 4; x++) { + int y = (rollAngle * x) / 64; + y -= pitchAngle; + // y += 41; // == 4 * 9 + 5 if (y >= 0 && y <= 81) { - uint16_t pos = position - 7 + LINE * (y / 9) + 3 - 4 * LINE + x; - screenBuffer[pos] = (SYM_AH_BAR9_0 + (y % 9)); + max7456WriteChar(elemPosX + x, elemPosY + (y / 9), (SYM_AH_BAR9_0 + (y % 9))); } } @@ -1652,23 +312,23 @@ void osdDrawSingleElement(uint8_t item) case OSD_HORIZON_SIDEBARS: { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; + elemPosX = 14; + elemPosY = 6; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; + ++elemPosY; // Draw AH sides int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; - for (int8_t x = -hudheight; x <= hudheight; x++) { - screenBuffer[position - hudwidth + (x * LINE)] = (SYM_AH_DECORATION); - screenBuffer[position + hudwidth + (x * LINE)] = (SYM_AH_DECORATION); + for (int8_t y = -hudheight; y <= hudheight; y++) { + max7456WriteChar(elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION); + max7456WriteChar(elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION); } // AH level indicators - screenBuffer[position - hudwidth + 1] = (SYM_AH_LEFT); - screenBuffer[position + hudwidth - 1] = (SYM_AH_RIGHT); + max7456WriteChar(elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT); + max7456WriteChar(elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT); return; } @@ -1680,4 +340,323 @@ void osdDrawSingleElement(uint8_t item) max7456Write(elemPosX, elemPosY, buff); } +void osdDrawElements(void) +{ + max7456ClearScreen(); + +#if 0 + if (currentElement) + osdDrawElementPositioningHelp(); +#else + if (false) + ; +#endif +#ifdef CMS + else if (sensors(SENSOR_ACC) || displayIsGrabbed(osd7456DisplayPort)) +#else + else if (sensors(SENSOR_ACC)) +#endif + { + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + osdDrawSingleElement(OSD_CROSSHAIRS); + } + + osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); + osdDrawSingleElement(OSD_RSSI_VALUE); + osdDrawSingleElement(OSD_FLYTIME); + osdDrawSingleElement(OSD_ONTIME); + osdDrawSingleElement(OSD_FLYMODE); + osdDrawSingleElement(OSD_THROTTLE_POS); + osdDrawSingleElement(OSD_VTX_CHANNEL); + osdDrawSingleElement(OSD_CURRENT_DRAW); + osdDrawSingleElement(OSD_MAH_DRAWN); + osdDrawSingleElement(OSD_CRAFT_NAME); + osdDrawSingleElement(OSD_ALTITUDE); + +#ifdef GPS +#ifdef CMS + if (sensors(SENSOR_GPS) || displayIsGrabbed(osd7456DisplayPort)) +#else + if (sensors(SENSOR_GPS)) +#endif + { + osdDrawSingleElement(OSD_GPS_SATS); + osdDrawSingleElement(OSD_GPS_SPEED); + } +#endif // GPS +} + +void osdResetConfig(osd_profile_t *osdProfile) +{ + osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(22, 12) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(12, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(12, 12); + osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 4); + osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6); + osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 3); + osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(15, 3); + osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); + osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); + osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); + + osdProfile->rssi_alarm = 20; + osdProfile->cap_alarm = 2200; + osdProfile->time_alarm = 10; // in minutes + osdProfile->alt_alarm = 100; // meters or feet depend on configuration + + osdProfile->video_system = 0; +} + +void osdInit(void) +{ + char x, string_buffer[30]; + + armState = ARMING_FLAG(ARMED); + + max7456Init(masterConfig.osdProfile.video_system); + + max7456ClearScreen(); + + // display logo and help + x = 160; + for (int i = 1; i < 5; i++) { + for (int j = 3; j < 27; j++) { + if (x != 255) + max7456WriteChar(j, i, x++); + } + } + + sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); + max7456Write(5, 6, string_buffer); +#ifdef CMS + max7456Write(7, 7, CMS_STARTUP_HELP_TEXT1); + max7456Write(11, 8, CMS_STARTUP_HELP_TEXT2); + max7456Write(11, 9, CMS_STARTUP_HELP_TEXT3); +#endif + + max7456RefreshAll(); + + refreshTimeout = 4 * REFRESH_1S; + + osd7456DisplayPort = max7456DisplayPortInit(); +#ifdef CMS + cmsDisplayPortRegister(osd7456DisplayPort); +#endif +} + +void osdUpdateAlarms(void) +{ + osd_profile_t *pOsdProfile = &masterConfig.osdProfile; + + // This is overdone? + // uint16_t *itemPos = masterConfig.osdProfile.item_pos; + + int32_t alt = osdGetAltitude(BaroAlt) / 100; + statRssi = rssi * 100 / 1024; + + if (statRssi < pOsdProfile->rssi_alarm) + pOsdProfile->item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + + if (vbat <= (batteryWarningVoltage - 1)) + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + + if (STATE(GPS_FIX) == 0) + pOsdProfile->item_pos[OSD_GPS_SATS] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + + if (flyTime / 60 >= pOsdProfile->time_alarm && ARMING_FLAG(ARMED)) + pOsdProfile->item_pos[OSD_FLYTIME] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + + if (mAhDrawn >= pOsdProfile->cap_alarm) + pOsdProfile->item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + + if (alt >= pOsdProfile->alt_alarm) + pOsdProfile->item_pos[OSD_ALTITUDE] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; +} + +void osdResetAlarms(void) +{ + osd_profile_t *pOsdProfile = &masterConfig.osdProfile; + + pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; +} + +static void osdResetStats(void) +{ + stats.max_current = 0; + stats.max_speed = 0; + stats.min_voltage = 500; + stats.max_current = 0; + stats.min_rssi = 99; + stats.max_altitude = 0; +} + +static void osdUpdateStats(void) +{ + int16_t value; + + value = GPS_speed * 36 / 1000; + if (stats.max_speed < value) + stats.max_speed = value; + + if (stats.min_voltage > vbat) + stats.min_voltage = vbat; + + value = amperage / 100; + if (stats.max_current < value) + stats.max_current = value; + + if (stats.min_rssi > statRssi) + stats.min_rssi = statRssi; + + if (stats.max_altitude < BaroAlt) + stats.max_altitude = BaroAlt; +} + +static void osdShowStats(void) +{ + uint8_t top = 2; + char buff[10]; + + max7456ClearScreen(); + max7456Write(2, top++, " --- STATS ---"); + + if (STATE(GPS_FIX)) { + max7456Write(2, top, "MAX SPEED :"); + itoa(stats.max_speed, buff, 10); + max7456Write(22, top++, buff); + } + + max7456Write(2, top, "MIN BATTERY :"); + sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10); + max7456Write(22, top++, buff); + + max7456Write(2, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + max7456Write(22, top++, buff); + + if (feature(FEATURE_CURRENT_METER)) { + max7456Write(2, top, "MAX CURRENT :"); + itoa(stats.max_current, buff, 10); + strcat(buff, "A"); + max7456Write(22, top++, buff); + + max7456Write(2, top, "USED MAH :"); + itoa(mAhDrawn, buff, 10); + strcat(buff, "\x07"); + max7456Write(22, top++, buff); + } + + max7456Write(2, top, "MAX ALTITUDE :"); + int32_t alt = osdGetAltitude(stats.max_altitude); + sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); + max7456Write(22, top++, buff); + + refreshTimeout = 60 * REFRESH_1S; +} + +// called when motors armed +static void osdArmMotors(void) +{ + max7456ClearScreen(); + max7456Write(12, 7, "ARMED"); + refreshTimeout = REFRESH_1S / 2; + osdResetStats(); +} + +static void osdRefresh(uint32_t currentTime) +{ + static uint8_t lastSec = 0; + uint8_t sec; + + // detect arm/disarm + if (armState != ARMING_FLAG(ARMED)) { + if (ARMING_FLAG(ARMED)) + osdArmMotors(); // reset statistic etc + else + osdShowStats(); // show statistic + + armState = ARMING_FLAG(ARMED); + } + + osdUpdateStats(); + + sec = currentTime / 1000000; + + if (ARMING_FLAG(ARMED) && sec != lastSec) { + flyTime++; + lastSec = sec; + } + + if (refreshTimeout) { + if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics + refreshTimeout = 1; + refreshTimeout--; + if (!refreshTimeout) + max7456ClearScreen(); + return; + } + + blinkState = (currentTime / 200000) % 2; + +#ifdef CMS + if (!displayIsGrabbed(osd7456DisplayPort)) { + osdUpdateAlarms(); + osdDrawElements(); +#ifdef OSD_CALLS_CMS + } else { + cmsUpdate(currentTime); +#endif + } +#endif +} + +/* + * Called periodically by the scheduler + */ +void osdUpdate(uint32_t currentTime) +{ + static uint32_t counter = 0; +#ifdef MAX7456_DMA_CHANNEL_TX + // don't touch buffers if DMA transaction is in progress + if (max7456DmaInProgres()) + return; +#endif // MAX7456_DMA_CHANNEL_TX + + // redraw values in buffer + if (counter++ % 5 == 0) + osdRefresh(currentTime); + else // rest of time redraw screen 10 chars per idle to don't lock the main idle + max7456DrawScreen(); + +#ifdef CMS + // do not allow ARM if we are in menu + if (displayIsGrabbed(osd7456DisplayPort)) { + DISABLE_ARMING_FLAG(OK_TO_ARM); + } +#endif +} + + #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7b5eae891f..e1c542ca77 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -17,7 +17,11 @@ #pragma once -#include +#define VISIBLE_FLAG 0x0800 +#define BLINK_FLAG 0x0400 +#define VISIBLE(x) (x & VISIBLE_FLAG) +#define BLINK(x) ((x & BLINK_FLAG) && blinkState) +#define BLINK_OFF(x) (x & ~BLINK_FLAG) typedef enum { OSD_RSSI_VALUE, @@ -36,16 +40,16 @@ typedef enum { OSD_GPS_SPEED, OSD_GPS_SATS, OSD_ALTITUDE, - OSD_MAX_ITEMS, // MUST BE LAST -} osd_items_t; + OSD_ITEM_COUNT // MUST BE LAST +} osd_items_e; typedef enum { OSD_UNIT_IMPERIAL, OSD_UNIT_METRIC -} osd_unit_t; +} osd_unit_e; -typedef struct { - uint16_t item_pos[OSD_MAX_ITEMS]; +typedef struct osd_profile_s { + uint16_t item_pos[OSD_ITEM_COUNT]; // Alarms uint8_t rssi_alarm; @@ -54,17 +58,12 @@ typedef struct { uint16_t alt_alarm; uint8_t video_system; - osd_unit_t units; + uint8_t row_shiftdown; + + osd_unit_e units; } osd_profile_t; -typedef struct { - int16_t max_speed; - int16_t min_voltage; // /10 - int16_t max_current; // /10 - int16_t min_rssi; - int16_t max_altitude; -} statistic_t; - -void updateOsd(uint32_t currentTime); void osdInit(void); -void resetOsdConfig(osd_profile_t *osdProfile); +void osdResetConfig(osd_profile_t *osdProfile); +void osdResetAlarms(void); +void osdUpdate(uint32_t currentTime); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index bcb7f1542f..4ee0324e62 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -35,6 +35,8 @@ uint8_t cliMode = 0; #include "build/debug.h" #include "build/version.h" +#include "cms/cms.h" + #include "common/axis.h" #include "common/color.h" #include "common/maths.h" @@ -944,6 +946,7 @@ const clivalue_t valueTable[] = { #ifdef OSD { "osd_video_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.video_system, .config.minmax = { 0, 2 } }, + { "osd_row_shiftdown", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.row_shiftdown, .config.minmax = { 0, 1 } }, { "osd_units", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.osdProfile.units, .config.lookup = { TABLE_UNIT } }, { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.rssi_alarm, .config.minmax = { 0, 100 } }, diff --git a/src/main/main.c b/src/main/main.c index 6a0cdf2514..6ad8fbc9c7 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -29,8 +29,9 @@ #include "common/maths.h" #include "common/printf.h" -#include "drivers/nvic.h" +#include "cms/cms.h" +#include "drivers/nvic.h" #include "drivers/sensor.h" #include "drivers/system.h" #include "drivers/dma.h" @@ -87,6 +88,7 @@ #include "io/serial_cli.h" #include "io/transponder_ir.h" #include "io/osd.h" +#include "io/displayport_msp.h" #include "io/vtx.h" #include "scheduler/scheduler.h" @@ -395,6 +397,10 @@ void init(void) initBoardAlignment(&masterConfig.boardAlignment); +#ifdef CMS + cmsInit(); +#endif + #ifdef USE_DASHBOARD if (feature(FEATURE_DASHBOARD)) { dashboardInit(&masterConfig.rxConfig); @@ -454,6 +460,10 @@ void init(void) mspFcInit(); mspSerialInit(); +#if defined(USE_MSP_DISPLAYPORT) && defined(CMS) + cmsDisplayPortRegister(displayPortMspInit()); +#endif + #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 338867e55f..84f0a60a1b 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -216,6 +216,9 @@ #define MSP_OSD_VIDEO_CONFIG 180 #define MSP_SET_OSD_VIDEO_CONFIG 181 +// External OSD displayport mode messages +#define MSP_DISPLAYPORT 182 + // // Multwii original MSP commands // diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index d3f68b125a..9b38932bfd 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -120,7 +120,7 @@ static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int l #define JUMBO_FRAME_SIZE_LIMIT 255 -static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) +static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) { serialBeginWrite(msp->port); const int len = sbufBytesRemaining(&packet->buf); @@ -140,6 +140,7 @@ static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) } serialWrite(msp->port, checksum); serialEndWrite(msp->port); + return sizeof(hdr) + len + 1; // header, data, and checksum } static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn) @@ -211,3 +212,57 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } +int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) +{ + static uint8_t pushBuf[30]; + int ret = 0; + + mspPacket_t push = { + .buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), }, + .cmd = cmd, + .result = 0, + }; + + for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) + if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { + continue; + } + + sbufWriteData(&push.buf, data, datalen); + + sbufSwitchToReader(&push.buf, pushBuf); + + ret = mspSerialEncode(mspPort, &push); + } + return ret; // return the number of bytes written +} + +uint32_t mspSerialTxBytesFree() +{ + uint32_t ret = UINT32_MAX; + + for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) + if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { + continue; + } + + const uint32_t bytesFree = serialTxBytesFree(mspPort->port); + if (bytesFree < ret) { + ret = bytesFree; + } + } + + return ret; +} diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index f2f071464b..f6e2954a24 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -66,3 +66,5 @@ void mspSerialInit(void); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); +int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen); +uint32_t mspSerialTxBytesFree(void); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index ed1b50f0a5..d43b778aa5 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -85,6 +85,9 @@ typedef enum { #ifdef USE_BST TASK_BST_MASTER_PROCESS, #endif +#ifdef CMS + TASK_CMS, +#endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 2eaa230431..beadee7a0d 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -91,10 +91,20 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +#define USE_DASHBOARD + +// Configuratoin Menu System +#define CMS +#define CMS_MAX_DEVICE 4 + +// Use external display connected by MSP to run CMS +#define USE_MSP_DISPLAYPORT + // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk // include the osd code #define OSD + // include the max7456 driver #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 67a0132080..5878246a44 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -9,9 +9,8 @@ TARGET_SRC = \ drivers/compass_ak8963.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ + drivers/max7456.c \ drivers/serial_usb_vcp.c \ drivers/transponder_ir.c \ drivers/transponder_ir_stm32f30x.c \ - io/transponder_ir.c \ - drivers/max7456.c \ - io/osd.c + io/transponder_ir.c diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index e4893c33c0..0fba1225a4 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -70,6 +70,9 @@ //#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER +#define CMS +#define USE_MSP_DISPLAYPORT + //#define PITOT //#define USE_PITOT_MS4525 //#define MS4525_BUS I2C_DEVICE_EXT diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index 18034c1332..333e4cdcb9 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -5,6 +5,5 @@ TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ - drivers/max7456.c \ - io/osd.c + drivers/max7456.c diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 80ff2728cf..02080b4f52 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -167,3 +167,6 @@ #endif #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) + +#define CMS +#define USE_MSP_DISPLAYPORT diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 8062644aa9..8f311405d7 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -137,8 +137,16 @@ //#define USE_QUAD_MIXER_ONLY #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_DASHBOARD + #define OSD +// Configuratoin Menu System +#define CMS + +// Use external display connected by MSP to run CMS +#define USE_MSP_DISPLAYPORT + #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_SERVOS diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index eac7fbb6ec..23a8b46be3 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -11,5 +11,4 @@ TARGET_SRC = \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ drivers/vtx_soft_spi_rtc6705.c \ - drivers/max7456.c \ - io/osd.c + drivers/max7456.c diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 718f15c982..6a4e297877 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -132,3 +132,10 @@ #define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define USE_DASHBOARD + +// Configuratoin Menu System +#define CMS + +// Use external display connected by MSP to run CMS +#define USE_MSP_DISPLAYPORT diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 97a409253e..017b143372 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -120,6 +120,8 @@ #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +#define CMS + //#define USE_SDCARD //#define USE_SDCARD_SPI2 // diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 971ddf04a2..cef064b7e3 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -25,5 +25,4 @@ TARGET_SRC = \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ - drivers/max7456.c \ - io/osd.c + drivers/max7456.c diff --git a/src/main/target/common.h b/src/main/target/common.h index 42dfed36de..c6b1ecefa3 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -55,7 +55,9 @@ #endif #if (FLASH_SIZE > 128) +#define CMS #define USE_DASHBOARD +#define USE_MSP_DISPLAYPORT #define TELEMETRY_MAVLINK #else #define SKIP_CLI_COMMAND_HELP diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 15030e1517..b4cd33fb02 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -61,8 +61,6 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/beeper.h" -#include "io/osd.h" -#include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 0076b35425..b8c446721d 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -30,9 +30,6 @@ #include "io/motors.h" #include "io/gps.h" #include "io/serial.h" -#include "io/ledstrip.h" -#include "io/osd.h" -#include "io/vtx.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" diff --git a/src/test/Makefile b/src/test/Makefile index 6581e7ae2b..d8eb183f34 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -110,6 +110,14 @@ $(OBJECT_DIR)/common/maths.o : \ @mkdir -p $(dir $@) $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@ +$(OBJECT_DIR)/common/typeconversion.o : \ + $(USER_DIR)/common/typeconversion.c \ + $(USER_DIR)/common/typeconversion.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/typeconversion.c -o $@ + $(OBJECT_DIR)/common/filter.o : \ $(USER_DIR)/common/filter.c \ $(USER_DIR)/common/filter.h \ @@ -574,6 +582,39 @@ $(OBJECT_DIR)/alignsensor_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/drivers/display.o : \ + $(USER_DIR)/drivers/display.c \ + $(USER_DIR)/drivers/display.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/display.c -o $@ + +$(OBJECT_DIR)/cms/cms.o : \ + $(USER_DIR)/cms/cms.c \ + $(USER_DIR)/cms/cms.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/cms/cms.c -o $@ + +$(OBJECT_DIR)/cms_unittest.o : \ + $(TEST_DIR)/cms_unittest.cc \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/cms_unittest.cc -o $@ + +$(OBJECT_DIR)/cms_unittest : \ + $(OBJECT_DIR)/cms_unittest.o \ + $(OBJECT_DIR)/common/typeconversion.o \ + $(OBJECT_DIR)/drivers/display.o \ + $(OBJECT_DIR)/cms/cms.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + + ## test : Build and run the Unit Tests test: $(TESTS:%=test-%) diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc new file mode 100644 index 0000000000..a417e8b7ee --- /dev/null +++ b/src/test/unit/cms_unittest.cc @@ -0,0 +1,207 @@ +/* + * 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 + +#include + +#define BARO + +extern "C" { + #include "target.h" + #include "drivers/display.h" + #include "cms/cms.h" + #include "cms/cms_types.h" + void cmsMenuOpen(void); + long cmsMenuBack(displayPort_t *pDisplay); + uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key); + extern CMS_Menu *currentMenu; // Points to top entry of the current page +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +static displayPort_t testDisplayPort; +static int displayPortTestGrab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestRelease(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestClear(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) +{ + UNUSED(displayPort); + UNUSED(x); + UNUSED(y); + UNUSED(s); + return 0; +} + +static int displayPortTestHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static void displayPortTestResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static const displayPortVTable_t testDisplayPortVTable = { + .grab = displayPortTestGrab, + .release = displayPortTestRelease, + .clear = displayPortTestClear, + .write = displayPortTestWrite, + .heartbeat = displayPortTestHeartbeat, + .resync = displayPortTestResync, + .txBytesFree = displayPortTestTxBytesFree +}; + +displayPort_t *displayPortTestInit(void) +{ + testDisplayPort.vTable = &testDisplayPortVTable; + testDisplayPort.rows = 10; + testDisplayPort.cols = 40; + testDisplayPort.isGrabbed = false; + return &testDisplayPort; +} + +TEST(CMSUnittest, TestCmsDisplayPortRegister) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + for (int ii = 0; ii < CMS_MAX_DEVICE; ++ii) { + const bool registered = cmsDisplayPortRegister(displayPort); + EXPECT_EQ(true, registered); + } + const bool registered = cmsDisplayPortRegister(displayPort); + EXPECT_EQ(false, registered); +} + +TEST(CMSUnittest, TestCmsMenuOpen) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + displayGrab(displayPort); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); +} + +TEST(CMSUnittest, TestCmsMenuExit0) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuExit(displayPort, (void*)0); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuExit1) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuExit(displayPort, (void*)0); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuBack) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuBack(displayPort); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuKey) +{ +#define KEY_ENTER 0 +#define KEY_UP 1 +#define KEY_DOWN 2 +#define KEY_LEFT 3 +#define KEY_RIGHT 4 +#define KEY_ESC 5 +#define BUTTON_TIME 250 // msec +#define BUTTON_PAUSE 500 // msec + cmsInit(); + displayPort_t *displayPort = &testDisplayPort; + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + uint16_t result = cmsHandleKey(displayPort, KEY_ESC); + EXPECT_EQ(BUTTON_PAUSE, result); +} +// STUBS + +extern "C" { +static OSD_Entry menuMainEntries[] = +{ + {"-- MAIN MENU --", OME_Label, NULL, NULL, 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; +CMS_Menu menuMain = { + "MENUMAIN", + OME_MENU, + NULL, + NULL, + NULL, + menuMainEntries, +}; +uint8_t armingFlags; +int16_t debug[4]; +int16_t rcData[18]; +void delay(uint32_t) {} +uint32_t micros(void) { return 0; } +uint32_t millis(void) { return 0; } +void saveConfigAndNotify(void) {} +void stopMotors(void) {} +void stopPwmAllMotors(void) {} +void systemReset(void) {} +} \ No newline at end of file diff --git a/src/test/unit/target.h b/src/test/unit/target.h index b7899e43a2..c2ebf479ed 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -17,6 +17,8 @@ #pragma once +#define CMS +#define CMS_MAX_DEVICE 4 #define MAG #define BARO #define GPS