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"