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