From 6793692217bc3e1c5afcb989c9194c65156d9064 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 19 Oct 2016 02:39:52 +0900 Subject: [PATCH 01/19] Configuration Menu System support for external OSD A quick hack to support config menu on external OSD, consisiting of: - CMS-OSD partial separation (CMS and OSD reside in a same file: osd.c) - MSP message injection (take it as server-push in client-server model) --- src/main/fc/fc_tasks.c | 14 + src/main/io/cms.h | 1 + src/main/io/osd.c | 1576 +++++++++++++++----------- src/main/io/serial_msp.c | 28 + src/main/io/serial_msp.h | 1 + src/main/main.c | 18 + src/main/msp/msp.h | 2 + src/main/msp/msp_protocol.h | 3 + src/main/msp/msp_server_fc.c | 23 + src/main/scheduler/scheduler.h | 3 + src/main/target/OMNIBUS/target.h | 12 +- src/main/target/SPRACINGF3/target.h | 5 + src/main/target/SPRACINGF3/target.mk | 3 +- 13 files changed, 1027 insertions(+), 662 deletions(-) create mode 100644 src/main/io/cms.h diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 01d9e9e9b9..5862ac29d3 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -47,6 +47,7 @@ #include "io/serial_msp.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" +#include "io/cms.h" #include "rx/rx.h" @@ -331,6 +332,10 @@ void fcTasksInit(void) #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif +#ifdef CMS + // XXX Should check FEATURE + setTaskEnabled(TASK_CMS, true); +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -490,4 +495,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/cms.h b/src/main/io/cms.h new file mode 100644 index 0000000000..e6d9887938 --- /dev/null +++ b/src/main/io/cms.h @@ -0,0 +1 @@ +void cmsHandler(uint32_t); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ac099ab7de..eb44fa2466 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -27,8 +27,6 @@ #include "platform.h" -#ifdef OSD - #include "build/version.h" #include "common/utils.h" @@ -61,6 +59,177 @@ #include "common/printf.h" +#include "build/debug.h" + +#ifdef USE_DPRINTF +extern serialPort_t *debugSerialPort; +#define dprintf(x) if (debugSerialPort) printf x +#else +#define dprintf(x) +#endif + +// Configuration Menu System forwards + +uint8_t cmsHandleKey(uint8_t); +void cmsUpdateMaxRows(void); +void cmsOpenMenu(void); +void cmsExitMenu(void * ptr); +void cmsChangeScreen(void * ptr); +void cmsMenuBack(void); +void cmsDrawMenu(void); + +// OSD forwards + +void osdMenuBegin(void); +void osdMenuEnd(void); +void osdUpdate(uint32_t currentTime); +char osdGetAltitudeSymbol(); +int32_t osdGetAltitude(int32_t alt); +void osdEditElement(void *ptr); +void cmsEraseFlash(void *ptr); +void osdDrawElements(void); +void osdDrawSingleElement(uint8_t item); +void osdResetAlarms(void); + +#ifdef CANVAS +// +// canvasXXX() should goto io/canvas.c +// cmsXXX() should goto io/cms.c and then call display device +// specific functions; max7456XXX(), canvasXXX(), oledXXX(), ... +// + +#include "io/serial_msp.h" +#include "msp/msp_protocol.h" + +void canvasBegin(void) +{ + uint8_t subcmd = 0; + + mspSerialPush(MSP_CANVAS, &subcmd, 1); +} + +void canvasHeartBeat(void) +{ + canvasBegin(); +} + +void canvasEnd(void) +{ + uint8_t subcmd = 1; + + mspSerialPush(MSP_CANVAS, &subcmd, 1); +} + +void canvasClear(void) +{ + uint8_t subcmd = 2; + + mspSerialPush(MSP_CANVAS, &subcmd, 1); +} + +void canvasWrite(int col, int row, char *string) +{ + +//debug[0]++; // Let's capture excess canvas writes + + int len; + char buf[30 + 4]; + + if ((len = strlen(string)) >= 30) + len = 30; + + buf[0] = 3; + buf[1] = row; + buf[2] = col; + buf[3] = 0; + memcpy((char *)&buf[4], string, len); + + mspSerialPush(MSP_CANVAS, (uint8_t *)buf, len + 4); +} +#endif + +// Force draw all elements if true +bool cmsScreenCleared; + +// Function vector may be good here. + +uint8_t cmsGetRowsCount() +{ +#ifdef OSD + return max7456GetRowsCount(); +#endif + +#ifdef CANVAS + return 13; +#endif +} + +void cmsScreenClear(void) +{ +#ifdef OSD + max7456ClearScreen(); +#endif + +#ifdef CANVAS + canvasClear(); +#endif + cmsScreenCleared = true; +} + +void cmsScreenBegin(void) +{ +#ifdef OSD + osdMenuBegin(); +#endif + +#ifdef CANVAS + canvasBegin(); +#endif + + cmsScreenClear(); +} + +void cmsScreenEnd(void) +{ +#ifdef OSD + osdMenuEnd(); +#endif + +#ifdef CANVAS + canvasEnd(); +#endif +} + +void cmsScreenWrite(uint8_t x, uint8_t y, char *s) +{ +#ifdef OSD + max7456Write(x, y, s); +#endif + +#ifdef CANVAS + canvasWrite(x, y, s); +#endif +} + +void cmsScreenHeartBeat(void) +{ +#ifdef CANVAS + canvasHeartBeat(); +#endif +} + +// Find wedged device and restart (kludge!) +void cmsScreenResync(void) +{ +#ifdef OSD + max7456RefreshAll(); +#endif +} + +// +// Lots of things not separated yet. +// + #define IS_HI(X) (rcData[X] > 1750) #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) @@ -73,9 +242,11 @@ #define KEY_RIGHT 4 #define KEY_ESC 5 +#define curr_profile masterConfig.profile[masterConfig.current_profile_index] + +#ifdef OSD //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; @@ -96,6 +267,7 @@ static uint16_t flyTime = 0; uint8_t statRssi; statistic_t stats; +#endif #define BUTTON_TIME 2 #define BUTTON_PAUSE 5 @@ -103,9 +275,14 @@ statistic_t stats; #define LEFT_MENU_COLUMN 1 #define RIGHT_MENU_COLUMN 23 -#define MAX_MENU_ITEMS (max7456GetRowsCount() - 2) +#define MAX_MENU_ITEMS (cmsGetRowsCount() - 2) -uint8_t osdRows; +// +// Menu element types +// XXX Upon separation, all OME would be renamed to CME_ or similar. +// + +typedef void (* OSDMenuFuncPtr)(void *data); //type of elements typedef enum @@ -136,30 +313,13 @@ uint8_t featureLedstrip = 0; 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; + bool changed; } OSD_Entry; typedef struct @@ -218,13 +378,16 @@ OSD_Entry *currentMenu; OSD_Entry *nextPage = NULL; int8_t currentMenuPos = 0; +int8_t lastMenuPos; uint8_t currentMenuIdx = 0; uint16_t *currentElement = NULL; +#ifdef OSD OSD_Entry menuAlarms[]; OSD_Entry menuOsdLayout[]; OSD_Entry menuOsdActiveElems[]; OSD_Entry menuOsdElemsPositions[]; +#endif OSD_Entry menuFeatures[]; OSD_Entry menuBlackbox[]; #ifdef LED_STRIP @@ -241,42 +404,44 @@ 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} + {"----MAIN MENU----", OME_Label, NULL, NULL, true}, +#ifdef OSD + {"SCREEN LAYOUT", OME_Submenu, cmsChangeScreen, &menuOsdLayout[0], true}, + {"ALARMS", OME_Submenu, cmsChangeScreen, &menuAlarms[0], true}, +#endif + {"CFG. IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], true}, + {"FEATURES", OME_Submenu, cmsChangeScreen, &menuFeatures[0], true}, + {"SAVE & EXIT", OME_OSD_Exit, cmsExitMenu, (void*)1, true}, + {"EXIT", OME_OSD_Exit, cmsExitMenu, (void*)0, true}, + {NULL,OME_END, NULL, NULL, true} }; OSD_Entry menuFeatures[] = { - {"----- FEATURES -----", OME_Label, NULL, NULL}, - {"BLACKBOX", OME_Submenu, osdChangeScreen, &menuBlackbox[0]}, + {"----- FEATURES -----", OME_Label, NULL, NULL, true}, + {"BLACKBOX", OME_Submenu, cmsChangeScreen, &menuBlackbox[0], true}, #ifdef LED_STRIP - {"LED STRIP", OME_Submenu, osdChangeScreen, &menuLedstrip[0]}, + {"LED STRIP", OME_Submenu, cmsChangeScreen, &menuLedstrip[0], true}, #endif // LED_STRIP #if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, osdChangeScreen, &menu_vtx[0]}, + {"VTX", OME_Submenu, cmsChangeScreen, &menu_vtx[0], true}, #endif // VTX || USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; 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}, + {"--- BLACKBOX ---", OME_Label, NULL, NULL, true}, + {"ENABLED", OME_Bool, NULL, &featureBlackbox, true}, + {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, true}, #ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, osdEraseFlash, NULL}, + {"ERASE FLASH", OME_Submenu, cmsEraseFlash, NULL, true}, #endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; #ifdef LED_STRIP @@ -317,7 +482,7 @@ void getLedColor(void) } //udate all leds with flag color -void applyLedColor(void * ptr) +static void applyLedColor(void * ptr) { UNUSED(ptr); for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { @@ -331,11 +496,11 @@ 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} + {"--- LED TRIP ---", OME_Label, NULL, NULL, true}, + {"ENABLED", OME_Bool, NULL, &featureLedstrip, true}, + {"LED COLOR", OME_TAB, applyLedColor, &entryLed, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; #endif // LED_STRIP @@ -358,19 +523,19 @@ OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; OSD_Entry menu_vtx[] = { - {"--- VTX ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureVtx}, + {"--- VTX ---", OME_Label, NULL, NULL, true}, + {"ENABLED", OME_Bool, NULL, &featureVtx, true}, #ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz}, + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, true}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, true}, #endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel}, + {"BAND", OME_TAB, NULL, &entryVtxBand, true}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, true}, #ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power}, + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, true}, #endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; #endif // VTX || USE_RTC6705 @@ -384,30 +549,30 @@ OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 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} + {"----- MISC -----", OME_Label, NULL, NULL, true}, + {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz, true}, + {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, true}, + {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, true}, + {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit, true}, + {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle, true}, + {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, true}, + {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; 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} + {"-----CFG. IMU-----", OME_Label, NULL, NULL, true}, + {"PID", OME_Submenu, cmsChangeScreen, &menuPid[0], true}, + {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile, true}, + {"RATE & RXPO", OME_Submenu, cmsChangeScreen, &menuRateExpo[0], true}, + {"RC PREVIEW", OME_Submenu, cmsChangeScreen, &menuRc[0], true}, + {"MISC", OME_Submenu, cmsChangeScreen, &menuMisc[0], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; uint8_t tempPid[4][3]; @@ -426,21 +591,21 @@ 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}, + {"------- PID -------", OME_Label, NULL, NULL, true}, + {"ROLL P", OME_UINT8, NULL, &entryRollP, true}, + {"ROLL I", OME_UINT8, NULL, &entryRollI, true}, + {"ROLL D", OME_UINT8, NULL, &entryRollD, true}, - {"PITCH P", OME_UINT8, NULL, &entryPitchP}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD}, + {"PITCH P", OME_UINT8, NULL, &entryPitchP, true}, + {"PITCH I", OME_UINT8, NULL, &entryPitchI, true}, + {"PITCH D", OME_UINT8, NULL, &entryPitchD, true}, - {"YAW P", OME_UINT8, NULL, &entryYawP}, - {"YAW I", OME_UINT8, NULL, &entryYawI}, - {"YAW D", OME_UINT8, NULL, &entryYawD}, + {"YAW P", OME_UINT8, NULL, &entryYawP, true}, + {"YAW I", OME_UINT8, NULL, &entryYawI, true}, + {"YAW D", OME_UINT8, NULL, &entryYawD, true}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; controlRateConfig_t rateProfile; @@ -458,19 +623,19 @@ static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSe 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} + {"----RATE & EXPO----", OME_Label, NULL, NULL, true}, + {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate, true}, + {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate, true}, + {"YAW RATE", OME_FLOAT, NULL, &entryYawRate, true}, + {"RC RATE", OME_FLOAT, NULL, &entryRcRate, true}, + {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, true}, + {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, true}, + {"THR. PID ATT.", OME_FLOAT, NULL, &extryTpaEntry, true}, + {"TPA BREAKPOINT", OME_UINT16, NULL, &entryTpaBreak, true}, + {"D SETPOINT", OME_FLOAT, NULL, &entryDSetpoint, true}, + {"D SETPOINT TRANSITION", OME_FLOAT, NULL, &entryPSetpoint, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; @@ -484,26 +649,30 @@ 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} + {"---- RC PREVIEW ----", OME_Label, NULL, NULL, true}, + {"ROLL", OME_INT16, NULL, &entryRcRoll, true}, + {"PITCH", OME_INT16, NULL, &entryRcPitch, true}, + {"THROTTLE", OME_INT16, NULL, &entryRcThrottle, true}, + {"YAW", OME_INT16, NULL, &entryRcYaw, true}, + {"AUX1", OME_INT16, NULL, &entryRcAux1, true}, + {"AUX2", OME_INT16, NULL, &entryRcAux2, true}, + {"AUX3", OME_INT16, NULL, &entryRcAux3, true}, + {"AUX4", OME_INT16, NULL, &entryRcAux4, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; +#ifdef OSD +// +// OSD specific menu pages and items for +// 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} + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, true}, + {"ACTIVE ELEM.", OME_Submenu, cmsChangeScreen, &menuOsdActiveElems[0], true}, + {"POSITION", OME_Submenu, cmsChangeScreen, &menuOsdElemsPositions[0], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; OSD_UINT8_t entryAlarmRssi = {&OSD_cfg.rssi_alarm, 5, 90, 5}; @@ -513,193 +682,88 @@ 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} + {"------ ALARMS ------", OME_Label, NULL, NULL, true}, + {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, true}, + {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity, true}, + {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, true}, + {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; 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]}, + {"---POSITION---", OME_Label, NULL, NULL, true}, + {"RSSI", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], true}, + {"MAIN BATTERY", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, + {"UPTIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ONTIME], true}, + {"FLY TIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], true}, + {"FLY MODE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], true}, + {"NAME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], true}, + {"THROTTLE POS", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], true}, #ifdef VTX - {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, + {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL], true}, #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]}, + {"CURRENT (A)", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], true}, + {"USED MAH", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], true}, #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]}, + {"GPS SPEED", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], true}, + {"GPS SATS.", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], true}, #endif // GPS - {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; 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]}, + {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL, true}, + {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], true}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, + {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], true}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], true}, + {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], true}, + {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], true}, + {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], true}, + {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], true}, + {"THROTTLE POS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], true}, #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]}, + {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], true}, + {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], true}, #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]}, + {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], true}, + {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], true}, #endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; +#endif -void resetOsdConfig(osd_profile_t *osdProfile) +uint8_t cmsRows; +bool cmsInMenu = false; + +// +// CMS specific functions +// + +void cmsUpdateMaxRows(void) { - 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); + OSD_Entry *ptr; - osdProfile->rssi_alarm = 20; - osdProfile->cap_alarm = 2200; - osdProfile->time_alarm = 10; // in minutes - osdProfile->alt_alarm = 100; // meters or feet depend on configuration + currentMenuIdx = 0; + for (ptr = currentMenu; ptr->type != OME_END; ptr++) + currentMenuIdx++; - osdProfile->video_system = 0; + if (currentMenuIdx > MAX_MENU_ITEMS) + currentMenuIdx = MAX_MENU_ITEMS; + + currentMenuIdx--; } -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; -} - -/** - * Gets the correct altitude symbol for the current unit system - */ -char osdGetAltitudeSymbol() -{ - switch (masterConfig.osdProfile.units) { - case OSD_UNIT_IMPERIAL: - return 0xF; - default: - return 0xC; - } -} - -/** - * Converts altitude based on the current unit system. - * @param alt Raw altitude (i.e. as taken from BaroAlt) - */ -int32_t osdGetAltitude(int32_t alt) -{ - switch (masterConfig.osdProfile.units) { - case OSD_UNIT_IMPERIAL: - return (alt * 328) / 100; // Convert to feet / 100 - default: - return alt; // Already in metre / 100 - } -} - -void osdUpdateAlarms(void) -{ - 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 cmsHandleKey(uint8_t key) { uint8_t res = BUTTON_TIME; OSD_Entry *p; @@ -708,24 +772,26 @@ uint8_t osdHandleKey(uint8_t key) return res; if (key == KEY_ESC) { - osdMenuBack(); + cmsMenuBack(); return BUTTON_PAUSE; } if (key == KEY_DOWN) { - if (currentMenuPos < currentMenuIdx) + if (currentMenuPos < currentMenuIdx) { currentMenuPos++; - else { + } else { if (nextPage) // we have more pages { - max7456ClearScreen(); + cmsScreenClear(); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; currentMenuPos = 0; - osdUpdateMaxRows(); + lastMenuPos = -1; + cmsUpdateMaxRows(); + } else { + currentMenuPos = 0; } - currentMenuPos = 0; } } @@ -737,14 +803,17 @@ uint8_t osdHandleKey(uint8_t key) if (currentMenuPos == -1 || (currentMenu + currentMenuPos)->type == OME_Label) { if (nextPage) { - max7456ClearScreen(); + cmsScreenClear(); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; currentMenuPos = 0; - osdUpdateMaxRows(); + lastMenuPos = -1; + cmsUpdateMaxRows(); + } else { + currentMenuPos = currentMenuIdx; + // lastMenuPos = -1; } - currentMenuPos = currentMenuIdx; } } @@ -755,6 +824,7 @@ uint8_t osdHandleKey(uint8_t key) switch (p->type) { case OME_POS: +#ifdef OSD if (key == KEY_RIGHT) { uint32_t address = (uint32_t)p->data; uint16_t *val; @@ -763,6 +833,7 @@ uint8_t osdHandleKey(uint8_t key) if (!(*val & VISIBLE_FLAG)) // no submenu for hidden elements break; } +#endif case OME_Submenu: case OME_OSD_Exit: if (p->func && key == KEY_RIGHT) { @@ -771,7 +842,7 @@ uint8_t osdHandleKey(uint8_t key) } break; case OME_Back: - osdMenuBack(); + cmsMenuBack(); res = BUTTON_PAUSE; break; case OME_Bool: @@ -781,9 +852,11 @@ uint8_t osdHandleKey(uint8_t key) *val = 1; else *val = 0; + p->changed = true; } break; case OME_VISIBLE: +#ifdef OSD if (p->data) { uint32_t address = (uint32_t)p->data; uint16_t *val; @@ -794,7 +867,9 @@ uint8_t osdHandleKey(uint8_t key) *val |= VISIBLE_FLAG; else *val %= ~VISIBLE_FLAG; + p->changed = true; } +#endif break; case OME_UINT8: case OME_FLOAT: @@ -808,6 +883,7 @@ uint8_t osdHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } + p->changed = true; } break; case OME_TAB: @@ -824,6 +900,7 @@ uint8_t osdHandleKey(uint8_t key) } if (p->func) p->func(p->data); + p->changed = true; } break; case OME_INT8: @@ -837,6 +914,7 @@ uint8_t osdHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } + p->changed = true; } break; case OME_UINT16: @@ -850,6 +928,7 @@ uint8_t osdHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } + p->changed = true; } break; case OME_INT16: @@ -863,6 +942,7 @@ uint8_t osdHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } + p->changed = true; } break; case OME_Label: @@ -872,56 +952,7 @@ uint8_t osdHandleKey(uint8_t key) 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) +static void simple_ftoa(int32_t value, char *floatString) { uint8_t k; // np. 3450 @@ -947,101 +978,125 @@ void simple_ftoa(int32_t value, char *floatString) floatString[0] = ' '; } -void osdDrawMenu(void) +void cmsDrawMenu(void) { uint8_t i = 0; OSD_Entry *p; char buff[10]; - uint8_t top = (osdRows - currentMenuIdx) / 2 - 1; + uint8_t top = (cmsRows - currentMenuIdx) / 2 - 1; + if (!currentMenu) return; if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label currentMenuPos++; + if (lastMenuPos >= 0 && currentMenuPos != lastMenuPos) + cmsScreenWrite(LEFT_MENU_COLUMN, lastMenuPos + top, " "); + 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); + + if (currentMenuPos == i && lastMenuPos != currentMenuPos) { + cmsScreenWrite(LEFT_MENU_COLUMN, i + top, " >"); + lastMenuPos = currentMenuPos; + } + + if (cmsScreenCleared) + cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text); switch (p->type) { - case OME_POS: { + case OME_POS: +#ifdef OSD uint32_t address = (uint32_t)p->data; uint16_t *val; val = (uint16_t *)address; if (!(*val & VISIBLE_FLAG)) break; - } +#endif + case OME_Submenu: - max7456Write(RIGHT_MENU_COLUMN, i + top, ">"); + if (cmsScreenCleared) + cmsScreenWrite(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 "); + if ((p->changed || cmsScreenCleared) && p->data) { + if (*((uint8_t *)(p->data))) { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES"); + } else { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO "); + } + p->changed = false; } break; case OME_TAB: { - OSD_TAB_t *ptr = p->data; - max7456Write(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); + if (p->changed || cmsScreenCleared) { + OSD_TAB_t *ptr = p->data; + cmsScreenWrite(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); + p->changed = false; + } break; } case OME_VISIBLE: - if (p->data) { +#ifdef OSD + if ((p->changed || cmsScreenCleared) && 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 "); + if (VISIBLE(*val)) { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES"); + } else { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO "); + } + p->changed = false; } +#endif break; case OME_UINT8: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; } break; case OME_INT8: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; } break; case OME_UINT16: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; } break; case OME_INT16: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; } break; case OME_FLOAT: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, buff); + p->changed = false; } break; case OME_OSD_Exit: @@ -1050,6 +1105,7 @@ void osdDrawMenu(void) case OME_Back: break; } + i++; if (i == MAX_MENU_ITEMS) // max per page @@ -1060,233 +1116,14 @@ void osdDrawMenu(void) break; } } + cmsScreenCleared = false; } -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) +void cmsChangeScreen(void *ptr) { uint8_t i; if (ptr) { - max7456ClearScreen(); + cmsScreenClear(); // hack - save profile to temp if (ptr == &menuPid[0]) { for (i = 0; i < 3; i++) { @@ -1308,48 +1145,96 @@ void osdChangeScreen(void *ptr) currentMenu = (OSD_Entry *)ptr; currentMenuPos = 0; - osdUpdateMaxRows(); + lastMenuPos = -1; // XXX this? + cmsUpdateMaxRows(); } } -#ifdef USE_FLASHFS -void osdEraseFlash(void *ptr) +void cmsMenuBack(void) { - UNUSED(ptr); + uint8_t i; - max7456ClearScreen(); - max7456Write(5, 3, "ERASING FLASH..."); - max7456RefreshAll(); + // 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]; + } - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); + curr_profile.pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + curr_profile.pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + curr_profile.pidProfile.D8[PIDLEVEL] = tempPid[3][2]; } - max7456ClearScreen(); - max7456RefreshAll(); -} -#endif // USE_FLASHFS -void osdEditElement(void *ptr) -{ - uint32_t address = (uint32_t)ptr; + // 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)); - // zsave position on menu stack - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; + if (menuStackIdx) { + cmsScreenClear(); + menuStackIdx--; + nextPage = NULL; + currentMenu = menuStack[menuStackIdx]; + currentMenuPos = menuStackHistory[menuStackIdx]; + lastMenuPos = -1; - currentElement = (uint16_t *)address; - - *currentElement |= BLINK_FLAG; - max7456ClearScreen(); + cmsUpdateMaxRows(); + } + else { + cmsOpenMenu(); + } } -void osdExitMenu(void *ptr) +void cmsOpenMenu(void) { - max7456ClearScreen(); - max7456Write(5, 3, "RESTARTING IMU..."); - max7456RefreshAll(); + if (cmsInMenu) + 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 + + cmsRows = cmsGetRowsCount(); + cmsInMenu = true; +#ifdef OSD +// XXX Do we need this here? + refreshTimeout = 0; +#endif + cmsScreenBegin(); + cmsScreenClear(); + currentMenu = &menuMain[0]; + cmsChangeScreen(currentMenu); +#ifdef LED_STRIP + getLedColor(); +#endif // LED_STRIP +} + +void cmsExitMenu(void *ptr) +{ + cmsScreenClear(); + + cmsScreenWrite(5, 3, "RESTARTING IMU..."); + cmsScreenResync(); // Was max7456RefreshAll(); why at this timing? + stopMotors(); stopPwmAllMotors(); delay(200); @@ -1384,47 +1269,113 @@ void osdExitMenu(void *ptr) saveConfigAndNotify(); } + cmsInMenu = false; + + cmsScreenEnd(); + systemReset(); } -void osdOpenMenu(void) +void cmsProcess(uint32_t currentTime) { - if (inMenu) - return; + static uint8_t rcDelay = BUTTON_TIME; + uint8_t key = 0; + static uint32_t lastCmsHeartBeat = 0; - if (feature(FEATURE_LED_STRIP)) - featureLedstrip = 1; +//debug[1]++; - if (feature(FEATURE_BLACKBOX)) - featureBlackbox = 1; + // detect enter to menu + if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsOpenMenu(); + } -#if defined(VTX) || defined(USE_RTC6705) - if (feature(FEATURE_VTX)) - featureVtx = 1; -#endif // VTX || USE_RTC6705 + if (cmsInMenu) { + 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; + } -#ifdef VTX - vtxBand = masterConfig.vtxBand; - vtxChannel = masterConfig.vtx_channel + 1; -#endif // VTX + // XXX Element position adjustment is hard to separate. + // XXX May need to drop it upon real separation. + // XXX Don't know if this still works + if (key && !currentElement) { + rcDelay = cmsHandleKey(key); + return; + } -#ifdef USE_RTC6705 - vtxBand = masterConfig.vtx_channel / 8; - vtxChannel = masterConfig.vtx_channel % 8 + 1; -#endif // USE_RTC6705 + cmsDrawMenu(); - osdRows = max7456GetRowsCount(); - inMenu = true; - refreshTimeout = 0; - max7456ClearScreen(); - currentMenu = &menuMain[0]; - osdResetAlarms(); - osdChangeScreen(currentMenu); -#ifdef LED_STRIP - getLedColor(); -#endif // LED_STRIP + if (currentTime > lastCmsHeartBeat + 500) { + // Heart beat for external CMS display device @ 500msec + // (Timeout @ 1000msec) + cmsScreenHeartBeat(); + lastCmsHeartBeat = currentTime; + } + } } +void cmsHandler(uint32_t currentTime) +{ + static uint32_t counter = 0; + + if (counter++ % 5 == 0) { + cmsProcess(currentTime); + } + + // do not allow ARM if we are in menu + if (cmsInMenu) + DISABLE_ARMING_FLAG(OK_TO_ARM); + +} + +// Does this belong here? + +#ifdef USE_FLASHFS +void cmsEraseFlash(void *ptr) +{ + UNUSED(ptr); + + cmsScreenClear(); + cmsScreenWrite(5, 3, "ERASING FLASH..."); + cmsScreenResync(); // Was max7456RefreshAll(); Why at this timing? + + flashfsEraseCompletely(); + while (!flashfsIsReady()) { + delay(100); + } + + cmsScreenClear(); + cmsScreenResync(); // Was max7456RefreshAll(); wedges during heavy SPI? +} +#endif // USE_FLASHFS + +#ifdef OSD +bool osdInMenu = false; + +// +// OSD specific CMS function +// + void osdDrawElementPositioningHelp(void) { max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); @@ -1434,13 +1385,26 @@ void osdDrawElementPositioningHelp(void) max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); } +// +// OSD spcific functions +// + +void osdMenuBegin(void) { + osdResetAlarms(); + osdInMenu = true; +} + +void osdMenuEnd(void) { + osdInMenu = false; +} + void osdDrawElements(void) { max7456ClearScreen(); if (currentElement) osdDrawElementPositioningHelp(); - else if (sensors(SENSOR_ACC) || inMenu) + else if (sensors(SENSOR_ACC) || osdInMenu) { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); osdDrawSingleElement(OSD_CROSSHAIRS); @@ -1459,7 +1423,7 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_ALTITUDE); #ifdef GPS - if (sensors(SENSOR_GPS) || inMenu) { + if (sensors(SENSOR_GPS) || osdInMenu) { osdDrawSingleElement(OSD_GPS_SATS); osdDrawSingleElement(OSD_GPS_SPEED); } @@ -1680,4 +1644,298 @@ void osdDrawSingleElement(uint8_t item) max7456Write(elemPosX, elemPosY, buff); } +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"); + cmsScreenResync(); // Was max7456RefreshAll(); may be okay. + + refreshTimeout = 4 * REFRESH_1S; +} + +/** + * Gets the correct altitude symbol for the current unit system + */ +char osdGetAltitudeSymbol() +{ + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return 0xF; + default: + return 0xC; + } +} + +/** + * Converts altitude based on the current unit system. + * @param alt Raw altitude (i.e. as taken from BaroAlt) + */ +int32_t osdGetAltitude(int32_t alt) +{ + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return (alt * 328) / 100; // Convert to feet / 100 + default: + return alt; // Already in metre / 100 + } +} + +void osdUpdateAlarms(void) +{ + 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; +} + +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 = 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) + 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 (osdInMenu) + DISABLE_ARMING_FLAG(OK_TO_ARM); +} + +void osdUpdate(uint32_t currentTime) +{ + static uint8_t lastSec = 0; + uint8_t sec; + + // detect enter to menu + if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsOpenMenu(); + } + + // 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 (!osdInMenu) { + osdUpdateAlarms(); + osdDrawElements(); + } else { + cmsProcess(currentTime); + } +} + +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(); +} + #endif // OSD diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3fc7cbc55b..f05323a17d 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -84,6 +84,34 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } +#ifdef USE_DPRINTF +#include "common/printf.h" +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 +extern serialPort_t *debugSerialPort; +#define dprintf(x) if (debugSerialPort) printf x +#else +#define dprintf(x) +#endif + +void mspSerialPush(int cmd, uint8_t *data, int buflen) +{ + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // Big enough for a OSD line + uint8_t buf[sizeof(bufWriter_t) + 30]; + + writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port); + + mspServerPush(mspPort, cmd, data, buflen); + + bufWriterFlush(writer); + } +} + /* * Process MSP commands from serial ports configured as MSP ports. * diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index dd552da664..cac35d4eba 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -52,3 +52,4 @@ void mspSerialInit(void); void mspSerialProcess(void); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); +void mspSerialPush(int, uint8_t *, int); diff --git a/src/main/main.c b/src/main/main.c index 2a61781213..10eb3db37a 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -126,6 +126,14 @@ extern uint8_t motorControlEnable; serialPort_t *loopbackPort; #endif +#ifdef USE_DPRINTF +#include "common/printf.h" +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 +serialPort_t *debugSerialPort = NULL; +#define dprintf(x) if (debugSerialPort) printf x +#else +#define dprintf(x) +#endif typedef enum { SYSTEM_STATE_INITIALISING = 0, @@ -244,6 +252,16 @@ void init(void) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif +#ifdef USE_DPRINTF + // Setup debugSerialPort + + debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0); + if (debugSerialPort) { + setPrintfSerialPort(debugSerialPort); + dprintf(("debugSerialPort: OK\r\n")); + } +#endif + mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #ifdef USE_SERVOS servoMixerInit(masterConfig.customServoMixer); diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index e49dd6bb77..9185aef992 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -23,3 +23,5 @@ typedef void (*mspPostProcessFuncPtr)(struct mspPort_s *); // msp post process f void mspInit(void); bool mspProcessReceivedData(struct mspPort_s *mspPort, uint8_t c); mspPostProcessFuncPtr mspProcessReceivedCommand(struct mspPort_s *mspPort); + +void mspServerPush(mspPort_t *, int, uint8_t *, int); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 8d14b4d698..a0fee2efb7 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 canvas mode messages +#define MSP_CANVAS 182 + // // Multwii original MSP commands // diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c index 75e81e1dd0..2b36e8d248 100755 --- a/src/main/msp/msp_server_fc.c +++ b/src/main/msp/msp_server_fc.c @@ -100,6 +100,15 @@ #include "config/config_master.h" #include "config/feature.h" +#ifdef USE_DPRINTF +#include "common/printf.h" +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 +extern serialPort_t *debugSerialPort; +#define dprintf(x) if (debugSerialPort) printf x +#else +#define dprintf(x) +#endif + #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -1968,3 +1977,17 @@ bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c) } return true; } + +void mspServerPush(mspPort_t *mspPort, int cmd, uint8_t *data, int len) +{ + currentPort = mspPort; + mspPort->cmdMSP = cmd; + + headSerialReply(len); + + while (len--) { + serialize8(*data++); + } + + tailSerialReply(); +} diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index f44cd497f9..5b5eccaca1 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 86c9d1699b..c8b434d69a 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -1,3 +1,4 @@ +#define USE_DPRINTF /* * This file is part of Cleanflight. * @@ -89,12 +90,19 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +// Configuratoin Menu System +#define CMS + +// Use external OSD to run CMS +#define CANVAS + // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk // include the osd code -#define OSD +//#define OSD + // include the max7456 driver -#define USE_MAX7456 +//#define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 #define MAX7456_SPI_CS_PIN PB1 #define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 543a91d20b..d337d42552 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -129,3 +129,8 @@ #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) ) +// Configuratoin Menu System +#define CMS + +// Use external OSD to run CMS +#define CANVAS diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index 5b3a330295..22dcede4b5 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -8,5 +8,6 @@ TARGET_SRC = \ drivers/barometer_bmp085.c \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c + drivers/compass_hmc5883l.c \ + io/osd.c From 01eab00fd74668f19247c1dcb28b7d6d738e8b22 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 19 Oct 2016 02:39:52 +0900 Subject: [PATCH 02/19] Configuration Menu System support for external OSD A quick hack to support config menu on external OSD, consisiting of: - CMS-OSD partial separation (CMS and OSD reside in a same file: osd.c) - MSP message injection (take it as server-push in client-server model) --- src/main/fc/fc_msp.c | 23 + src/main/fc/fc_tasks.c | 14 + src/main/io/cms.h | 1 + src/main/io/osd.c | 1576 +++++++++++++++----------- src/main/main.c | 18 + src/main/msp/msp_protocol.h | 3 + src/main/msp/msp_serial.c | 28 + src/main/msp/msp_serial.h | 1 + src/main/scheduler/scheduler.h | 3 + src/main/target/OMNIBUS/target.h | 12 +- src/main/target/SPRACINGF3/target.h | 5 + src/main/target/SPRACINGF3/target.mk | 3 +- 12 files changed, 1025 insertions(+), 662 deletions(-) create mode 100644 src/main/io/cms.h diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e5529f7c75..2d8a8193b0 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -102,6 +102,15 @@ #include "config/config_master.h" #include "config/feature.h" +#ifdef USE_DPRINTF +#include "common/printf.h" +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 +extern serialPort_t *debugSerialPort; +#define dprintf(x) if (debugSerialPort) printf x +#else +#define dprintf(x) +#endif + #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -1945,3 +1954,17 @@ mspProcessCommandFnPtr mspFcInit(void) initActiveBoxIds(); return mspFcProcessCommand; } + +void mspServerPush(mspPort_t *mspPort, int cmd, uint8_t *data, int len) +{ + currentPort = mspPort; + mspPort->cmdMSP = cmd; + + headSerialReply(len); + + while (len--) { + serialize8(*data++); + } + + tailSerialReply(); +} diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index d3d088f29e..468c4c9cdd 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -47,6 +47,7 @@ #include "io/serial.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" +#include "io/cms.h" #include "msp/msp_serial.h" @@ -328,6 +329,10 @@ void fcTasksInit(void) #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif +#ifdef CMS + // XXX Should check FEATURE + setTaskEnabled(TASK_CMS, true); +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -487,4 +492,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/cms.h b/src/main/io/cms.h new file mode 100644 index 0000000000..e6d9887938 --- /dev/null +++ b/src/main/io/cms.h @@ -0,0 +1 @@ +void cmsHandler(uint32_t); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c9ad4c1d58..94b4d68f4b 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -27,8 +27,6 @@ #include "platform.h" -#ifdef OSD - #include "build/version.h" #include "common/utils.h" @@ -61,6 +59,177 @@ #include "common/printf.h" +#include "build/debug.h" + +#ifdef USE_DPRINTF +extern serialPort_t *debugSerialPort; +#define dprintf(x) if (debugSerialPort) printf x +#else +#define dprintf(x) +#endif + +// Configuration Menu System forwards + +uint8_t cmsHandleKey(uint8_t); +void cmsUpdateMaxRows(void); +void cmsOpenMenu(void); +void cmsExitMenu(void * ptr); +void cmsChangeScreen(void * ptr); +void cmsMenuBack(void); +void cmsDrawMenu(void); + +// OSD forwards + +void osdMenuBegin(void); +void osdMenuEnd(void); +void osdUpdate(uint32_t currentTime); +char osdGetAltitudeSymbol(); +int32_t osdGetAltitude(int32_t alt); +void osdEditElement(void *ptr); +void cmsEraseFlash(void *ptr); +void osdDrawElements(void); +void osdDrawSingleElement(uint8_t item); +void osdResetAlarms(void); + +#ifdef CANVAS +// +// canvasXXX() should goto io/canvas.c +// cmsXXX() should goto io/cms.c and then call display device +// specific functions; max7456XXX(), canvasXXX(), oledXXX(), ... +// + +#include "io/serial_msp.h" +#include "msp/msp_protocol.h" + +void canvasBegin(void) +{ + uint8_t subcmd = 0; + + mspSerialPush(MSP_CANVAS, &subcmd, 1); +} + +void canvasHeartBeat(void) +{ + canvasBegin(); +} + +void canvasEnd(void) +{ + uint8_t subcmd = 1; + + mspSerialPush(MSP_CANVAS, &subcmd, 1); +} + +void canvasClear(void) +{ + uint8_t subcmd = 2; + + mspSerialPush(MSP_CANVAS, &subcmd, 1); +} + +void canvasWrite(int col, int row, char *string) +{ + +//debug[0]++; // Let's capture excess canvas writes + + int len; + char buf[30 + 4]; + + if ((len = strlen(string)) >= 30) + len = 30; + + buf[0] = 3; + buf[1] = row; + buf[2] = col; + buf[3] = 0; + memcpy((char *)&buf[4], string, len); + + mspSerialPush(MSP_CANVAS, (uint8_t *)buf, len + 4); +} +#endif + +// Force draw all elements if true +bool cmsScreenCleared; + +// Function vector may be good here. + +uint8_t cmsGetRowsCount() +{ +#ifdef OSD + return max7456GetRowsCount(); +#endif + +#ifdef CANVAS + return 13; +#endif +} + +void cmsScreenClear(void) +{ +#ifdef OSD + max7456ClearScreen(); +#endif + +#ifdef CANVAS + canvasClear(); +#endif + cmsScreenCleared = true; +} + +void cmsScreenBegin(void) +{ +#ifdef OSD + osdMenuBegin(); +#endif + +#ifdef CANVAS + canvasBegin(); +#endif + + cmsScreenClear(); +} + +void cmsScreenEnd(void) +{ +#ifdef OSD + osdMenuEnd(); +#endif + +#ifdef CANVAS + canvasEnd(); +#endif +} + +void cmsScreenWrite(uint8_t x, uint8_t y, char *s) +{ +#ifdef OSD + max7456Write(x, y, s); +#endif + +#ifdef CANVAS + canvasWrite(x, y, s); +#endif +} + +void cmsScreenHeartBeat(void) +{ +#ifdef CANVAS + canvasHeartBeat(); +#endif +} + +// Find wedged device and restart (kludge!) +void cmsScreenResync(void) +{ +#ifdef OSD + max7456RefreshAll(); +#endif +} + +// +// Lots of things not separated yet. +// + #define IS_HI(X) (rcData[X] > 1750) #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) @@ -73,9 +242,11 @@ #define KEY_RIGHT 4 #define KEY_ESC 5 +#define curr_profile masterConfig.profile[masterConfig.current_profile_index] + +#ifdef OSD //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; @@ -96,6 +267,7 @@ static uint16_t flyTime = 0; uint8_t statRssi; statistic_t stats; +#endif #define BUTTON_TIME 2 #define BUTTON_PAUSE 5 @@ -103,9 +275,14 @@ statistic_t stats; #define LEFT_MENU_COLUMN 1 #define RIGHT_MENU_COLUMN 23 -#define MAX_MENU_ITEMS (max7456GetRowsCount() - 2) +#define MAX_MENU_ITEMS (cmsGetRowsCount() - 2) -uint8_t osdRows; +// +// Menu element types +// XXX Upon separation, all OME would be renamed to CME_ or similar. +// + +typedef void (* OSDMenuFuncPtr)(void *data); //type of elements typedef enum @@ -136,30 +313,13 @@ uint8_t featureLedstrip = 0; 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; + bool changed; } OSD_Entry; typedef struct @@ -218,13 +378,16 @@ OSD_Entry *currentMenu; OSD_Entry *nextPage = NULL; int8_t currentMenuPos = 0; +int8_t lastMenuPos; uint8_t currentMenuIdx = 0; uint16_t *currentElement = NULL; +#ifdef OSD OSD_Entry menuAlarms[]; OSD_Entry menuOsdLayout[]; OSD_Entry menuOsdActiveElems[]; OSD_Entry menuOsdElemsPositions[]; +#endif OSD_Entry menuFeatures[]; OSD_Entry menuBlackbox[]; #ifdef LED_STRIP @@ -241,42 +404,44 @@ 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} + {"----MAIN MENU----", OME_Label, NULL, NULL, true}, +#ifdef OSD + {"SCREEN LAYOUT", OME_Submenu, cmsChangeScreen, &menuOsdLayout[0], true}, + {"ALARMS", OME_Submenu, cmsChangeScreen, &menuAlarms[0], true}, +#endif + {"CFG. IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], true}, + {"FEATURES", OME_Submenu, cmsChangeScreen, &menuFeatures[0], true}, + {"SAVE & EXIT", OME_OSD_Exit, cmsExitMenu, (void*)1, true}, + {"EXIT", OME_OSD_Exit, cmsExitMenu, (void*)0, true}, + {NULL,OME_END, NULL, NULL, true} }; OSD_Entry menuFeatures[] = { - {"----- FEATURES -----", OME_Label, NULL, NULL}, - {"BLACKBOX", OME_Submenu, osdChangeScreen, &menuBlackbox[0]}, + {"----- FEATURES -----", OME_Label, NULL, NULL, true}, + {"BLACKBOX", OME_Submenu, cmsChangeScreen, &menuBlackbox[0], true}, #ifdef LED_STRIP - {"LED STRIP", OME_Submenu, osdChangeScreen, &menuLedstrip[0]}, + {"LED STRIP", OME_Submenu, cmsChangeScreen, &menuLedstrip[0], true}, #endif // LED_STRIP #if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, osdChangeScreen, &menu_vtx[0]}, + {"VTX", OME_Submenu, cmsChangeScreen, &menu_vtx[0], true}, #endif // VTX || USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; 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}, + {"--- BLACKBOX ---", OME_Label, NULL, NULL, true}, + {"ENABLED", OME_Bool, NULL, &featureBlackbox, true}, + {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, true}, #ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, osdEraseFlash, NULL}, + {"ERASE FLASH", OME_Submenu, cmsEraseFlash, NULL, true}, #endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; #ifdef LED_STRIP @@ -317,7 +482,7 @@ void getLedColor(void) } //udate all leds with flag color -void applyLedColor(void * ptr) +static void applyLedColor(void * ptr) { UNUSED(ptr); for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { @@ -331,11 +496,11 @@ 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} + {"--- LED TRIP ---", OME_Label, NULL, NULL, true}, + {"ENABLED", OME_Bool, NULL, &featureLedstrip, true}, + {"LED COLOR", OME_TAB, applyLedColor, &entryLed, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; #endif // LED_STRIP @@ -358,19 +523,19 @@ OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; OSD_Entry menu_vtx[] = { - {"--- VTX ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureVtx}, + {"--- VTX ---", OME_Label, NULL, NULL, true}, + {"ENABLED", OME_Bool, NULL, &featureVtx, true}, #ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz}, + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, true}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, true}, #endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel}, + {"BAND", OME_TAB, NULL, &entryVtxBand, true}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, true}, #ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power}, + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, true}, #endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; #endif // VTX || USE_RTC6705 @@ -384,30 +549,30 @@ OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 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} + {"----- MISC -----", OME_Label, NULL, NULL, true}, + {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz, true}, + {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, true}, + {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, true}, + {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit, true}, + {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle, true}, + {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, true}, + {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; 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} + {"-----CFG. IMU-----", OME_Label, NULL, NULL, true}, + {"PID", OME_Submenu, cmsChangeScreen, &menuPid[0], true}, + {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile, true}, + {"RATE & RXPO", OME_Submenu, cmsChangeScreen, &menuRateExpo[0], true}, + {"RC PREVIEW", OME_Submenu, cmsChangeScreen, &menuRc[0], true}, + {"MISC", OME_Submenu, cmsChangeScreen, &menuMisc[0], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; uint8_t tempPid[4][3]; @@ -426,21 +591,21 @@ 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}, + {"------- PID -------", OME_Label, NULL, NULL, true}, + {"ROLL P", OME_UINT8, NULL, &entryRollP, true}, + {"ROLL I", OME_UINT8, NULL, &entryRollI, true}, + {"ROLL D", OME_UINT8, NULL, &entryRollD, true}, - {"PITCH P", OME_UINT8, NULL, &entryPitchP}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD}, + {"PITCH P", OME_UINT8, NULL, &entryPitchP, true}, + {"PITCH I", OME_UINT8, NULL, &entryPitchI, true}, + {"PITCH D", OME_UINT8, NULL, &entryPitchD, true}, - {"YAW P", OME_UINT8, NULL, &entryYawP}, - {"YAW I", OME_UINT8, NULL, &entryYawI}, - {"YAW D", OME_UINT8, NULL, &entryYawD}, + {"YAW P", OME_UINT8, NULL, &entryYawP, true}, + {"YAW I", OME_UINT8, NULL, &entryYawI, true}, + {"YAW D", OME_UINT8, NULL, &entryYawD, true}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; controlRateConfig_t rateProfile; @@ -458,19 +623,19 @@ static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSe 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} + {"----RATE & EXPO----", OME_Label, NULL, NULL, true}, + {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate, true}, + {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate, true}, + {"YAW RATE", OME_FLOAT, NULL, &entryYawRate, true}, + {"RC RATE", OME_FLOAT, NULL, &entryRcRate, true}, + {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, true}, + {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, true}, + {"THR. PID ATT.", OME_FLOAT, NULL, &extryTpaEntry, true}, + {"TPA BREAKPOINT", OME_UINT16, NULL, &entryTpaBreak, true}, + {"D SETPOINT", OME_FLOAT, NULL, &entryDSetpoint, true}, + {"D SETPOINT TRANSITION", OME_FLOAT, NULL, &entryPSetpoint, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; @@ -484,26 +649,30 @@ 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} + {"---- RC PREVIEW ----", OME_Label, NULL, NULL, true}, + {"ROLL", OME_INT16, NULL, &entryRcRoll, true}, + {"PITCH", OME_INT16, NULL, &entryRcPitch, true}, + {"THROTTLE", OME_INT16, NULL, &entryRcThrottle, true}, + {"YAW", OME_INT16, NULL, &entryRcYaw, true}, + {"AUX1", OME_INT16, NULL, &entryRcAux1, true}, + {"AUX2", OME_INT16, NULL, &entryRcAux2, true}, + {"AUX3", OME_INT16, NULL, &entryRcAux3, true}, + {"AUX4", OME_INT16, NULL, &entryRcAux4, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; +#ifdef OSD +// +// OSD specific menu pages and items for +// 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} + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, true}, + {"ACTIVE ELEM.", OME_Submenu, cmsChangeScreen, &menuOsdActiveElems[0], true}, + {"POSITION", OME_Submenu, cmsChangeScreen, &menuOsdElemsPositions[0], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; OSD_UINT8_t entryAlarmRssi = {&OSD_cfg.rssi_alarm, 5, 90, 5}; @@ -513,193 +682,88 @@ 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} + {"------ ALARMS ------", OME_Label, NULL, NULL, true}, + {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, true}, + {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity, true}, + {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, true}, + {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; 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]}, + {"---POSITION---", OME_Label, NULL, NULL, true}, + {"RSSI", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], true}, + {"MAIN BATTERY", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, + {"UPTIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ONTIME], true}, + {"FLY TIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], true}, + {"FLY MODE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], true}, + {"NAME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], true}, + {"THROTTLE POS", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], true}, #ifdef VTX - {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, + {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL], true}, #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]}, + {"CURRENT (A)", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], true}, + {"USED MAH", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], true}, #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]}, + {"GPS SPEED", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], true}, + {"GPS SATS.", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], true}, #endif // GPS - {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; 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]}, + {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL, true}, + {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], true}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, + {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], true}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], true}, + {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], true}, + {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], true}, + {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], true}, + {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], true}, + {"THROTTLE POS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], true}, #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]}, + {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], true}, + {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], true}, #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]}, + {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], true}, + {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], true}, #endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} }; +#endif -void resetOsdConfig(osd_profile_t *osdProfile) +uint8_t cmsRows; +bool cmsInMenu = false; + +// +// CMS specific functions +// + +void cmsUpdateMaxRows(void) { - 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); + OSD_Entry *ptr; - osdProfile->rssi_alarm = 20; - osdProfile->cap_alarm = 2200; - osdProfile->time_alarm = 10; // in minutes - osdProfile->alt_alarm = 100; // meters or feet depend on configuration + currentMenuIdx = 0; + for (ptr = currentMenu; ptr->type != OME_END; ptr++) + currentMenuIdx++; - osdProfile->video_system = 0; + if (currentMenuIdx > MAX_MENU_ITEMS) + currentMenuIdx = MAX_MENU_ITEMS; + + currentMenuIdx--; } -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; -} - -/** - * Gets the correct altitude symbol for the current unit system - */ -char osdGetAltitudeSymbol() -{ - switch (masterConfig.osdProfile.units) { - case OSD_UNIT_IMPERIAL: - return 0xF; - default: - return 0xC; - } -} - -/** - * Converts altitude based on the current unit system. - * @param alt Raw altitude (i.e. as taken from BaroAlt) - */ -int32_t osdGetAltitude(int32_t alt) -{ - switch (masterConfig.osdProfile.units) { - case OSD_UNIT_IMPERIAL: - return (alt * 328) / 100; // Convert to feet / 100 - default: - return alt; // Already in metre / 100 - } -} - -void osdUpdateAlarms(void) -{ - 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 cmsHandleKey(uint8_t key) { uint8_t res = BUTTON_TIME; OSD_Entry *p; @@ -708,24 +772,26 @@ uint8_t osdHandleKey(uint8_t key) return res; if (key == KEY_ESC) { - osdMenuBack(); + cmsMenuBack(); return BUTTON_PAUSE; } if (key == KEY_DOWN) { - if (currentMenuPos < currentMenuIdx) + if (currentMenuPos < currentMenuIdx) { currentMenuPos++; - else { + } else { if (nextPage) // we have more pages { - max7456ClearScreen(); + cmsScreenClear(); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; currentMenuPos = 0; - osdUpdateMaxRows(); + lastMenuPos = -1; + cmsUpdateMaxRows(); + } else { + currentMenuPos = 0; } - currentMenuPos = 0; } } @@ -737,14 +803,17 @@ uint8_t osdHandleKey(uint8_t key) if (currentMenuPos == -1 || (currentMenu + currentMenuPos)->type == OME_Label) { if (nextPage) { - max7456ClearScreen(); + cmsScreenClear(); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; currentMenuPos = 0; - osdUpdateMaxRows(); + lastMenuPos = -1; + cmsUpdateMaxRows(); + } else { + currentMenuPos = currentMenuIdx; + // lastMenuPos = -1; } - currentMenuPos = currentMenuIdx; } } @@ -755,6 +824,7 @@ uint8_t osdHandleKey(uint8_t key) switch (p->type) { case OME_POS: +#ifdef OSD if (key == KEY_RIGHT) { uint32_t address = (uint32_t)p->data; uint16_t *val; @@ -763,6 +833,7 @@ uint8_t osdHandleKey(uint8_t key) if (!(*val & VISIBLE_FLAG)) // no submenu for hidden elements break; } +#endif case OME_Submenu: case OME_OSD_Exit: if (p->func && key == KEY_RIGHT) { @@ -771,7 +842,7 @@ uint8_t osdHandleKey(uint8_t key) } break; case OME_Back: - osdMenuBack(); + cmsMenuBack(); res = BUTTON_PAUSE; break; case OME_Bool: @@ -781,9 +852,11 @@ uint8_t osdHandleKey(uint8_t key) *val = 1; else *val = 0; + p->changed = true; } break; case OME_VISIBLE: +#ifdef OSD if (p->data) { uint32_t address = (uint32_t)p->data; uint16_t *val; @@ -794,7 +867,9 @@ uint8_t osdHandleKey(uint8_t key) *val |= VISIBLE_FLAG; else *val %= ~VISIBLE_FLAG; + p->changed = true; } +#endif break; case OME_UINT8: case OME_FLOAT: @@ -808,6 +883,7 @@ uint8_t osdHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } + p->changed = true; } break; case OME_TAB: @@ -824,6 +900,7 @@ uint8_t osdHandleKey(uint8_t key) } if (p->func) p->func(p->data); + p->changed = true; } break; case OME_INT8: @@ -837,6 +914,7 @@ uint8_t osdHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } + p->changed = true; } break; case OME_UINT16: @@ -850,6 +928,7 @@ uint8_t osdHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } + p->changed = true; } break; case OME_INT16: @@ -863,6 +942,7 @@ uint8_t osdHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } + p->changed = true; } break; case OME_Label: @@ -872,56 +952,7 @@ uint8_t osdHandleKey(uint8_t key) 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) +static void simple_ftoa(int32_t value, char *floatString) { uint8_t k; // np. 3450 @@ -947,101 +978,125 @@ void simple_ftoa(int32_t value, char *floatString) floatString[0] = ' '; } -void osdDrawMenu(void) +void cmsDrawMenu(void) { uint8_t i = 0; OSD_Entry *p; char buff[10]; - uint8_t top = (osdRows - currentMenuIdx) / 2 - 1; + uint8_t top = (cmsRows - currentMenuIdx) / 2 - 1; + if (!currentMenu) return; if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label currentMenuPos++; + if (lastMenuPos >= 0 && currentMenuPos != lastMenuPos) + cmsScreenWrite(LEFT_MENU_COLUMN, lastMenuPos + top, " "); + 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); + + if (currentMenuPos == i && lastMenuPos != currentMenuPos) { + cmsScreenWrite(LEFT_MENU_COLUMN, i + top, " >"); + lastMenuPos = currentMenuPos; + } + + if (cmsScreenCleared) + cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text); switch (p->type) { - case OME_POS: { + case OME_POS: +#ifdef OSD uint32_t address = (uint32_t)p->data; uint16_t *val; val = (uint16_t *)address; if (!(*val & VISIBLE_FLAG)) break; - } +#endif + case OME_Submenu: - max7456Write(RIGHT_MENU_COLUMN, i + top, ">"); + if (cmsScreenCleared) + cmsScreenWrite(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 "); + if ((p->changed || cmsScreenCleared) && p->data) { + if (*((uint8_t *)(p->data))) { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES"); + } else { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO "); + } + p->changed = false; } break; case OME_TAB: { - OSD_TAB_t *ptr = p->data; - max7456Write(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); + if (p->changed || cmsScreenCleared) { + OSD_TAB_t *ptr = p->data; + cmsScreenWrite(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); + p->changed = false; + } break; } case OME_VISIBLE: - if (p->data) { +#ifdef OSD + if ((p->changed || cmsScreenCleared) && 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 "); + if (VISIBLE(*val)) { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES"); + } else { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO "); + } + p->changed = false; } +#endif break; case OME_UINT8: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; } break; case OME_INT8: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; } break; case OME_UINT16: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; } break; case OME_INT16: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; } break; case OME_FLOAT: - if (p->data) { + if ((p->changed || cmsScreenCleared) && 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); + cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, buff); + p->changed = false; } break; case OME_OSD_Exit: @@ -1050,6 +1105,7 @@ void osdDrawMenu(void) case OME_Back: break; } + i++; if (i == MAX_MENU_ITEMS) // max per page @@ -1060,233 +1116,14 @@ void osdDrawMenu(void) break; } } + cmsScreenCleared = false; } -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) +void cmsChangeScreen(void *ptr) { uint8_t i; if (ptr) { - max7456ClearScreen(); + cmsScreenClear(); // hack - save profile to temp if (ptr == &menuPid[0]) { for (i = 0; i < 3; i++) { @@ -1308,48 +1145,96 @@ void osdChangeScreen(void *ptr) currentMenu = (OSD_Entry *)ptr; currentMenuPos = 0; - osdUpdateMaxRows(); + lastMenuPos = -1; // XXX this? + cmsUpdateMaxRows(); } } -#ifdef USE_FLASHFS -void osdEraseFlash(void *ptr) +void cmsMenuBack(void) { - UNUSED(ptr); + uint8_t i; - max7456ClearScreen(); - max7456Write(5, 3, "ERASING FLASH..."); - max7456RefreshAll(); + // 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]; + } - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); + curr_profile.pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + curr_profile.pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + curr_profile.pidProfile.D8[PIDLEVEL] = tempPid[3][2]; } - max7456ClearScreen(); - max7456RefreshAll(); -} -#endif // USE_FLASHFS -void osdEditElement(void *ptr) -{ - uint32_t address = (uint32_t)ptr; + // 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)); - // zsave position on menu stack - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; + if (menuStackIdx) { + cmsScreenClear(); + menuStackIdx--; + nextPage = NULL; + currentMenu = menuStack[menuStackIdx]; + currentMenuPos = menuStackHistory[menuStackIdx]; + lastMenuPos = -1; - currentElement = (uint16_t *)address; - - *currentElement |= BLINK_FLAG; - max7456ClearScreen(); + cmsUpdateMaxRows(); + } + else { + cmsOpenMenu(); + } } -void osdExitMenu(void *ptr) +void cmsOpenMenu(void) { - max7456ClearScreen(); - max7456Write(5, 3, "RESTARTING IMU..."); - max7456RefreshAll(); + if (cmsInMenu) + 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 + + cmsRows = cmsGetRowsCount(); + cmsInMenu = true; +#ifdef OSD +// XXX Do we need this here? + refreshTimeout = 0; +#endif + cmsScreenBegin(); + cmsScreenClear(); + currentMenu = &menuMain[0]; + cmsChangeScreen(currentMenu); +#ifdef LED_STRIP + getLedColor(); +#endif // LED_STRIP +} + +void cmsExitMenu(void *ptr) +{ + cmsScreenClear(); + + cmsScreenWrite(5, 3, "RESTARTING IMU..."); + cmsScreenResync(); // Was max7456RefreshAll(); why at this timing? + stopMotors(); stopPwmAllMotors(); delay(200); @@ -1384,47 +1269,113 @@ void osdExitMenu(void *ptr) saveConfigAndNotify(); } + cmsInMenu = false; + + cmsScreenEnd(); + systemReset(); } -void osdOpenMenu(void) +void cmsProcess(uint32_t currentTime) { - if (inMenu) - return; + static uint8_t rcDelay = BUTTON_TIME; + uint8_t key = 0; + static uint32_t lastCmsHeartBeat = 0; - if (feature(FEATURE_LED_STRIP)) - featureLedstrip = 1; +//debug[1]++; - if (feature(FEATURE_BLACKBOX)) - featureBlackbox = 1; + // detect enter to menu + if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsOpenMenu(); + } -#if defined(VTX) || defined(USE_RTC6705) - if (feature(FEATURE_VTX)) - featureVtx = 1; -#endif // VTX || USE_RTC6705 + if (cmsInMenu) { + 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; + } -#ifdef VTX - vtxBand = masterConfig.vtxBand; - vtxChannel = masterConfig.vtx_channel + 1; -#endif // VTX + // XXX Element position adjustment is hard to separate. + // XXX May need to drop it upon real separation. + // XXX Don't know if this still works + if (key && !currentElement) { + rcDelay = cmsHandleKey(key); + return; + } -#ifdef USE_RTC6705 - vtxBand = masterConfig.vtx_channel / 8; - vtxChannel = masterConfig.vtx_channel % 8 + 1; -#endif // USE_RTC6705 + cmsDrawMenu(); - osdRows = max7456GetRowsCount(); - inMenu = true; - refreshTimeout = 0; - max7456ClearScreen(); - currentMenu = &menuMain[0]; - osdResetAlarms(); - osdChangeScreen(currentMenu); -#ifdef LED_STRIP - getLedColor(); -#endif // LED_STRIP + if (currentTime > lastCmsHeartBeat + 500) { + // Heart beat for external CMS display device @ 500msec + // (Timeout @ 1000msec) + cmsScreenHeartBeat(); + lastCmsHeartBeat = currentTime; + } + } } +void cmsHandler(uint32_t currentTime) +{ + static uint32_t counter = 0; + + if (counter++ % 5 == 0) { + cmsProcess(currentTime); + } + + // do not allow ARM if we are in menu + if (cmsInMenu) + DISABLE_ARMING_FLAG(OK_TO_ARM); + +} + +// Does this belong here? + +#ifdef USE_FLASHFS +void cmsEraseFlash(void *ptr) +{ + UNUSED(ptr); + + cmsScreenClear(); + cmsScreenWrite(5, 3, "ERASING FLASH..."); + cmsScreenResync(); // Was max7456RefreshAll(); Why at this timing? + + flashfsEraseCompletely(); + while (!flashfsIsReady()) { + delay(100); + } + + cmsScreenClear(); + cmsScreenResync(); // Was max7456RefreshAll(); wedges during heavy SPI? +} +#endif // USE_FLASHFS + +#ifdef OSD +bool osdInMenu = false; + +// +// OSD specific CMS function +// + void osdDrawElementPositioningHelp(void) { max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); @@ -1434,13 +1385,26 @@ void osdDrawElementPositioningHelp(void) max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); } +// +// OSD spcific functions +// + +void osdMenuBegin(void) { + osdResetAlarms(); + osdInMenu = true; +} + +void osdMenuEnd(void) { + osdInMenu = false; +} + void osdDrawElements(void) { max7456ClearScreen(); if (currentElement) osdDrawElementPositioningHelp(); - else if (sensors(SENSOR_ACC) || inMenu) + else if (sensors(SENSOR_ACC) || osdInMenu) { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); osdDrawSingleElement(OSD_CROSSHAIRS); @@ -1459,7 +1423,7 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_ALTITUDE); #ifdef GPS - if (sensors(SENSOR_GPS) || inMenu) { + if (sensors(SENSOR_GPS) || osdInMenu) { osdDrawSingleElement(OSD_GPS_SATS); osdDrawSingleElement(OSD_GPS_SPEED); } @@ -1680,4 +1644,298 @@ void osdDrawSingleElement(uint8_t item) max7456Write(elemPosX, elemPosY, buff); } +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"); + cmsScreenResync(); // Was max7456RefreshAll(); may be okay. + + refreshTimeout = 4 * REFRESH_1S; +} + +/** + * Gets the correct altitude symbol for the current unit system + */ +char osdGetAltitudeSymbol() +{ + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return 0xF; + default: + return 0xC; + } +} + +/** + * Converts altitude based on the current unit system. + * @param alt Raw altitude (i.e. as taken from BaroAlt) + */ +int32_t osdGetAltitude(int32_t alt) +{ + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return (alt * 328) / 100; // Convert to feet / 100 + default: + return alt; // Already in metre / 100 + } +} + +void osdUpdateAlarms(void) +{ + 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; +} + +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 = 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) + 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 (osdInMenu) + DISABLE_ARMING_FLAG(OK_TO_ARM); +} + +void osdUpdate(uint32_t currentTime) +{ + static uint8_t lastSec = 0; + uint8_t sec; + + // detect enter to menu + if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsOpenMenu(); + } + + // 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 (!osdInMenu) { + osdUpdateAlarms(); + osdDrawElements(); + } else { + cmsProcess(currentTime); + } +} + +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(); +} + #endif // OSD diff --git a/src/main/main.c b/src/main/main.c index 3ff79d02ad..6bf9b1e7df 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -127,6 +127,14 @@ extern uint8_t motorControlEnable; serialPort_t *loopbackPort; #endif +#ifdef USE_DPRINTF +#include "common/printf.h" +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 +serialPort_t *debugSerialPort = NULL; +#define dprintf(x) if (debugSerialPort) printf x +#else +#define dprintf(x) +#endif typedef enum { SYSTEM_STATE_INITIALISING = 0, @@ -245,6 +253,16 @@ void init(void) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif +#ifdef USE_DPRINTF + // Setup debugSerialPort + + debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0); + if (debugSerialPort) { + setPrintfSerialPort(debugSerialPort); + dprintf(("debugSerialPort: OK\r\n")); + } +#endif + mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #ifdef USE_SERVOS servoMixerInit(masterConfig.customServoMixer); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 8d14b4d698..a0fee2efb7 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 canvas mode messages +#define MSP_CANVAS 182 + // // Multwii original MSP commands // diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index a26a0f99db..3bb28db637 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -128,6 +128,34 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort) return mspPostProcessFn; } +#ifdef USE_DPRINTF +#include "common/printf.h" +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 +extern serialPort_t *debugSerialPort; +#define dprintf(x) if (debugSerialPort) printf x +#else +#define dprintf(x) +#endif + +void mspSerialPush(int cmd, uint8_t *data, int buflen) +{ + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // Big enough for a OSD line + uint8_t buf[sizeof(bufWriter_t) + 30]; + + writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port); + + mspServerPush(mspPort, cmd, data, buflen); + + bufWriterFlush(writer); + } +} + /* * Process MSP commands from serial ports configured as MSP ports. * diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 8382a0d656..b6b3b319f4 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -60,3 +60,4 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); +void mspSerialPush(int, uint8_t *, int); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index f44cd497f9..5b5eccaca1 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 4b392c32fd..92d25c77d1 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -1,3 +1,4 @@ +#define USE_DPRINTF /* * This file is part of Cleanflight. * @@ -88,12 +89,19 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +// Configuratoin Menu System +#define CMS + +// Use external OSD to run CMS +#define CANVAS + // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk // include the osd code -#define OSD +//#define OSD + // include the max7456 driver -#define USE_MAX7456 +//#define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 #define MAX7456_SPI_CS_PIN PB1 #define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index d7c7d42f44..3b888267c0 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -130,3 +130,8 @@ #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) ) +// Configuratoin Menu System +#define CMS + +// Use external OSD to run CMS +#define CANVAS diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index 5b3a330295..22dcede4b5 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -8,5 +8,6 @@ TARGET_SRC = \ drivers/barometer_bmp085.c \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c + drivers/compass_hmc5883l.c \ + io/osd.c From 0249443862a9d003239e15c965a91136472ea9d6 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 20 Oct 2016 23:21:32 +0900 Subject: [PATCH 03/19] Rebased onto super-abstracted msp --- src/main/fc/fc_msp.c | 34 +++++++++++-------------- src/main/fc/fc_msp.h | 1 + src/main/io/cms.h | 1 + src/main/io/osd.c | 21 +++++++++++++++- src/main/main.c | 5 ++++ src/main/msp/msp.h | 1 + src/main/msp/msp_serial.c | 53 ++++++++++++++++++--------------------- src/main/msp/msp_serial.h | 4 ++- 8 files changed, 71 insertions(+), 49 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2d8a8193b0..c69738c7fa 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -102,15 +102,6 @@ #include "config/config_master.h" #include "config/feature.h" -#ifdef USE_DPRINTF -#include "common/printf.h" -#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 -extern serialPort_t *debugSerialPort; -#define dprintf(x) if (debugSerialPort) printf x -#else -#define dprintf(x) -#endif - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -1946,16 +1937,7 @@ mspResult_e mspFcProcessCommand(mspPort_t *mspPort, mspPostProcessFnPtr *mspPost return ret; } -/* - * Return a pointer to the process command function - */ -mspProcessCommandFnPtr mspFcInit(void) -{ - initActiveBoxIds(); - return mspFcProcessCommand; -} - -void mspServerPush(mspPort_t *mspPort, int cmd, uint8_t *data, int len) +void mspServerPush(mspPort_t *mspPort, uint8_t cmd, uint8_t *data, int len) { currentPort = mspPort; mspPort->cmdMSP = cmd; @@ -1968,3 +1950,17 @@ void mspServerPush(mspPort_t *mspPort, int cmd, uint8_t *data, int len) tailSerialReply(); } + +/* + * Return a pointer to the process command function + */ +mspProcessCommandFnPtr mspFcInit(void) +{ + initActiveBoxIds(); + return mspFcProcessCommand; +} + +mspPushCommandFnPtr mspFcPushInit(void) +{ + return mspServerPush; +} diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index 745fd551a9..be575e72e4 100644 --- a/src/main/fc/fc_msp.h +++ b/src/main/fc/fc_msp.h @@ -20,3 +20,4 @@ #include "msp/msp.h" mspProcessCommandFnPtr mspFcInit(void); +mspPushCommandFnPtr mspFcPushInit(void); diff --git a/src/main/io/cms.h b/src/main/io/cms.h index e6d9887938..61b059b1a3 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -1 +1,2 @@ +void cmsInit(void); void cmsHandler(uint32_t); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 94b4d68f4b..71d5dc8a67 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -98,8 +98,9 @@ void osdResetAlarms(void); // specific functions; max7456XXX(), canvasXXX(), oledXXX(), ... // -#include "io/serial_msp.h" +#include "fc/fc_msp.h" #include "msp/msp_protocol.h" +#include "msp/msp_serial.h" void canvasBegin(void) { @@ -146,6 +147,12 @@ void canvasWrite(int col, int row, char *string) mspSerialPush(MSP_CANVAS, (uint8_t *)buf, len + 4); } + +// Called once at startup to initialize push function in msp +void canvasInit(void) +{ + mspSerialPushInit(mspFcPushInit()); +} #endif // Force draw all elements if true @@ -226,6 +233,13 @@ void cmsScreenResync(void) #endif } +void cmsScreenInit(void) +{ +#ifdef CANVAS + canvasInit(); +#endif +} + // // Lots of things not separated yet. // @@ -1348,6 +1362,11 @@ void cmsHandler(uint32_t currentTime) } +void cmsInit(void) +{ + cmsScreenInit(); +} + // Does this belong here? #ifdef USE_FLASHFS diff --git a/src/main/main.c b/src/main/main.c index 6bf9b1e7df..4cf62bf83f 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -88,6 +88,7 @@ #include "io/transponder_ir.h" #include "io/osd.h" #include "io/vtx.h" +#include "io/cms.h" #include "scheduler/scheduler.h" @@ -424,6 +425,10 @@ void init(void) } #endif +#ifdef CMS + cmsInit(); +#endif + if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index 3f3216df29..268c621433 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -29,3 +29,4 @@ struct serialPort_s; typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc. struct mspPort_s; typedef mspResult_e (*mspProcessCommandFnPtr)(struct mspPort_s *mspPort, mspPostProcessFnPtr *mspPostProcessFn); +typedef void (*mspPushCommandFnPtr)(struct mspPort_s *, uint8_t, uint8_t *, int); diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 3bb28db637..29d23ce8be 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -35,6 +35,7 @@ static mspProcessCommandFnPtr mspProcessCommandFn; +static mspPushCommandFnPtr mspPushCommandFn; static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; bufWriter_t *writer; @@ -128,34 +129,6 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort) return mspPostProcessFn; } -#ifdef USE_DPRINTF -#include "common/printf.h" -#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 -extern serialPort_t *debugSerialPort; -#define dprintf(x) if (debugSerialPort) printf x -#else -#define dprintf(x) -#endif - -void mspSerialPush(int cmd, uint8_t *data, int buflen) -{ - for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { - mspPort_t * const mspPort = &mspPorts[portIndex]; - if (!mspPort->port) { - continue; - } - - // Big enough for a OSD line - uint8_t buf[sizeof(bufWriter_t) + 30]; - - writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port); - - mspServerPush(mspPort, cmd, data, buflen); - - bufWriterFlush(writer); - } -} - /* * Process MSP commands from serial ports configured as MSP ports. * @@ -204,3 +177,27 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse) memset(mspPorts, 0, sizeof(mspPorts)); mspSerialAllocatePorts(); } + +void mspSerialPush(uint8_t cmd, uint8_t *data, int buflen) +{ + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // Big enough for a OSD line + uint8_t buf[sizeof(bufWriter_t) + 30]; + + writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port); + + mspPushCommandFn(mspPort, cmd, data, buflen); + + bufWriterFlush(writer); + } +} + +void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFnToUse) +{ + mspPushCommandFn = mspPushCommandFnToUse; +} diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index b6b3b319f4..19ab30dead 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -60,4 +60,6 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); -void mspSerialPush(int, uint8_t *, int); + +void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFn); +void mspSerialPush(uint8_t, uint8_t *, int); From 7f2ebbd2fa9406a0e6422dc1da660a49f3e2a22b Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 20 Oct 2016 23:49:24 +0900 Subject: [PATCH 04/19] Touch ups --- src/main/fc/fc_msp.c | 32 ++----- src/main/io/cms.h | 3 - src/main/io/osd.c | 188 -------------------------------------- src/main/msp/msp.h | 9 -- src/main/msp/msp_serial.c | 28 ------ src/main/msp/msp_serial.h | 5 - 6 files changed, 9 insertions(+), 256 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bd30e76bf0..01750caefa 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1946,6 +1946,15 @@ mspResult_e mspFcProcessCommand(mspPort_t *mspPort, mspPostProcessFnPtr *mspPost return ret; } +/* + * Return a pointer to the process command function + */ +mspProcessCommandFnPtr mspFcInit(void) +{ + initActiveBoxIds(); + return mspFcProcessCommand; +} + void mspServerPush(mspPort_t *mspPort, uint8_t cmd, uint8_t *data, int len) { currentPort = mspPort; @@ -1960,30 +1969,7 @@ void mspServerPush(mspPort_t *mspPort, uint8_t cmd, uint8_t *data, int len) tailSerialReply(); } -/* - * Return a pointer to the process command function - */ -mspProcessCommandFnPtr mspFcInit(void) -{ - initActiveBoxIds(); - return mspFcProcessCommand; -} - mspPushCommandFnPtr mspFcPushInit(void) { return mspServerPush; } - -void mspServerPush(mspPort_t *mspPort, int cmd, uint8_t *data, int len) -{ - currentPort = mspPort; - mspPort->cmdMSP = cmd; - - headSerialReply(len); - - while (len--) { - serialize8(*data++); - } - - tailSerialReply(); -} diff --git a/src/main/io/cms.h b/src/main/io/cms.h index 8607551fb2..61b059b1a3 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -1,5 +1,2 @@ -<<<<<<< HEAD void cmsInit(void); -======= ->>>>>>> origin/bfdev-osd-cms-separation-poc void cmsHandler(uint32_t); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 05f75eebd9..71d5dc8a67 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -98,14 +98,9 @@ void osdResetAlarms(void); // specific functions; max7456XXX(), canvasXXX(), oledXXX(), ... // -<<<<<<< HEAD #include "fc/fc_msp.h" #include "msp/msp_protocol.h" #include "msp/msp_serial.h" -======= -#include "io/serial_msp.h" -#include "msp/msp_protocol.h" ->>>>>>> origin/bfdev-osd-cms-separation-poc void canvasBegin(void) { @@ -152,15 +147,12 @@ void canvasWrite(int col, int row, char *string) mspSerialPush(MSP_CANVAS, (uint8_t *)buf, len + 4); } -<<<<<<< HEAD // Called once at startup to initialize push function in msp void canvasInit(void) { mspSerialPushInit(mspFcPushInit()); } -======= ->>>>>>> origin/bfdev-osd-cms-separation-poc #endif // Force draw all elements if true @@ -241,7 +233,6 @@ void cmsScreenResync(void) #endif } -<<<<<<< HEAD void cmsScreenInit(void) { #ifdef CANVAS @@ -249,8 +240,6 @@ void cmsScreenInit(void) #endif } -======= ->>>>>>> origin/bfdev-osd-cms-separation-poc // // Lots of things not separated yet. // @@ -301,21 +290,12 @@ statistic_t stats; #define LEFT_MENU_COLUMN 1 #define RIGHT_MENU_COLUMN 23 #define MAX_MENU_ITEMS (cmsGetRowsCount() - 2) -<<<<<<< HEAD // // Menu element types // XXX Upon separation, all OME would be renamed to CME_ or similar. // -======= - -// -// Menu element types -// XXX Upon separation, all OME would be renamed to CME_ or similar. -// - ->>>>>>> origin/bfdev-osd-cms-separation-poc typedef void (* OSDMenuFuncPtr)(void *data); //type of elements @@ -1154,7 +1134,6 @@ void cmsDrawMenu(void) } void cmsChangeScreen(void *ptr) -<<<<<<< HEAD { uint8_t i; if (ptr) { @@ -1317,170 +1296,6 @@ void cmsProcess(uint32_t currentTime) uint8_t key = 0; static uint32_t lastCmsHeartBeat = 0; -======= -{ - uint8_t i; - if (ptr) { - cmsScreenClear(); - // 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; - lastMenuPos = -1; // XXX this? - cmsUpdateMaxRows(); - } -} - -void cmsMenuBack(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) { - cmsScreenClear(); - menuStackIdx--; - nextPage = NULL; - currentMenu = menuStack[menuStackIdx]; - currentMenuPos = menuStackHistory[menuStackIdx]; - lastMenuPos = -1; - - cmsUpdateMaxRows(); - } - else { - cmsOpenMenu(); - } -} - -void cmsOpenMenu(void) -{ - if (cmsInMenu) - 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 - - cmsRows = cmsGetRowsCount(); - cmsInMenu = true; -#ifdef OSD -// XXX Do we need this here? - refreshTimeout = 0; -#endif - cmsScreenBegin(); - cmsScreenClear(); - currentMenu = &menuMain[0]; - cmsChangeScreen(currentMenu); -#ifdef LED_STRIP - getLedColor(); -#endif // LED_STRIP -} - -void cmsExitMenu(void *ptr) -{ - cmsScreenClear(); - - cmsScreenWrite(5, 3, "RESTARTING IMU..."); - cmsScreenResync(); // Was max7456RefreshAll(); why at this timing? - - 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(); - } - - cmsInMenu = false; - - cmsScreenEnd(); - - systemReset(); -} - -void cmsProcess(uint32_t currentTime) -{ - static uint8_t rcDelay = BUTTON_TIME; - uint8_t key = 0; - static uint32_t lastCmsHeartBeat = 0; - ->>>>>>> origin/bfdev-osd-cms-separation-poc //debug[1]++; // detect enter to menu @@ -1547,14 +1362,11 @@ void cmsHandler(uint32_t currentTime) } -<<<<<<< HEAD void cmsInit(void) { cmsScreenInit(); } -======= ->>>>>>> origin/bfdev-osd-cms-separation-poc // Does this belong here? #ifdef USE_FLASHFS diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index cc4497a68b..1f8aae7487 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -24,17 +24,8 @@ typedef enum { MSP_RESULT_NO_REPLY = 0 } mspResult_e; - -<<<<<<< HEAD struct serialPort_s; typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc. struct mspPort_s; typedef mspResult_e (*mspProcessCommandFnPtr)(struct mspPort_s *mspPort, mspPostProcessFnPtr *mspPostProcessFn); typedef void (*mspPushCommandFnPtr)(struct mspPort_s *, uint8_t, uint8_t *, int); -======= -void mspInit(void); -bool mspProcessReceivedData(struct mspPort_s *mspPort, uint8_t c); -mspPostProcessFuncPtr mspProcessReceivedCommand(struct mspPort_s *mspPort); - -void mspServerPush(mspPort_t *, int, uint8_t *, int); ->>>>>>> origin/bfdev-osd-cms-separation-poc diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 71736cd9a3..29d23ce8be 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -129,34 +129,6 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort) return mspPostProcessFn; } -#ifdef USE_DPRINTF -#include "common/printf.h" -#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 -extern serialPort_t *debugSerialPort; -#define dprintf(x) if (debugSerialPort) printf x -#else -#define dprintf(x) -#endif - -void mspSerialPush(int cmd, uint8_t *data, int buflen) -{ - for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { - mspPort_t * const mspPort = &mspPorts[portIndex]; - if (!mspPort->port) { - continue; - } - - // Big enough for a OSD line - uint8_t buf[sizeof(bufWriter_t) + 30]; - - writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port); - - mspServerPush(mspPort, cmd, data, buflen); - - bufWriterFlush(writer); - } -} - /* * Process MSP commands from serial ports configured as MSP ports. * diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index aa0ce22931..ceb6b2cf89 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -60,10 +60,5 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); -<<<<<<< HEAD:src/main/msp/msp_serial.h - void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFn); void mspSerialPush(uint8_t, uint8_t *, int); -======= -void mspSerialPush(int, uint8_t *, int); ->>>>>>> origin/bfdev-osd-cms-separation-poc:src/main/io/serial_msp.h From c795135542d6b1f4f9e5468d5e6247e497f7bf20 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 21 Oct 2016 00:41:15 +0900 Subject: [PATCH 05/19] Further touch-ups --- src/main/io/osd.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 71d5dc8a67..6b9a366ba8 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -104,9 +104,9 @@ void osdResetAlarms(void); void canvasBegin(void) { - uint8_t subcmd = 0; + uint8_t subcmd[] = { 0 }; - mspSerialPush(MSP_CANVAS, &subcmd, 1); + mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); } void canvasHeartBeat(void) @@ -116,16 +116,16 @@ void canvasHeartBeat(void) void canvasEnd(void) { - uint8_t subcmd = 1; + uint8_t subcmd[] = { 1 }; - mspSerialPush(MSP_CANVAS, &subcmd, 1); + mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); } void canvasClear(void) { - uint8_t subcmd = 2; + uint8_t subcmd[] = { 2 }; - mspSerialPush(MSP_CANVAS, &subcmd, 1); + mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); } void canvasWrite(int col, int row, char *string) @@ -1019,7 +1019,7 @@ void cmsDrawMenu(void) cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text); switch (p->type) { - case OME_POS: + case OME_POS:; // Semi-colon required to add an empty statement #ifdef OSD uint32_t address = (uint32_t)p->data; uint16_t *val; From 6cf9086f5686c2dd6e42a0ce1e7ae504f26cde95 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 21 Oct 2016 11:22:29 +0900 Subject: [PATCH 06/19] CMS-OSD separation --- src/main/config/config_master.h | 1 + src/main/fc/config.c | 1 + src/main/fc/fc_msp.c | 4 +- src/main/fc/fc_tasks.c | 3 +- src/main/io/canvas.c | 86 ++ src/main/io/canvas.h | 3 + src/main/io/cms.c | 1207 ++++++++++++++++++++++ src/main/io/cms.h | 11 + src/main/io/cms_types.h | 96 ++ src/main/io/ledstrip.c | 4 +- src/main/io/osd.c | 1423 ++------------------------ src/main/io/osd.h | 4 + src/main/io/serial_cli.c | 1 + src/main/main.c | 3 +- src/main/target/OMNIBUS/target.h | 6 +- src/main/target/OMNIBUS/target.mk | 3 +- src/main/target/SPRACINGF3/target.mk | 3 +- src/main/telemetry/ltm.c | 4 +- src/main/telemetry/smartport.c | 6 +- 19 files changed, 1518 insertions(+), 1351 deletions(-) create mode 100644 src/main/io/canvas.c create mode 100644 src/main/io/canvas.h create mode 100644 src/main/io/cms.c create mode 100644 src/main/io/cms_types.h diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 64132e301e..6070482cb3 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -37,6 +37,7 @@ #include "io/motors.h" #include "io/servos.h" #include "io/gps.h" +#include "io/cms_types.h" #include "io/osd.h" #include "io/ledstrip.h" #include "io/vtx.h" diff --git a/src/main/fc/config.c b/src/main/fc/config.c index db7ff721f6..015a603394 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -63,6 +63,7 @@ #include "io/servos.h" #include "io/ledstrip.h" #include "io/gps.h" +#include "io/cms_types.h" #include "io/osd.h" #include "io/vtx.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 01750caefa..db9043f439 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -66,9 +66,9 @@ #include "io/flashfs.h" #include "io/transponder_ir.h" #include "io/asyncfatfs/asyncfatfs.h" -#include "io/osd.h" +//#include "io/osd.h" #include "io/serial_4way.h" -#include "io/vtx.h" +//#include "io/vtx.h" #include "msp/msp_protocol.h" #include "msp/msp.h" diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 468c4c9cdd..cb20e0555d 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -43,11 +43,12 @@ #include "io/display.h" #include "io/gps.h" #include "io/ledstrip.h" +#include "io/cms_types.h" +#include "io/cms.h" #include "io/osd.h" #include "io/serial.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" -#include "io/cms.h" #include "msp/msp_serial.h" diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c new file mode 100644 index 0000000000..3c9ccd3807 --- /dev/null +++ b/src/main/io/canvas.c @@ -0,0 +1,86 @@ +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CANVAS + +#include "io/cms_types.h" + +#include "fc/fc_msp.h" +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +void canvasGetSize(uint8_t *pRows, uint8_t *pCols) +{ + *pRows = 13; + *pCols = 30; +} + +void canvasBegin(void) +{ + uint8_t subcmd[] = { 0 }; + + mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); +} + +void canvasHeartBeat(void) +{ + canvasBegin(); +} + +void canvasEnd(void) +{ + uint8_t subcmd[] = { 1 }; + + mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); +} + +void canvasClear(void) +{ + uint8_t subcmd[] = { 2 }; + + mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); +} + +void canvasWrite(uint8_t col, uint8_t row, char *string) +{ + +//debug[0]++; // Let's capture excess canvas writes + + int len; + char buf[30 + 4]; + + if ((len = strlen(string)) >= 30) + len = 30; + + buf[0] = 3; + buf[1] = row; + buf[2] = col; + buf[3] = 0; + memcpy((char *)&buf[4], string, len); + + mspSerialPush(MSP_CANVAS, (uint8_t *)buf, len + 4); +} + +screenFnVTable_t canvasVTable = { + canvasGetSize, + canvasBegin, + canvasEnd, + canvasClear, + canvasWrite, + canvasHeartBeat, + NULL, +}; + +screenFnVTable_t *canvasInit(void) +{ + mspSerialPushInit(mspFcPushInit()); // Called once at startup to initialize push function in msp + + return &canvasVTable; +} +#endif diff --git a/src/main/io/canvas.h b/src/main/io/canvas.h new file mode 100644 index 0000000000..a34bc06e5a --- /dev/null +++ b/src/main/io/canvas.h @@ -0,0 +1,3 @@ +#pragma once + +screenFnVTable_t *canvasInit(void); diff --git a/src/main/io/cms.c b/src/main/io/cms.c new file mode 100644 index 0000000000..a507f22b6c --- /dev/null +++ b/src/main/io/cms.c @@ -0,0 +1,1207 @@ +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#include "drivers/system.h" + +#include "io/cms_types.h" +#include "io/canvas.h" + +#include "io/flashfs.h" +#include "io/osd.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" + +#include "build/debug.h" + +// Configuration Menu System forwards + +uint8_t cmsHandleKey(uint8_t); +void cmsUpdateMaxRows(void); +void cmsOpenMenu(void); +void cmsExitMenu(void * ptr); +void cmsChangeScreen(void * ptr); +void cmsMenuBack(void); +void cmsDrawMenu(void); +void cmsGetSize(uint8_t *, uint8_t *); +void cmsEraseFlash(void *ptr); + +screenFnVTable_t *pScreenFnVTable; + +// Force draw all elements if true +bool cmsScreenCleared; + +// Function vector may be good here. + +uint8_t cmsRows; +uint8_t cmsCols; + +void cmsGetSize(uint8_t *pRows, uint8_t *pCols) +{ + pScreenFnVTable->getsize(pRows, pCols); +} + +void cmsScreenClear(void) +{ + pScreenFnVTable->clear(); + cmsScreenCleared = true; +} + +void cmsScreenBegin(void) +{ + pScreenFnVTable->begin(); + pScreenFnVTable->clear(); +} + +void cmsScreenEnd(void) +{ + pScreenFnVTable->end(); +} + +void cmsScreenWrite(uint8_t x, uint8_t y, char *s) +{ + pScreenFnVTable->write(x, y, s); +} + +void cmsScreenHeartBeat(void) +{ + if (pScreenFnVTable->heartbeat) + pScreenFnVTable->heartbeat(); +} + +void cmsScreenResync(void) +{ + if (pScreenFnVTable->resync) + pScreenFnVTable->resync(); + + pScreenFnVTable->getsize(&cmsRows, &cmsCols); +} + +void cmsScreenInit(void) +{ +#ifdef OSD +pScreenFnVTable = osdCmsInit(); +#endif + +#ifdef CANVAS +pScreenFnVTable = canvasInit(); +#endif +} + +// +// Lots of things not separated yet. +// + +#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 curr_profile masterConfig.profile[masterConfig.current_profile_index] + +//osd current screen - to reduce long lines ;-) +#define OSD_cfg masterConfig.osdProfile + +#if 0 +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? + +static uint16_t flyTime = 0; +uint8_t statRssi; + +statistic_t stats; +#endif + +#define BUTTON_TIME 2 +#define BUTTON_PAUSE 5 + +// 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 +// +// Right column size be 5 chars??? (now 7) + +#define LEFT_MENU_COLUMN 1 +#define RIGHT_MENU_COLUMN 23 + +//#define MAX_MENU_ITEMS (cmsGetRowsCount() - 2) +#define MAX_MENU_ITEMS (cmsRows - 2) + +//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 + +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; +int8_t lastMenuPos; +uint8_t currentMenuIdx = 0; +uint16_t *currentElement = NULL; + +#ifdef OSD +OSD_Entry menuAlarms[]; +OSD_Entry menuOsdLayout[]; +OSD_Entry menuOsdActiveElems[]; +#if 0 // Not supported yet (or drop GUI position editing) +OSD_Entry menuOsdElemsPositions[]; +#endif +#endif + +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, true}, +#ifdef OSD + {"SCREEN LAYOUT", OME_Submenu, cmsChangeScreen, &menuOsdLayout[0], true}, + {"ALARMS", OME_Submenu, cmsChangeScreen, &menuAlarms[0], true}, +#endif + {"CFG. IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], true}, + {"FEATURES", OME_Submenu, cmsChangeScreen, &menuFeatures[0], true}, + {"SAVE & EXIT", OME_OSD_Exit, cmsExitMenu, (void*)1, true}, + {"EXIT", OME_OSD_Exit, cmsExitMenu, (void*)0, true}, + {NULL,OME_END, NULL, NULL, true} +}; + +OSD_Entry menuFeatures[] = +{ + {"----- FEATURES -----", OME_Label, NULL, NULL, true}, + {"BLACKBOX", OME_Submenu, cmsChangeScreen, &menuBlackbox[0], true}, +#ifdef LED_STRIP + {"LED STRIP", OME_Submenu, cmsChangeScreen, &menuLedstrip[0], true}, +#endif // LED_STRIP +#if defined(VTX) || defined(USE_RTC6705) + {"VTX", OME_Submenu, cmsChangeScreen, &menu_vtx[0], true}, +#endif // VTX || USE_RTC6705 + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + +OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; + +OSD_Entry menuBlackbox[] = +{ + {"--- BLACKBOX ---", OME_Label, NULL, NULL, true}, + {"ENABLED", OME_Bool, NULL, &featureBlackbox, true}, + {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, true}, +#ifdef USE_FLASHFS + {"ERASE FLASH", OME_Submenu, cmsEraseFlash, NULL, true}, +#endif // USE_FLASHFS + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + +#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.ledConfigs[ledIndex]; + + int fn = ledGetFunction(ledConfig); + + if (fn == LED_FUNCTION_COLOR) { + ledColor = ledGetColor(ledConfig); + break; + } + } +} + +//udate all leds with flag color +static void applyLedColor(void * ptr) +{ + UNUSED(ptr); + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + ledConfig_t *ledConfig = &masterConfig.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, true}, + {"ENABLED", OME_Bool, NULL, &featureLedstrip, true}, + {"LED COLOR", OME_TAB, applyLedColor, &entryLed, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; +#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, true}, + {"ENABLED", OME_Bool, NULL, &featureVtx, true}, +#ifdef VTX + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, true}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, true}, +#endif // VTX + {"BAND", OME_TAB, NULL, &entryVtxBand, true}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, true}, +#ifdef USE_RTC6705 + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, true}, +#endif // USE_RTC6705 + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; +#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, true}, + {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz, true}, + {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, true}, + {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, true}, + {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit, true}, + {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle, true}, + {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, true}, + {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + +OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; + +OSD_Entry menuImu[] = +{ + {"-----CFG. IMU-----", OME_Label, NULL, NULL, true}, + {"PID", OME_Submenu, cmsChangeScreen, &menuPid[0], true}, + {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile, true}, + {"RATE & RXPO", OME_Submenu, cmsChangeScreen, &menuRateExpo[0], true}, + {"RC PREVIEW", OME_Submenu, cmsChangeScreen, &menuRc[0], true}, + {"MISC", OME_Submenu, cmsChangeScreen, &menuMisc[0], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + +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, true}, + {"ROLL P", OME_UINT8, NULL, &entryRollP, true}, + {"ROLL I", OME_UINT8, NULL, &entryRollI, true}, + {"ROLL D", OME_UINT8, NULL, &entryRollD, true}, + + {"PITCH P", OME_UINT8, NULL, &entryPitchP, true}, + {"PITCH I", OME_UINT8, NULL, &entryPitchI, true}, + {"PITCH D", OME_UINT8, NULL, &entryPitchD, true}, + + {"YAW P", OME_UINT8, NULL, &entryYawP, true}, + {"YAW I", OME_UINT8, NULL, &entryYawI, true}, + {"YAW D", OME_UINT8, NULL, &entryYawD, true}, + + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + +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, true}, + {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate, true}, + {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate, true}, + {"YAW RATE", OME_FLOAT, NULL, &entryYawRate, true}, + {"RC RATE", OME_FLOAT, NULL, &entryRcRate, true}, + {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, true}, + {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, true}, + {"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, true}, + {"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, true}, + {"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, true}, + {"D SETPT TRNS", OME_FLOAT, NULL, &entryPSetpoint, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + +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, true}, + {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, true}, + {"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, true}, + {"THROTTLE", OME_Poll_INT16, NULL, &entryRcThrottle, true}, + {"YAW", OME_Poll_INT16, NULL, &entryRcYaw, true}, + {"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, true}, + {"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, true}, + {"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, true}, + {"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + + +bool cmsInMenu = false; + +// +// CMS specific functions +// + +void cmsUpdateMaxRows(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--; +} + +uint8_t cmsHandleKey(uint8_t key) +{ + uint8_t res = BUTTON_TIME; + OSD_Entry *p; + + if (!currentMenu) + return res; + + if (key == KEY_ESC) { + cmsMenuBack(); + return BUTTON_PAUSE; + } + + if (key == KEY_DOWN) { + if (currentMenuPos < currentMenuIdx) { + currentMenuPos++; + } else { + if (nextPage) // we have more pages + { + cmsScreenClear(); + p = nextPage; + nextPage = currentMenu; + currentMenu = (OSD_Entry *)p; + currentMenuPos = 0; + lastMenuPos = -1; + cmsUpdateMaxRows(); + } else { + 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) { + cmsScreenClear(); + p = nextPage; + nextPage = currentMenu; + currentMenu = (OSD_Entry *)p; + currentMenuPos = 0; + lastMenuPos = -1; + cmsUpdateMaxRows(); + } else { + currentMenuPos = currentMenuIdx; + // lastMenuPos = -1; + } + } + } + + if (key == KEY_DOWN || key == KEY_UP) + return res; + + p = currentMenu + currentMenuPos; + + switch (p->type) { + case OME_POS: +#if 0 +#ifdef OSD + 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; + } +#endif +#endif + case OME_Submenu: + case OME_OSD_Exit: + if (p->func && key == KEY_RIGHT) { + p->func(p->data); + res = BUTTON_PAUSE; + } + break; + case OME_Back: + cmsMenuBack(); + res = BUTTON_PAUSE; + break; + case OME_Bool: + if (p->data) { + uint8_t *val = p->data; + if (key == KEY_RIGHT) + *val = 1; + else + *val = 0; + p->changed = true; + } + break; + case OME_VISIBLE: +#ifdef OSD + 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; + p->changed = true; + } +#endif + 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; + } + p->changed = true; + } + 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); + p->changed = true; + } + 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; + } + p->changed = true; + } + 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; + } + p->changed = true; + } + 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; + } + p->changed = true; + } + break; + case OME_Poll_INT16: + case OME_Label: + case OME_END: + break; + } + return res; +} + +static 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 cmsDrawMenu(void) +{ + uint8_t i = 0; + OSD_Entry *p; + char buff[10]; + uint8_t top = (cmsRows - currentMenuIdx) / 2 - 1; + + // XXX Need denom based on absolute time? + static uint8_t pollDenom = 0; + bool drawPolled = (++pollDenom % 8 == 0); + + if (!currentMenu) + return; + + if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label + currentMenuPos++; + + if (lastMenuPos >= 0 && currentMenuPos != lastMenuPos) + cmsScreenWrite(LEFT_MENU_COLUMN, lastMenuPos + top, " "); + + for (p = currentMenu; p->type != OME_END; p++) { + + if (currentMenuPos == i && lastMenuPos != currentMenuPos) { + cmsScreenWrite(LEFT_MENU_COLUMN, i + top, " >"); + lastMenuPos = currentMenuPos; + } + + if (cmsScreenCleared) + cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text); + + switch (p->type) { + case OME_POS:; // Semi-colon required to add an empty statement +#ifdef OSD + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + if (!(*val & VISIBLE_FLAG)) + break; +#endif + + case OME_Submenu: + if (cmsScreenCleared) + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, ">"); + break; + case OME_Bool: + if ((p->changed || cmsScreenCleared) && p->data) { + if (*((uint8_t *)(p->data))) { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES"); + } else { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO "); + } + p->changed = false; + } + break; + case OME_TAB: { + if (p->changed || cmsScreenCleared) { + OSD_TAB_t *ptr = p->data; + cmsScreenWrite(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); + p->changed = false; + } + break; + } + case OME_VISIBLE: +#ifdef OSD + if ((p->changed || cmsScreenCleared) && p->data) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + + if (VISIBLE(*val)) { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES"); + } else { + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO "); + } + p->changed = false; + } +#endif + break; + case OME_UINT8: + if ((p->changed || cmsScreenCleared) && p->data) { + OSD_UINT8_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; + } + break; + case OME_INT8: + if ((p->changed || cmsScreenCleared) && p->data) { + OSD_INT8_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; + } + break; + case OME_UINT16: + if ((p->changed || cmsScreenCleared) && p->data) { + OSD_UINT16_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; + } + break; + case OME_INT16: + if ((p->changed || cmsScreenCleared) && p->data) { + OSD_UINT16_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + p->changed = false; + } + break; + case OME_Poll_INT16: + if (p->data && drawPolled) { + OSD_UINT16_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); + } + break; + case OME_FLOAT: + if ((p->changed || cmsScreenCleared) && p->data) { + OSD_FLOAT_t *ptr = p->data; + simple_ftoa(*ptr->val * ptr->multipler, buff); + cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, " "); + cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, buff); + p->changed = false; + } + 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; + } + } + cmsScreenCleared = false; +} + +void cmsChangeScreen(void *ptr) +{ + uint8_t i; + if (ptr) { + cmsScreenClear(); + // 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; + lastMenuPos = -1; // XXX this? + cmsUpdateMaxRows(); + } +} + +void cmsMenuBack(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) { + cmsScreenClear(); + menuStackIdx--; + nextPage = NULL; + currentMenu = menuStack[menuStackIdx]; + currentMenuPos = menuStackHistory[menuStackIdx]; + lastMenuPos = -1; + + cmsUpdateMaxRows(); + } + else { + cmsOpenMenu(); + } +} + +void cmsOpenMenu(void) +{ + if (cmsInMenu) + 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 + +#ifdef LED_STRIP + getLedColor(); +#endif // LED_STRIP + + // cmsRows = cmsGetRowsCount(); + cmsGetSize(&cmsRows, &cmsCols); + cmsInMenu = true; + cmsScreenBegin(); + cmsScreenClear(); + currentMenu = &menuMain[0]; + cmsChangeScreen(currentMenu); +} + +void cmsExitMenu(void *ptr) +{ + cmsScreenClear(); + + cmsScreenWrite(5, 3, "RESTARTING IMU..."); + cmsScreenResync(); // Was max7456RefreshAll(); why at this timing? + + 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(); + } + + cmsInMenu = false; + + cmsScreenEnd(); + + systemReset(); +} + +void cmsUpdate(uint32_t currentTime) +{ + static uint8_t rcDelay = BUTTON_TIME; + uint8_t key = 0; + static uint32_t lastCmsHeartBeat = 0; + + // detect enter to menu + if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + // XXX Double enter!? + cmsOpenMenu(); + } + + if (cmsInMenu) { + 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; + } + + // XXX Element position adjustment is hard to separate. + // XXX May need to drop it upon real separation. + // XXX Don't know if this still works + + if (key && !currentElement) { + rcDelay = cmsHandleKey(key); + return; + } + + cmsDrawMenu(); + + if (currentTime > lastCmsHeartBeat + 500) { + // Heart beat for external CMS display device @ 500msec + // (Timeout @ 1000msec) + cmsScreenHeartBeat(); + lastCmsHeartBeat = currentTime; + } + } +} + +void cmsHandler(uint32_t currentTime) +{ + static uint32_t counter = 0; + + if (counter++ % 5 == 0) { + cmsUpdate(currentTime); + } + + // do not allow ARM if we are in menu + if (cmsInMenu) + DISABLE_ARMING_FLAG(OK_TO_ARM); +} + +void cmsInit(void) +{ + cmsScreenInit(); +} + +// Does this belong here? + +#ifdef USE_FLASHFS +void cmsEraseFlash(void *ptr) +{ + UNUSED(ptr); + + cmsScreenClear(); + cmsScreenWrite(5, 3, "ERASING FLASH..."); + cmsScreenResync(); // Was max7456RefreshAll(); Why at this timing? + + flashfsEraseCompletely(); + while (!flashfsIsReady()) { + delay(100); + } + + cmsScreenClear(); + cmsScreenResync(); // Was max7456RefreshAll(); wedges during heavy SPI? +} +#endif // USE_FLASHFS + +#ifdef OSD +// +// OSD specific menu pages and items +// XXX Should be part of the osd.c, or new osd_csm.c. +// +OSD_Entry menuOsdLayout[] = +{ + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, true}, + {"ACTIVE ELEM.", OME_Submenu, cmsChangeScreen, &menuOsdActiveElems[0], true}, +#if 0 + {"POSITION", OME_Submenu, cmsChangeScreen, &menuOsdElemsPositions[0], true}, +#endif + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + +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, true}, + {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, true}, + {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity, true}, + {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, true}, + {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude, true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; + +#if 0 // Not supported yet (or drop support for GUI position editing) +OSD_Entry menuOsdElemsPositions[] = +{ + {"---POSITION---", OME_Label, NULL, NULL, true}, + {"RSSI", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_RSSI_VALUE], true}, + {"MAIN BATTERY", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, + {"UPTIME", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_ONTIME], true}, + {"FLY TIME", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_FLYTIME], true}, + {"FLY MODE", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_FLYMODE], true}, + {"NAME", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_CRAFT_NAME], true}, + {"THROTTLE", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_THROTTLE_POS], true}, + +#ifdef VTX + {"VTX CHAN", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_VTX_CHANNEL], true}, +#endif // VTX + {"CURRENT (A)", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_CURRENT_DRAW], true}, + {"USED MAH", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_MAH_DRAWN], true}, +#ifdef GPS + {"GPS SPEED", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_GPS_SPEED], true}, + {"GPS SATS.", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_GPS_SATS], true}, +#endif // GPS + {"ALTITUDE", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_ALTITUDE], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; +#endif + +OSD_Entry menuOsdActiveElems[] = +{ + {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL, true}, + {"RSSI", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_RSSI_VALUE], true}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, + {"HORIZON", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON], true}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_HORIZON_SIDEBARS], true}, + {"UPTIME", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_ONTIME], true}, + {"FLY TIME", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_FLYTIME], true}, + {"FLY MODE", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_FLYMODE], true}, + {"NAME", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_CRAFT_NAME], true}, + {"THROTTLE", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_THROTTLE_POS], true}, +#ifdef VTX + {"VTX CHAN", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_VTX_CHANNEL]}, +#endif // VTX + {"CURRENT (A)", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_CURRENT_DRAW], true}, + {"USED MAH", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_MAH_DRAWN], true}, +#ifdef GPS + {"GPS SPEED", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_GPS_SPEED], true}, + {"GPS SATS.", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_GPS_SATS], true}, +#endif // GPS + {"ALTITUDE", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_ALTITUDE], true}, + {"BACK", OME_Back, NULL, NULL, true}, + {NULL, OME_END, NULL, NULL, true} +}; +#endif diff --git a/src/main/io/cms.h b/src/main/io/cms.h index 61b059b1a3..ed54214f42 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -1,2 +1,13 @@ +#pragma once + void cmsInit(void); void cmsHandler(uint32_t); + +void cmsOpenMenu(); +void cmsUpdate(uint32_t); +void cmsScreenResync(void); + +// Required for external CMS tables + +void cmsChangeScreen(void * ptr); +void cmsExitMenu(void * ptr); diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h new file mode 100644 index 0000000000..e7950c2d18 --- /dev/null +++ b/src/main/io/cms_types.h @@ -0,0 +1,96 @@ +// +// Menu element types +// XXX Upon separation, all OME would be renamed to CME_ or similar. +// + +#pragma once + +typedef void (*OSDMenuFuncPtr)(void *data); + +//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_Poll_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; + +typedef struct +{ + char *text; + OSD_MenuElement type; + OSDMenuFuncPtr func; + void *data; + bool changed; +} 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; + +typedef struct screenFnVTable_s { + void (*getsize)(uint8_t *, uint8_t *); + void (*begin)(void); + void (*end)(void); + void (*clear)(void); + void (*write)(uint8_t, uint8_t, char *); + void (*heartbeat)(void); + void (*resync)(void); +} screenFnVTable_t; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index baeb80c957..55432bcb9f 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -62,8 +62,8 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/gps.h" -#include "io/osd.h" -#include "io/vtx.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 6b9a366ba8..a9ed93f6c8 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 @@ -29,10 +31,15 @@ #include "build/version.h" +#ifdef OSD + #include "common/utils.h" #include "drivers/system.h" +#include "io/cms_types.h" +#include "io/cms.h" + #include "io/flashfs.h" #include "io/osd.h" @@ -40,12 +47,15 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/pid.h" +//#include "flight/pid.h" #include "config/config_profile.h" #include "config/config_master.h" #include "config/feature.h" +// Short hands +#define OSD_cfg masterConfig.osdProfile + #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -53,10 +63,6 @@ #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" @@ -68,1361 +74,62 @@ extern serialPort_t *debugSerialPort; #define dprintf(x) #endif -// Configuration Menu System forwards - -uint8_t cmsHandleKey(uint8_t); -void cmsUpdateMaxRows(void); -void cmsOpenMenu(void); -void cmsExitMenu(void * ptr); -void cmsChangeScreen(void * ptr); -void cmsMenuBack(void); -void cmsDrawMenu(void); - -// OSD forwards - -void osdMenuBegin(void); -void osdMenuEnd(void); -void osdUpdate(uint32_t currentTime); -char osdGetAltitudeSymbol(); -int32_t osdGetAltitude(int32_t alt); -void osdEditElement(void *ptr); -void cmsEraseFlash(void *ptr); -void osdDrawElements(void); -void osdDrawSingleElement(uint8_t item); -void osdResetAlarms(void); - -#ifdef CANVAS -// -// canvasXXX() should goto io/canvas.c -// cmsXXX() should goto io/cms.c and then call display device -// specific functions; max7456XXX(), canvasXXX(), oledXXX(), ... -// - -#include "fc/fc_msp.h" -#include "msp/msp_protocol.h" -#include "msp/msp_serial.h" - -void canvasBegin(void) -{ - uint8_t subcmd[] = { 0 }; - - mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); -} - -void canvasHeartBeat(void) -{ - canvasBegin(); -} - -void canvasEnd(void) -{ - uint8_t subcmd[] = { 1 }; - - mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); -} - -void canvasClear(void) -{ - uint8_t subcmd[] = { 2 }; - - mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd)); -} - -void canvasWrite(int col, int row, char *string) -{ - -//debug[0]++; // Let's capture excess canvas writes - - int len; - char buf[30 + 4]; - - if ((len = strlen(string)) >= 30) - len = 30; - - buf[0] = 3; - buf[1] = row; - buf[2] = col; - buf[3] = 0; - memcpy((char *)&buf[4], string, len); - - mspSerialPush(MSP_CANVAS, (uint8_t *)buf, len + 4); -} - -// Called once at startup to initialize push function in msp -void canvasInit(void) -{ - mspSerialPushInit(mspFcPushInit()); -} -#endif - -// Force draw all elements if true -bool cmsScreenCleared; - -// Function vector may be good here. - -uint8_t cmsGetRowsCount() -{ -#ifdef OSD - return max7456GetRowsCount(); -#endif - -#ifdef CANVAS - return 13; -#endif -} - -void cmsScreenClear(void) -{ -#ifdef OSD - max7456ClearScreen(); -#endif - -#ifdef CANVAS - canvasClear(); -#endif - cmsScreenCleared = true; -} - -void cmsScreenBegin(void) -{ -#ifdef OSD - osdMenuBegin(); -#endif - -#ifdef CANVAS - canvasBegin(); -#endif - - cmsScreenClear(); -} - -void cmsScreenEnd(void) -{ -#ifdef OSD - osdMenuEnd(); -#endif - -#ifdef CANVAS - canvasEnd(); -#endif -} - -void cmsScreenWrite(uint8_t x, uint8_t y, char *s) -{ -#ifdef OSD - max7456Write(x, y, s); -#endif - -#ifdef CANVAS - canvasWrite(x, y, s); -#endif -} - -void cmsScreenHeartBeat(void) -{ -#ifdef CANVAS - canvasHeartBeat(); -#endif -} - -// Find wedged device and restart (kludge!) -void cmsScreenResync(void) -{ -#ifdef OSD - max7456RefreshAll(); -#endif -} - -void cmsScreenInit(void) -{ -#ifdef CANVAS - canvasInit(); -#endif -} - -// -// Lots of things not separated yet. -// +// 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 - -#define curr_profile masterConfig.profile[masterConfig.current_profile_index] - -#ifdef OSD -//osd current screen - to reduce long lines ;-) -#define OSD_cfg masterConfig.osdProfile - -uint16_t refreshTimeout = 0; - -#define VISIBLE_FLAG 0x0800 +//#define VISIBLE_FLAG 0x0800 // defined in osd.h #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 VISIBLE(x) (x & VISIBLE_FLAG) // defined in osd.h #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; statistic_t stats; -#endif -#define BUTTON_TIME 2 -#define BUTTON_PAUSE 5 +uint16_t refreshTimeout = 0; #define REFRESH_1S 12 -#define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN 23 -#define MAX_MENU_ITEMS (cmsGetRowsCount() - 2) - -// -// Menu element types -// XXX Upon separation, all OME would be renamed to CME_ or similar. -// - -typedef void (* OSDMenuFuncPtr)(void *data); - -//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 +// OSD forwards -typedef struct -{ - char *text; - OSD_MenuElement type; - OSDMenuFuncPtr func; - void *data; - bool changed; -} OSD_Entry; +void osdUpdate(uint32_t currentTime); +char osdGetAltitudeSymbol(); +int32_t osdGetAltitude(int32_t alt); +void osdEditElement(void *ptr); +void osdDrawElements(void); +void osdDrawSingleElement(uint8_t item); -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; -int8_t lastMenuPos; -uint8_t currentMenuIdx = 0; -uint16_t *currentElement = NULL; - -#ifdef OSD -OSD_Entry menuAlarms[]; -OSD_Entry menuOsdLayout[]; -OSD_Entry menuOsdActiveElems[]; -OSD_Entry menuOsdElemsPositions[]; -#endif -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, true}, -#ifdef OSD - {"SCREEN LAYOUT", OME_Submenu, cmsChangeScreen, &menuOsdLayout[0], true}, - {"ALARMS", OME_Submenu, cmsChangeScreen, &menuAlarms[0], true}, -#endif - {"CFG. IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], true}, - {"FEATURES", OME_Submenu, cmsChangeScreen, &menuFeatures[0], true}, - {"SAVE & EXIT", OME_OSD_Exit, cmsExitMenu, (void*)1, true}, - {"EXIT", OME_OSD_Exit, cmsExitMenu, (void*)0, true}, - {NULL,OME_END, NULL, NULL, true} -}; - -OSD_Entry menuFeatures[] = -{ - {"----- FEATURES -----", OME_Label, NULL, NULL, true}, - {"BLACKBOX", OME_Submenu, cmsChangeScreen, &menuBlackbox[0], true}, -#ifdef LED_STRIP - {"LED STRIP", OME_Submenu, cmsChangeScreen, &menuLedstrip[0], true}, -#endif // LED_STRIP -#if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, cmsChangeScreen, &menu_vtx[0], true}, -#endif // VTX || USE_RTC6705 - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; - -OSD_Entry menuBlackbox[] = -{ - {"--- BLACKBOX ---", OME_Label, NULL, NULL, true}, - {"ENABLED", OME_Bool, NULL, &featureBlackbox, true}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, true}, -#ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, cmsEraseFlash, NULL, true}, -#endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -#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.ledConfigs[ledIndex]; - - int fn = ledGetFunction(ledConfig); - - if (fn == LED_FUNCTION_COLOR) { - ledColor = ledGetColor(ledConfig); - break; - } - } -} - -//udate all leds with flag color -static void applyLedColor(void * ptr) -{ - UNUSED(ptr); - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - ledConfig_t *ledConfig = &masterConfig.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, true}, - {"ENABLED", OME_Bool, NULL, &featureLedstrip, true}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; -#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, true}, - {"ENABLED", OME_Bool, NULL, &featureVtx, true}, -#ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, true}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, true}, -#endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand, true}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, true}, -#ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, true}, -#endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; -#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, true}, - {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz, true}, - {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, true}, - {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, true}, - {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit, true}, - {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle, true}, - {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, true}, - {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; - -OSD_Entry menuImu[] = -{ - {"-----CFG. IMU-----", OME_Label, NULL, NULL, true}, - {"PID", OME_Submenu, cmsChangeScreen, &menuPid[0], true}, - {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile, true}, - {"RATE & RXPO", OME_Submenu, cmsChangeScreen, &menuRateExpo[0], true}, - {"RC PREVIEW", OME_Submenu, cmsChangeScreen, &menuRc[0], true}, - {"MISC", OME_Submenu, cmsChangeScreen, &menuMisc[0], true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -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, true}, - {"ROLL P", OME_UINT8, NULL, &entryRollP, true}, - {"ROLL I", OME_UINT8, NULL, &entryRollI, true}, - {"ROLL D", OME_UINT8, NULL, &entryRollD, true}, - - {"PITCH P", OME_UINT8, NULL, &entryPitchP, true}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI, true}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD, true}, - - {"YAW P", OME_UINT8, NULL, &entryYawP, true}, - {"YAW I", OME_UINT8, NULL, &entryYawI, true}, - {"YAW D", OME_UINT8, NULL, &entryYawD, true}, - - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -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, true}, - {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate, true}, - {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate, true}, - {"YAW RATE", OME_FLOAT, NULL, &entryYawRate, true}, - {"RC RATE", OME_FLOAT, NULL, &entryRcRate, true}, - {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, true}, - {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, true}, - {"THR. PID ATT.", OME_FLOAT, NULL, &extryTpaEntry, true}, - {"TPA BREAKPOINT", OME_UINT16, NULL, &entryTpaBreak, true}, - {"D SETPOINT", OME_FLOAT, NULL, &entryDSetpoint, true}, - {"D SETPOINT TRANSITION", OME_FLOAT, NULL, &entryPSetpoint, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -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, true}, - {"ROLL", OME_INT16, NULL, &entryRcRoll, true}, - {"PITCH", OME_INT16, NULL, &entryRcPitch, true}, - {"THROTTLE", OME_INT16, NULL, &entryRcThrottle, true}, - {"YAW", OME_INT16, NULL, &entryRcYaw, true}, - {"AUX1", OME_INT16, NULL, &entryRcAux1, true}, - {"AUX2", OME_INT16, NULL, &entryRcAux2, true}, - {"AUX3", OME_INT16, NULL, &entryRcAux3, true}, - {"AUX4", OME_INT16, NULL, &entryRcAux4, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -#ifdef OSD -// -// OSD specific menu pages and items for -// -OSD_Entry menuOsdLayout[] = -{ - {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, true}, - {"ACTIVE ELEM.", OME_Submenu, cmsChangeScreen, &menuOsdActiveElems[0], true}, - {"POSITION", OME_Submenu, cmsChangeScreen, &menuOsdElemsPositions[0], true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -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, true}, - {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, true}, - {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity, true}, - {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, true}, - {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -OSD_Entry menuOsdElemsPositions[] = -{ - {"---POSITION---", OME_Label, NULL, NULL, true}, - {"RSSI", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], true}, - {"MAIN BATTERY", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, - {"UPTIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ONTIME], true}, - {"FLY TIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], true}, - {"FLY MODE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], true}, - {"NAME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], true}, - {"THROTTLE POS", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], true}, -#ifdef VTX - {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL], true}, -#endif // VTX - {"CURRENT (A)", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], true}, - {"USED MAH", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], true}, -#ifdef GPS - {"GPS SPEED", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], true}, - {"GPS SATS.", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], true}, -#endif // GPS - {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; - -OSD_Entry menuOsdActiveElems[] = -{ - {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL, true}, - {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], true}, - {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, - {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], true}, - {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], true}, - {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], true}, - {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], true}, - {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], true}, - {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], true}, - {"THROTTLE POS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], true}, -#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], true}, - {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], true}, -#ifdef GPS - {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], true}, - {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], true}, -#endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; -#endif - -uint8_t cmsRows; -bool cmsInMenu = false; - -// -// CMS specific functions -// - -void cmsUpdateMaxRows(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--; -} - -uint8_t cmsHandleKey(uint8_t key) -{ - uint8_t res = BUTTON_TIME; - OSD_Entry *p; - - if (!currentMenu) - return res; - - if (key == KEY_ESC) { - cmsMenuBack(); - return BUTTON_PAUSE; - } - - if (key == KEY_DOWN) { - if (currentMenuPos < currentMenuIdx) { - currentMenuPos++; - } else { - if (nextPage) // we have more pages - { - cmsScreenClear(); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - lastMenuPos = -1; - cmsUpdateMaxRows(); - } else { - 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) { - cmsScreenClear(); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - lastMenuPos = -1; - cmsUpdateMaxRows(); - } else { - currentMenuPos = currentMenuIdx; - // lastMenuPos = -1; - } - } - } - - if (key == KEY_DOWN || key == KEY_UP) - return res; - - p = currentMenu + currentMenuPos; - - switch (p->type) { - case OME_POS: -#ifdef OSD - 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; - } -#endif - case OME_Submenu: - case OME_OSD_Exit: - if (p->func && key == KEY_RIGHT) { - p->func(p->data); - res = BUTTON_PAUSE; - } - break; - case OME_Back: - cmsMenuBack(); - res = BUTTON_PAUSE; - break; - case OME_Bool: - if (p->data) { - uint8_t *val = p->data; - if (key == KEY_RIGHT) - *val = 1; - else - *val = 0; - p->changed = true; - } - break; - case OME_VISIBLE: -#ifdef OSD - 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; - p->changed = true; - } -#endif - 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; - } - p->changed = true; - } - 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); - p->changed = true; - } - 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; - } - p->changed = true; - } - 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; - } - p->changed = true; - } - 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; - } - p->changed = true; - } - break; - case OME_Label: - case OME_END: - break; - } - return res; -} - -static 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 cmsDrawMenu(void) -{ - uint8_t i = 0; - OSD_Entry *p; - char buff[10]; - uint8_t top = (cmsRows - currentMenuIdx) / 2 - 1; - - if (!currentMenu) - return; - - if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label - currentMenuPos++; - - if (lastMenuPos >= 0 && currentMenuPos != lastMenuPos) - cmsScreenWrite(LEFT_MENU_COLUMN, lastMenuPos + top, " "); - - for (p = currentMenu; p->type != OME_END; p++) { - - if (currentMenuPos == i && lastMenuPos != currentMenuPos) { - cmsScreenWrite(LEFT_MENU_COLUMN, i + top, " >"); - lastMenuPos = currentMenuPos; - } - - if (cmsScreenCleared) - cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text); - - switch (p->type) { - case OME_POS:; // Semi-colon required to add an empty statement -#ifdef OSD - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - if (!(*val & VISIBLE_FLAG)) - break; -#endif - - case OME_Submenu: - if (cmsScreenCleared) - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, ">"); - break; - case OME_Bool: - if ((p->changed || cmsScreenCleared) && p->data) { - if (*((uint8_t *)(p->data))) { - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES"); - } else { - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO "); - } - p->changed = false; - } - break; - case OME_TAB: { - if (p->changed || cmsScreenCleared) { - OSD_TAB_t *ptr = p->data; - cmsScreenWrite(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); - p->changed = false; - } - break; - } - case OME_VISIBLE: -#ifdef OSD - if ((p->changed || cmsScreenCleared) && p->data) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - - if (VISIBLE(*val)) { - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES"); - } else { - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO "); - } - p->changed = false; - } -#endif - break; - case OME_UINT8: - if ((p->changed || cmsScreenCleared) && p->data) { - OSD_UINT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); - p->changed = false; - } - break; - case OME_INT8: - if ((p->changed || cmsScreenCleared) && p->data) { - OSD_INT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); - p->changed = false; - } - break; - case OME_UINT16: - if ((p->changed || cmsScreenCleared) && p->data) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); - p->changed = false; - } - break; - case OME_INT16: - if ((p->changed || cmsScreenCleared) && p->data) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff); - p->changed = false; - } - break; - case OME_FLOAT: - if ((p->changed || cmsScreenCleared) && p->data) { - OSD_FLOAT_t *ptr = p->data; - simple_ftoa(*ptr->val * ptr->multipler, buff); - cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, buff); - p->changed = false; - } - 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; - } - } - cmsScreenCleared = false; -} - -void cmsChangeScreen(void *ptr) -{ - uint8_t i; - if (ptr) { - cmsScreenClear(); - // 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; - lastMenuPos = -1; // XXX this? - cmsUpdateMaxRows(); - } -} - -void cmsMenuBack(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) { - cmsScreenClear(); - menuStackIdx--; - nextPage = NULL; - currentMenu = menuStack[menuStackIdx]; - currentMenuPos = menuStackHistory[menuStackIdx]; - lastMenuPos = -1; - - cmsUpdateMaxRows(); - } - else { - cmsOpenMenu(); - } -} - -void cmsOpenMenu(void) -{ - if (cmsInMenu) - 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 - - cmsRows = cmsGetRowsCount(); - cmsInMenu = true; -#ifdef OSD -// XXX Do we need this here? - refreshTimeout = 0; -#endif - cmsScreenBegin(); - cmsScreenClear(); - currentMenu = &menuMain[0]; - cmsChangeScreen(currentMenu); -#ifdef LED_STRIP - getLedColor(); -#endif // LED_STRIP -} - -void cmsExitMenu(void *ptr) -{ - cmsScreenClear(); - - cmsScreenWrite(5, 3, "RESTARTING IMU..."); - cmsScreenResync(); // Was max7456RefreshAll(); why at this timing? - - 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(); - } - - cmsInMenu = false; - - cmsScreenEnd(); - - systemReset(); -} - -void cmsProcess(uint32_t currentTime) -{ - static uint8_t rcDelay = BUTTON_TIME; - uint8_t key = 0; - static uint32_t lastCmsHeartBeat = 0; - -//debug[1]++; - - // detect enter to menu - if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { - cmsOpenMenu(); - } - - if (cmsInMenu) { - 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; - } - - // XXX Element position adjustment is hard to separate. - // XXX May need to drop it upon real separation. - // XXX Don't know if this still works - if (key && !currentElement) { - rcDelay = cmsHandleKey(key); - return; - } - - cmsDrawMenu(); - - if (currentTime > lastCmsHeartBeat + 500) { - // Heart beat for external CMS display device @ 500msec - // (Timeout @ 1000msec) - cmsScreenHeartBeat(); - lastCmsHeartBeat = currentTime; - } - } -} - -void cmsHandler(uint32_t currentTime) -{ - static uint32_t counter = 0; - - if (counter++ % 5 == 0) { - cmsProcess(currentTime); - } - - // do not allow ARM if we are in menu - if (cmsInMenu) - DISABLE_ARMING_FLAG(OK_TO_ARM); - -} - -void cmsInit(void) -{ - cmsScreenInit(); -} - -// Does this belong here? - -#ifdef USE_FLASHFS -void cmsEraseFlash(void *ptr) -{ - UNUSED(ptr); - - cmsScreenClear(); - cmsScreenWrite(5, 3, "ERASING FLASH..."); - cmsScreenResync(); // Was max7456RefreshAll(); Why at this timing? - - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); - } - - cmsScreenClear(); - cmsScreenResync(); // Was max7456RefreshAll(); wedges during heavy SPI? -} -#endif // USE_FLASHFS - -#ifdef OSD bool osdInMenu = false; -// -// OSD specific CMS function -// - -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 "); -} - -// -// OSD spcific functions -// - -void osdMenuBegin(void) { - osdResetAlarms(); - osdInMenu = true; -} - -void osdMenuEnd(void) { - osdInMenu = false; -} +#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 osdDrawElements(void) { max7456ClearScreen(); +#if 0 if (currentElement) osdDrawElementPositioningHelp(); +#else + if (false) + ; +#endif else if (sensors(SENSOR_ACC) || osdInMenu) { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); @@ -1449,12 +156,6 @@ void osdDrawElements(void) #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])) @@ -1695,7 +396,7 @@ void osdInit(void) armState = ARMING_FLAG(ARMED); - max7456Init(masterConfig.osdProfile.video_system); + max7456Init(OSD_cfg.video_system); max7456ClearScreen(); @@ -1708,11 +409,14 @@ void osdInit(void) } } + cmsInit(); + 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"); + cmsScreenResync(); // Was max7456RefreshAll(); may be okay. refreshTimeout = 4 * REFRESH_1S; @@ -1723,7 +427,7 @@ void osdInit(void) */ char osdGetAltitudeSymbol() { - switch (masterConfig.osdProfile.units) { + switch (OSD_cfg.units) { case OSD_UNIT_IMPERIAL: return 0xF; default: @@ -1737,7 +441,7 @@ char osdGetAltitudeSymbol() */ int32_t osdGetAltitude(int32_t alt) { - switch (masterConfig.osdProfile.units) { + switch (OSD_cfg.units) { case OSD_UNIT_IMPERIAL: return (alt * 328) / 100; // Convert to feet / 100 default: @@ -1899,10 +603,12 @@ void osdUpdate(uint32_t currentTime) static uint8_t lastSec = 0; uint8_t sec; +#ifdef OSD_CALLS_CMS // detect enter to menu if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { cmsOpenMenu(); } +#endif // detect arm/disarm if (armState != ARMING_FLAG(ARMED)) { @@ -1937,11 +643,34 @@ void osdUpdate(uint32_t currentTime) if (!osdInMenu) { osdUpdateAlarms(); osdDrawElements(); +#ifdef OSD_CALLS_CMS } else { - cmsProcess(currentTime); + cmsUpdate(currentTime); +#endif } } +// +// OSD specific CMS functions +// + +void osdGetSize(uint8_t *pRows, uint8_t *pCols) +{ + *pRows = max7456GetRowsCount(); + *pCols = 30; +} + +void osdMenuBegin(void) { + osdResetAlarms(); + osdInMenu = true; + refreshTimeout = 0; +} + +void osdMenuEnd(void) { + osdInMenu = false; +} + +#ifdef EDIT_ELEMENT_SUPPORT void osdEditElement(void *ptr) { uint32_t address = (uint32_t)ptr; @@ -1957,4 +686,28 @@ void osdEditElement(void *ptr) max7456ClearScreen(); } +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 "); +} +#endif + +screenFnVTable_t osdVTable = { + osdGetSize, + osdMenuBegin, + osdMenuEnd, + max7456ClearScreen, + max7456Write, + NULL, + max7456RefreshAll, +}; + +screenFnVTable_t *osdCmsInit(void) +{ + return &osdVTable; +} #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7b5eae891f..150786a578 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -68,3 +68,7 @@ typedef struct { void updateOsd(uint32_t currentTime); void osdInit(void); void resetOsdConfig(osd_profile_t *osdProfile); +screenFnVTable_t *osdCmsInit(void); + +#define VISIBLE_FLAG 0x0800 +#define VISIBLE(x) (x & VISIBLE_FLAG) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d98b5d0793..ed62918554 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -70,6 +70,7 @@ uint8_t cliMode = 0; #include "io/flashfs.h" #include "io/beeper.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/cms_types.h" #include "io/osd.h" #include "io/vtx.h" diff --git a/src/main/main.c b/src/main/main.c index 4cf62bf83f..216c2c1dcd 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -86,9 +86,10 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" +#include "io/cms_types.h" +#include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" -#include "io/cms.h" #include "scheduler/scheduler.h" diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 92d25c77d1..9718b5bc97 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -93,15 +93,15 @@ #define CMS // Use external OSD to run CMS -#define CANVAS +//#define CANVAS // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk // include the osd code -//#define OSD +#define OSD // include the max7456 driver -//#define USE_MAX7456 +#define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 #define MAX7456_SPI_CS_PIN PB1 #define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 67a0132080..01d31c3708 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -14,4 +14,5 @@ TARGET_SRC = \ drivers/transponder_ir_stm32f30x.c \ io/transponder_ir.c \ drivers/max7456.c \ - io/osd.c + io/osd.c \ + io/cms.c diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index 22dcede4b5..dd7702375e 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -9,5 +9,6 @@ TARGET_SRC = \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ - io/osd.c + io/cms.c \ + io/canvas.c diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 691d208c6d..0a8e5ffc1b 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -65,8 +65,8 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/beeper.h" -#include "io/osd.h" -#include "io/vtx.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 0e9fb6fa0e..8e8f908413 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -36,9 +36,9 @@ #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" -#include "io/ledstrip.h" -#include "io/osd.h" -#include "io/vtx.h" +//#include "io/ledstrip.h" +//#include "io/osd.h" +//#include "io/vtx.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" From c3c13cf454c36190b5e56e428f666789eace0259 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 24 Oct 2016 02:47:33 +0900 Subject: [PATCH 07/19] Touch up for SIRINFPV and STM32F3DISCOVERY --- src/main/target/SIRINFPV/target.h | 1 + src/main/target/SIRINFPV/target.mk | 3 ++- src/main/target/STM32F3DISCOVERY/target.h | 2 ++ src/main/target/STM32F3DISCOVERY/target.mk | 3 ++- 4 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index cab5ad2edd..474dd9b87e 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -135,6 +135,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define OSD +#define CMS #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index eac7fbb6ec..2597e6e977 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -12,4 +12,5 @@ TARGET_SRC = \ drivers/flash_m25p16.c \ drivers/vtx_soft_spi_rtc6705.c \ drivers/max7456.c \ - io/osd.c + io/osd.c \ + io/cms.c diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 687fa1924a..6b5b5c23b6 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..6b718b5b0a 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -26,4 +26,5 @@ TARGET_SRC = \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ drivers/max7456.c \ - io/osd.c + io/osd.c \ + io/cms.c From 38660aa8a60c2ca3c17e1120af9ebbb3223b5f80 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 25 Oct 2016 14:34:08 +0900 Subject: [PATCH 08/19] Touch-ups, tidy, bandwidth mgmt on serial canvas --- src/main/fc/fc_msp.c | 2 - src/main/io/canvas.c | 39 +- src/main/io/cms.c | 824 ++++++++++++++++----------------- src/main/io/cms.h | 4 + src/main/io/cms_types.h | 27 +- src/main/io/ledstrip.c | 2 - src/main/io/osd.c | 116 ++--- src/main/io/osd.h | 12 +- src/main/telemetry/ltm.c | 2 - src/main/telemetry/smartport.c | 3 - 10 files changed, 524 insertions(+), 507 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d3b2c92e16..b367c7f645 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -67,9 +67,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" diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index fa684bdcdb..bde2031674 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -17,45 +17,48 @@ #include "msp/msp_protocol.h" #include "msp/msp_serial.h" -void canvasOutput(uint8_t cmd, uint8_t *buf, int len) -{ - mspSerialPush(cmd, buf, len); - delayMicroseconds(len * 150); // XXX Kludge!!! -} - -void canvasGetSize(uint8_t *pRows, uint8_t *pCols) +void canvasGetDevParam(uint8_t *pRows, uint8_t *pCols, uint16_t *pBuftime, uint16_t *pBufsize) { *pRows = 13; *pCols = 30; + *pBuftime = 23; // = 256/(115200/10) + *pBufsize = 192; // 256 * 3/4 (Be conservative) } -void canvasBegin(void) +int canvasOutput(uint8_t cmd, uint8_t *buf, int len) +{ + mspSerialPush(cmd, buf, len); + + return 6 + len; +} + +int canvasBegin(void) { uint8_t subcmd[] = { 0 }; - canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); + return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); } -void canvasHeartBeat(void) +int canvasHeartBeat(void) { - canvasBegin(); + return canvasBegin(); } -void canvasEnd(void) +int canvasEnd(void) { uint8_t subcmd[] = { 1 }; - canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); + return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); } -void canvasClear(void) +int canvasClear(void) { uint8_t subcmd[] = { 2 }; - canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); + return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); } -void canvasWrite(uint8_t col, uint8_t row, char *string) +int canvasWrite(uint8_t col, uint8_t row, char *string) { int len; char buf[30 + 4]; @@ -69,11 +72,11 @@ void canvasWrite(uint8_t col, uint8_t row, char *string) buf[3] = 0; memcpy((char *)&buf[4], string, len); - canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4); + return canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4); } screenFnVTable_t canvasVTable = { - canvasGetSize, + canvasGetDevParam, canvasBegin, canvasEnd, canvasClear, diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 014f9d00ac..83cdcb1aee 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -1,3 +1,25 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +/* + Created by Marcin Baliniak + OSD-CMS separation by jflyper + */ + #include #include #include @@ -7,8 +29,12 @@ #include "build/version.h" +#ifdef CMS + #include "drivers/system.h" +#include "common/typeconversion.h" + #include "io/cms_types.h" #include "io/canvas.h" @@ -29,35 +55,64 @@ // Configuration Menu System forwards -uint8_t cmsHandleKey(uint8_t); -void cmsUpdateMaxRows(void); void cmsOpenMenu(void); -void cmsExitMenu(void * ptr); -void cmsChangeScreen(void * ptr); +void cmsExitMenu(void *); +void cmsChangeScreen(void *); void cmsMenuBack(void); -void cmsDrawMenu(void); -void cmsGetSize(uint8_t *, uint8_t *); -void cmsEraseFlash(void *ptr); +void cmsEraseFlash(void *); screenFnVTable_t *pScreenFnVTable; -// Force draw all elements if true -bool cmsScreenCleared; - -// Function vector may be good here. - uint8_t cmsRows; uint8_t cmsCols; +uint16_t cmsBuftime; +uint16_t cmsBufsize; +uint16_t cmsBatchsize; -void cmsGetSize(uint8_t *pRows, uint8_t *pCols) +#define CMS_UPDATE_INTERVAL 50 // msec + +//#define MAX_MENU_ITEMS (cmsGetRowsCount() - 2) +#define MAX_MENU_ITEMS (cmsRows - 2) + +// 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 (cmsCols - 7) +#define RIGHT_MENU_COLUMN (cmsCols - 9 - 7) + +bool cmsScreenCleared; +OSD_Entry *currentMenu; +OSD_Entry *nextPage = NULL; + +int8_t currentCursorPos = 0; +int8_t lastCursorPos; +uint8_t currentMenuIdx = 0; +uint16_t *currentElement = NULL; + +void cmsGetDevParam(uint8_t *pRows, uint8_t *pCols, uint16_t *pBuftime, uint16_t *pBufsize) { - pScreenFnVTable->getsize(pRows, pCols); + pScreenFnVTable->getDevParam(pRows, pCols, pBuftime, pBufsize); } void cmsScreenClear(void) { pScreenFnVTable->clear(); cmsScreenCleared = true; + lastCursorPos = -1; } void cmsScreenBegin(void) @@ -71,9 +126,9 @@ void cmsScreenEnd(void) pScreenFnVTable->end(); } -void cmsScreenWrite(uint8_t x, uint8_t y, char *s) +int cmsScreenWrite(uint8_t x, uint8_t y, char *s) { - pScreenFnVTable->write(x, y, s); + return pScreenFnVTable->write(x, y, s); } void cmsScreenHeartBeat(void) @@ -87,7 +142,7 @@ void cmsScreenResync(void) if (pScreenFnVTable->resync) pScreenFnVTable->resync(); - pScreenFnVTable->getsize(&cmsRows, &cmsCols); + pScreenFnVTable->getDevParam(&cmsRows, &cmsCols, &cmsBuftime, &cmsBufsize); } void cmsScreenInit(void) @@ -117,58 +172,8 @@ pScreenFnVTable = canvasInit(); #define KEY_RIGHT 4 #define KEY_ESC 5 -#define curr_profile masterConfig.profile[masterConfig.current_profile_index] - -//osd current screen - to reduce long lines ;-) -#define OSD_cfg masterConfig.osdProfile - -#if 0 -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? - -static uint16_t flyTime = 0; -uint8_t statRssi; - -statistic_t stats; -#endif - -#define BUTTON_TIME 2 -#define BUTTON_PAUSE 5 - -// 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 -// -// Right column size be 5 chars??? (now 7) - -#define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN 23 - -//#define MAX_MENU_ITEMS (cmsGetRowsCount() - 2) -#define MAX_MENU_ITEMS (cmsRows - 2) +#define BUTTON_TIME 250 // msec +#define BUTTON_PAUSE 500 // msec //uint8_t armState; uint8_t featureBlackbox = 0; @@ -182,21 +187,10 @@ 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; -int8_t lastMenuPos; -uint8_t currentMenuIdx = 0; -uint16_t *currentElement = NULL; - #ifdef OSD OSD_Entry menuAlarms[]; OSD_Entry menuOsdLayout[]; OSD_Entry menuOsdActiveElems[]; -#if 0 // Not supported yet (or drop GUI position editing) -OSD_Entry menuOsdElemsPositions[]; -#endif #endif OSD_Entry menuFeatures[]; @@ -218,44 +212,44 @@ OSD_Entry menuMisc[]; OSD_Entry menuMain[] = { - {"----MAIN MENU----", OME_Label, NULL, NULL, true}, + {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, + {"CFG. IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], 0}, + {"FEATURES", OME_Submenu, cmsChangeScreen, &menuFeatures[0], 0}, #ifdef OSD - {"SCREEN LAYOUT", OME_Submenu, cmsChangeScreen, &menuOsdLayout[0], true}, - {"ALARMS", OME_Submenu, cmsChangeScreen, &menuAlarms[0], true}, + {"SCR LAYOUT", OME_Submenu, cmsChangeScreen, &menuOsdLayout[0], 0}, + {"ALARMS", OME_Submenu, cmsChangeScreen, &menuAlarms[0], 0}, #endif - {"CFG. IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], true}, - {"FEATURES", OME_Submenu, cmsChangeScreen, &menuFeatures[0], true}, - {"SAVE & EXIT", OME_OSD_Exit, cmsExitMenu, (void*)1, true}, - {"EXIT", OME_OSD_Exit, cmsExitMenu, (void*)0, true}, - {NULL,OME_END, NULL, NULL, true} + {"SAVE&REBOOT", OME_OSD_Exit, cmsExitMenu, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsExitMenu, (void*)0, 0}, + {NULL,OME_END, NULL, NULL, 0} }; OSD_Entry menuFeatures[] = { - {"----- FEATURES -----", OME_Label, NULL, NULL, true}, - {"BLACKBOX", OME_Submenu, cmsChangeScreen, &menuBlackbox[0], true}, -#ifdef LED_STRIP - {"LED STRIP", OME_Submenu, cmsChangeScreen, &menuLedstrip[0], true}, -#endif // LED_STRIP + {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, + {"BLACKBOX", OME_Submenu, cmsChangeScreen, &menuBlackbox[0], 0}, #if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, cmsChangeScreen, &menu_vtx[0], true}, + {"VTX", OME_Submenu, cmsChangeScreen, &menu_vtx[0], 0}, #endif // VTX || USE_RTC6705 - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} +#ifdef LED_STRIP + {"LED STRIP", OME_Submenu, cmsChangeScreen, &menuLedstrip[0], 0}, +#endif // LED_STRIP + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; OSD_Entry menuBlackbox[] = { - {"--- BLACKBOX ---", OME_Label, NULL, NULL, true}, - {"ENABLED", OME_Bool, NULL, &featureBlackbox, true}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, true}, + {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &featureBlackbox, 0}, + {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0}, #ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, cmsEraseFlash, NULL, true}, + {"ERASE FLASH", OME_Submenu, cmsEraseFlash, NULL, 0}, #endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; #ifdef LED_STRIP @@ -263,25 +257,25 @@ OSD_Entry menuBlackbox[] = 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" + "BLACK ", + "WHITE ", + "RED ", + "ORANGE ", + "YELLOW ", + "LIME GRN", + "GREEN ", + "MINT GRN", + "CYAN ", + "LT BLUE ", + "BLUE ", + "DK VIOLT", + "MAGENTA ", + "DEEP PNK" }; //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) +static void getLedColor(void) { for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; @@ -310,11 +304,11 @@ OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; OSD_Entry menuLedstrip[] = { - {"--- LED TRIP ---", OME_Label, NULL, NULL, true}, - {"ENABLED", OME_Bool, NULL, &featureLedstrip, true}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &featureLedstrip, 0}, + {"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; #endif // LED_STRIP @@ -337,19 +331,19 @@ OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; OSD_Entry menu_vtx[] = { - {"--- VTX ---", OME_Label, NULL, NULL, true}, - {"ENABLED", OME_Bool, NULL, &featureVtx, true}, + {"--- VTX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &featureVtx, 0}, #ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, true}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, true}, + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, #endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand, true}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, true}, + {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, #ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, true}, + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0}, #endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; #endif // VTX || USE_RTC6705 @@ -363,30 +357,30 @@ OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, OSD_Entry menuMisc[]= { - {"----- MISC -----", OME_Label, NULL, NULL, true}, - {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz, true}, - {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, true}, - {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, true}, - {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit, true}, - {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle, true}, - {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, true}, - {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"--- MISC ---", OME_Label, NULL, NULL, 0}, + {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, + {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, 0}, + {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, 0}, + {"YAW P LIM", OME_UINT16, NULL, &entryYawPLimit, 0}, + {"MIN THR", OME_UINT16, NULL, &entryMinThrottle, 0}, + {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, 0}, + {"VBAT CLMAX", OME_UINT8, NULL, &entryVbatMaxCell, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; OSD_Entry menuImu[] = { - {"-----CFG. IMU-----", OME_Label, NULL, NULL, true}, - {"PID", OME_Submenu, cmsChangeScreen, &menuPid[0], true}, - {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile, true}, - {"RATE & RXPO", OME_Submenu, cmsChangeScreen, &menuRateExpo[0], true}, - {"RC PREVIEW", OME_Submenu, cmsChangeScreen, &menuRc[0], true}, - {"MISC", OME_Submenu, cmsChangeScreen, &menuMisc[0], true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"--- CFG. IMU ---", OME_Label, NULL, NULL, 0}, + {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, + {"PID", OME_Submenu, cmsChangeScreen, &menuPid[0], 0}, + {"RATE&RXPO", OME_Submenu, cmsChangeScreen, &menuRateExpo[0], 0}, + {"RC PREV", OME_Submenu, cmsChangeScreen, &menuRc[0], 0}, + {"MISC", OME_Submenu, cmsChangeScreen, &menuMisc[0], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; uint8_t tempPid[4][3]; @@ -405,21 +399,21 @@ static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1}; OSD_Entry menuPid[] = { - {"------- PID -------", OME_Label, NULL, NULL, true}, - {"ROLL P", OME_UINT8, NULL, &entryRollP, true}, - {"ROLL I", OME_UINT8, NULL, &entryRollI, true}, - {"ROLL D", OME_UINT8, NULL, &entryRollD, true}, + {"--- PID ---", OME_Label, NULL, NULL, 0}, + {"ROLL P", OME_UINT8, NULL, &entryRollP, 0}, + {"ROLL I", OME_UINT8, NULL, &entryRollI, 0}, + {"ROLL D", OME_UINT8, NULL, &entryRollD, 0}, - {"PITCH P", OME_UINT8, NULL, &entryPitchP, true}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI, true}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD, true}, + {"PITCH P", OME_UINT8, NULL, &entryPitchP, 0}, + {"PITCH I", OME_UINT8, NULL, &entryPitchI, 0}, + {"PITCH D", OME_UINT8, NULL, &entryPitchD, 0}, - {"YAW P", OME_UINT8, NULL, &entryYawP, true}, - {"YAW I", OME_UINT8, NULL, &entryYawI, true}, - {"YAW D", OME_UINT8, NULL, &entryYawD, true}, + {"YAW P", OME_UINT8, NULL, &entryYawP, 0}, + {"YAW I", OME_UINT8, NULL, &entryYawI, 0}, + {"YAW D", OME_UINT8, NULL, &entryYawD, 0}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; controlRateConfig_t rateProfile; @@ -428,6 +422,7 @@ 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 entryRcYawRate = {&rateProfile.rcYawRate8, 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}; @@ -437,19 +432,20 @@ static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSe OSD_Entry menuRateExpo[] = { - {"----RATE & EXPO----", OME_Label, NULL, NULL, true}, - {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate, true}, - {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate, true}, - {"YAW RATE", OME_FLOAT, NULL, &entryYawRate, true}, - {"RC RATE", OME_FLOAT, NULL, &entryRcRate, true}, - {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, true}, - {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, true}, - {"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, true}, - {"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, true}, - {"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, true}, - {"D SETPT TRNS", OME_FLOAT, NULL, &entryPSetpoint, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0}, + {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0}, + {"RC YAW RATE", OME_FLOAT, NULL, &entryRcRate, 0}, + {"ROLL SUPER", OME_FLOAT, NULL, &entryRollRate, 0}, + {"PITCH SUPER", OME_FLOAT, NULL, &entryPitchRate, 0}, + {"YAW SUPER", OME_FLOAT, NULL, &entryYawRate, 0}, + {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, 0}, + {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, 0}, + {"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, 0}, + {"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, 0}, + {"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, 0}, + {"D SETPT TRN", OME_FLOAT, NULL, &entryPSetpoint, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; @@ -463,26 +459,22 @@ static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0}; OSD_Entry menuRc[] = { - {"---- RC PREVIEW ----", OME_Label, NULL, NULL, true}, - {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, true}, - {"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, true}, - {"THROTTLE", OME_Poll_INT16, NULL, &entryRcThrottle, true}, - {"YAW", OME_Poll_INT16, NULL, &entryRcYaw, true}, - {"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, true}, - {"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, true}, - {"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, true}, - {"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"--- RC PREV ---", OME_Label, NULL, NULL, 0}, + {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0}, + {"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, 0}, + {"THROTTLE", OME_Poll_INT16, NULL, &entryRcThrottle, 0}, + {"YAW", OME_Poll_INT16, NULL, &entryRcYaw, 0}, + {"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, 0}, + {"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, 0}, + {"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, 0}, + {"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; bool cmsInMenu = false; -// -// CMS specific functions -// - void cmsUpdateMaxRows(void) { OSD_Entry *ptr; @@ -497,9 +489,9 @@ void cmsUpdateMaxRows(void) currentMenuIdx--; } -uint8_t cmsHandleKey(uint8_t key) +uint16_t cmsHandleKey(uint8_t key) { - uint8_t res = BUTTON_TIME; + uint16_t res = BUTTON_TIME; OSD_Entry *p; if (!currentMenu) @@ -511,8 +503,8 @@ uint8_t cmsHandleKey(uint8_t key) } if (key == KEY_DOWN) { - if (currentMenuPos < currentMenuIdx) { - currentMenuPos++; + if (currentCursorPos < currentMenuIdx) { + currentCursorPos++; } else { if (nextPage) // we have more pages { @@ -520,33 +512,30 @@ uint8_t cmsHandleKey(uint8_t key) p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - lastMenuPos = -1; + currentCursorPos = 0; cmsUpdateMaxRows(); } else { - currentMenuPos = 0; + currentCursorPos = 0; } } } if (key == KEY_UP) { - currentMenuPos--; + currentCursorPos--; - if ((currentMenu + currentMenuPos)->type == OME_Label && currentMenuPos > 0) - currentMenuPos--; + if ((currentMenu + currentCursorPos)->type == OME_Label && currentCursorPos > 0) + currentCursorPos--; - if (currentMenuPos == -1 || (currentMenu + currentMenuPos)->type == OME_Label) { + if (currentCursorPos == -1 || (currentMenu + currentCursorPos)->type == OME_Label) { if (nextPage) { cmsScreenClear(); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - lastMenuPos = -1; + currentCursorPos = 0; cmsUpdateMaxRows(); } else { - currentMenuPos = currentMenuIdx; - // lastMenuPos = -1; + currentCursorPos = currentMenuIdx; } } } @@ -554,22 +543,9 @@ uint8_t cmsHandleKey(uint8_t key) if (key == KEY_DOWN || key == KEY_UP) return res; - p = currentMenu + currentMenuPos; + p = currentMenu + currentCursorPos; switch (p->type) { - case OME_POS: -#if 0 -#ifdef OSD - 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; - } -#endif -#endif case OME_Submenu: case OME_OSD_Exit: if (p->func && key == KEY_RIGHT) { @@ -588,7 +564,7 @@ uint8_t cmsHandleKey(uint8_t key) *val = 1; else *val = 0; - p->changed = true; + SET_PRINTVALUE(p); } break; case OME_VISIBLE: @@ -603,7 +579,7 @@ uint8_t cmsHandleKey(uint8_t key) *val |= VISIBLE_FLAG; else *val %= ~VISIBLE_FLAG; - p->changed = true; + SET_PRINTVALUE(p); } #endif break; @@ -619,7 +595,7 @@ uint8_t cmsHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } - p->changed = true; + SET_PRINTVALUE(p); } break; case OME_TAB: @@ -636,7 +612,7 @@ uint8_t cmsHandleKey(uint8_t key) } if (p->func) p->func(p->data); - p->changed = true; + SET_PRINTVALUE(p); } break; case OME_INT8: @@ -650,7 +626,7 @@ uint8_t cmsHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } - p->changed = true; + SET_PRINTVALUE(p); } break; case OME_UINT16: @@ -664,7 +640,7 @@ uint8_t cmsHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } - p->changed = true; + SET_PRINTVALUE(p); } break; case OME_INT16: @@ -678,7 +654,7 @@ uint8_t cmsHandleKey(uint8_t key) if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } - p->changed = true; + SET_PRINTVALUE(p); } break; case OME_Poll_INT16: @@ -689,7 +665,7 @@ uint8_t cmsHandleKey(uint8_t key) return res; } -static void simple_ftoa(int32_t value, char *floatString) +static void cmsFtoa(int32_t value, char *floatString) { uint8_t k; // np. 3450 @@ -715,111 +691,124 @@ static void simple_ftoa(int32_t value, char *floatString) floatString[0] = ' '; } -void cmsDrawMenuEntry(OSD_Entry *p, uint8_t row, bool drawPolled) +void cmsPad(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; +} + +int cmsDrawMenuEntry(OSD_Entry *p, uint8_t row, bool drawPolled) { char buff[10]; + int cnt = 0; switch (p->type) { - case OME_POS:; // Semi-colon required to add an empty statement -#ifdef OSD - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - if (!(*val & VISIBLE_FLAG)) - break; -#endif - case OME_Submenu: - if (cmsScreenCleared) - cmsScreenWrite(RIGHT_MENU_COLUMN, row, ">"); + if (IS_PRINTVALUE(p)) { + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, ">"); + CLR_PRINTVALUE(p); + } break; case OME_Bool: - if ((p->changed || cmsScreenCleared) && p->data) { + if (IS_PRINTVALUE(p) && p->data) { if (*((uint8_t *)(p->data))) { - cmsScreenWrite(RIGHT_MENU_COLUMN, row, "YES"); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, "YES"); } else { - cmsScreenWrite(RIGHT_MENU_COLUMN, row, "NO "); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, "NO "); } - p->changed = false; + CLR_PRINTVALUE(p); } break; case OME_TAB: { - if (p->changed || cmsScreenCleared) { + if (IS_PRINTVALUE(p)) { OSD_TAB_t *ptr = p->data; - cmsScreenWrite(RIGHT_MENU_COLUMN - 5, row, (char *)ptr->names[*ptr->val]); - p->changed = false; + //cnt = cmsScreenWrite(RIGHT_MENU_COLUMN - 5, row, (char *)ptr->names[*ptr->val]); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, (char *)ptr->names[*ptr->val]); + CLR_PRINTVALUE(p); } break; } case OME_VISIBLE: #ifdef OSD - if ((p->changed || cmsScreenCleared) && p->data) { + if (IS_PRINTVALUE(p) && p->data) { uint32_t address = (uint32_t)p->data; uint16_t *val; val = (uint16_t *)address; if (VISIBLE(*val)) { - cmsScreenWrite(RIGHT_MENU_COLUMN, row, "YES"); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, "YES"); } else { - cmsScreenWrite(RIGHT_MENU_COLUMN, row, "NO "); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, "NO "); } - p->changed = false; + CLR_PRINTVALUE(p); } #endif break; case OME_UINT8: - if ((p->changed || cmsScreenCleared) && p->data) { + if (IS_PRINTVALUE(p) && p->data) { OSD_UINT8_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); - p->changed = false; + cmsPad(buff, 5); + //cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, " "); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); + CLR_PRINTVALUE(p); } break; case OME_INT8: - if ((p->changed || cmsScreenCleared) && p->data) { + if (IS_PRINTVALUE(p) && p->data) { OSD_INT8_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); - p->changed = false; + cmsPad(buff, 5); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); + CLR_PRINTVALUE(p); } break; case OME_UINT16: - if ((p->changed || cmsScreenCleared) && p->data) { + if (IS_PRINTVALUE(p) && p->data) { OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); - p->changed = false; + cmsPad(buff, 5); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); + CLR_PRINTVALUE(p); } break; case OME_INT16: - if ((p->changed || cmsScreenCleared) && p->data) { + if (IS_PRINTVALUE(p) && p->data) { OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); - p->changed = false; + cmsPad(buff, 5); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); + CLR_PRINTVALUE(p); } break; case OME_Poll_INT16: if (p->data && drawPolled) { OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); + cmsPad(buff, 5); + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); + // PRINTVALUE not cleared on purpose } break; case OME_FLOAT: - if ((p->changed || cmsScreenCleared) && p->data) { + if (IS_PRINTVALUE(p) && p->data) { OSD_FLOAT_t *ptr = p->data; - simple_ftoa(*ptr->val * ptr->multipler, buff); - cmsScreenWrite(RIGHT_MENU_COLUMN - 1, row, " "); - cmsScreenWrite(RIGHT_MENU_COLUMN - 1, row, buff); - p->changed = false; + cmsFtoa(*ptr->val * ptr->multipler, buff); + cmsPad(buff, 5); + // XXX One char left ??? + cnt = cmsScreenWrite(RIGHT_MENU_COLUMN - 1, row, buff); + CLR_PRINTVALUE(p); } break; case OME_OSD_Exit: @@ -828,79 +817,108 @@ void cmsDrawMenuEntry(OSD_Entry *p, uint8_t row, bool drawPolled) case OME_Back: break; } + + return cnt; } -void cmsDrawMenu(void) +void cmsDrawMenu(uint32_t currentTime) { - uint8_t i = 0; + UNUSED(currentTime); + + uint8_t i; OSD_Entry *p; uint8_t top = (cmsRows - currentMenuIdx) / 2 - 1; + // Polled (dynamic) value display denominator. // XXX Need to denom based on absolute time static uint8_t pollDenom = 0; bool drawPolled = (++pollDenom % 8 == 0); + int16_t cnt = 0; + if (!currentMenu) return; - if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label - currentMenuPos++; - - if (lastMenuPos >= 0 && currentMenuPos != lastMenuPos) - cmsScreenWrite(LEFT_MENU_COLUMN, lastMenuPos + top, " "); - - for (p = currentMenu; p->type != OME_END; p++) { - - if (currentMenuPos == i && lastMenuPos != currentMenuPos) { - cmsScreenWrite(LEFT_MENU_COLUMN, i + top, " >"); - lastMenuPos = currentMenuPos; + if (cmsScreenCleared) { + for (p = currentMenu, i= 0; p->type != OME_END; p++, i++) { + SET_PRINTLABEL(p); + SET_PRINTVALUE(p); } - if (cmsScreenCleared) - cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text); - - cmsDrawMenuEntry(p, top + i, drawPolled); - - i++; - - if (i == MAX_MENU_ITEMS) // max per page + if (i > MAX_MENU_ITEMS) // max per page { - nextPage = currentMenu + i; + nextPage = currentMenu + MAX_MENU_ITEMS; if (nextPage->type == OME_END) nextPage = NULL; - break; + } + + cmsScreenCleared = false; + } + + // Cursor manipulation + + if ((currentMenu + currentCursorPos)->type == OME_Label) // skip label + currentCursorPos++; + + if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) { + cnt += cmsScreenWrite(LEFT_MENU_COLUMN, lastCursorPos + top, " "); + } + + if (lastCursorPos != currentCursorPos) { + cnt += cmsScreenWrite(LEFT_MENU_COLUMN, currentCursorPos + top, " >"); + lastCursorPos = currentCursorPos; + } + + // Print text labels + for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS && p->type != OME_END; i++, p++) { + if (IS_PRINTLABEL(p)) { + cnt += cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text); + CLR_PRINTLABEL(p); + if (cnt > cmsBatchsize) + return; + } + } + + // Print values + + // XXX Polled values at latter positions in the list may not be + // XXX printed if the cnt exceeds cmsBatchsize in the middle of the list. + + for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS && p->type != OME_END; i++, p++) { + if (IS_PRINTVALUE(p)) { + cnt += cmsDrawMenuEntry(p, top + i, drawPolled); + if (cnt > cmsBatchsize) + return; } } - cmsScreenCleared = false; } void cmsChangeScreen(void *ptr) { uint8_t i; if (ptr) { - cmsScreenClear(); // 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[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i]; + tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i]; + tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].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]; + tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL]; + tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL]; + tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].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; + menuStackHistory[menuStackIdx] = currentCursorPos; menuStackIdx++; currentMenu = (OSD_Entry *)ptr; - currentMenuPos = 0; - lastMenuPos = -1; // XXX this? + currentCursorPos = 0; + cmsScreenClear(); cmsUpdateMaxRows(); } } @@ -913,14 +931,14 @@ void cmsMenuBack(void) // 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]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1]; + masterConfig.profile[masterConfig.current_profile_index].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]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; } // hack - save rate config for current profile @@ -932,14 +950,10 @@ void cmsMenuBack(void) menuStackIdx--; nextPage = NULL; currentMenu = menuStack[menuStackIdx]; - currentMenuPos = menuStackHistory[menuStackIdx]; - lastMenuPos = -1; + currentCursorPos = menuStackHistory[menuStackIdx]; cmsUpdateMaxRows(); } - else { - cmsOpenMenu(); - } } void cmsOpenMenu(void) @@ -947,15 +961,11 @@ void cmsOpenMenu(void) if (cmsInMenu) return; - if (feature(FEATURE_LED_STRIP)) - featureLedstrip = 1; - - if (feature(FEATURE_BLACKBOX)) - featureBlackbox = 1; + featureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; + featureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; #if defined(VTX) || defined(USE_RTC6705) - if (feature(FEATURE_VTX)) - featureVtx = 1; + featureVtx = feature(FEATURE_VTX) ? 1 : 0; #endif // VTX || USE_RTC6705 #ifdef VTX @@ -972,27 +982,28 @@ void cmsOpenMenu(void) getLedColor(); #endif // LED_STRIP - // cmsRows = cmsGetRowsCount(); - cmsGetSize(&cmsRows, &cmsCols); + cmsGetDevParam(&cmsRows, &cmsCols, &cmsBuftime, &cmsBufsize); + + cmsBatchsize = (cmsBuftime < CMS_UPDATE_INTERVAL) ? cmsBufsize : (cmsBufsize * CMS_UPDATE_INTERVAL) / cmsBuftime; + cmsInMenu = true; cmsScreenBegin(); - cmsScreenClear(); currentMenu = &menuMain[0]; cmsChangeScreen(currentMenu); } void cmsExitMenu(void *ptr) { - cmsScreenClear(); - - cmsScreenWrite(5, 3, "RESTARTING IMU..."); - cmsScreenResync(); // Was max7456RefreshAll(); why at this timing? - - stopMotors(); - stopPwmAllMotors(); - delay(200); - if (ptr) { + cmsScreenClear(); + + cmsScreenWrite(5, 3, "RESTARTING IMU..."); + cmsScreenResync(); // Was max7456RefreshAll(); why at this timing? + + stopMotors(); + stopPwmAllMotors(); + delay(200); + // save local variables to configuration if (featureBlackbox) featureSet(FEATURE_BLACKBOX); @@ -1026,24 +1037,31 @@ void cmsExitMenu(void *ptr) cmsScreenEnd(); - systemReset(); + if (ptr) + systemReset(); } void cmsUpdate(uint32_t currentTime) { - static uint8_t rcDelay = BUTTON_TIME; - uint8_t key = 0; + static int16_t rcDelay = BUTTON_TIME; + static uint32_t lastCalled = 0; static uint32_t lastCmsHeartBeat = 0; - // detect enter to menu - if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + uint8_t key = 0; + + // Detect menu invocation + if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { // XXX Double enter!? cmsOpenMenu(); + rcDelay = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME + lastCalled = currentTime; + return; } if (cmsInMenu) { - if (rcDelay) { - rcDelay--; + if (rcDelay > 0) { + rcDelay -= currentTime - lastCalled; + debug[0] = rcDelay; } else if (IS_HI(PITCH)) { key = KEY_UP; @@ -1067,16 +1085,14 @@ void cmsUpdate(uint32_t currentTime) rcDelay = BUTTON_TIME; } - // XXX Element position adjustment is hard to separate. - // XXX May need to drop it upon real separation. - // XXX Don't know if this still works + lastCalled = currentTime; if (key && !currentElement) { rcDelay = cmsHandleKey(key); return; } - cmsDrawMenu(); + cmsDrawMenu(currentTime); if (currentTime > lastCmsHeartBeat + 500) { // Heart beat for external CMS display device @ 500msec @@ -1087,12 +1103,16 @@ void cmsUpdate(uint32_t currentTime) } } -void cmsHandler(uint32_t currentTime) +void cmsHandler(uint32_t unusedTime) { - static uint32_t counter = 0; + UNUSED(unusedTime); - if (counter++ % 5 == 0) { - cmsUpdate(currentTime); + static uint32_t lastCalled = 0; + uint32_t now = millis(); + + if (now - lastCalled >= CMS_UPDATE_INTERVAL) { + cmsUpdate(now); + lastCalled = now; } // do not allow ARM if we are in menu @@ -1129,85 +1149,57 @@ void cmsEraseFlash(void *ptr) #ifdef OSD // // OSD specific menu pages and items -// XXX Should be part of the osd.c, or new osd_csm.c. +// XXX Should be part of the osd.c, or new osd_cms.c. // OSD_Entry menuOsdLayout[] = { - {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, true}, - {"ACTIVE ELEM.", OME_Submenu, cmsChangeScreen, &menuOsdActiveElems[0], true}, -#if 0 - {"POSITION", OME_Submenu, cmsChangeScreen, &menuOsdElemsPositions[0], true}, -#endif - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, + {"ACTIVE ELEM.", OME_Submenu, cmsChangeScreen, &menuOsdActiveElems[0], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; -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_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 menuAlarms[] = { - {"------ ALARMS ------", OME_Label, NULL, NULL, true}, - {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, true}, - {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity, true}, - {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, true}, - {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude, true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"--- 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} }; -#if 0 // Not supported yet (or drop support for GUI position editing) -OSD_Entry menuOsdElemsPositions[] = -{ - {"---POSITION---", OME_Label, NULL, NULL, true}, - {"RSSI", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_RSSI_VALUE], true}, - {"MAIN BATTERY", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, - {"UPTIME", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_ONTIME], true}, - {"FLY TIME", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_FLYTIME], true}, - {"FLY MODE", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_FLYMODE], true}, - {"NAME", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_CRAFT_NAME], true}, - {"THROTTLE", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_THROTTLE_POS], true}, - -#ifdef VTX - {"VTX CHAN", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_VTX_CHANNEL], true}, -#endif // VTX - {"CURRENT (A)", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_CURRENT_DRAW], true}, - {"USED MAH", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_MAH_DRAWN], true}, -#ifdef GPS - {"GPS SPEED", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_GPS_SPEED], true}, - {"GPS SATS.", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_GPS_SATS], true}, -#endif // GPS - {"ALTITUDE", OME_POS, osdEditElement, &OSD_cfg.item_pos[OSD_ALTITUDE], true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} -}; -#endif - OSD_Entry menuOsdActiveElems[] = { - {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL, true}, - {"RSSI", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_RSSI_VALUE], true}, - {"MAIN BATTERY", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE], true}, - {"HORIZON", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON], true}, - {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_HORIZON_SIDEBARS], true}, - {"UPTIME", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_ONTIME], true}, - {"FLY TIME", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_FLYTIME], true}, - {"FLY MODE", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_FLYMODE], true}, - {"NAME", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_CRAFT_NAME], true}, - {"THROTTLE", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_THROTTLE_POS], true}, + {"--- 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, &OSD_cfg.item_pos[OSD_VTX_CHANNEL]}, + {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, #endif // VTX - {"CURRENT (A)", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_CURRENT_DRAW], true}, - {"USED MAH", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_MAH_DRAWN], true}, + {"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, &OSD_cfg.item_pos[OSD_GPS_SPEED], true}, - {"GPS SATS.", OME_VISIBLE, NULL, &OSD_cfg.item_pos[OSD_GPS_SATS], true}, + {"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, &OSD_cfg.item_pos[OSD_ALTITUDE], true}, - {"BACK", OME_Back, NULL, NULL, true}, - {NULL, OME_END, NULL, NULL, true} + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} }; #endif + +#endif // CMS diff --git a/src/main/io/cms.h b/src/main/io/cms.h index ed54214f42..382c724dab 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -11,3 +11,7 @@ void cmsScreenResync(void); void cmsChangeScreen(void * ptr); void cmsExitMenu(void * ptr); + +#define STARTUP_HELP_TEXT1 "MENU: THR MID" +#define STARTUP_HELP_TEXT2 "+ YAW LEFT" +#define STARTUP_HELP_TEXT3 "+ PITCH UP" diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index e7950c2d18..8c35b4a185 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -23,7 +23,6 @@ typedef enum 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; @@ -34,9 +33,21 @@ typedef struct OSD_MenuElement type; OSDMenuFuncPtr func; void *data; - bool changed; + 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 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; } + typedef struct { uint8_t *val; @@ -86,11 +97,11 @@ typedef struct } OSD_TAB_t; typedef struct screenFnVTable_s { - void (*getsize)(uint8_t *, uint8_t *); - void (*begin)(void); - void (*end)(void); - void (*clear)(void); - void (*write)(uint8_t, uint8_t, char *); - void (*heartbeat)(void); + void (*getDevParam)(uint8_t *, uint8_t *, uint16_t *, uint16_t *); + int (*begin)(void); + int (*end)(void); + int (*clear)(void); + int (*write)(uint8_t, uint8_t, char *); + int (*heartbeat)(void); void (*resync)(void); } screenFnVTable_t; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 55432bcb9f..0ed87081d2 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 a9ed93f6c8..2691e44638 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -53,9 +53,6 @@ #include "config/config_master.h" #include "config/feature.h" -// Short hands -#define OSD_cfg masterConfig.osdProfile - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -80,17 +77,8 @@ extern serialPort_t *debugSerialPort; #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) -//#define VISIBLE_FLAG 0x0800 // defined in osd.h -#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) // defined in osd.h -#define BLINK(x) ((x & BLINK_FLAG) && blinkState) -#define BLINK_OFF(x) (x & ~BLINK_FLAG) - //extern uint8_t RSSI; // TODO: not used? static uint16_t flyTime = 0; @@ -158,11 +146,11 @@ void osdDrawElements(void) void osdDrawSingleElement(uint8_t item) { - if (!VISIBLE(OSD_cfg.item_pos[item]) || BLINK(OSD_cfg.item_pos[item])) + if (!VISIBLE(masterConfig.osdProfile.item_pos[item]) || BLINK(masterConfig.osdProfile.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) { @@ -396,7 +384,7 @@ void osdInit(void) armState = ARMING_FLAG(ARMED); - max7456Init(OSD_cfg.video_system); + max7456Init(masterConfig.osdProfile.video_system); max7456ClearScreen(); @@ -413,9 +401,9 @@ void osdInit(void) 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"); + max7456Write(7, 7, STARTUP_HELP_TEXT1); + max7456Write(12, 8, STARTUP_HELP_TEXT2); + max7456Write(12, 9, STARTUP_HELP_TEXT3); cmsScreenResync(); // Was max7456RefreshAll(); may be okay. @@ -427,7 +415,7 @@ void osdInit(void) */ char osdGetAltitudeSymbol() { - switch (OSD_cfg.units) { + switch (masterConfig.osdProfile.units) { case OSD_UNIT_IMPERIAL: return 0xF; default: @@ -441,7 +429,7 @@ char osdGetAltitudeSymbol() */ int32_t osdGetAltitude(int32_t alt) { - switch (OSD_cfg.units) { + switch (masterConfig.osdProfile.units) { case OSD_UNIT_IMPERIAL: return (alt * 328) / 100; // Convert to feet / 100 default: @@ -454,44 +442,44 @@ void osdUpdateAlarms(void) 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; + if (statRssi < masterConfig.osdProfile.rssi_alarm) + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; else - OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; if (vbat <= (batteryWarningVoltage - 1)) - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; else - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; if (STATE(GPS_FIX) == 0) - OSD_cfg.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; else - OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + masterConfig.osdProfile.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; + if (flyTime / 60 >= masterConfig.osdProfile.time_alarm && ARMING_FLAG(ARMED)) + masterConfig.osdProfile.item_pos[OSD_FLYTIME] |= BLINK_FLAG; else - OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - if (mAhDrawn >= OSD_cfg.cap_alarm) - OSD_cfg.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; + if (mAhDrawn >= masterConfig.osdProfile.cap_alarm) + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; else - OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; - if (alt >= OSD_cfg.alt_alarm) - OSD_cfg.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; + if (alt >= masterConfig.osdProfile.alt_alarm) + masterConfig.osdProfile.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; else - OSD_cfg.item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; + masterConfig.osdProfile.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; + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; } void osdResetStats(void) @@ -654,20 +642,42 @@ void osdUpdate(uint32_t currentTime) // OSD specific CMS functions // -void osdGetSize(uint8_t *pRows, uint8_t *pCols) +void osdGetDevParam(uint8_t *pRows, uint8_t *pCols, uint16_t *pBuftime, uint16_t *pBufsize) { *pRows = max7456GetRowsCount(); *pCols = 30; + *pBuftime = 1; // Very fast + *pBufsize = 50000; // Very large } -void osdMenuBegin(void) { +int osdMenuBegin(void) +{ osdResetAlarms(); osdInMenu = true; refreshTimeout = 0; + + return 0; } -void osdMenuEnd(void) { +int osdMenuEnd(void) +{ osdInMenu = false; + + return 0; +} + +int osdClearScreen(void) +{ + max7456ClearScreen(); + + return 0; +} + +int osdWrite(uint8_t x, uint8_t y, char *s) +{ + max7456Write(x, y, s); + + return 0; } #ifdef EDIT_ELEMENT_SUPPORT @@ -688,20 +698,20 @@ void osdEditElement(void *ptr) 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 "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 1, "USE ROLL/PITCH"); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 2, "TO MOVE ELEM. "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 3, " "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); } #endif screenFnVTable_t osdVTable = { - osdGetSize, + osdGetDevParam, osdMenuBegin, osdMenuEnd, - max7456ClearScreen, - max7456Write, + osdClearScreen, + osdWrite, NULL, max7456RefreshAll, }; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 150786a578..0c5f29c5f8 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -17,8 +17,6 @@ #pragma once -#include - typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -70,5 +68,13 @@ void osdInit(void); void resetOsdConfig(osd_profile_t *osdProfile); screenFnVTable_t *osdCmsInit(void); -#define VISIBLE_FLAG 0x0800 +// 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) +#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) diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 0a8e5ffc1b..e10f13555d 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -65,8 +65,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 8e8f908413..1ea6c579b1 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -36,9 +36,6 @@ #include "io/gps.h" #include "io/gimbal.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" From 761e1c5bf261bbfcb4d924e52bc42d0934816e5f Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 25 Oct 2016 17:28:56 +0900 Subject: [PATCH 09/19] Add OLED CMS support --- src/main/fc/fc_tasks.c | 3 +- src/main/fc/rc_controls.c | 2 + src/main/io/cms.c | 13 ++++-- src/main/io/display.c | 68 ++++++++++++++++++++++++++++++++ src/main/io/display.h | 2 + src/main/io/gps.c | 2 + src/main/main.c | 3 +- src/main/target/OMNIBUS/target.h | 4 +- 8 files changed, 91 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index cb20e0555d..c8ab268d37 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -39,11 +39,12 @@ #include "flight/pid.h" #include "flight/altitudehold.h" +#include "io/cms_types.h" + #include "io/beeper.h" #include "io/display.h" #include "io/gps.h" #include "io/ledstrip.h" -#include "io/cms_types.h" #include "io/cms.h" #include "io/osd.h" #include "io/serial.h" diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 314ee07ecd..c63c976aa0 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -47,6 +47,8 @@ #include "rx/rx.h" +#include "io/cms_types.h" + #include "io/gps.h" #include "io/beeper.h" #include "io/motors.h" diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 83cdcb1aee..d2bfc441f7 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -40,6 +40,7 @@ #include "io/flashfs.h" #include "io/osd.h" +#include "io/display.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -61,7 +62,7 @@ void cmsChangeScreen(void *); void cmsMenuBack(void); void cmsEraseFlash(void *); -screenFnVTable_t *pScreenFnVTable; +screenFnVTable_t *pScreenFnVTable = NULL; uint8_t cmsRows; uint8_t cmsCols; @@ -91,8 +92,7 @@ uint16_t cmsBatchsize; // #define LEFT_MENU_COLUMN 1 -//#define RIGHT_MENU_COLUMN (cmsCols - 7) -#define RIGHT_MENU_COLUMN (cmsCols - 9 - 7) +#define RIGHT_MENU_COLUMN (cmsCols - 7) bool cmsScreenCleared; OSD_Entry *currentMenu; @@ -154,6 +154,10 @@ pScreenFnVTable = osdCmsInit(); #ifdef CANVAS pScreenFnVTable = canvasInit(); #endif + +#ifdef OLEDCMS +pScreenFnVTable = displayCMSInit(); +#endif } // @@ -1107,6 +1111,9 @@ void cmsHandler(uint32_t unusedTime) { UNUSED(unusedTime); + if (pScreenFnVTable == NULL) + return; + static uint32_t lastCalled = 0; uint32_t now = millis(); diff --git a/src/main/io/display.c b/src/main/io/display.c index f2bf158a4c..aec3466a1f 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -54,6 +54,8 @@ #include "flight/imu.h" #include "flight/failsafe.h" +#include "io/cms_types.h" + #ifdef GPS #include "io/gps.h" #include "flight/navigation.h" @@ -581,10 +583,19 @@ void showDebugPage(void) } #endif +#ifdef OLEDCMS +static bool displayInCMS = false; +#endif + void displayUpdate(uint32_t currentTime) { static uint8_t previousArmedState = 0; +#ifdef OLEDCMS + if (displayInCMS) + return; +#endif + const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L; if (!updateNow) { return; @@ -733,4 +744,61 @@ void displayDisablePageCycling(void) pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED; } +#ifdef OLEDCMS +#include "io/cms_types.h" + +void displayCMSGetDevParam(uint8_t *pRows, uint8_t *pCols, uint16_t *pBuftime, uint16_t *pBufsize) +{ + *pRows = 8; + *pCols = 21; + *pBuftime = 1; + *pBufsize = 50000; +} + +int displayCMSBegin(void) +{ + displayInCMS = true; + + return 0; +} + +int displayCMSEnd(void) +{ + displayInCMS = false; + + return 0; +} + +int displayCMSClear(void) +{ + i2c_OLED_clear_display_quick(); + + return 0; +} + +int displayCMSWrite(uint8_t x, uint8_t y, char *s) +{ + i2c_OLED_set_xy(x, y); + i2c_OLED_send_string(s); + + return 0; +} + +screenFnVTable_t displayCMSVTable = { + displayCMSGetDevParam, + displayCMSBegin, + displayCMSEnd, + displayCMSClear, + displayCMSWrite, + NULL, + NULL, +}; + +screenFnVTable_t *displayCMSInit(void) +{ + return &displayCMSVTable; +} + +#endif // OLEDCMS + #endif diff --git a/src/main/io/display.h b/src/main/io/display.h index 75abffbe3c..6b20100cdd 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -45,3 +45,5 @@ void displayEnablePageCycling(void); void displayDisablePageCycling(void); void displayResetPageCycling(void); void displaySetNextPageChangeAt(uint32_t futureMicros); + +screenFnVTable_t *displayCMSInit(void); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 59fca7deef..cdf82a3dec 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -41,6 +41,8 @@ #include "sensors/sensors.h" +#include "io/cms_types.h" + #include "io/serial.h" #include "io/display.h" #include "io/gps.h" diff --git a/src/main/main.c b/src/main/main.c index af2b46896e..08dfe07b1f 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -74,6 +74,8 @@ #include "rx/rx.h" #include "rx/spektrum.h" +#include "io/cms_types.h" + #include "io/beeper.h" #include "io/serial.h" #include "io/flashfs.h" @@ -86,7 +88,6 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" -#include "io/cms_types.h" #include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 9718b5bc97..570ffc3c6a 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -95,10 +95,12 @@ // Use external OSD to run CMS //#define CANVAS +#define OLEDCMS + // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk // include the osd code -#define OSD +//#define OSD // include the max7456 driver #define USE_MAX7456 From 2e2bac6da3d68d2d29b5de4dc5b210363b395908 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 25 Oct 2016 18:45:36 +0900 Subject: [PATCH 10/19] Prevent osd.c from calling cmsInit and cmsResync if CMS is not defined --- src/main/io/osd.c | 4 ++++ src/main/target/OMNIBUS/target.h | 5 +++-- src/main/target/SPRACINGF3/target.h | 5 ++++- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2691e44638..b398b9118e 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -397,7 +397,9 @@ void osdInit(void) } } +#ifdef CMS cmsInit(); +#endif sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); max7456Write(5, 6, string_buffer); @@ -405,7 +407,9 @@ void osdInit(void) max7456Write(12, 8, STARTUP_HELP_TEXT2); max7456Write(12, 9, STARTUP_HELP_TEXT3); +#ifdef CMS cmsScreenResync(); // Was max7456RefreshAll(); may be okay. +#endif refreshTimeout = 4 * REFRESH_1S; } diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 570ffc3c6a..d059b58d11 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -95,12 +95,13 @@ // Use external OSD to run CMS //#define CANVAS -#define OLEDCMS +// USE I2C OLED display to run CMS +//#define OLEDCMS // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk // include the osd code -//#define OSD +#define OSD // include the max7456 driver #define USE_MAX7456 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 3b888267c0..a00f3a84bc 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -134,4 +134,7 @@ #define CMS // Use external OSD to run CMS -#define CANVAS +//#define CANVAS + +// USE I2C OLED display to run CMS +#define OLEDCMS From e26258e6860c69d5470bc8a54c1b8caa6ba3cfd4 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 26 Oct 2016 19:02:15 +0900 Subject: [PATCH 11/19] Add info menu --- src/main/io/cms.c | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index d2bfc441f7..c8fad2b3de 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -214,15 +214,28 @@ OSD_Entry menuRc[]; OSD_Entry menuRateExpo[]; OSD_Entry menuMisc[]; +static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; +static char infoTargetName[] = __TARGET__; + +OSD_Entry menuInfo[] = { + { "--- INFO ---", OME_Label, NULL, NULL, 0 }, + { FC_VERSION_STRING, OME_Label, NULL, NULL, 0 }, + { infoGitRev, OME_Label, NULL, NULL, 0 }, + { infoTargetName, OME_Label, NULL, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + OSD_Entry menuMain[] = { {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, - {"CFG. IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], 0}, + {"CFG&IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], 0}, {"FEATURES", OME_Submenu, cmsChangeScreen, &menuFeatures[0], 0}, #ifdef OSD {"SCR LAYOUT", OME_Submenu, cmsChangeScreen, &menuOsdLayout[0], 0}, {"ALARMS", OME_Submenu, cmsChangeScreen, &menuAlarms[0], 0}, #endif + {"INFO", OME_Submenu, cmsChangeScreen, &menuInfo[0], 0}, {"SAVE&REBOOT", OME_OSD_Exit, cmsExitMenu, (void*)1, 0}, {"EXIT", OME_OSD_Exit, cmsExitMenu, (void*)0, 0}, {NULL,OME_END, NULL, NULL, 0} @@ -861,7 +874,7 @@ void cmsDrawMenu(uint32_t currentTime) // Cursor manipulation - if ((currentMenu + currentCursorPos)->type == OME_Label) // skip label + while ((currentMenu + currentCursorPos)->type == OME_Label) // skip label currentCursorPos++; if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) { @@ -1129,6 +1142,14 @@ void cmsHandler(uint32_t unusedTime) void cmsInit(void) { + int i; + for (i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { + if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') + infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; + else + infoGitRev[i] = shortGitRevision[i]; + } + cmsScreenInit(); } From 7960b1665d77f83155a1f3edd32d20e5be0b0fe9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 27 Oct 2016 18:16:51 +0900 Subject: [PATCH 12/19] Introduction of displayPort_t --- src/main/config/config_master.h | 2 +- src/main/fc/config.c | 2 +- src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_controls.c | 2 +- src/main/io/canvas.c | 22 +- src/main/io/canvas.h | 2 +- src/main/io/cms.c | 1650 ++++++++++++++++--------------- src/main/io/cms.h | 30 +- src/main/io/cms_types.h | 12 +- src/main/io/display.c | 25 +- src/main/io/display.h | 2 +- src/main/io/gps.c | 2 +- src/main/io/osd.c | 31 +- src/main/io/osd.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/main.c | 2 +- 16 files changed, 907 insertions(+), 883 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 60bdf4a800..a548e2e4da 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -38,7 +38,7 @@ #include "io/motors.h" #include "io/servos.h" #include "io/gps.h" -#include "io/cms_types.h" +#include "io/cms.h" #include "io/osd.h" #include "io/ledstrip.h" #include "io/vtx.h" diff --git a/src/main/fc/config.c b/src/main/fc/config.c index cf88d51829..830d5e8276 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -63,7 +63,7 @@ #include "io/servos.h" #include "io/ledstrip.h" #include "io/gps.h" -#include "io/cms_types.h" +#include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 37e05b936b..9a99de11b9 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -40,7 +40,7 @@ #include "flight/pid.h" #include "flight/altitudehold.h" -#include "io/cms_types.h" +#include "io/cms.h" #include "io/beeper.h" #include "io/display.h" diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index f87f00b27a..29f1945b53 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -47,7 +47,7 @@ #include "rx/rx.h" -#include "io/cms_types.h" +#include "io/cms.h" #include "io/gps.h" #include "io/beeper.h" diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index bde2031674..db80e003e0 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -11,20 +11,12 @@ #include "drivers/system.h" -#include "io/cms_types.h" +#include "io/cms.h" #include "fc/fc_msp.h" #include "msp/msp_protocol.h" #include "msp/msp_serial.h" -void canvasGetDevParam(uint8_t *pRows, uint8_t *pCols, uint16_t *pBuftime, uint16_t *pBufsize) -{ - *pRows = 13; - *pCols = 30; - *pBuftime = 23; // = 256/(115200/10) - *pBufsize = 192; // 256 * 3/4 (Be conservative) -} - int canvasOutput(uint8_t cmd, uint8_t *buf, int len) { mspSerialPush(cmd, buf, len); @@ -85,10 +77,18 @@ screenFnVTable_t canvasVTable = { NULL, }; -screenFnVTable_t *canvasInit(void) +displayPort_t canvasDisplayPort = { + .rows = 13, + .cols = 30, + .pBuftime = 23, // = 256/(115200/10) + .pBufsize = 192, // 256 * 3/4 (Be conservative) + .VTable = canvasVTable, +}; + +displayPort_t *canvasInit(void) { mspSerialPushInit(mspFcPushInit()); // Called once at startup to initialize push function in msp - return &canvasVTable; + return &canvasDisplayPort; } #endif diff --git a/src/main/io/canvas.h b/src/main/io/canvas.h index a34bc06e5a..f6a574f03f 100644 --- a/src/main/io/canvas.h +++ b/src/main/io/canvas.h @@ -1,3 +1,3 @@ #pragma once -screenFnVTable_t *canvasInit(void); +displayPort_t *canvasInit(void); diff --git a/src/main/io/cms.c b/src/main/io/cms.c index c8fad2b3de..1580cbd0d5 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -35,6 +35,7 @@ #include "common/typeconversion.h" +#include "io/cms.h" #include "io/cms_types.h" #include "io/canvas.h" @@ -52,28 +53,67 @@ #include "config/config_master.h" #include "config/feature.h" +#include "io/cms.h" + #include "build/debug.h" -// Configuration Menu System forwards - -void cmsOpenMenu(void); -void cmsExitMenu(void *); -void cmsChangeScreen(void *); -void cmsMenuBack(void); -void cmsEraseFlash(void *); - -screenFnVTable_t *pScreenFnVTable = NULL; - -uint8_t cmsRows; -uint8_t cmsCols; -uint16_t cmsBuftime; -uint16_t cmsBufsize; -uint16_t cmsBatchsize; - #define CMS_UPDATE_INTERVAL 50 // msec -//#define MAX_MENU_ITEMS (cmsGetRowsCount() - 2) -#define MAX_MENU_ITEMS (cmsRows - 2) +// XXX Why is this here? Something is wrong? +int8_t lastCursorPos; + +void cmsScreenClear(displayPort_t *instance) +{ + instance->VTable->clear(); + instance->cleared = true; + lastCursorPos = -1; // XXX Here +} + +void cmsScreenBegin(displayPort_t *instance) +{ + instance->VTable->begin(); + instance->VTable->clear(); +} + +void cmsScreenEnd(displayPort_t *instance) +{ + instance->VTable->end(); +} + +int cmsScreenWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s) +{ + return instance->VTable->write(x, y, s); +} + +void cmsScreenHeartBeat(displayPort_t *instance) +{ + if (instance->VTable->heartbeat) + instance->VTable->heartbeat(); +} + +void cmsScreenResync(displayPort_t *instance) +{ + if (instance->VTable->resync) + instance->VTable->resync(); +} + +displayPort_t *cmsScreenInit(void) +{ +#ifdef OSD + return osdCmsInit(); +#endif + +#ifdef CANVAS + return canvasCmsInit(); +#endif + +#ifdef OLEDCMS + return displayCmsInit(); +#endif + + return NULL; +} + // XXX LEFT_MENU_COLUMN and RIGHT_MENU_COLUMN must be adjusted // dynamically depending on size of the active output device, @@ -92,78 +132,23 @@ uint16_t cmsBatchsize; // #define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN (cmsCols - 7) +#define RIGHT_MENU_COLUMN(p) ((p)->cols - 7) +#define MAX_MENU_ITEMS(p) ((p)->rows - 2) -bool cmsScreenCleared; -OSD_Entry *currentMenu; +bool cmsInMenu = false; + +OSD_Entry *menuStack[10]; //tab to save menu stack +uint8_t menuStackHistory[10]; //current position in menu stack +uint8_t menuStackIdx = 0; + +OSD_Entry menuMain[]; +OSD_Entry *currentMenu = NULL; OSD_Entry *nextPage = NULL; int8_t currentCursorPos = 0; -int8_t lastCursorPos; uint8_t currentMenuIdx = 0; uint16_t *currentElement = NULL; -void cmsGetDevParam(uint8_t *pRows, uint8_t *pCols, uint16_t *pBuftime, uint16_t *pBufsize) -{ - pScreenFnVTable->getDevParam(pRows, pCols, pBuftime, pBufsize); -} - -void cmsScreenClear(void) -{ - pScreenFnVTable->clear(); - cmsScreenCleared = true; - lastCursorPos = -1; -} - -void cmsScreenBegin(void) -{ - pScreenFnVTable->begin(); - pScreenFnVTable->clear(); -} - -void cmsScreenEnd(void) -{ - pScreenFnVTable->end(); -} - -int cmsScreenWrite(uint8_t x, uint8_t y, char *s) -{ - return pScreenFnVTable->write(x, y, s); -} - -void cmsScreenHeartBeat(void) -{ - if (pScreenFnVTable->heartbeat) - pScreenFnVTable->heartbeat(); -} - -void cmsScreenResync(void) -{ - if (pScreenFnVTable->resync) - pScreenFnVTable->resync(); - - pScreenFnVTable->getDevParam(&cmsRows, &cmsCols, &cmsBuftime, &cmsBufsize); -} - -void cmsScreenInit(void) -{ -#ifdef OSD -pScreenFnVTable = osdCmsInit(); -#endif - -#ifdef CANVAS -pScreenFnVTable = canvasInit(); -#endif - -#ifdef OLEDCMS -pScreenFnVTable = displayCMSInit(); -#endif -} - -// -// Lots of things not separated yet. -// - #define IS_HI(X) (rcData[X] > 1750) #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) @@ -179,320 +164,7 @@ pScreenFnVTable = displayCMSInit(); #define BUTTON_TIME 250 // msec #define BUTTON_PAUSE 500 // msec -//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 - -OSD_Entry *menuStack[10]; //tab to save menu stack -uint8_t menuStackHistory[10]; //current position in menu stack -uint8_t menuStackIdx = 0; - -#ifdef OSD -OSD_Entry menuAlarms[]; -OSD_Entry menuOsdLayout[]; -OSD_Entry menuOsdActiveElems[]; -#endif - -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[]; - -static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; -static char infoTargetName[] = __TARGET__; - -OSD_Entry menuInfo[] = { - { "--- INFO ---", OME_Label, NULL, NULL, 0 }, - { FC_VERSION_STRING, OME_Label, NULL, NULL, 0 }, - { infoGitRev, OME_Label, NULL, NULL, 0 }, - { infoTargetName, OME_Label, NULL, NULL, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } -}; - -OSD_Entry menuMain[] = -{ - {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, - {"CFG&IMU", OME_Submenu, cmsChangeScreen, &menuImu[0], 0}, - {"FEATURES", OME_Submenu, cmsChangeScreen, &menuFeatures[0], 0}, -#ifdef OSD - {"SCR LAYOUT", OME_Submenu, cmsChangeScreen, &menuOsdLayout[0], 0}, - {"ALARMS", OME_Submenu, cmsChangeScreen, &menuAlarms[0], 0}, -#endif - {"INFO", OME_Submenu, cmsChangeScreen, &menuInfo[0], 0}, - {"SAVE&REBOOT", OME_OSD_Exit, cmsExitMenu, (void*)1, 0}, - {"EXIT", OME_OSD_Exit, cmsExitMenu, (void*)0, 0}, - {NULL,OME_END, NULL, NULL, 0} -}; - -OSD_Entry menuFeatures[] = -{ - {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, - {"BLACKBOX", OME_Submenu, cmsChangeScreen, &menuBlackbox[0], 0}, -#if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, cmsChangeScreen, &menu_vtx[0], 0}, -#endif // VTX || USE_RTC6705 -#ifdef LED_STRIP - {"LED STRIP", OME_Submenu, cmsChangeScreen, &menuLedstrip[0], 0}, -#endif // LED_STRIP - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; - -OSD_Entry menuBlackbox[] = -{ - {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &featureBlackbox, 0}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0}, -#ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, cmsEraseFlash, NULL, 0}, -#endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -#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 GRN", - "GREEN ", - "MINT GRN", - "CYAN ", - "LT BLUE ", - "BLUE ", - "DK VIOLT", - "MAGENTA ", - "DEEP PNK" -}; - -//find first led with color flag and restore color index -//after saving all leds with flags color will have color set in OSD -static void getLedColor(void) -{ - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - - int fn = ledGetFunction(ledConfig); - - if (fn == LED_FUNCTION_COLOR) { - ledColor = ledGetColor(ledConfig); - break; - } - } -} - -//udate all leds with flag color -static void applyLedColor(void * ptr) -{ - UNUSED(ptr); - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - ledConfig_t *ledConfig = &masterConfig.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 STRIP ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &featureLedstrip, 0}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; -#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, 0}, - {"ENABLED", OME_Bool, NULL, &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} -}; -#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, 0}, - {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, - {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, 0}, - {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, 0}, - {"YAW P LIM", OME_UINT16, NULL, &entryYawPLimit, 0}, - {"MIN THR", OME_UINT16, NULL, &entryMinThrottle, 0}, - {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, 0}, - {"VBAT CLMAX", OME_UINT8, NULL, &entryVbatMaxCell, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; - -OSD_Entry menuImu[] = -{ - {"--- CFG. IMU ---", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, - {"PID", OME_Submenu, cmsChangeScreen, &menuPid[0], 0}, - {"RATE&RXPO", OME_Submenu, cmsChangeScreen, &menuRateExpo[0], 0}, - {"RC PREV", OME_Submenu, cmsChangeScreen, &menuRc[0], 0}, - {"MISC", OME_Submenu, cmsChangeScreen, &menuMisc[0], 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -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, 0}, - {"ROLL P", OME_UINT8, NULL, &entryRollP, 0}, - {"ROLL I", OME_UINT8, NULL, &entryRollI, 0}, - {"ROLL D", OME_UINT8, NULL, &entryRollD, 0}, - - {"PITCH P", OME_UINT8, NULL, &entryPitchP, 0}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI, 0}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD, 0}, - - {"YAW P", OME_UINT8, NULL, &entryYawP, 0}, - {"YAW I", OME_UINT8, NULL, &entryYawI, 0}, - {"YAW D", OME_UINT8, NULL, &entryYawD, 0}, - - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -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 entryRcYawRate = {&rateProfile.rcYawRate8, 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, 0}, - {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0}, - {"RC YAW RATE", OME_FLOAT, NULL, &entryRcRate, 0}, - {"ROLL SUPER", OME_FLOAT, NULL, &entryRollRate, 0}, - {"PITCH SUPER", OME_FLOAT, NULL, &entryPitchRate, 0}, - {"YAW SUPER", OME_FLOAT, NULL, &entryYawRate, 0}, - {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, 0}, - {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, 0}, - {"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, 0}, - {"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, 0}, - {"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, 0}, - {"D SETPT TRN", OME_FLOAT, NULL, &entryPSetpoint, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -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 PREV ---", OME_Label, NULL, NULL, 0}, - {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0}, - {"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, 0}, - {"THROTTLE", OME_Poll_INT16, NULL, &entryRcThrottle, 0}, - {"YAW", OME_Poll_INT16, NULL, &entryRcYaw, 0}, - {"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, 0}, - {"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, 0}, - {"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, 0}, - {"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - - -bool cmsInMenu = false; - -void cmsUpdateMaxRows(void) +void cmsUpdateMaxRows(displayPort_t *instance) { OSD_Entry *ptr; @@ -500,13 +172,364 @@ void cmsUpdateMaxRows(void) for (ptr = currentMenu; ptr->type != OME_END; ptr++) currentMenuIdx++; - if (currentMenuIdx > MAX_MENU_ITEMS) - currentMenuIdx = MAX_MENU_ITEMS; + if (currentMenuIdx > MAX_MENU_ITEMS(instance)) + currentMenuIdx = MAX_MENU_ITEMS(instance); currentMenuIdx--; } -uint16_t cmsHandleKey(uint8_t key) +static void cmsFtoa(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 cmsPad(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; +} + +int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool drawPolled) +{ + char buff[10]; + int cnt = 0; + + switch (p->type) { + case OME_Submenu: + if (IS_PRINTVALUE(p)) { + cnt = cmsScreenWrite(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 = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + } else { + cnt = cmsScreenWrite(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 = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]); + cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, (char *)ptr->names[*ptr->val]); + CLR_PRINTVALUE(p); + } + break; + } + case OME_VISIBLE: +#ifdef OSD + if (IS_PRINTVALUE(p) && p->data) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + + if (VISIBLE(*val)) { + cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + } else { + cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + } + CLR_PRINTVALUE(p); + } +#endif + break; + case OME_UINT8: + if (IS_PRINTVALUE(p) && p->data) { + OSD_UINT8_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsPad(buff, 5); + //cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, " "); + cnt = cmsScreenWrite(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); + cmsPad(buff, 5); + cnt = cmsScreenWrite(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); + cmsPad(buff, 5); + cnt = cmsScreenWrite(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); + cmsPad(buff, 5); + cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + CLR_PRINTVALUE(p); + } + break; + case OME_Poll_INT16: + if (p->data && drawPolled) { + OSD_UINT16_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsPad(buff, 5); + cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + // PRINTVALUE not cleared on purpose + } + break; + case OME_FLOAT: + if (IS_PRINTVALUE(p) && p->data) { + OSD_FLOAT_t *ptr = p->data; + cmsFtoa(*ptr->val * ptr->multipler, buff); + cmsPad(buff, 5); + cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ??? + CLR_PRINTVALUE(p); + } + break; + case OME_OSD_Exit: + case OME_Label: + case OME_END: + case OME_Back: + break; + } + + return cnt; +} + +void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) +{ + UNUSED(currentTime); + + uint8_t i; + OSD_Entry *p; + uint8_t top = (pDisplay->rows - currentMenuIdx) / 2 - 1; + + // Polled (dynamic) value display denominator. + // XXX Need to denom based on absolute time + static uint8_t pollDenom = 0; + bool drawPolled = (++pollDenom % 8 == 0); + + int16_t cnt = 0; + + if (!currentMenu) + return; + + if (pDisplay->cleared) { + for (p = currentMenu, i= 0; p->type != OME_END; p++, i++) { + SET_PRINTLABEL(p); + SET_PRINTVALUE(p); + } + + if (i > MAX_MENU_ITEMS(pDisplay)) // max per page + { + nextPage = currentMenu + MAX_MENU_ITEMS(pDisplay); + if (nextPage->type == OME_END) + nextPage = NULL; + } + + pDisplay->cleared = false; + } + + // Cursor manipulation + + while ((currentMenu + currentCursorPos)->type == OME_Label) // skip label + currentCursorPos++; + + if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) { + cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, lastCursorPos + top, " "); + } + + if (lastCursorPos != currentCursorPos) { + cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >"); + lastCursorPos = currentCursorPos; + } + + // Print text labels + for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + if (IS_PRINTLABEL(p)) { + cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text); + CLR_PRINTLABEL(p); + if (cnt > pDisplay->batchsize) + return; + } + } + + // Print values + + // XXX Polled values at latter positions in the list may not be + // XXX printed if the cnt exceeds batchsize in the middle of the list. + + for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + if (IS_PRINTVALUE(p)) { + cnt += cmsDrawMenuEntry(pDisplay, p, top + i, drawPolled); + if (cnt > pDisplay->batchsize) + return; + } + } +} + +// XXX Needs separation +OSD_Entry menuPid[]; +OSD_Entry menuRateExpo[]; +uint8_t tempPid[4][3]; +controlRateConfig_t rateProfile; + +void cmsMenuChange(displayPort_t *pDisplay, void *ptr) +{ + uint8_t i; + if (ptr) { + // hack - save profile to temp + // XXX (jflyper) This hack could be avoided by adding pre- and post- + // (onEntry and onExit?) functions to OSD_Entry, that are called + // before and after the menu item is displayed. + if (ptr == &menuPid[0]) { + for (i = 0; i < 3; i++) { + tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i]; + tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i]; + tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i]; + } + tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL]; + tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL]; + tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].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] = currentCursorPos; + menuStackIdx++; + + currentMenu = (OSD_Entry *)ptr; + currentCursorPos = 0; + cmsScreenClear(pDisplay); + cmsUpdateMaxRows(pDisplay); + } +} + +void cmsMenuBack(displayPort_t *pDisplay) +{ + 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++) { + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2]; + } + + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + masterConfig.profile[masterConfig.current_profile_index].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) { + cmsScreenClear(pDisplay); + menuStackIdx--; + nextPage = NULL; + currentMenu = menuStack[menuStackIdx]; + currentCursorPos = menuStackHistory[menuStackIdx]; + + cmsUpdateMaxRows(pDisplay); + } +} + +// XXX Separation +void cmsx_FeatureRead(void); +void cmsx_FeatureWriteback(void); +void cmsx_InfoInit(void); + +void cmsMenuOpen(displayPort_t *pDisplay) +{ + if (cmsInMenu) + return; + + cmsx_FeatureRead(); + + if (!pDisplay) + return; + + pDisplay->batchsize = (pDisplay->buftime < CMS_UPDATE_INTERVAL) ? pDisplay->bufsize : (pDisplay->bufsize * CMS_UPDATE_INTERVAL) / pDisplay->buftime; + + cmsInMenu = true; + DISABLE_ARMING_FLAG(OK_TO_ARM); + + cmsScreenBegin(pDisplay); + currentMenu = &menuMain[0]; + cmsMenuChange(pDisplay, currentMenu); +} + +void cmsMenuExit(displayPort_t *pDisplay, void *ptr) +{ + if (ptr) { + cmsScreenClear(pDisplay); + + cmsScreenWrite(pDisplay, 5, 3, "REBOOTING..."); + cmsScreenResync(pDisplay); // Was max7456RefreshAll(); why at this timing? + + stopMotors(); + stopPwmAllMotors(); + delay(200); + + // save local variables to configuration + cmsx_FeatureWriteback(); + } + + cmsInMenu = false; + + cmsScreenEnd(pDisplay); + currentMenu = NULL; + + if (ptr) + systemReset(); + + ENABLE_ARMING_FLAG(OK_TO_ARM); +} + +uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) { uint16_t res = BUTTON_TIME; OSD_Entry *p; @@ -515,7 +538,7 @@ uint16_t cmsHandleKey(uint8_t key) return res; if (key == KEY_ESC) { - cmsMenuBack(); + cmsMenuBack(pDisplay); return BUTTON_PAUSE; } @@ -525,12 +548,12 @@ uint16_t cmsHandleKey(uint8_t key) } else { if (nextPage) // we have more pages { - cmsScreenClear(); + cmsScreenClear(pDisplay); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; currentCursorPos = 0; - cmsUpdateMaxRows(); + cmsUpdateMaxRows(pDisplay); } else { currentCursorPos = 0; } @@ -545,12 +568,12 @@ uint16_t cmsHandleKey(uint8_t key) if (currentCursorPos == -1 || (currentMenu + currentCursorPos)->type == OME_Label) { if (nextPage) { - cmsScreenClear(); + cmsScreenClear(pDisplay); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; currentCursorPos = 0; - cmsUpdateMaxRows(); + cmsUpdateMaxRows(pDisplay); } else { currentCursorPos = currentMenuIdx; } @@ -566,12 +589,12 @@ uint16_t cmsHandleKey(uint8_t key) case OME_Submenu: case OME_OSD_Exit: if (p->func && key == KEY_RIGHT) { - p->func(p->data); + p->func(pDisplay, p->data); res = BUTTON_PAUSE; } break; case OME_Back: - cmsMenuBack(); + cmsMenuBack(pDisplay); res = BUTTON_PAUSE; break; case OME_Bool: @@ -628,7 +651,7 @@ uint16_t cmsHandleKey(uint8_t key) *ptr->val -= 1; } if (p->func) - p->func(p->data); + p->func(pDisplay, p->data); SET_PRINTVALUE(p); } break; @@ -682,383 +705,9 @@ uint16_t cmsHandleKey(uint8_t key) return res; } -static void cmsFtoa(int32_t value, char *floatString) -{ - uint8_t k; - // np. 3450 +OSD_Entry menuRc[]; - 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 cmsPad(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; -} - -int cmsDrawMenuEntry(OSD_Entry *p, uint8_t row, bool drawPolled) -{ - char buff[10]; - int cnt = 0; - - switch (p->type) { - case OME_Submenu: - if (IS_PRINTVALUE(p)) { - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, ">"); - CLR_PRINTVALUE(p); - } - break; - case OME_Bool: - if (IS_PRINTVALUE(p) && p->data) { - if (*((uint8_t *)(p->data))) { - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, "YES"); - } else { - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, "NO "); - } - CLR_PRINTVALUE(p); - } - break; - case OME_TAB: { - if (IS_PRINTVALUE(p)) { - OSD_TAB_t *ptr = p->data; - //cnt = cmsScreenWrite(RIGHT_MENU_COLUMN - 5, row, (char *)ptr->names[*ptr->val]); - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, (char *)ptr->names[*ptr->val]); - CLR_PRINTVALUE(p); - } - break; - } - case OME_VISIBLE: -#ifdef OSD - if (IS_PRINTVALUE(p) && p->data) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - - if (VISIBLE(*val)) { - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, "YES"); - } else { - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, "NO "); - } - CLR_PRINTVALUE(p); - } -#endif - break; - case OME_UINT8: - if (IS_PRINTVALUE(p) && p->data) { - OSD_UINT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - cmsPad(buff, 5); - //cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, " "); - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, 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); - cmsPad(buff, 5); - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, 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); - cmsPad(buff, 5); - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, 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); - cmsPad(buff, 5); - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); - CLR_PRINTVALUE(p); - } - break; - case OME_Poll_INT16: - if (p->data && drawPolled) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - cmsPad(buff, 5); - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff); - // PRINTVALUE not cleared on purpose - } - break; - case OME_FLOAT: - if (IS_PRINTVALUE(p) && p->data) { - OSD_FLOAT_t *ptr = p->data; - cmsFtoa(*ptr->val * ptr->multipler, buff); - cmsPad(buff, 5); - // XXX One char left ??? - cnt = cmsScreenWrite(RIGHT_MENU_COLUMN - 1, row, buff); - CLR_PRINTVALUE(p); - } - break; - case OME_OSD_Exit: - case OME_Label: - case OME_END: - case OME_Back: - break; - } - - return cnt; -} - -void cmsDrawMenu(uint32_t currentTime) -{ - UNUSED(currentTime); - - uint8_t i; - OSD_Entry *p; - uint8_t top = (cmsRows - currentMenuIdx) / 2 - 1; - - // Polled (dynamic) value display denominator. - // XXX Need to denom based on absolute time - static uint8_t pollDenom = 0; - bool drawPolled = (++pollDenom % 8 == 0); - - int16_t cnt = 0; - - if (!currentMenu) - return; - - if (cmsScreenCleared) { - for (p = currentMenu, i= 0; p->type != OME_END; p++, i++) { - SET_PRINTLABEL(p); - SET_PRINTVALUE(p); - } - - if (i > MAX_MENU_ITEMS) // max per page - { - nextPage = currentMenu + MAX_MENU_ITEMS; - if (nextPage->type == OME_END) - nextPage = NULL; - } - - cmsScreenCleared = false; - } - - // Cursor manipulation - - while ((currentMenu + currentCursorPos)->type == OME_Label) // skip label - currentCursorPos++; - - if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) { - cnt += cmsScreenWrite(LEFT_MENU_COLUMN, lastCursorPos + top, " "); - } - - if (lastCursorPos != currentCursorPos) { - cnt += cmsScreenWrite(LEFT_MENU_COLUMN, currentCursorPos + top, " >"); - lastCursorPos = currentCursorPos; - } - - // Print text labels - for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS && p->type != OME_END; i++, p++) { - if (IS_PRINTLABEL(p)) { - cnt += cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text); - CLR_PRINTLABEL(p); - if (cnt > cmsBatchsize) - return; - } - } - - // Print values - - // XXX Polled values at latter positions in the list may not be - // XXX printed if the cnt exceeds cmsBatchsize in the middle of the list. - - for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS && p->type != OME_END; i++, p++) { - if (IS_PRINTVALUE(p)) { - cnt += cmsDrawMenuEntry(p, top + i, drawPolled); - if (cnt > cmsBatchsize) - return; - } - } -} - -void cmsChangeScreen(void *ptr) -{ - uint8_t i; - if (ptr) { - // hack - save profile to temp - if (ptr == &menuPid[0]) { - for (i = 0; i < 3; i++) { - tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i]; - tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i]; - tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i]; - } - tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].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] = currentCursorPos; - menuStackIdx++; - - currentMenu = (OSD_Entry *)ptr; - currentCursorPos = 0; - cmsScreenClear(); - cmsUpdateMaxRows(); - } -} - -void cmsMenuBack(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++) { - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2]; - } - - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - masterConfig.profile[masterConfig.current_profile_index].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) { - cmsScreenClear(); - menuStackIdx--; - nextPage = NULL; - currentMenu = menuStack[menuStackIdx]; - currentCursorPos = menuStackHistory[menuStackIdx]; - - cmsUpdateMaxRows(); - } -} - -void cmsOpenMenu(void) -{ - if (cmsInMenu) - return; - - featureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; - featureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; - -#if defined(VTX) || defined(USE_RTC6705) - featureVtx = feature(FEATURE_VTX) ? 1 : 0; -#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 - -#ifdef LED_STRIP - getLedColor(); -#endif // LED_STRIP - - cmsGetDevParam(&cmsRows, &cmsCols, &cmsBuftime, &cmsBufsize); - - cmsBatchsize = (cmsBuftime < CMS_UPDATE_INTERVAL) ? cmsBufsize : (cmsBufsize * CMS_UPDATE_INTERVAL) / cmsBuftime; - - cmsInMenu = true; - cmsScreenBegin(); - currentMenu = &menuMain[0]; - cmsChangeScreen(currentMenu); -} - -void cmsExitMenu(void *ptr) -{ - if (ptr) { - cmsScreenClear(); - - cmsScreenWrite(5, 3, "RESTARTING IMU..."); - cmsScreenResync(); // Was max7456RefreshAll(); why at this timing? - - stopMotors(); - stopPwmAllMotors(); - delay(200); - - // 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(); - } - - cmsInMenu = false; - - cmsScreenEnd(); - - if (ptr) - systemReset(); -} - -void cmsUpdate(uint32_t currentTime) +void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) { static int16_t rcDelay = BUTTON_TIME; static uint32_t lastCalled = 0; @@ -1069,7 +718,7 @@ void cmsUpdate(uint32_t currentTime) // Detect menu invocation if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { // XXX Double enter!? - cmsOpenMenu(); + cmsMenuOpen(pDisplay); rcDelay = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME lastCalled = currentTime; return; @@ -1096,7 +745,7 @@ void cmsUpdate(uint32_t currentTime) 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 + else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != menuRc) // this menu is used to check transmitter signals so can't exit using YAW { key = KEY_ESC; rcDelay = BUTTON_TIME; @@ -1105,72 +754,327 @@ void cmsUpdate(uint32_t currentTime) lastCalled = currentTime; if (key && !currentElement) { - rcDelay = cmsHandleKey(key); + rcDelay = cmsHandleKey(pDisplay, key); return; } - cmsDrawMenu(currentTime); + cmsDrawMenu(pDisplay, currentTime); if (currentTime > lastCmsHeartBeat + 500) { // Heart beat for external CMS display device @ 500msec // (Timeout @ 1000msec) - cmsScreenHeartBeat(); + cmsScreenHeartBeat(pDisplay); lastCmsHeartBeat = currentTime; } } } +displayPort_t *currentDisplay = NULL; + void cmsHandler(uint32_t unusedTime) { UNUSED(unusedTime); - if (pScreenFnVTable == NULL) + if (!currentDisplay) return; static uint32_t lastCalled = 0; uint32_t now = millis(); if (now - lastCalled >= CMS_UPDATE_INTERVAL) { - cmsUpdate(now); + cmsUpdate(currentDisplay, now); lastCalled = now; } - - // do not allow ARM if we are in menu - if (cmsInMenu) - DISABLE_ARMING_FLAG(OK_TO_ARM); } void cmsInit(void) { - int i; - for (i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { - if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') - infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; - else - infoGitRev[i] = shortGitRevision[i]; - } - - cmsScreenInit(); + currentDisplay = cmsScreenInit(); + cmsx_InfoInit(); } -// Does this belong here? +// +// Menu tables, should eventually be all GONE!? +// +// +// IMU +// + +OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; + +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, 0}, + {"ROLL P", OME_UINT8, NULL, &entryRollP, 0}, + {"ROLL I", OME_UINT8, NULL, &entryRollI, 0}, + {"ROLL D", OME_UINT8, NULL, &entryRollD, 0}, + + {"PITCH P", OME_UINT8, NULL, &entryPitchP, 0}, + {"PITCH I", OME_UINT8, NULL, &entryPitchI, 0}, + {"PITCH D", OME_UINT8, NULL, &entryPitchD, 0}, + + {"YAW P", OME_UINT8, NULL, &entryYawP, 0}, + {"YAW I", OME_UINT8, NULL, &entryYawI, 0}, + {"YAW D", OME_UINT8, NULL, &entryYawD, 0}, + + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +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 entryRcYawRate = {&rateProfile.rcYawRate8, 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, 0}, + {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0}, + {"RC YAW RATE", OME_FLOAT, NULL, &entryRcRate, 0}, + {"ROLL SUPER", OME_FLOAT, NULL, &entryRollRate, 0}, + {"PITCH SUPER", OME_FLOAT, NULL, &entryPitchRate, 0}, + {"YAW SUPER", OME_FLOAT, NULL, &entryYawRate, 0}, + {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, 0}, + {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, 0}, + {"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, 0}, + {"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, 0}, + {"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, 0}, + {"D SETPT TRN", OME_FLOAT, NULL, &entryPSetpoint, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +// +// RC preview +// +static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; +static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0}; +static OSD_INT16_t entryRcThr = {&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 PREV ---", OME_Label, NULL, NULL, 0}, + {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0}, + {"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, 0}, + {"THR", OME_Poll_INT16, NULL, &entryRcThr, 0}, + {"YAW", OME_Poll_INT16, NULL, &entryRcYaw, 0}, + {"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, 0}, + {"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, 0}, + {"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, 0}, + {"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +// +// Misc +// +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, 0}, + {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, + {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, 0}, + {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, 0}, + {"YAW P LIM", OME_UINT16, NULL, &entryYawPLimit, 0}, + {"MIN THR", OME_UINT16, NULL, &entryMinThrottle, 0}, + {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, 0}, + {"VBAT CLMAX", OME_UINT8, NULL, &entryVbatMaxCell, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +OSD_Entry menuImu[] = +{ + {"--- CFG. IMU ---", OME_Label, NULL, NULL, 0}, + {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, + {"PID", OME_Submenu, cmsMenuChange, &menuPid[0], 0}, + {"RATE&RXPO", OME_Submenu, cmsMenuChange, &menuRateExpo[0], 0}, + {"RC PREV", OME_Submenu, cmsMenuChange, &menuRc[0], 0}, + {"MISC", OME_Submenu, cmsMenuChange, &menuMisc[0], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +// +// Black box +// +uint8_t featureBlackbox = 0; + +OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; + +OSD_Entry menuBlackbox[] = +{ + {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &featureBlackbox, 0}, + {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0}, #ifdef USE_FLASHFS -void cmsEraseFlash(void *ptr) + {"ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0}, +#endif // USE_FLASHFS + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +// +// VTX +// +#if defined(VTX) || defined(USE_RTC6705) + +uint8_t featureVtx = 0, vtxBand, vtxChannel; + +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, 0}, + {"ENABLED", OME_Bool, NULL, &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} +}; +#endif // VTX || USE_RTC6705 + +// +// LED_STRIP +// +#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 GRN", + "GREEN ", + "MINT GRN", + "CYAN ", + "LT BLUE ", + "BLUE ", + "DK VIOLT", + "MAGENTA ", + "DEEP PNK" +}; + +//find first led with color flag and restore color index +//after saving all leds with flags color will have color set in OSD +static void getLedColor(void) +{ + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + int fn = ledGetFunction(ledConfig); + + if (fn == LED_FUNCTION_COLOR) { + ledColor = ledGetColor(ledConfig); + break; + } + } +} + +//udate all leds with flag color +static void applyLedColor(displayPort_t *pDisplay, void *ptr) +{ + UNUSED(ptr); + UNUSED(pDisplay); // Arrgh + + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) + *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); + } +} + +static uint8_t featureLedstrip; + +OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; + +OSD_Entry menuLedstrip[] = +{ + {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &featureLedstrip, 0}, + {"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; +#endif // LED_STRIP + +// +// Should goto flashfs eventually. +// +#ifdef USE_FLASHFS +void cmsx_EraseFlash(void *ptr) { UNUSED(ptr); - cmsScreenClear(); - cmsScreenWrite(5, 3, "ERASING FLASH..."); - cmsScreenResync(); // Was max7456RefreshAll(); Why at this timing? + cmsScreenClear(currentDisplay); + cmsScreenWrite(currentDisplay5, 3, "ERASING FLASH..."); + cmsScreenResync(currentDisplay); // Was max7456RefreshAll(); Why at this timing? flashfsEraseCompletely(); while (!flashfsIsReady()) { delay(100); } - cmsScreenClear(); - cmsScreenResync(); // Was max7456RefreshAll(); wedges during heavy SPI? + cmsScreenClear(currentDisplay); + cmsScreenResync(currentDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? } #endif // USE_FLASHFS @@ -1179,13 +1083,6 @@ void cmsEraseFlash(void *ptr) // OSD specific menu pages and items // XXX Should be part of the osd.c, or new osd_cms.c. // -OSD_Entry menuOsdLayout[] = -{ - {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, - {"ACTIVE ELEM.", OME_Submenu, cmsChangeScreen, &menuOsdActiveElems[0], 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; @@ -1228,6 +1125,123 @@ OSD_Entry menuOsdActiveElems[] = {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; + +OSD_Entry menuOsdLayout[] = +{ + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, + {"ACTIVE ELEM.", OME_Submenu, cmsMenuChange, &menuOsdActiveElems[0], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; +#endif // OSD + +// +// Info +// +static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; +static char infoTargetName[] = __TARGET__; + +OSD_Entry menuInfo[] = { + { "--- INFO ---", OME_Label, NULL, NULL, 0 }, + { FC_VERSION_STRING, OME_Label, NULL, NULL, 0 }, + { infoGitRev, OME_Label, NULL, NULL, 0 }, + { infoTargetName, OME_Label, NULL, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +void cmsx_InfoInit(void) +{ + int i; + for (i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { + if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') + infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; + else + infoGitRev[i] = shortGitRevision[i]; + } +} + +OSD_Entry menuFeatures[] = +{ + {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, + {"BLACKBOX", OME_Submenu, cmsMenuChange, &menuBlackbox[0], 0}, +#if defined(VTX) || defined(USE_RTC6705) + {"VTX", OME_Submenu, cmsMenuChange, &menu_vtx[0], 0}, +#endif // VTX || USE_RTC6705 +#ifdef LED_STRIP + {"LED STRIP", OME_Submenu, cmsMenuChange, &menuLedstrip[0], 0}, +#endif // LED_STRIP + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +OSD_Entry menuMain[] = +{ + {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, + {"CFG&IMU", OME_Submenu, cmsMenuChange, &menuImu[0], 0}, + {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures[0], 0}, +#ifdef OSD + {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &menuOsdLayout[0], 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, &menuAlarms[0], 0}, +#endif + {"INFO", OME_Submenu, cmsMenuChange, &menuInfo[0], 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, + {NULL,OME_END, NULL, NULL, 0} +}; + +void cmsx_FeatureRead(void) +{ + featureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; + +#ifdef LED_STRIP + featureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; + getLedColor(); #endif +#if defined(VTX) || defined(USE_RTC6705) + featureVtx = feature(FEATURE_VTX) ? 1 : 0; +#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 +} + +void cmsx_FeatureWriteback(void) +{ + 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(); +} + #endif // CMS diff --git a/src/main/io/cms.h b/src/main/io/cms.h index 382c724dab..3228af396f 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -1,17 +1,41 @@ #pragma once +typedef struct screenFnVTable_s { + int (*begin)(void); + int (*end)(void); + int (*clear)(void); + int (*write)(uint8_t, uint8_t, char *); + int (*heartbeat)(void); + void (*resync)(void); +} screenFnVTable_t; + +typedef struct displayPort_s { + uint8_t rows; + uint8_t cols; + uint16_t buftime; + uint16_t bufsize; + uint16_t batchsize; // Computed by CMS + screenFnVTable_t *VTable; + + // CMS state + bool cleared; +} displayPort_t; + void cmsInit(void); void cmsHandler(uint32_t); +#if 0 void cmsOpenMenu(); void cmsUpdate(uint32_t); -void cmsScreenResync(void); +void cmsScreenResync(displayPort_t *); +#endif // Required for external CMS tables -void cmsChangeScreen(void * ptr); -void cmsExitMenu(void * ptr); +void cmsChangeScreen(displayPort_t *, void *); +void cmsExitMenu(displayPort_t *, void *); #define STARTUP_HELP_TEXT1 "MENU: THR MID" #define STARTUP_HELP_TEXT2 "+ YAW LEFT" #define STARTUP_HELP_TEXT3 "+ PITCH UP" + diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index 8c35b4a185..35f3270418 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -5,7 +5,7 @@ #pragma once -typedef void (*OSDMenuFuncPtr)(void *data); +typedef void (*OSDMenuFuncPtr)(displayPort_t *, void *); //type of elements typedef enum @@ -95,13 +95,3 @@ typedef struct uint8_t max; const char * const *names; } OSD_TAB_t; - -typedef struct screenFnVTable_s { - void (*getDevParam)(uint8_t *, uint8_t *, uint16_t *, uint16_t *); - int (*begin)(void); - int (*end)(void); - int (*clear)(void); - int (*write)(uint8_t, uint8_t, char *); - int (*heartbeat)(void); - void (*resync)(void); -} screenFnVTable_t; diff --git a/src/main/io/display.c b/src/main/io/display.c index aec3466a1f..7e08ffd4bd 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -54,7 +54,7 @@ #include "flight/imu.h" #include "flight/failsafe.h" -#include "io/cms_types.h" +#include "io/cms.h" #ifdef GPS #include "io/gps.h" @@ -745,15 +745,7 @@ void displayDisablePageCycling(void) } #ifdef OLEDCMS -#include "io/cms_types.h" - -void displayCMSGetDevParam(uint8_t *pRows, uint8_t *pCols, uint16_t *pBuftime, uint16_t *pBufsize) -{ - *pRows = 8; - *pCols = 21; - *pBuftime = 1; - *pBufsize = 50000; -} +#include "io/cms.h" int displayCMSBegin(void) { @@ -785,7 +777,6 @@ int displayCMSWrite(uint8_t x, uint8_t y, char *s) } screenFnVTable_t displayCMSVTable = { - displayCMSGetDevParam, displayCMSBegin, displayCMSEnd, displayCMSClear, @@ -794,9 +785,17 @@ screenFnVTable_t displayCMSVTable = { NULL, }; -screenFnVTable_t *displayCMSInit(void) +displayPort_t displayCMSDisplayPort = { + .rows = 8, + .cols = 21, + .buftime = 1, + .bufsize = 50000, + .VTable = &displayCMSVTable, +}; + +displayPort_t *displayCmsInit(void) { - return &displayCMSVTable; + return &displayCMSDisplayPort; } #endif // OLEDCMS diff --git a/src/main/io/display.h b/src/main/io/display.h index 6b20100cdd..f9fd7e682e 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -46,4 +46,4 @@ void displayDisablePageCycling(void); void displayResetPageCycling(void); void displaySetNextPageChangeAt(uint32_t futureMicros); -screenFnVTable_t *displayCMSInit(void); +displayPort_t *displayCmsInit(void); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index cdf82a3dec..b9248532df 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -41,7 +41,7 @@ #include "sensors/sensors.h" -#include "io/cms_types.h" +#include "io/cms.h" #include "io/serial.h" #include "io/display.h" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b398b9118e..c3954b7573 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -37,8 +37,8 @@ #include "drivers/system.h" -#include "io/cms_types.h" #include "io/cms.h" +#include "io/cms_types.h" #include "io/flashfs.h" #include "io/osd.h" @@ -404,12 +404,10 @@ void osdInit(void) sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); max7456Write(5, 6, string_buffer); max7456Write(7, 7, STARTUP_HELP_TEXT1); - max7456Write(12, 8, STARTUP_HELP_TEXT2); - max7456Write(12, 9, STARTUP_HELP_TEXT3); + max7456Write(11, 8, STARTUP_HELP_TEXT2); + max7456Write(11, 9, STARTUP_HELP_TEXT3); -#ifdef CMS - cmsScreenResync(); // Was max7456RefreshAll(); may be okay. -#endif + max7456RefreshAll(); refreshTimeout = 4 * REFRESH_1S; } @@ -646,14 +644,6 @@ void osdUpdate(uint32_t currentTime) // OSD specific CMS functions // -void osdGetDevParam(uint8_t *pRows, uint8_t *pCols, uint16_t *pBuftime, uint16_t *pBufsize) -{ - *pRows = max7456GetRowsCount(); - *pCols = 30; - *pBuftime = 1; // Very fast - *pBufsize = 50000; // Very large -} - int osdMenuBegin(void) { osdResetAlarms(); @@ -711,7 +701,6 @@ void osdDrawElementPositioningHelp(void) #endif screenFnVTable_t osdVTable = { - osdGetDevParam, osdMenuBegin, osdMenuEnd, osdClearScreen, @@ -720,8 +709,16 @@ screenFnVTable_t osdVTable = { max7456RefreshAll, }; -screenFnVTable_t *osdCmsInit(void) +displayPort_t osdDisplayPort = { + .buftime = 1, // Very fast + .bufsize = 50000, // Very large + .VTable = &osdVTable, +}; + +displayPort_t *osdCmsInit(void) { - return &osdVTable; + osdDisplayPort.rows = max7456GetRowsCount(); + osdDisplayPort.cols = 30; + return &osdDisplayPort; } #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 0c5f29c5f8..8fdf99dd00 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -66,7 +66,7 @@ typedef struct { void updateOsd(uint32_t currentTime); void osdInit(void); void resetOsdConfig(osd_profile_t *osdProfile); -screenFnVTable_t *osdCmsInit(void); +displayPort_t *osdCmsInit(void); // Character coordinate and attributes diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 64c9fdc4c5..a07f72bf36 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -71,7 +71,7 @@ uint8_t cliMode = 0; #include "io/flashfs.h" #include "io/beeper.h" #include "io/asyncfatfs/asyncfatfs.h" -#include "io/cms_types.h" +#include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" diff --git a/src/main/main.c b/src/main/main.c index 3b811a2976..dac4339be3 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -74,7 +74,7 @@ #include "rx/rx.h" #include "rx/spektrum.h" -#include "io/cms_types.h" +#include "io/cms.h" #include "io/beeper.h" #include "io/serial.h" From 720a841008680efa6513ebdd19c4c8c831aea49d Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 28 Oct 2016 13:22:28 +0900 Subject: [PATCH 13/19] More displayPort_t, CANVAS as new feature, multi-display cycling MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Further touch-ups with displayPort_t handling. - CANVAS is a FEATURE now. - Device specific xxxCmsInit() are registered by device’s init, and called by CMS upon entry to menu. - Multiple display devices can be cycled by entering menu invocation command while in menu. - The menu invocation command is (was) changed to THR-Mid + YAW-Left + Pitch-Full to avoid collision with MWOSD menu invocation command. - More separation attempt. --- src/main/fc/config.h | 1 + src/main/fc/fc_msp.c | 9 - src/main/io/canvas.c | 20 +- src/main/io/canvas.h | 4 +- src/main/io/cms.c | 299 +++++++++++++++++----------- src/main/io/cms.h | 6 + src/main/io/display.c | 39 ++-- src/main/io/display.h | 4 +- src/main/io/osd.c | 25 +-- src/main/io/osd.h | 5 +- src/main/io/serial_cli.c | 2 +- src/main/main.c | 15 +- src/main/target/OMNIBUS/target.h | 4 +- src/main/target/OMNIBUS/target.mk | 3 +- src/main/target/SIRINFPV/target.h | 8 + src/main/target/SIRINFPV/target.mk | 3 +- src/main/target/SPRACINGF3/target.h | 2 +- 17 files changed, 267 insertions(+), 182 deletions(-) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 1432b6882b..60c2f517c6 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -55,6 +55,7 @@ typedef enum { FEATURE_VTX = 1 << 24, FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, + FEATURE_CANVAS = 1 << 27, } features_e; void beeperOffSet(uint32_t mask); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 4edd2c4a6e..4f35eb4cb4 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -101,15 +101,6 @@ #include "config/config_master.h" #include "config/feature.h" -#ifdef USE_DPRINTF -#include "common/printf.h" -#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 -extern serialPort_t *debugSerialPort; -#define dprintf(x) if (debugSerialPort) printf x -#else -#define dprintf(x) -#endif - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index db80e003e0..97b12f03d9 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -68,7 +68,6 @@ int canvasWrite(uint8_t col, uint8_t row, char *string) } screenFnVTable_t canvasVTable = { - canvasGetDevParam, canvasBegin, canvasEnd, canvasClear, @@ -77,18 +76,19 @@ screenFnVTable_t canvasVTable = { NULL, }; -displayPort_t canvasDisplayPort = { - .rows = 13, - .cols = 30, - .pBuftime = 23, // = 256/(115200/10) - .pBufsize = 192, // 256 * 3/4 (Be conservative) - .VTable = canvasVTable, -}; +void canvasCmsInit(displayPort_t *pPort) +{ + pPort->rows = 13; + pPort->cols = 30; + pPort->buftime = 23; // = 256/(115200/10) + pPort->bufsize = 192; // 256 * 3/4 (Be conservative) + pPort->VTable = &canvasVTable; +} -displayPort_t *canvasInit(void) +void canvasInit() { mspSerialPushInit(mspFcPushInit()); // Called once at startup to initialize push function in msp - return &canvasDisplayPort; + cmsDeviceRegister(canvasCmsInit); } #endif diff --git a/src/main/io/canvas.h b/src/main/io/canvas.h index f6a574f03f..c7f96207ca 100644 --- a/src/main/io/canvas.h +++ b/src/main/io/canvas.h @@ -1,3 +1,3 @@ #pragma once - -displayPort_t *canvasInit(void); +void canvasInit(void); +void canvasCmsInit(displayPort_t *); diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 1580cbd0d5..e83ac375f9 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -37,7 +37,9 @@ #include "io/cms.h" #include "io/cms_types.h" +#ifdef CANVAS #include "io/canvas.h" +#endif #include "io/flashfs.h" #include "io/osd.h" @@ -57,9 +59,50 @@ #include "build/debug.h" +// Device management + +#define CMS_MAX_DEVICE 4 + +cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE]; +int cmsDeviceCount; +int cmsCurrentDevice = -1; +int cmsLastDevice = -1; + +bool cmsDeviceRegister(cmsDeviceInitFuncPtr func) +{ + if (cmsDeviceCount == CMS_MAX_DEVICE) + return false; + + cmsDeviceInitFunc[cmsDeviceCount++] = func; + + return true; +} + +cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void) +{ + if (cmsDeviceCount == 0) + return NULL; + + if (cmsCurrentDevice < 0) + cmsCurrentDevice = 0; + + return cmsDeviceInitFunc[cmsCurrentDevice]; +} + +cmsDeviceInitFuncPtr cmsDeviceSelectNext(void) +{ + if (cmsDeviceCount == 0) + return NULL; + + cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay + + return cmsDeviceInitFunc[cmsCurrentDevice]; +} + #define CMS_UPDATE_INTERVAL 50 // msec -// XXX Why is this here? Something is wrong? +// XXX Why is this here? Something wrong? +// XXX We need something like Drawing Context that holds all state variables? int8_t lastCursorPos; void cmsScreenClear(displayPort_t *instance) @@ -97,21 +140,9 @@ void cmsScreenResync(displayPort_t *instance) instance->VTable->resync(); } -displayPort_t *cmsScreenInit(void) +void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) { -#ifdef OSD - return osdCmsInit(); -#endif - -#ifdef CANVAS - return canvasCmsInit(); -#endif - -#ifdef OLEDCMS - return displayCmsInit(); -#endif - - return NULL; + cmsDeviceInitFunc(pDisp); } @@ -407,38 +438,31 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) // XXX Needs separation OSD_Entry menuPid[]; +void cmsx_PidRead(void); +void cmsx_PidWriteback(void); OSD_Entry menuRateExpo[]; -uint8_t tempPid[4][3]; -controlRateConfig_t rateProfile; +void cmsx_RateExpoRead(void); +void cmsx_RateExpoWriteback(void); void cmsMenuChange(displayPort_t *pDisplay, void *ptr) { - uint8_t i; if (ptr) { - // hack - save profile to temp - // XXX (jflyper) This hack could be avoided by adding pre- and post- - // (onEntry and onExit?) functions to OSD_Entry, that are called - // before and after the menu item is displayed. - if (ptr == &menuPid[0]) { - for (i = 0; i < 3; i++) { - tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i]; - tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i]; - tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i]; - } - tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]; - } - + // XXX (jflyper): This can be avoided by adding pre- and post- + // XXX (or onEnter and onExit) functions? + if (ptr == &menuPid[0]) + cmsx_PidRead(); if (ptr == &menuRateExpo[0]) - memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); + cmsx_RateExpoRead(); - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentCursorPos; - menuStackIdx++; - - currentMenu = (OSD_Entry *)ptr; - currentCursorPos = 0; + if ((OSD_Entry *)ptr != currentMenu) { + // Stack it and move to a new menu. + // (ptr == curretMenu case occurs when reopening for display sw) + menuStack[menuStackIdx] = currentMenu; + menuStackHistory[menuStackIdx] = currentCursorPos; + menuStackIdx++; + currentMenu = (OSD_Entry *)ptr; + currentCursorPos = 0; + } cmsScreenClear(pDisplay); cmsUpdateMaxRows(pDisplay); } @@ -446,25 +470,14 @@ void cmsMenuChange(displayPort_t *pDisplay, void *ptr) void cmsMenuBack(displayPort_t *pDisplay) { - 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++) { - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2]; - } - - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; - } + if (currentMenu == &menuPid[0]) + cmsx_PidWriteback(); // 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)); + cmsx_RateExpoWriteback(); if (menuStackIdx) { cmsScreenClear(pDisplay); @@ -477,29 +490,43 @@ void cmsMenuBack(displayPort_t *pDisplay) } } +// XXX This should go to device +void cmsComputeBatchsize(displayPort_t *pDisplay) +{ + pDisplay->batchsize = (pDisplay->buftime < CMS_UPDATE_INTERVAL) ? pDisplay->bufsize : (pDisplay->bufsize * CMS_UPDATE_INTERVAL) / pDisplay->buftime; +} + // XXX Separation void cmsx_FeatureRead(void); void cmsx_FeatureWriteback(void); void cmsx_InfoInit(void); -void cmsMenuOpen(displayPort_t *pDisplay) +displayPort_t currentDisplay; + +void cmsMenuOpen(void) { - if (cmsInMenu) + cmsDeviceInitFuncPtr initfunc; + + if (!cmsInMenu) { + // New open + cmsInMenu = true; + DISABLE_ARMING_FLAG(OK_TO_ARM); + initfunc = cmsDeviceSelectCurrent(); + cmsx_FeatureRead(); + currentMenu = &menuMain[0]; + } else { + // Switch display + cmsScreenEnd(¤tDisplay); + initfunc = cmsDeviceSelectNext(); + } + + if (!initfunc) return; - cmsx_FeatureRead(); - - if (!pDisplay) - return; - - pDisplay->batchsize = (pDisplay->buftime < CMS_UPDATE_INTERVAL) ? pDisplay->bufsize : (pDisplay->bufsize * CMS_UPDATE_INTERVAL) / pDisplay->buftime; - - cmsInMenu = true; - DISABLE_ARMING_FLAG(OK_TO_ARM); - - cmsScreenBegin(pDisplay); - currentMenu = &menuMain[0]; - cmsMenuChange(pDisplay, currentMenu); + cmsScreenInit(¤tDisplay, initfunc); + cmsComputeBatchsize(¤tDisplay); + cmsScreenBegin(¤tDisplay); + cmsMenuChange(¤tDisplay, currentMenu); } void cmsMenuExit(displayPort_t *pDisplay, void *ptr) @@ -715,19 +742,20 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) uint8_t key = 0; - // Detect menu invocation - if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { - // XXX Double enter!? - cmsMenuOpen(pDisplay); - rcDelay = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME - lastCalled = currentTime; - return; - } - - if (cmsInMenu) { + if (!cmsInMenu) { + // Detect menu invocation + if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsMenuOpen(); + rcDelay = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME + } + } else { if (rcDelay > 0) { - rcDelay -= currentTime - lastCalled; - debug[0] = rcDelay; + rcDelay -= (currentTime - lastCalled); + } + else if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + // Double enter = display switching + cmsMenuOpen(); + rcDelay = BUTTON_PAUSE; } else if (IS_HI(PITCH)) { key = KEY_UP; @@ -751,10 +779,10 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) rcDelay = BUTTON_TIME; } - lastCalled = currentTime; + //lastCalled = currentTime; if (key && !currentElement) { - rcDelay = cmsHandleKey(pDisplay, key); + rcDelay = cmsHandleKey(¤tDisplay, key); return; } @@ -763,33 +791,31 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) if (currentTime > lastCmsHeartBeat + 500) { // Heart beat for external CMS display device @ 500msec // (Timeout @ 1000msec) - cmsScreenHeartBeat(pDisplay); + cmsScreenHeartBeat(¤tDisplay); lastCmsHeartBeat = currentTime; } } + lastCalled = currentTime; } -displayPort_t *currentDisplay = NULL; - void cmsHandler(uint32_t unusedTime) { UNUSED(unusedTime); - if (!currentDisplay) + if (cmsDeviceCount < 0) return; static uint32_t lastCalled = 0; uint32_t now = millis(); if (now - lastCalled >= CMS_UPDATE_INTERVAL) { - cmsUpdate(currentDisplay, now); + cmsUpdate(¤tDisplay, now); lastCalled = now; } } void cmsInit(void) { - currentDisplay = cmsScreenInit(); cmsx_InfoInit(); } @@ -803,6 +829,8 @@ void cmsInit(void) OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; +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}; @@ -815,6 +843,35 @@ 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}; +void cmsx_PidRead(void) +{ + uint8_t i; + + for (i = 0; i < 3; i++) { + tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i]; + tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i]; + tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i]; + } + tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL]; + tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL]; + tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]; +} + +void cmsx_PidWriteback(void) +{ + uint8_t i; + + for (i = 0; i < 3; i++) { + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2]; + } + + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; +} + OSD_Entry menuPid[] = { {"--- PID ---", OME_Label, NULL, NULL, 0}, @@ -834,6 +891,11 @@ OSD_Entry menuPid[] = {NULL, OME_END, NULL, NULL, 0} }; +// +// Rate & Expo +// +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}; @@ -846,6 +908,16 @@ 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}; +void cmsx_RateExpoRead() +{ + memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); +} + +void cmsx_RateExpoWriteback() +{ + memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); +} + OSD_Entry menuRateExpo[] = { {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0}, @@ -931,6 +1003,29 @@ OSD_Entry menuImu[] = // // Black box // + +// +// Should goto flashfs eventually. +// +#ifdef USE_FLASHFS +void cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) +{ + UNUSED(ptr); + + cmsScreenClear(pDisplay); + cmsScreenWrite(pDisplay, 5, 3, "ERASING FLASH..."); + cmsScreenResync(pDisplay); // Was max7456RefreshAll(); Why at this timing? + + flashfsEraseCompletely(); + while (!flashfsIsReady()) { + delay(100); + } + + cmsScreenClear(pDisplay); + cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? +} +#endif // USE_FLASHFS + uint8_t featureBlackbox = 0; OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; @@ -1056,28 +1151,6 @@ OSD_Entry menuLedstrip[] = }; #endif // LED_STRIP -// -// Should goto flashfs eventually. -// -#ifdef USE_FLASHFS -void cmsx_EraseFlash(void *ptr) -{ - UNUSED(ptr); - - cmsScreenClear(currentDisplay); - cmsScreenWrite(currentDisplay5, 3, "ERASING FLASH..."); - cmsScreenResync(currentDisplay); // Was max7456RefreshAll(); Why at this timing? - - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); - } - - cmsScreenClear(currentDisplay); - cmsScreenResync(currentDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? -} -#endif // USE_FLASHFS - #ifdef OSD // // OSD specific menu pages and items @@ -1221,10 +1294,13 @@ void cmsx_FeatureWriteback(void) else featureClear(FEATURE_BLACKBOX); +#ifdef LED_STRIP if (featureLedstrip) featureSet(FEATURE_LED_STRIP); else featureClear(FEATURE_LED_STRIP); +#endif + #if defined(VTX) || defined(USE_RTC6705) if (featureVtx) featureSet(FEATURE_VTX); @@ -1244,4 +1320,5 @@ void cmsx_FeatureWriteback(void) saveConfigAndNotify(); } + #endif // CMS diff --git a/src/main/io/cms.h b/src/main/io/cms.h index 3228af396f..79ea1461ab 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -21,9 +21,15 @@ typedef struct displayPort_s { bool cleared; } displayPort_t; +// Device management +typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *); +bool cmsDeviceRegister(cmsDeviceInitFuncPtr); + +// For main.c and scheduler void cmsInit(void); void cmsHandler(uint32_t); + #if 0 void cmsOpenMenu(); void cmsUpdate(uint32_t); diff --git a/src/main/io/display.c b/src/main/io/display.c index 7e08ffd4bd..a620629932 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -707,6 +707,10 @@ void displayInit(rxConfig_t *rxConfigToUse) resetDisplay(); delay(200); +#ifdef CMS + cmsDeviceRegister(displayCmsInit); +#endif + rxConfig = rxConfigToUse; memset(&pageState, 0, sizeof(pageState)); @@ -747,28 +751,28 @@ void displayDisablePageCycling(void) #ifdef OLEDCMS #include "io/cms.h" -int displayCMSBegin(void) +int displayCmsBegin(void) { displayInCMS = true; return 0; } -int displayCMSEnd(void) +int displayCmsEnd(void) { displayInCMS = false; return 0; } -int displayCMSClear(void) +int displayCmsClear(void) { i2c_OLED_clear_display_quick(); return 0; } -int displayCMSWrite(uint8_t x, uint8_t y, char *s) +int displayCmsWrite(uint8_t x, uint8_t y, char *s) { i2c_OLED_set_xy(x, y); i2c_OLED_send_string(s); @@ -776,28 +780,23 @@ int displayCMSWrite(uint8_t x, uint8_t y, char *s) return 0; } -screenFnVTable_t displayCMSVTable = { - displayCMSBegin, - displayCMSEnd, - displayCMSClear, - displayCMSWrite, +screenFnVTable_t displayCmsVTable = { + displayCmsBegin, + displayCmsEnd, + displayCmsClear, + displayCmsWrite, NULL, NULL, }; -displayPort_t displayCMSDisplayPort = { - .rows = 8, - .cols = 21, - .buftime = 1, - .bufsize = 50000, - .VTable = &displayCMSVTable, -}; - -displayPort_t *displayCmsInit(void) +void displayCmsInit(displayPort_t *pPort) { - return &displayCMSDisplayPort; + pPort->rows = 8; + pPort->cols = 21; + pPort->buftime = 1; + pPort->bufsize = 50000; + pPort->VTable = &displayCmsVTable; } - #endif // OLEDCMS #endif diff --git a/src/main/io/display.h b/src/main/io/display.h index f9fd7e682e..e2c6c2b967 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -46,4 +46,6 @@ void displayDisablePageCycling(void); void displayResetPageCycling(void); void displaySetNextPageChangeAt(uint32_t futureMicros); -displayPort_t *displayCmsInit(void); +#ifdef CMS +void displayCmsInit(displayPort_t *); +#endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c3954b7573..4b7df072e6 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -64,13 +64,6 @@ #include "build/debug.h" -#ifdef USE_DPRINTF -extern serialPort_t *debugSerialPort; -#define dprintf(x) if (debugSerialPort) printf x -#else -#define dprintf(x) -#endif - // Things in both OSD and CMS #define IS_HI(X) (rcData[X] > 1750) @@ -398,7 +391,7 @@ void osdInit(void) } #ifdef CMS - cmsInit(); + cmsDeviceRegister(osdCmsInit); #endif sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); @@ -709,16 +702,12 @@ screenFnVTable_t osdVTable = { max7456RefreshAll, }; -displayPort_t osdDisplayPort = { - .buftime = 1, // Very fast - .bufsize = 50000, // Very large - .VTable = &osdVTable, -}; - -displayPort_t *osdCmsInit(void) +void osdCmsInit(displayPort_t *pPort) { - osdDisplayPort.rows = max7456GetRowsCount(); - osdDisplayPort.cols = 30; - return &osdDisplayPort; + pPort->rows = max7456GetRowsCount(); + pPort->cols = 30; + pPort->buftime = 1; // Very fast + pPort->bufsize = 50000; // Very large + pPort->VTable = &osdVTable; } #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 8fdf99dd00..09d3a21843 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -66,7 +66,10 @@ typedef struct { void updateOsd(uint32_t currentTime); void osdInit(void); void resetOsdConfig(osd_profile_t *osdProfile); -displayPort_t *osdCmsInit(void); + +#ifdef CMS +void osdCmsInit(displayPort_t *); +#endif // Character coordinate and attributes diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a07f72bf36..a9ccf40404 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -227,7 +227,7 @@ static const char * const featureNames[] = { "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", - " ", "VTX", "RX_SPI", "SOFTSPI", NULL + " ", "VTX", "RX_SPI", "SOFTSPI", "CANVAS", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/main.c b/src/main/main.c index dac4339be3..618409f3a9 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -91,6 +91,7 @@ #include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" +#include "io/canvas.h" #include "scheduler/scheduler.h" @@ -398,6 +399,10 @@ void init(void) initBoardAlignment(&masterConfig.boardAlignment); +#ifdef CMS + cmsInit(); +#endif + #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); @@ -419,10 +424,6 @@ void init(void) } #endif -#ifdef CMS - cmsInit(); -#endif - if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, @@ -461,6 +462,12 @@ void init(void) mspFcInit(); mspSerialInit(); +#ifdef CANVAS + if (feature(FEATURE_CANVAS)) { + canvasInit(); + } +#endif + #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 259fde884b..9bc30bf941 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -96,10 +96,10 @@ #define CMS // Use external OSD to run CMS -//#define CANVAS +#define CANVAS // USE I2C OLED display to run CMS -//#define OLEDCMS +#define OLEDCMS // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 01d31c3708..4ec2d2df37 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -15,4 +15,5 @@ TARGET_SRC = \ io/transponder_ir.c \ drivers/max7456.c \ io/osd.c \ - io/cms.c + io/cms.c \ + io/canvas.c diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index e4e23315b6..9a9f16e331 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -138,8 +138,16 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define OSD + +// Configuratoin Menu System #define CMS +// Use external OSD to run CMS +#define CANVAS + +// USE I2C OLED display to run CMS +#define OLEDCMS + #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 2597e6e977..6481f94182 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -13,4 +13,5 @@ TARGET_SRC = \ drivers/vtx_soft_spi_rtc6705.c \ drivers/max7456.c \ io/osd.c \ - io/cms.c + io/cms.c \ + io/canvas.c diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 6ab414085a..0ded226cf6 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -140,4 +140,4 @@ #define CANVAS // USE I2C OLED display to run CMS -//#define OLEDCMS +#define OLEDCMS From 853e830c6f4d7c26fe97cc6381337b9935168787 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 29 Oct 2016 03:31:14 +0900 Subject: [PATCH 14/19] Take care of a complex config case --- src/main/io/display.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/display.c b/src/main/io/display.c index 87b3292c60..d8b5a8b82e 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -703,7 +703,7 @@ void displayInit(rxConfig_t *rxConfigToUse) resetDisplay(); delay(200); -#ifdef CMS +#if defined(CMS) && defined(OLEDCMS) cmsDeviceRegister(displayCmsInit); #endif From 1aaf2d9d47808dbd723c0c7f4ca390ea5fbe0bb3 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 29 Oct 2016 13:40:27 +0900 Subject: [PATCH 15/19] Minor touch ups --- src/main/io/cms.c | 2 +- src/main/io/osd.c | 15 +++++++++------ src/main/io/osd.h | 2 ++ src/main/io/serial_cli.c | 1 + 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index e83ac375f9..6b076b14e2 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -470,7 +470,7 @@ void cmsMenuChange(displayPort_t *pDisplay, void *ptr) void cmsMenuBack(displayPort_t *pDisplay) { - // becasue pids and rates meybe stored in profiles we need some thicks to manipulate it + // becasue pids and rates may be stored in profiles we need some thicks to manipulate it // hack to save pid profile if (currentMenu == &menuPid[0]) cmsx_PidWriteback(); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4b7df072e6..c67c07ce64 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -390,10 +390,6 @@ void osdInit(void) } } -#ifdef CMS - cmsDeviceRegister(osdCmsInit); -#endif - sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); max7456Write(5, 6, string_buffer); max7456Write(7, 7, STARTUP_HELP_TEXT1); @@ -403,6 +399,10 @@ void osdInit(void) max7456RefreshAll(); refreshTimeout = 4 * REFRESH_1S; + +#ifdef CMS + cmsDeviceRegister(osdCmsInit); +#endif } /** @@ -637,6 +637,8 @@ void osdUpdate(uint32_t currentTime) // OSD specific CMS functions // +uint8_t shiftdown; + int osdMenuBegin(void) { osdResetAlarms(); @@ -662,7 +664,7 @@ int osdClearScreen(void) int osdWrite(uint8_t x, uint8_t y, char *s) { - max7456Write(x, y, s); + max7456Write(x, y + shiftdown, s); return 0; } @@ -704,7 +706,8 @@ screenFnVTable_t osdVTable = { void osdCmsInit(displayPort_t *pPort) { - pPort->rows = max7456GetRowsCount(); + shiftdown = masterConfig.osdProfile.row_shiftdown; + pPort->rows = max7456GetRowsCount() - shiftdown; pPort->cols = 30; pPort->buftime = 1; // Very fast pPort->bufsize = 50000; // Very large diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 09d3a21843..24cf3ad99b 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -52,6 +52,8 @@ typedef struct { uint16_t alt_alarm; uint8_t video_system; + uint8_t row_shiftdown; + osd_unit_t units; } osd_profile_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1656b82cd1..d0e021691e 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -943,6 +943,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 } }, From a5c14eed9aa2d375aa6a9f2701e71303310942f4 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 31 Oct 2016 02:26:50 +0900 Subject: [PATCH 16/19] Avoid zombie VCP --- src/main/msp/msp_serial.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index a64b9f2e30..de04e3ed78 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -229,6 +229,11 @@ void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) continue; } + // XXX Avoid zombie VCP port + if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { + continue; + } + mspPushCommandFn(&push, data, datalen); sbufSwitchToReader(&push.buf, pushBuf); From 8f26ce95f02f724335fb4f043ea078c0bba056b1 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 31 Oct 2016 02:35:56 +0900 Subject: [PATCH 17/19] Comment tidy --- src/main/msp/msp_serial.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index de04e3ed78..ef39598c96 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -229,7 +229,7 @@ void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) continue; } - // XXX Avoid zombie VCP port + // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { continue; } From e8a22fa60e9ff86d6e8812d703c8f7109cdbfdc5 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 31 Oct 2016 03:31:39 +0900 Subject: [PATCH 18/19] Added FC identifier... (Argggh) --- src/main/io/cms.c | 5 ++++- src/main/target/OMNIBUSF4/target.h | 3 +++ src/main/target/OMNIBUSF4/target.mk | 4 +++- src/main/target/REVO/target.h | 3 +++ src/main/target/REVO/target.mk | 4 +++- 5 files changed, 16 insertions(+), 3 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 6b076b14e2..539f4c9939 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -102,7 +102,7 @@ cmsDeviceInitFuncPtr cmsDeviceSelectNext(void) #define CMS_UPDATE_INTERVAL 50 // msec // XXX Why is this here? Something wrong? -// XXX We need something like Drawing Context that holds all state variables? +// XXX Something like Drawing Context that holds all state variables would be the way... int8_t lastCursorPos; void cmsScreenClear(displayPort_t *instance) @@ -1214,8 +1214,11 @@ OSD_Entry menuOsdLayout[] = static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; static char infoTargetName[] = __TARGET__; +#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere + OSD_Entry menuInfo[] = { { "--- INFO ---", OME_Label, NULL, NULL, 0 }, + { BETAFLIGHT_IDENTIFIER, OME_Label, NULL, NULL, 0 }, { FC_VERSION_STRING, OME_Label, NULL, NULL, 0 }, { infoGitRev, OME_Label, NULL, NULL, 0 }, { infoTargetName, OME_Label, NULL, NULL, 0 }, diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index d7137502d8..a1dd90dfaf 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 CANVAS + //#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..c7cf0ad933 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -6,5 +6,7 @@ TARGET_SRC = \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ drivers/max7456.c \ - io/osd.c + io/osd.c \ + io/cms.c \ + io/canvas.c diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 4d73c99355..eed70b40de 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -144,3 +144,6 @@ #define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) + +#define CMS +#define CANVAS diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index 2711b19dac..6f70ecbe7e 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -4,4 +4,6 @@ FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c + drivers/compass_hmc5883l.c \ + io/cms.c \ + io/canvas.c From b42de4f2a41e27f89b36ef5992df550360dbc7f3 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 1 Nov 2016 01:31:09 +0900 Subject: [PATCH 19/19] Provision OSDMenuFuncPtr to return long --- src/main/io/cms.c | 16 ++++++++++++---- src/main/io/cms.h | 11 ++--------- src/main/io/cms_types.h | 2 +- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 539f4c9939..c83a71d286 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -444,7 +444,7 @@ OSD_Entry menuRateExpo[]; void cmsx_RateExpoRead(void); void cmsx_RateExpoWriteback(void); -void cmsMenuChange(displayPort_t *pDisplay, void *ptr) +long cmsMenuChange(displayPort_t *pDisplay, void *ptr) { if (ptr) { // XXX (jflyper): This can be avoided by adding pre- and post- @@ -466,9 +466,11 @@ void cmsMenuChange(displayPort_t *pDisplay, void *ptr) cmsScreenClear(pDisplay); cmsUpdateMaxRows(pDisplay); } + + return 0; } -void cmsMenuBack(displayPort_t *pDisplay) +long cmsMenuBack(displayPort_t *pDisplay) { // becasue pids and rates may be stored in profiles we need some thicks to manipulate it // hack to save pid profile @@ -488,6 +490,8 @@ void cmsMenuBack(displayPort_t *pDisplay) cmsUpdateMaxRows(pDisplay); } + + return 0; } // XXX This should go to device @@ -529,7 +533,7 @@ void cmsMenuOpen(void) cmsMenuChange(¤tDisplay, currentMenu); } -void cmsMenuExit(displayPort_t *pDisplay, void *ptr) +long cmsMenuExit(displayPort_t *pDisplay, void *ptr) { if (ptr) { cmsScreenClear(pDisplay); @@ -554,6 +558,8 @@ void cmsMenuExit(displayPort_t *pDisplay, void *ptr) systemReset(); ENABLE_ARMING_FLAG(OK_TO_ARM); + + return 0; } uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) @@ -1125,7 +1131,7 @@ static void getLedColor(void) } //udate all leds with flag color -static void applyLedColor(displayPort_t *pDisplay, void *ptr) +static long applyLedColor(displayPort_t *pDisplay, void *ptr) { UNUSED(ptr); UNUSED(pDisplay); // Arrgh @@ -1135,6 +1141,8 @@ static void applyLedColor(displayPort_t *pDisplay, void *ptr) if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); } + + return 0; } static uint8_t featureLedstrip; diff --git a/src/main/io/cms.h b/src/main/io/cms.h index 79ea1461ab..a9ab352cd3 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -29,17 +29,10 @@ bool cmsDeviceRegister(cmsDeviceInitFuncPtr); void cmsInit(void); void cmsHandler(uint32_t); - -#if 0 -void cmsOpenMenu(); -void cmsUpdate(uint32_t); -void cmsScreenResync(displayPort_t *); -#endif - // Required for external CMS tables -void cmsChangeScreen(displayPort_t *, void *); -void cmsExitMenu(displayPort_t *, void *); +long cmsChangeScreen(displayPort_t *, void *); +long cmsExitMenu(displayPort_t *, void *); #define STARTUP_HELP_TEXT1 "MENU: THR MID" #define STARTUP_HELP_TEXT2 "+ YAW LEFT" diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index 35f3270418..df68379ebf 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -5,7 +5,7 @@ #pragma once -typedef void (*OSDMenuFuncPtr)(displayPort_t *, void *); +typedef long (*OSDMenuFuncPtr)(displayPort_t *, void *); //type of elements typedef enum