From 6793692217bc3e1c5afcb989c9194c65156d9064 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 19 Oct 2016 02:39:52 +0900 Subject: [PATCH 001/188] 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 002/188] 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 003/188] 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 004/188] 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 005/188] 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 006/188] 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 007/188] 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 008/188] 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 009/188] 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 010/188] 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 9384a8db84929df01dd44fb390262eb2157574fd Mon Sep 17 00:00:00 2001 From: Patrick Johnson Date: Tue, 25 Oct 2016 21:07:21 -0500 Subject: [PATCH 011/188] Changed FLASH_PAGE_COUNT to "3" for STM32FX411xE --- src/main/config/config_eeprom.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index d9ceba8828..cbbb1f795d 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -76,7 +76,7 @@ #if defined(STM32F40_41xxx) #define FLASH_PAGE_COUNT 4 // just to make calculations work #elif defined (STM32F411xE) -#define FLASH_PAGE_COUNT 4 // just to make calculations work +#define FLASH_PAGE_COUNT 3 // just to make calculations work #elif defined (STM32F745xx) #define FLASH_PAGE_COUNT 4 // just to make calculations work #else From 06239d2a0d63b1c458a123166e809fae7368e20b Mon Sep 17 00:00:00 2001 From: Patrick Johnson Date: Tue, 25 Oct 2016 21:34:27 -0500 Subject: [PATCH 012/188] Changed FLASH_PAGE_COUNT to 3 for STM32FX411xE --- src/main/config/config_eeprom.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index cbbb1f795d..63477173c3 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -76,7 +76,7 @@ #if defined(STM32F40_41xxx) #define FLASH_PAGE_COUNT 4 // just to make calculations work #elif defined (STM32F411xE) -#define FLASH_PAGE_COUNT 3 // just to make calculations work +#define FLASH_PAGE_COUNT 3 // just to make calculations work #elif defined (STM32F745xx) #define FLASH_PAGE_COUNT 4 // just to make calculations work #else From a0b3afae31a54b9d77c9d9af3a11dd0514f69e22 Mon Sep 17 00:00:00 2001 From: Patrick Johnson Date: Tue, 25 Oct 2016 21:48:53 -0500 Subject: [PATCH 013/188] Updated to correctly support STM32FX411 device. Removed HSE bypass references. Minor cleanup. --- src/main/target/system_stm32f4xx.c | 187 +++++------------------------ 1 file changed, 27 insertions(+), 160 deletions(-) diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index 879acff784..43b21f83ea 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -346,21 +346,6 @@ uint32_t hse_value = HSE_VALUE; /* #define DATA_IN_ExtSDRAM */ #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ -#if defined(STM32F410xx) || defined(STM32F411xE) -/*!< Uncomment the following line if you need to clock the STM32F410xx/STM32F411xE by HSE Bypass - through STLINK MCO pin of STM32F103 microcontroller. The frequency cannot be changed - and is fixed at 8 MHz. - Hardware configuration needed for Nucleo Board: - – SB54, SB55 OFF - – R35 removed - – SB16, SB50 ON */ -/* #define USE_HSE_BYPASS */ - -#if defined(USE_HSE_BYPASS) -#define HSE_BYPASS_INPUT_FREQUENCY 8000000 -#endif /* USE_HSE_BYPASS */ -#endif /* STM32F410xx || STM32F411xE */ - /*!< Uncomment the following line if you need to relocate your vector Table in Internal SRAM. */ /* #define VECT_TAB_SRAM */ @@ -379,17 +364,10 @@ uint32_t hse_value = HSE_VALUE; #elif defined (STM32F446xx) #define PLL_M 8 #elif defined (STM32F410xx) || defined (STM32F411xE) - #if defined(USE_HSE_BYPASS) - #define PLL_M 8 - #else /* !USE_HSE_BYPASS */ - #define PLL_M 8 - #endif /* USE_HSE_BYPASS */ + #define PLL_M 8 #else #endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F469_479xx */ -/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ -#define PLL_Q 7 - #if defined(STM32F446xx) /* PLL division factor for I2S, SAI, SYSTEM and SPDIF: Clock = PLL_VCO / PLLR */ #define PLL_R 7 @@ -399,24 +377,32 @@ uint32_t hse_value = HSE_VALUE; #define PLL_N 360 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 2 +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 7 #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ #if defined (STM32F40_41xxx) #define PLL_N 336 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 2 +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 7 #endif /* STM32F40_41xxx */ #if defined(STM32F401xx) #define PLL_N 336 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 4 +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 7 #endif /* STM32F401xx */ #if defined(STM32F410xx) || defined(STM32F411xE) -#define PLL_N 400 +#define PLL_N 384 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 4 +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 8 #endif /* STM32F410xx || STM32F411xE */ /******************************************************************************/ @@ -450,7 +436,7 @@ uint32_t hse_value = HSE_VALUE; #endif /* STM32F401xx */ #if defined(STM32F410xx) || defined(STM32F411xE) - uint32_t SystemCoreClock = 100000000; + uint32_t SystemCoreClock = 96000000; #endif /* STM32F410xx || STM32F401xE */ __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; @@ -585,7 +571,6 @@ void SystemCoreClockUpdate(void) pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; -#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F446xx) || defined(STM32F469_479xx) if (pllsource != 0) { /* HSE used as PLL clock source */ @@ -596,21 +581,7 @@ void SystemCoreClockUpdate(void) /* HSI used as PLL clock source */ pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); } -#elif defined(STM32F410xx) || defined(STM32F411xE) -#if defined(USE_HSE_BYPASS) - if (pllsource != 0) - { - /* HSE used as PLL clock source */ - pllvco = (HSE_BYPASS_INPUT_FREQUENCY / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); - } -#else - if (pllsource == 0) - { - /* HSI used as PLL clock source */ - pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); - } -#endif /* USE_HSE_BYPASS */ -#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F446xx || STM32F469_479xx */ + pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; SystemCoreClock = pllvco/pllp; break; @@ -657,7 +628,6 @@ void SystemCoreClockUpdate(void) */ void SetSysClock(void) { -#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F446xx)|| defined(STM32F469_479xx) /******************************************************************************/ /* PLL (clocked by HSE) used as System clock source */ /******************************************************************************/ @@ -707,16 +677,22 @@ void SetSysClock(void) RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; #endif /* STM32F401xx */ -#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F469_479xx) - /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); -#endif /* STM32F40_41xxx || STM32F401xx || STM32F427_437x || STM32F429_439xx || STM32F469_479xx */ +#if defined(STM32F410xx) || defined(STM32F411xE) + /* PCLK2 = HCLK / 2*/ + RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 4*/ + RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; +#endif /* STM32F410xx || STM32F411xE */ #if defined(STM32F446xx) /* Configure the main PLL */ RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24) | (PLL_R << 28); +#else + /* Configure the main PLL */ + RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); #endif /* STM32F446xx */ /* Enable the main PLL */ @@ -737,19 +713,17 @@ void SetSysClock(void) while((PWR->CSR & PWR_CSR_ODSWRDY) == 0) { } - /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ - FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS; #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ -#if defined(STM32F40_41xxx) +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx) /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS; -#endif /* STM32F40_41xxx */ +#endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ -#if defined(STM32F401xx) +#if defined(STM32F401xx) || defined(STM32F410xx) || defined(STM32F411xE) /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; -#endif /* STM32F401xx */ +#endif /* STM32F401xx || STM32F410xx || STM32F411xE*/ /* Select the main PLL as system clock source */ RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); @@ -764,113 +738,6 @@ void SetSysClock(void) { /* If HSE fails to start-up, the application will have wrong clock configuration. User can add here some code to deal with this error */ } -#elif defined(STM32F410xx) || defined(STM32F411xE) -#if defined(USE_HSE_BYPASS) -/******************************************************************************/ -/* PLL (clocked by HSE) used as System clock source */ -/******************************************************************************/ - __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - - /* Enable HSE and HSE BYPASS */ - RCC->CR |= ((uint32_t)RCC_CR_HSEON | RCC_CR_HSEBYP); - - /* Wait till HSE is ready and if Time out is reached exit */ - do - { - HSEStatus = RCC->CR & RCC_CR_HSERDY; - StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - - if ((RCC->CR & RCC_CR_HSERDY) != RESET) - { - HSEStatus = (uint32_t)0x01; - } - else - { - HSEStatus = (uint32_t)0x00; - } - - if (HSEStatus == (uint32_t)0x01) - { - /* Select regulator voltage output Scale 1 mode */ - RCC->APB1ENR |= RCC_APB1ENR_PWREN; - PWR->CR |= PWR_CR_VOS; - - /* HCLK = SYSCLK / 1*/ - RCC->CFGR |= RCC_CFGR_HPRE_DIV1; - - /* PCLK2 = HCLK / 2*/ - RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; - - /* PCLK1 = HCLK / 4*/ - RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; - - /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); - - /* Enable the main PLL */ - RCC->CR |= RCC_CR_PLLON; - - /* Wait till the main PLL is ready */ - while((RCC->CR & RCC_CR_PLLRDY) == 0) - { - } - - /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ - FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; - - /* Select the main PLL as system clock source */ - RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); - RCC->CFGR |= RCC_CFGR_SW_PLL; - - /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); - { - } - } - else - { /* If HSE fails to start-up, the application will have wrong clock - configuration. User can add here some code to deal with this error */ - } -#else /* HSI will be used as PLL clock source */ - /* Select regulator voltage output Scale 1 mode */ - RCC->APB1ENR |= RCC_APB1ENR_PWREN; - PWR->CR |= PWR_CR_VOS; - - /* HCLK = SYSCLK / 1*/ - RCC->CFGR |= RCC_CFGR_HPRE_DIV1; - - /* PCLK2 = HCLK / 2*/ - RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; - - /* PCLK1 = HCLK / 4*/ - RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; - - /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | (PLL_Q << 24); - - /* Enable the main PLL */ - RCC->CR |= RCC_CR_PLLON; - - /* Wait till the main PLL is ready */ - while((RCC->CR & RCC_CR_PLLRDY) == 0) - { - } - - /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ - FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; - - /* Select the main PLL as system clock source */ - RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); - RCC->CFGR |= RCC_CFGR_SW_PLL; - - /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); - { - } -#endif /* USE_HSE_BYPASS */ -#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F469_479xx */ } /** From e26258e6860c69d5470bc8a54c1b8caa6ba3cfd4 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 26 Oct 2016 19:02:15 +0900 Subject: [PATCH 014/188] 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 0a4648c03e5e57233349d52683678d4411429a4a Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 25 Oct 2016 12:55:31 +1300 Subject: [PATCH 015/188] Fixed motor value translation to / from MSP for DShot. Fixed 'stopMotors' (used in 'HardfaultHandler' to actually stop motors with DShot. Also made 'disarmMotorOutput' used consistently. Renamed functions to be more clear. --- src/main/blackbox/blackbox.c | 13 ++++++- src/main/fc/fc_msp.c | 11 +++--- src/main/flight/mixer.c | 53 ++++++++++++++++++++------ src/main/flight/mixer.h | 4 +- src/main/io/serial_cli.c | 4 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- 6 files changed, 64 insertions(+), 23 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f039f7ec14..cddebdd4ca 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -36,6 +36,7 @@ #include "drivers/sensor.h" #include "drivers/compass.h" #include "drivers/system.h" +#include "drivers/pwm_output.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -895,7 +896,17 @@ void stopInTestMode(void) */ bool inMotorTestMode(void) { static uint32_t resetTime = 0; - uint16_t inactiveMotorCommand = (feature(FEATURE_3D) ? masterConfig.flight3DConfig.neutral3d : masterConfig.motorConfig.mincommand); + uint16_t inactiveMotorCommand; + if (feature(FEATURE_3D)) { + inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d; +#ifdef USE_DSHOT + } else if (isMotorProtocolDshot()) { + inactiveMotorCommand = DSHOT_DISARM_COMMAND; +#endif + } else { + inactiveMotorCommand = masterConfig.motorConfig.mincommand; + } + int i; bool atLeastOneMotorActivated = false; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bfeaf319ff..8fe284997f 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -648,10 +648,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, 0); continue; } - if (isMotorProtocolDshot()) - sbufWriteU16(dst, constrain((motor[i] / 2) + 1000, 1000, 2000)); // This is to get it working in the configurator - else - sbufWriteU16(dst, motor[i]); + + sbufWriteU16(dst, convertMotorToExternal(motor[i])); } break; case MSP_RC: @@ -1320,8 +1318,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert break; case MSP_SET_MOTOR: - for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 - motor_disarmed[i] = sbufReadU16(src); + for (i = 0; i < 8; i++) { // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 + motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src)); + } break; case MSP_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 7846b91714..a4e5ec37ef 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -47,6 +47,13 @@ #include "fc/runtime_config.h" #include "config/feature.h" +#include "config/config_master.h" + +#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 +// (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) +#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 +#define EXTERNAL_CONVERSION_MIN_VALUE 1000 +#define EXTERNAL_CONVERSION_MAX_VALUE 2000 uint8_t motorCount; @@ -234,21 +241,26 @@ static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, d static float rcCommandThrottleRange; bool isMotorProtocolDshot(void) { +#ifdef USE_DSHOT if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) return true; else +#endif return false; } // Add here scaled ESC outputs for digital protol void initEscEndpoints(void) { +#ifdef USE_DSHOT if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset; maxMotorOutputNormal = DSHOT_MAX_THROTTLE; deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH; deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; - } else { + } else +#endif + { disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand; minMotorOutputNormal = motorConfig->minthrottle; maxMotorOutputNormal = motorConfig->maxthrottle; @@ -359,7 +371,7 @@ void mixerResetDisarmedMotors(void) int i; // set disarmed motor values for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) - motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand; + motor_disarmed[i] = disarmMotorOutput; } void writeMotors(void) @@ -373,7 +385,7 @@ void writeMotors(void) } } -void writeAllMotors(int16_t mc) +static void writeAllMotors(int16_t mc) { // Sends commands to all motors for (uint8_t i = 0; i < motorCount; i++) { @@ -385,7 +397,7 @@ void writeAllMotors(int16_t mc) void stopMotors(void) { - writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand); + writeAllMotors(disarmMotorOutput); delay(50); // give the timers and ESCs a chance to react. } @@ -511,14 +523,31 @@ void mixTable(pidProfile_t *pidProfile) // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { - if (isMotorProtocolDshot()) { - motor[i] = (motor_disarmed[i] < motorOutputMin) ? disarmMotorOutput : motor_disarmed[i]; // Prevent getting into special reserved range - - if (motor_disarmed[i] != disarmMotorOutput) - motor[i] = (motor_disarmed[i] - 1000) * 2; // TODO - Perhaps needs rescaling as it will never reach 2047 during motor tests - } else { - motor[i] = motor_disarmed[i]; - } + motor[i] = motor_disarmed[i]; } } } + +uint16_t convertExternalToMotor(uint16_t externalValue) +{ + uint16_t motorValue = externalValue; +#ifdef USE_DSHOT + if (isMotorProtocolDshot()) { + motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); + } +#endif + + return motorValue; +} + +uint16_t convertMotorToExternal(uint16_t motorValue) +{ + uint16_t externalValue = motorValue; +#ifdef USE_DSHOT + if (isMotorProtocolDshot()) { + externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain((motorValue / EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE); + } +#endif + + return externalValue; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1a4949a3cc..8798361050 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -104,7 +104,6 @@ void mixerUseConfigs( airplaneConfig_t *airplaneConfigToUse, struct rxConfig_s *rxConfigToUse); -void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); @@ -117,4 +116,7 @@ void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); void stopPwmAllMotors(void); + bool isMotorProtocolDshot(void); +uint16_t convertExternalToMotor(uint16_t externalValue); +uint16_t convertMotorToExternal(uint16_t motorValue); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f024d1ae31..0af9572e5e 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3095,11 +3095,11 @@ static void cliMotor(char *cmdline) cliShowArgumentRangeError("value", 1000, 2000); return; } else { - motor_disarmed[motor_index] = motor_value; + motor_disarmed[motor_index] = convertExternalToMotor(motor_value); } } - cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]); + cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); } static void cliPlaySound(char *cmdline) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 980bde07d6..30dd0f082f 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1141,7 +1141,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) break; case BST_SET_MOTOR: for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 - motor_disarmed[i] = bstRead16(); + motor_disarmed[i] = convertExternalToMotor(bstRead16()); break; case BST_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS From a5894f1c24315688d50128fb29916cf12f24e0fe Mon Sep 17 00:00:00 2001 From: Nathan Date: Wed, 26 Oct 2016 20:33:45 -0600 Subject: [PATCH 016/188] disable uart tx dma when using dshot on the spracingf3 (#1412) --- src/main/drivers/serial_uart_stm32f30x.c | 2 ++ src/main/target/SPRACINGF3/target.h | 5 +++++ src/main/target/common.h | 1 + 3 files changed, 8 insertions(+) diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 304c702d51..5514b16ec8 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -158,7 +158,9 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option #ifdef USE_UART1_RX_DMA s->rxDMAChannel = DMA1_Channel5; #endif +#ifdef USE_UART1_TX_DMA s->txDMAChannel = DMA1_Channel4; +#endif s->USARTx = USART1; diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 634444f94c..207a7a2dd2 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -104,6 +104,11 @@ #define USE_DSHOT +// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4 +#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT) +#undef USE_UART1_TX_DMA +#endif + #define LED_STRIP #define USE_LED_STRIP_ON_DMA1_CHANNEL2 diff --git a/src/main/target/common.h b/src/main/target/common.h index 9072918f4f..8d0ea828d2 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -40,6 +40,7 @@ #ifdef STM32F1 // Using RX DMA disables the use of receive callbacks #define USE_UART1_RX_DMA +#define USE_UART1_TX_DMA #endif From 7960b1665d77f83155a1f3edd32d20e5be0b0fe9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 27 Oct 2016 18:16:51 +0900 Subject: [PATCH 017/188] 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 bd1d96dff92c0f843fa69d0e795f9a36621bd287 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 28 Oct 2016 00:00:21 +0200 Subject: [PATCH 018/188] BEEBRAIN defaults (bonus commit) --- src/main/target/NAZE/config.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 9efa2c5830..b47fe55744 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -46,6 +46,7 @@ void targetConfiguration(master_t *config) config->gyro_lpf = 1; config->gyro_soft_lpf_hz = 100; config->gyro_soft_notch_hz_1 = 0; + config->gyro_soft_notch_hz_2 = 0; /*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { config->rxConfig.channelRanges[channel].min = 1180; @@ -53,12 +54,12 @@ void targetConfiguration(master_t *config) }*/ for (int profileId = 0; profileId < 2; profileId++) { - config->profile[profileId].pidProfile.P8[ROLL] = 55; - config->profile[profileId].pidProfile.I8[ROLL] = 50; - config->profile[profileId].pidProfile.D8[ROLL] = 25; - config->profile[profileId].pidProfile.P8[PITCH] = 65; - config->profile[profileId].pidProfile.I8[PITCH] = 60; - config->profile[profileId].pidProfile.D8[PITCH] = 28; + config->profile[profileId].pidProfile.P8[ROLL] = 70; + config->profile[profileId].pidProfile.I8[ROLL] = 70; + config->profile[profileId].pidProfile.D8[ROLL] = 30; + config->profile[profileId].pidProfile.P8[PITCH] = 80; + config->profile[profileId].pidProfile.I8[PITCH] = 80; + config->profile[profileId].pidProfile.D8[PITCH] = 30; config->profile[profileId].pidProfile.P8[YAW] = 180; config->profile[profileId].pidProfile.I8[YAW] = 45; config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50; @@ -66,12 +67,14 @@ void targetConfiguration(master_t *config) config->profile[profileId].pidProfile.levelSensitivity = 1.0f; for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { - config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 110; + config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 100; config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110; + config->profile[profileId].controlRateProfile[rateProfileId].rcExpo8 = 20; config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 80; config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 80; config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80; + config->profile[profileId].pidProfile.dtermSetpointWeight = 254; config->profile[profileId].pidProfile.setpointRelaxRatio = 100; } } From 431295ab211b5e83cf02d8639283cd0ae6e65643 Mon Sep 17 00:00:00 2001 From: gaelj Date: Tue, 16 Aug 2016 13:01:22 +0200 Subject: [PATCH 019/188] Enable using of any RC channel to set the color hue modifier. Can be set via MSP (bumped to version 22) New CLI command: mode_color 7 0 AUX_Channel AUX_Channel: ROLL = 0 PITCH = 1 YAW = 2 THROTTLE = 3 AUX1 = 4 AUX2 = 5 AUX3 = 6 AUX4 = 7 AUX5 = 8 AUX6 = 9 AUX7 = 10 AUX8 = 11 Fixed CLI defaults printing for #1019. --- src/main/config/config_master.h | 1 + src/main/fc/config.c | 1 + src/main/fc/fc_msp.c | 5 +++++ src/main/io/ledstrip.c | 10 ++++++++-- src/main/io/ledstrip.h | 3 ++- src/main/io/serial_cli.c | 7 ++++++- src/main/msp/msp_protocol.h | 2 +- 7 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 2e74441969..4caf57f1e0 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -166,6 +166,7 @@ typedef struct master_s { modeColorIndexes_t modeColors[LED_MODE_COUNT]; specialColorIndexes_t specialColors; uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on + rc_alias_e ledstrip_aux_channel; #endif #ifdef TRANSPONDER diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c6a77b96a3..65d5f462bb 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -664,6 +664,7 @@ void createDefaultConfig(master_t *config) applyDefaultModeColors(config->modeColors); applyDefaultSpecialColors(&(config->specialColors)); config->ledstrip_visual_beeper = 0; + config->ledstrip_aux_channel = THROTTLE; #endif #ifdef VTX diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8fe284997f..1662bc1f7b 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -960,6 +960,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, j); sbufWriteU8(dst, masterConfig.specialColors.color[j]); } + + sbufWriteU8(dst, LED_AUX_CHANNEL); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, masterConfig.ledstrip_aux_channel); + break; #endif diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index baeb80c957..1892afa876 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -170,6 +170,7 @@ static const specialColorIndexes_t defaultSpecialColors[] = { }; static int scaledThrottle; +static int scaledAux; static void updateLedRingCounts(void); @@ -253,7 +254,7 @@ bool parseLedStripConfig(int ledIndex, const char *config) RING_COLORS, PARSE_STATE_COUNT }; - static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0'}; + static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', ':', '\0'}; ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfig, 0, sizeof(ledConfig_t)); @@ -492,7 +493,7 @@ static void applyLedFixedLayers() } if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { - hOffset += ((scaledThrottle - 10) * 4) / 3; + hOffset += scaledAux; } color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); @@ -962,6 +963,7 @@ void ledStripUpdate(uint32_t currentTime) // apply all layers; triggered timed functions has to update timers scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + scaledAux = scaleRange(rcData[masterConfig.ledstrip_aux_channel], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1); applyLedFixedLayers(); @@ -1035,6 +1037,10 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT) return false; masterConfig.specialColors.color[modeColorIndex] = colorIndex; + } else if (modeIndex == LED_AUX_CHANNEL) { + if (modeColorIndex < 0 || modeColorIndex >= 1) + return false; + masterConfig.ledstrip_aux_channel = colorIndex; } else { return false; } diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 351708abcb..de5439902a 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -75,7 +75,8 @@ typedef enum { LED_MODE_ANGLE, LED_MODE_MAG, LED_MODE_BARO, - LED_SPECIAL + LED_SPECIAL, + LED_AUX_CHANNEL } ledModeIndex_e; typedef enum { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0af9572e5e..a5dacf70f0 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1795,13 +1795,18 @@ static void printModeColor(uint8_t dumpMask, master_t *defaultConfig) } } + const char *format = "mode_color %u %u %u\r\n"; for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { int colorIndex = masterConfig.specialColors.color[j]; int colorIndexDefault = defaultConfig->specialColors.color[j]; - const char *format = "mode_color %u %u %u\r\n"; cliDefaultPrintf(dumpMask, colorIndex == colorIndexDefault, format, LED_SPECIAL, j, colorIndexDefault); cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, format, LED_SPECIAL, j, colorIndex); } + + int ledStripAuxChannel = masterConfig.ledstrip_aux_channel; + int ledStripAuxChannelDefault = defaultConfig->ledstrip_aux_channel; + cliDefaultPrintf(dumpMask, ledStripAuxChannel == ledStripAuxChannelDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannelDefault); + cliDumpPrintf(dumpMask, ledStripAuxChannel == ledStripAuxChannelDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannel); } static void cliModeColor(char *cmdline) diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 8d14b4d698..338867e55f 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 21 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 22 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 From 720a841008680efa6513ebdd19c4c8c831aea49d Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 28 Oct 2016 13:22:28 +0900 Subject: [PATCH 020/188] 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 6261e2b03bde05d88bca1fc0543123e74274bdd9 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 28 Oct 2016 12:17:59 +0200 Subject: [PATCH 021/188] Update to final BETAFLIGTHT3 target code --- src/main/target/BETAFLIGHTF3/target.h | 21 ++++----------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 9be40e8489..b258931c17 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -18,24 +18,18 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "BFFC" +#define TARGET_BOARD_IDENTIFIER "BFF3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE - -//#define LED0 PC14 #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USABLE_TIMER_CHANNEL_COUNT 10 -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH - -#define MPU6000_CS_PIN PC13 +#define MPU6000_CS_PIN PA15 #define MPU6000_SPI_INSTANCE SPI1 - #define GYRO #define USE_GYRO_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW180_DEG @@ -46,15 +40,12 @@ // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define EXTI_CALLBACK_HANDLER_COUNT 1 #define MPU_INT_EXTI PC13 #define USE_EXTI #define USB_IO -//#define USE_FLASHFS -//#define USE_FLASH_M25P16 - #define USE_VCP #define USE_UART1 #define USE_UART2 @@ -109,7 +100,6 @@ #define SDCARD_DETECT_PIN PC14 #define SDCARD_SPI_INSTANCE SPI2 -//#define SDCARD_SPI_CS_GPIO SPI2_GPIO #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 @@ -117,9 +107,6 @@ #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 -//#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -//#define SDCARD_DMA_CHANNEL DMA_Channel_0 - #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC From 82eb3ca967092e7202bb6c1d8f42db3242095d45 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 28 Oct 2016 15:50:09 +0200 Subject: [PATCH 022/188] Add DSHOT to BFF3 --- src/main/target/BETAFLIGHTF3/target.c | 23 ++++++++++++----------- src/main/target/BETAFLIGHTF3/target.h | 9 ++++++++- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 26c8c44a73..d6e93c8a89 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -14,26 +14,27 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ - //Target code By Hector "Hectech FPV" Hind + //Target code By BorisB and Hector "Hectech FPV" Hind #include #include #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel1, DMA1_CH1_HANDLER }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel4, DMA1_CH4_HANDLER }, // PWM4 - PB9 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index b258931c17..1dc435b492 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ - //Target code By Hector "Hectech FPV" Hind + //Target code By BorisB and Hector "Hectech FPV" Hind #pragma once @@ -44,6 +44,13 @@ #define MPU_INT_EXTI PC13 #define USE_EXTI +#define USE_DSHOT + +// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4 +#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT) +#undef USE_UART1_TX_DMA +#endif + #define USB_IO #define USE_VCP From af2a575460c8bb7c699c56a3ddd1886d453e96b6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 28 Oct 2016 16:21:03 +0200 Subject: [PATCH 023/188] Document 3D for DSHOT and special values --- src/main/flight/mixer.c | 4 ++-- src/main/flight/mixer.h | 22 ++++++++++++++++++++-- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a4e5ec37ef..aea7b68adc 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -256,8 +256,8 @@ void initEscEndpoints(void) { disarmMotorOutput = DSHOT_DISARM_COMMAND; minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset; maxMotorOutputNormal = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH; - deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; + deadbandMotor3dHigh = DSHOT_3D_MIN_NEGATIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dLow = DSHOT_3D_MAX_POSITIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes } else #endif { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8798361050..b329128b80 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -21,12 +21,30 @@ #define QUAD_MOTOR_COUNT 4 +/* + DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings + 0 = stop + 1-5: beep + 6: ESC info request (FW Version and SN sent over the tlm wire) + 7: spin direction 1 + 8: spin direction 2 + 9: 3d mode off + 10: 3d mode on + 11: ESC settings request (saved settings over the TLM wire) + 12: save Settings + + 3D Mode: + 0 = stop + 48 (low) - 1047 (high) -> positive direction + 1048 (low) - 2047 (high) -> negative direction +*/ + // Digital protocol has fixed values #define DSHOT_DISARM_COMMAND 0 #define DSHOT_MIN_THROTTLE 48 #define DSHOT_MAX_THROTTLE 2047 -#define DSHOT_3D_DEADBAND_LOW 900 // TODO - not agreed yet -#define DSHOT_3D_DEADBAND_HIGH 1100 // TODO - not agreed yet +#define DSHOT_3D_MAX_POSITIVE 1047 // TODO - Not working yet!! Mixer requires some throttle rescaling changes +#define DSHOT_3D_MIN_NEGATIVE 1048// TODO - Not working yet!! Mixer requires some throttle rescaling changes // Note: this is called MultiType/MULTITYPE_* in baseflight. typedef enum mixerMode From 853e830c6f4d7c26fe97cc6381337b9935168787 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 29 Oct 2016 03:31:14 +0900 Subject: [PATCH 024/188] 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 3665d763d4f72ad78cf0fc75d8f2b8bfe92db1bf Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 28 Oct 2016 21:57:59 -0600 Subject: [PATCH 025/188] cleanup stm32f3 serial uart1 to match the other uarts --- src/main/drivers/serial_uart_stm32f30x.c | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 5514b16ec8..41b222fd1b 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -99,6 +99,7 @@ static uartPort_t uartPort4; static uartPort_t uartPort5; #endif +#if defined(USE_UART1_TX_DMA) || defined(USE_UART2_TX_DMA) || defined(USE_UART3_TX_DMA) static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor) { uartPort_t *s = (uartPort_t*)(descriptor->userParam); @@ -110,6 +111,7 @@ static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor) else s->txDMAEmpty = true; } +#endif void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, uint8_t af, uint8_t index) { @@ -150,29 +152,33 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option s->port.baudRate = baudRate; - s->port.rxBuffer = rx1Buffer; - s->port.txBuffer = tx1Buffer; s->port.rxBufferSize = UART1_RX_BUFFER_SIZE; s->port.txBufferSize = UART1_TX_BUFFER_SIZE; - -#ifdef USE_UART1_RX_DMA - s->rxDMAChannel = DMA1_Channel5; -#endif -#ifdef USE_UART1_TX_DMA - s->txDMAChannel = DMA1_Channel4; -#endif + s->port.rxBuffer = rx1Buffer; + s->port.txBuffer = tx1Buffer; s->USARTx = USART1; +#ifdef USE_UART1_RX_DMA + s->rxDMAChannel = DMA1_Channel5; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; +#endif +#ifdef USE_UART1_TX_DMA + s->txDMAChannel = DMA1_Channel4; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; +#endif RCC_ClockCmd(RCC_APB2(USART1), ENABLE); + +#if defined(USE_UART1_TX_DMA) || defined(USE_UART1_RX_DMA) RCC_ClockCmd(RCC_AHB(DMA1), ENABLE); +#endif serialUARTInit(IOGetByTag(IO_TAG(UART1_TX_PIN)), IOGetByTag(IO_TAG(UART1_RX_PIN)), mode, options, GPIO_AF_7, 1); +#ifdef USE_UART1_TX_DMA dmaSetHandler(DMA1_CH4_HANDLER, handleUsartTxDma, NVIC_PRIO_SERIALUART1_TXDMA, (uint32_t)&uartPort1); +#endif #ifndef USE_UART1_RX_DMA NVIC_InitTypeDef NVIC_InitStructure; From b6ef0e984b5bc70394bd025d59ef684ea5b01802 Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 28 Oct 2016 22:01:53 -0600 Subject: [PATCH 026/188] omnibus tim8 pwm outputs to tim8 irq --- src/main/target/OMNIBUS/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index c194fc9752..1f8269072b 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -28,8 +28,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 From 1e9a627bd890be2e01bed0bdbf253d0f8af85806 Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 28 Oct 2016 22:12:37 -0600 Subject: [PATCH 027/188] omnibus, only use sdcard dma when dshot is not defined, until an alternative is found --- src/main/target/OMNIBUS/target.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 05676f0046..f2af32cd3d 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -127,9 +127,13 @@ // Divide to under 25MHz for normal operation: #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 -// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. +#define USE_DSHOT + +// DSHOT output 4 uses DMA1_Channel5, so don't use it for the SDCARD until we find an alternative +#ifndef USE_DSHOT #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +#endif // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING @@ -143,8 +147,6 @@ //#define RSSI_ADC_PIN PB1 //#define ADC_INSTANCE ADC3 -#define USE_DSHOT - #define LED_STRIP #define WS2811_PIN PA8 #define WS2811_TIMER TIM1 From 1aaf2d9d47808dbd723c0c7f4ca390ea5fbe0bb3 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 29 Oct 2016 13:40:27 +0900 Subject: [PATCH 028/188] 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 d49e66d4ce0379d4e069d9938d4870832d3c2085 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 29 Oct 2016 10:26:08 +0200 Subject: [PATCH 029/188] Update DMA mapping --- src/main/target/BETAFLIGHTF3/target.c | 6 +++--- src/main/target/BETAFLIGHTF3/target.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index d6e93c8a89..82adbb72fa 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -27,9 +27,9 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel1, DMA1_CH1_HANDLER }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel4, DMA1_CH4_HANDLER }, // PWM4 - PB9 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel1, DMA1_CH1_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10,DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM4 - PB9 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 1dc435b492..f7d6db277d 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -150,4 +150,4 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) \ No newline at end of file +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) \ No newline at end of file From 1712b03801930760a891dbdde952f2d181990dda Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sat, 29 Oct 2016 12:46:07 +0300 Subject: [PATCH 030/188] Add AnyFC-F7 README.md --- src/main/target/ANYFCF7/README.md | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 src/main/target/ANYFCF7/README.md diff --git a/src/main/target/ANYFCF7/README.md b/src/main/target/ANYFCF7/README.md new file mode 100644 index 0000000000..bf8b4c7406 --- /dev/null +++ b/src/main/target/ANYFCF7/README.md @@ -0,0 +1,21 @@ +# AnyFC-F7 + +* The first F7 board flown with betaflight and inavflight, made by [@sambas](https://github.com/sambas) +* OSHW CC BY-SA 3.0 +* Source: https://github.com/sambas/hw/tree/master/AnyFCF7 +* 1st betaflight: https://www.youtube.com/watch?v=tv7k3A0FG80 +* 1st inavflight: https://www.youtube.com/watch?v=kJvlZAzprBs + +## HW info + +* STM32F745VGT6 100lqfp 216MHz +* MPU6000 SPI +* MS5611 baro +* All 8 uarts available + VCP +* 10 pwm outputs + 6 inputs +* external I2C +* external SPI (shared with U4/5) +* support for CAN +* SD card logging (SPI) +* 3 AD channels, one with 10k/1k divider, two with 1k series resistor + From 00aabf97fe1eede06a8845f0ddcad8b7e24314aa Mon Sep 17 00:00:00 2001 From: Steffen Windoffer Date: Sat, 29 Oct 2016 18:57:19 +0200 Subject: [PATCH 031/188] moved f7 debug hardfaults to a familar place --- Makefile | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index d234823bce..983c51c904 100644 --- a/Makefile +++ b/Makefile @@ -158,6 +158,10 @@ else STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S endif +ifeq ($(DEBUG_HARDFAULTS),F7) +CFLAGS += -DDEBUG_HARDFAULTS +endif + REVISION = $(shell git log -1 --format="%h") FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' ) @@ -364,7 +368,7 @@ endif ARCH_FLAGS = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fsingle-precision-constant -Wdouble-promotion ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XG_TARGETS))) -DEVICE_FLAGS = -DSTM32F745xx -DUSE_HAL_DRIVER -D__FPU_PRESENT -DDEBUG_HARDFAULTS +DEVICE_FLAGS = -DSTM32F745xx -DUSE_HAL_DRIVER -D__FPU_PRESENT LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f745.ld else $(error Unknown MCU for F7 target) @@ -650,7 +654,7 @@ STM32F7xx_COMMON_SRC = \ drivers/pwm_output_hal.c \ drivers/system_stm32f7xx.c \ drivers/serial_uart_stm32f7xx.c \ - drivers/serial_uart_hal.c + drivers/serial_uart_hal.c F7EXCLUDES = drivers/bus_spi.c \ drivers/bus_i2c.c \ From bd84ca7610a2a2efd0291a2965a9286becda588e Mon Sep 17 00:00:00 2001 From: Steffen Windoffer Date: Sat, 29 Oct 2016 19:09:46 +0200 Subject: [PATCH 032/188] removed unused variable channelAddress --- src/main/drivers/light_ws2811strip_hal.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index a9e6061bba..28bafabbf7 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -85,7 +85,7 @@ void ws2811LedStripHardwareInit(void) __DMA1_CLK_ENABLE(); - + /* Set the parameters to be configured */ hdma_tim.Init.Channel = WS2811_DMA_CHANNEL; hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; @@ -103,23 +103,18 @@ void ws2811LedStripHardwareInit(void) /* Set hdma_tim instance */ hdma_tim.Instance = WS2811_DMA_STREAM; - uint32_t channelAddress = 0; switch (WS2811_TIMER_CHANNEL) { case TIM_CHANNEL_1: timDMASource = TIM_DMA_ID_CC1; - channelAddress = (uint32_t)(&WS2811_TIMER->CCR1); break; case TIM_CHANNEL_2: timDMASource = TIM_DMA_ID_CC2; - channelAddress = (uint32_t)(&WS2811_TIMER->CCR2); break; case TIM_CHANNEL_3: timDMASource = TIM_DMA_ID_CC3; - channelAddress = (uint32_t)(&WS2811_TIMER->CCR3); break; case TIM_CHANNEL_4: timDMASource = TIM_DMA_ID_CC4; - channelAddress = (uint32_t)(&WS2811_TIMER->CCR4); break; } From 194631a7968946d90ac488ac2fe7ebc8855edc9c Mon Sep 17 00:00:00 2001 From: Steffen Windoffer Date: Sat, 29 Oct 2016 19:20:05 +0200 Subject: [PATCH 033/188] removed unused status variable --- src/main/drivers/serial_uart_hal.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index 8fc2573293..0a00d2aacc 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -57,7 +57,6 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { static void uartReconfigure(uartPort_t *uartPort) { - HAL_StatusTypeDef status = HAL_ERROR; /*RCC_PeriphCLKInitTypeDef RCC_PeriphClkInit; RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART1|RCC_PERIPHCLK_USART2|RCC_PERIPHCLK_USART3| RCC_PERIPHCLK_UART4|RCC_PERIPHCLK_UART5|RCC_PERIPHCLK_USART6|RCC_PERIPHCLK_UART7|RCC_PERIPHCLK_UART8; @@ -90,11 +89,11 @@ static void uartReconfigure(uartPort_t *uartPort) if(uartPort->port.options & SERIAL_BIDIR) { - status = HAL_HalfDuplex_Init(&uartPort->Handle); + HAL_HalfDuplex_Init(&uartPort->Handle); } else { - status = HAL_UART_Init(&uartPort->Handle); + HAL_UART_Init(&uartPort->Handle); } // Receive DMA or IRQ @@ -216,7 +215,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, s->txDMAEmpty = true; - + // common serial initialisation code should move to serialPort::init() s->port.rxBufferHead = s->port.rxBufferTail = 0; s->port.txBufferHead = s->port.txBufferTail = 0; @@ -252,7 +251,7 @@ void uartStartTxDMA(uartPort_t *s) HAL_UART_StateTypeDef state = HAL_UART_GetState(&s->Handle); if((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX) return; - + if (s->port.txBufferHead > s->port.txBufferTail) { size = s->port.txBufferHead - s->port.txBufferTail; fromwhere = s->port.txBufferTail; @@ -387,4 +386,3 @@ const struct serialPortVTable uartVTable[] = { .endWrite = NULL, } }; - From 64bced32f0f59ffd43aafe8fa8dfe56226a564b7 Mon Sep 17 00:00:00 2001 From: Steffen Windoffer Date: Sat, 29 Oct 2016 19:33:05 +0200 Subject: [PATCH 034/188] add unused tim --- src/main/drivers/timer_stm32f7xx.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c index 180116c0fd..e9992d9b19 100644 --- a/src/main/drivers/timer_stm32f7xx.c +++ b/src/main/drivers/timer_stm32f7xx.c @@ -96,6 +96,7 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { uint8_t timerClockDivisor(TIM_TypeDef *tim) { + UNUSED(tim); return 1; } From a5c14eed9aa2d375aa6a9f2701e71303310942f4 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 31 Oct 2016 02:26:50 +0900 Subject: [PATCH 035/188] 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 036/188] 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 037/188] 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 bafc2e446e42101c63aba4e0c9fee7e2e00317b4 Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Mon, 31 Oct 2016 01:57:23 -0500 Subject: [PATCH 038/188] Initial FURYF7 Target --- src/main/drivers/accgyro_spi_icm20689.c | 6 +- src/main/target/FURYF7/README.md | 18 + src/main/target/FURYF7/stm32f7xx_hal_conf.h | 446 ++++++++++++++++++++ src/main/target/FURYF7/target.c | 36 ++ src/main/target/FURYF7/target.h | 176 ++++++++ src/main/target/FURYF7/target.mk | 7 + 6 files changed, 686 insertions(+), 3 deletions(-) create mode 100644 src/main/target/FURYF7/README.md create mode 100644 src/main/target/FURYF7/stm32f7xx_hal_conf.h create mode 100644 src/main/target/FURYF7/target.c create mode 100644 src/main/target/FURYF7/target.h create mode 100644 src/main/target/FURYF7/target.mk diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 648d3bd939..6458d2f510 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -73,7 +73,7 @@ static void icm20689SpiInit(void) IOInit(icmSpi20689CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0); IOConfigGPIO(icmSpi20689CsPin, SPI_IO_CS_CFG); - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); hardwareInitialised = true; } @@ -101,7 +101,7 @@ bool icm20689SpiDetect(void) } } while (attemptsRemaining--); - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); return true; @@ -175,6 +175,6 @@ void icm20689GyroInit(uint8_t lpf) mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); } diff --git a/src/main/target/FURYF7/README.md b/src/main/target/FURYF7/README.md new file mode 100644 index 0000000000..9b6255a618 --- /dev/null +++ b/src/main/target/FURYF7/README.md @@ -0,0 +1,18 @@ +# FURYF7 + +* STM32F745VGT6 +* ICM20689 SPI Gyro +* MS5611 baro - SPI (Not Currently Woerking. Need to write driver for MS5611 SPI) +* VCP +* 3 UARTS (UART1, UART3, UART6) +* 4 PWM outputs +* No PWM inputs (SBUS, PPM, Spek Sat) +* SD card (Not currently working, Fatal Error) +* 16MB Flash (Not currently working, no chip detected) +* ADC (RSSI, CURR, VBAT) +* I2C +* LED Strip +* Built in 5v 2A Regulator +* Spek Sat Connector +* SWD Port +* Beeper output \ No newline at end of file diff --git a/src/main/target/FURYF7/stm32f7xx_hal_conf.h b/src/main/target/FURYF7/stm32f7xx_hal_conf.h new file mode 100644 index 0000000000..20a305def5 --- /dev/null +++ b/src/main/target/FURYF7/stm32f7xx_hal_conf.h @@ -0,0 +1,446 @@ +/** + ****************************************************************************** + * @file stm32f7xx_hal_conf.h + * @author MCD Application Team + * @version V1.0.0 + * @date 22-April-2016 + * @brief HAL configuration file. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT(c) 2016 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F7xx_HAL_CONF_H +#define __STM32F7xx_HAL_CONF_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/* ########################## Module Selection ############################## */ +/** + * @brief This is the list of modules to be used in the HAL driver + */ +#define HAL_MODULE_ENABLED + #define HAL_ADC_MODULE_ENABLED +/* #define HAL_CAN_MODULE_ENABLED */ +/* #define HAL_CEC_MODULE_ENABLED */ +/* #define HAL_CRC_MODULE_ENABLED */ +/* #define HAL_CRYP_MODULE_ENABLED */ +/* #define HAL_DAC_MODULE_ENABLED */ +/* #define HAL_DCMI_MODULE_ENABLED */ + #define HAL_DMA_MODULE_ENABLED +// #define HAL_DMA2D_MODULE_ENABLED +/* #define HAL_ETH_MODULE_ENABLED */ +#define HAL_FLASH_MODULE_ENABLED +/* #define HAL_NAND_MODULE_ENABLED */ +/* #define HAL_NOR_MODULE_ENABLED */ +/* #define HAL_SRAM_MODULE_ENABLED */ +/* #define HAL_SDRAM_MODULE_ENABLED */ +/* #define HAL_HASH_MODULE_ENABLED */ +#define HAL_GPIO_MODULE_ENABLED + #define HAL_I2C_MODULE_ENABLED +/* #define HAL_I2S_MODULE_ENABLED */ +/* #define HAL_IWDG_MODULE_ENABLED */ +/* #define HAL_LPTIM_MODULE_ENABLED */ +/* #define HAL_LTDC_MODULE_ENABLED */ +#define HAL_PWR_MODULE_ENABLED +/* #define HAL_QSPI_MODULE_ENABLED */ +#define HAL_RCC_MODULE_ENABLED +/* #define HAL_RNG_MODULE_ENABLED */ +//#define HAL_RTC_MODULE_ENABLED +/* #define HAL_SAI_MODULE_ENABLED */ +/* #define HAL_SD_MODULE_ENABLED */ +/* #define HAL_SPDIFRX_MODULE_ENABLED */ + #define HAL_SPI_MODULE_ENABLED + #define HAL_TIM_MODULE_ENABLED + #define HAL_UART_MODULE_ENABLED + #define HAL_USART_MODULE_ENABLED +/* #define HAL_IRDA_MODULE_ENABLED */ +/* #define HAL_SMARTCARD_MODULE_ENABLED */ +/* #define HAL_WWDG_MODULE_ENABLED */ +#define HAL_CORTEX_MODULE_ENABLED +#define HAL_PCD_MODULE_ENABLED +/* #define HAL_HCD_MODULE_ENABLED */ +/* #define HAL_DFSDM_MODULE_ENABLED */ +/* #define HAL_DSI_MODULE_ENABLED */ +/* #define HAL_JPEG_MODULE_ENABLED */ +/* #define HAL_MDIOS_MODULE_ENABLED */ + + +/* ########################## HSE/HSI Values adaptation ##################### */ +/** + * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSE is used as system clock source, directly or through the PLL). + */ +#if !defined (HSE_VALUE) + #define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined (HSE_STARTUP_TIMEOUT) + #define HSE_STARTUP_TIMEOUT ((uint32_t)100U) /*!< Time out for HSE start up, in ms */ +#endif /* HSE_STARTUP_TIMEOUT */ + +/** + * @brief Internal High Speed oscillator (HSI) value. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSI is used as system clock source, directly or through the PLL). + */ +#if !defined (HSI_VALUE) + #define HSI_VALUE ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ + +/** + * @brief Internal Low Speed oscillator (LSI) value. + */ +#if !defined (LSI_VALUE) + #define LSI_VALUE ((uint32_t)32000U) /*!< LSI Typical Value in Hz*/ +#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz + The real value may vary depending on the variations + in voltage and temperature. */ +/** + * @brief External Low Speed oscillator (LSE) value. + */ +#if !defined (LSE_VALUE) + #define LSE_VALUE ((uint32_t)32768U) /*!< Value of the External Low Speed oscillator in Hz */ +#endif /* LSE_VALUE */ + +#if !defined (LSE_STARTUP_TIMEOUT) + #define LSE_STARTUP_TIMEOUT ((uint32_t)5000U) /*!< Time out for LSE start up, in ms */ +#endif /* LSE_STARTUP_TIMEOUT */ + +/** + * @brief External clock source for I2S peripheral + * This value is used by the I2S HAL module to compute the I2S clock source + * frequency, this source is inserted directly through I2S_CKIN pad. + */ +#if !defined (EXTERNAL_CLOCK_VALUE) + #define EXTERNAL_CLOCK_VALUE ((uint32_t)12288000U) /*!< Value of the Internal oscillator in Hz*/ +#endif /* EXTERNAL_CLOCK_VALUE */ + +/* Tip: To avoid modifying this file each time you need to use different HSE, + === you can define the HSE value in your toolchain compiler preprocessor. */ + +/* ########################### System Configuration ######################### */ +/** + * @brief This is the HAL system configuration section + */ +#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */ +#define TICK_INT_PRIORITY ((uint32_t)0x0FU) /*!< tick interrupt priority */ +#define USE_RTOS 0U +#define PREFETCH_ENABLE 1U +#define ART_ACCLERATOR_ENABLE 1U /* To enable instruction cache and prefetch */ + +/* ########################## Assert Selection ############################## */ +/** + * @brief Uncomment the line below to expanse the "assert_param" macro in the + * HAL drivers code + */ +/* #define USE_FULL_ASSERT 1 */ + +/* ################## Ethernet peripheral configuration for NUCLEO 144 board ##################### */ + +/* Section 1 : Ethernet peripheral configuration */ + +/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */ +#define MAC_ADDR0 2U +#define MAC_ADDR1 0U +#define MAC_ADDR2 0U +#define MAC_ADDR3 0U +#define MAC_ADDR4 0U +#define MAC_ADDR5 0U + +/* Definition of the Ethernet driver buffers size and count */ +#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ +#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ +#define ETH_RXBUFNB ((uint32_t)5) /* 5 Rx buffers of size ETH_RX_BUF_SIZE */ +#define ETH_TXBUFNB ((uint32_t)5) /* 5 Tx buffers of size ETH_TX_BUF_SIZE */ + +/* Section 2: PHY configuration section */ +/* LAN8742A PHY Address*/ +#define LAN8742A_PHY_ADDRESS 0x00 +/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/ +#define PHY_RESET_DELAY ((uint32_t)0x00000FFF) +/* PHY Configuration delay */ +#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFF) + +#define PHY_READ_TO ((uint32_t)0x0000FFFF) +#define PHY_WRITE_TO ((uint32_t)0x0000FFFF) + +/* Section 3: Common PHY Registers */ + +#define PHY_BCR ((uint16_t)0x00) /*!< Transceiver Basic Control Register */ +#define PHY_BSR ((uint16_t)0x01) /*!< Transceiver Basic Status Register */ + +#define PHY_RESET ((uint16_t)0x8000) /*!< PHY Reset */ +#define PHY_LOOPBACK ((uint16_t)0x4000) /*!< Select loop-back mode */ +#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100) /*!< Set the full-duplex mode at 100 Mb/s */ +#define PHY_HALFDUPLEX_100M ((uint16_t)0x2000) /*!< Set the half-duplex mode at 100 Mb/s */ +#define PHY_FULLDUPLEX_10M ((uint16_t)0x0100) /*!< Set the full-duplex mode at 10 Mb/s */ +#define PHY_HALFDUPLEX_10M ((uint16_t)0x0000) /*!< Set the half-duplex mode at 10 Mb/s */ +#define PHY_AUTONEGOTIATION ((uint16_t)0x1000) /*!< Enable auto-negotiation function */ +#define PHY_RESTART_AUTONEGOTIATION ((uint16_t)0x0200) /*!< Restart auto-negotiation function */ +#define PHY_POWERDOWN ((uint16_t)0x0800) /*!< Select the power down mode */ +#define PHY_ISOLATE ((uint16_t)0x0400) /*!< Isolate PHY from MII */ + +#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020) /*!< Auto-Negotiation process completed */ +#define PHY_LINKED_STATUS ((uint16_t)0x0004) /*!< Valid link established */ +#define PHY_JABBER_DETECTION ((uint16_t)0x0002) /*!< Jabber condition detected */ + +/* Section 4: Extended PHY Registers */ + +#define PHY_SR ((uint16_t)0x1F) /*!< PHY special control/ status register Offset */ + +#define PHY_SPEED_STATUS ((uint16_t)0x0004) /*!< PHY Speed mask */ +#define PHY_DUPLEX_STATUS ((uint16_t)0x0010) /*!< PHY Duplex mask */ + + +#define PHY_ISFR ((uint16_t)0x1D) /*!< PHY Interrupt Source Flag register Offset */ +#define PHY_ISFR_INT4 ((uint16_t)0x0010) /*!< PHY Link down inturrupt */ + +/* ################## SPI peripheral configuration ########################## */ + +/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver +* Activated: CRC code is present inside driver +* Deactivated: CRC code cleaned from driver +*/ + +#define USE_SPI_CRC 1U + +/* Includes ------------------------------------------------------------------*/ +/** + * @brief Include module's header file + */ + +#ifdef HAL_RCC_MODULE_ENABLED + #include "stm32f7xx_hal_rcc.h" +#endif /* HAL_RCC_MODULE_ENABLED */ + +#ifdef HAL_GPIO_MODULE_ENABLED + #include "stm32f7xx_hal_gpio.h" +#endif /* HAL_GPIO_MODULE_ENABLED */ + +#ifdef HAL_DMA_MODULE_ENABLED + #include "stm32f7xx_hal_dma.h" +#endif /* HAL_DMA_MODULE_ENABLED */ + +#ifdef HAL_CORTEX_MODULE_ENABLED + #include "stm32f7xx_hal_cortex.h" +#endif /* HAL_CORTEX_MODULE_ENABLED */ + +#ifdef HAL_ADC_MODULE_ENABLED + #include "stm32f7xx_hal_adc.h" +#endif /* HAL_ADC_MODULE_ENABLED */ + +#ifdef HAL_CAN_MODULE_ENABLED + #include "stm32f7xx_hal_can.h" +#endif /* HAL_CAN_MODULE_ENABLED */ + +#ifdef HAL_CEC_MODULE_ENABLED + #include "stm32f7xx_hal_cec.h" +#endif /* HAL_CEC_MODULE_ENABLED */ + +#ifdef HAL_CRC_MODULE_ENABLED + #include "stm32f7xx_hal_crc.h" +#endif /* HAL_CRC_MODULE_ENABLED */ + +#ifdef HAL_CRYP_MODULE_ENABLED + #include "stm32f7xx_hal_cryp.h" +#endif /* HAL_CRYP_MODULE_ENABLED */ + +#ifdef HAL_DMA2D_MODULE_ENABLED + #include "stm32f7xx_hal_dma2d.h" +#endif /* HAL_DMA2D_MODULE_ENABLED */ + +#ifdef HAL_DAC_MODULE_ENABLED + #include "stm32f7xx_hal_dac.h" +#endif /* HAL_DAC_MODULE_ENABLED */ + +#ifdef HAL_DCMI_MODULE_ENABLED + #include "stm32f7xx_hal_dcmi.h" +#endif /* HAL_DCMI_MODULE_ENABLED */ + +#ifdef HAL_ETH_MODULE_ENABLED + #include "stm32f7xx_hal_eth.h" +#endif /* HAL_ETH_MODULE_ENABLED */ + +#ifdef HAL_FLASH_MODULE_ENABLED + #include "stm32f7xx_hal_flash.h" +#endif /* HAL_FLASH_MODULE_ENABLED */ + +#ifdef HAL_SRAM_MODULE_ENABLED + #include "stm32f7xx_hal_sram.h" +#endif /* HAL_SRAM_MODULE_ENABLED */ + +#ifdef HAL_NOR_MODULE_ENABLED + #include "stm32f7xx_hal_nor.h" +#endif /* HAL_NOR_MODULE_ENABLED */ + +#ifdef HAL_NAND_MODULE_ENABLED + #include "stm32f7xx_hal_nand.h" +#endif /* HAL_NAND_MODULE_ENABLED */ + +#ifdef HAL_SDRAM_MODULE_ENABLED + #include "stm32f7xx_hal_sdram.h" +#endif /* HAL_SDRAM_MODULE_ENABLED */ + +#ifdef HAL_HASH_MODULE_ENABLED + #include "stm32f7xx_hal_hash.h" +#endif /* HAL_HASH_MODULE_ENABLED */ + +#ifdef HAL_I2C_MODULE_ENABLED + #include "stm32f7xx_hal_i2c.h" +#endif /* HAL_I2C_MODULE_ENABLED */ + +#ifdef HAL_I2S_MODULE_ENABLED + #include "stm32f7xx_hal_i2s.h" +#endif /* HAL_I2S_MODULE_ENABLED */ + +#ifdef HAL_IWDG_MODULE_ENABLED + #include "stm32f7xx_hal_iwdg.h" +#endif /* HAL_IWDG_MODULE_ENABLED */ + +#ifdef HAL_LPTIM_MODULE_ENABLED + #include "stm32f7xx_hal_lptim.h" +#endif /* HAL_LPTIM_MODULE_ENABLED */ + +#ifdef HAL_LTDC_MODULE_ENABLED + #include "stm32f7xx_hal_ltdc.h" +#endif /* HAL_LTDC_MODULE_ENABLED */ + +#ifdef HAL_PWR_MODULE_ENABLED + #include "stm32f7xx_hal_pwr.h" +#endif /* HAL_PWR_MODULE_ENABLED */ + +#ifdef HAL_QSPI_MODULE_ENABLED + #include "stm32f7xx_hal_qspi.h" +#endif /* HAL_QSPI_MODULE_ENABLED */ + +#ifdef HAL_RNG_MODULE_ENABLED + #include "stm32f7xx_hal_rng.h" +#endif /* HAL_RNG_MODULE_ENABLED */ + +#ifdef HAL_RTC_MODULE_ENABLED + #include "stm32f7xx_hal_rtc.h" +#endif /* HAL_RTC_MODULE_ENABLED */ + +#ifdef HAL_SAI_MODULE_ENABLED + #include "stm32f7xx_hal_sai.h" +#endif /* HAL_SAI_MODULE_ENABLED */ + +#ifdef HAL_SD_MODULE_ENABLED + #include "stm32f7xx_hal_sd.h" +#endif /* HAL_SD_MODULE_ENABLED */ + +#ifdef HAL_SPDIFRX_MODULE_ENABLED + #include "stm32f7xx_hal_spdifrx.h" +#endif /* HAL_SPDIFRX_MODULE_ENABLED */ + +#ifdef HAL_SPI_MODULE_ENABLED + #include "stm32f7xx_hal_spi.h" +#endif /* HAL_SPI_MODULE_ENABLED */ + +#ifdef HAL_TIM_MODULE_ENABLED + #include "stm32f7xx_hal_tim.h" +#endif /* HAL_TIM_MODULE_ENABLED */ + +#ifdef HAL_UART_MODULE_ENABLED + #include "stm32f7xx_hal_uart.h" +#endif /* HAL_UART_MODULE_ENABLED */ + +#ifdef HAL_USART_MODULE_ENABLED + #include "stm32f7xx_hal_usart.h" +#endif /* HAL_USART_MODULE_ENABLED */ + +#ifdef HAL_IRDA_MODULE_ENABLED + #include "stm32f7xx_hal_irda.h" +#endif /* HAL_IRDA_MODULE_ENABLED */ + +#ifdef HAL_SMARTCARD_MODULE_ENABLED + #include "stm32f7xx_hal_smartcard.h" +#endif /* HAL_SMARTCARD_MODULE_ENABLED */ + +#ifdef HAL_WWDG_MODULE_ENABLED + #include "stm32f7xx_hal_wwdg.h" +#endif /* HAL_WWDG_MODULE_ENABLED */ + +#ifdef HAL_PCD_MODULE_ENABLED + #include "stm32f7xx_hal_pcd.h" +#endif /* HAL_PCD_MODULE_ENABLED */ + +#ifdef HAL_HCD_MODULE_ENABLED + #include "stm32f7xx_hal_hcd.h" +#endif /* HAL_HCD_MODULE_ENABLED */ + +#ifdef HAL_DFSDM_MODULE_ENABLED + #include "stm32f7xx_hal_dfsdm.h" +#endif /* HAL_DFSDM_MODULE_ENABLED */ + +#ifdef HAL_DSI_MODULE_ENABLED + #include "stm32f7xx_hal_dsi.h" +#endif /* HAL_DSI_MODULE_ENABLED */ + +#ifdef HAL_JPEG_MODULE_ENABLED + #include "stm32f7xx_hal_jpeg.h" +#endif /* HAL_JPEG_MODULE_ENABLED */ + +#ifdef HAL_MDIOS_MODULE_ENABLED + #include "stm32f7xx_hal_mdios.h" +#endif /* HAL_MDIOS_MODULE_ENABLED */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ + #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ + void assert_failed(uint8_t* file, uint32_t line); +#else + #define assert_param(expr) ((void)0) +#endif /* USE_FULL_ASSERT */ + + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F7xx_HAL_CONF_H */ + + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file diff --git a/src/main/target/FURYF7/target.c b/src/main/target/FURYF7/target.c new file mode 100644 index 0000000000..9d934b12c6 --- /dev/null +++ b/src/main/target/FURYF7/target.c @@ -0,0 +1,36 @@ + +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/dma.h" + +#include "drivers/timer.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF3_TIM8, NULL, 0, 0 }, // PPM_IN + + { TIM3, IO_TAG(PB0), TIM_CHANNEL_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, DMA1_Stream7, DMA_CHANNEL_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, DMA1_Stream2, DMA_CHANNEL_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S4_OUT + +// { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5 }, // LED Strip +}; \ No newline at end of file diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h new file mode 100644 index 0000000000..8da6562712 --- /dev/null +++ b/src/main/target/FURYF7/target.h @@ -0,0 +1,176 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FYF7" + +#define CONFIG_START_FLASH_ADDRESS (0x080C0000) + +#define USBD_PRODUCT_STRING "FuryF7" + +#define USE_DSHOT + +#define LED0 PB5 +#define LED1 PB4 + +#define BEEPER PD10 +#define BEEPER_INVERTED + +#define USE_EXTI +#define MPU_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define ICM20689_CS_PIN PA4 +#define ICM20689_SPI_INSTANCE SPI1 + +#define GYRO +#define USE_GYRO_SPI_ICM20689 +#define GYRO_ICM20689_ALIGN CW180_DEG + +#define ACC +#define USE_ACC_SPI_ICM20689 +#define ACC_ICM20689_ALIGN CW180_DEG + +//#define BARO +//#define USE_BARO_MS5611 +//#define MS5611_I2C_INSTANCE I2CDEV_1 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +//#define USE_SPI_DEVICE_2 +//#define SPI2_NSS_PIN PB12 +//#define SPI2_SCK_PIN PB13 +//#define SPI2_MISO_PIN PB14 +//#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_SPI_DEVICE_4 +#define SPI4_NSS_PIN PE11 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_SDCARD +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PD2 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD +#define SDCARD_DETECT_EXTI_IRQn EXTI2_IRQn + +#define SDCARD_SPI_INSTANCE SPI4 +#define SDCARD_SPI_CS_PIN SPI4_NSS_PIN + +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 13.5MHz + +#define SDCARD_DMA_CHANNEL_TX DMA2_Stream1 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5 +#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA2 +#define SDCARD_DMA_CHANNEL DMA_CHANNEL_4 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PA15 +#define M25P16_SPI_INSTANCE SPI3 + +#define USE_VCP +#define VBUS_SENSING_PIN PC5 + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART3 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA +#define USE_I2C_PULLUP +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER +#define VBAT_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define CURRENT_METER_ADC_PIN PC3 + +//#define LED_STRIP +//#define WS2811_PIN PA0 +//#define WS2811_TIMER TIM5 +//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER +//#define WS2811_DMA_STREAM DMA1_Stream2 +//#define WS2811_DMA_IT DMA_IT_TCIF2 +//#define WS2811_DMA_CHANNEL DMA_Channel_6 +//#define WS2811_TIMER_CHANNEL TIM_Channel_1 + +// LED Strip can run off Pin 6 (PA0) of the ESC outputs. + +//#define WS2811_PIN PA1 +//#define WS2811_TIMER TIM5 +//#define WS2811_TIMER_CHANNEL TIM_CHANNEL_2 +//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER +//#define WS2811_DMA_STREAM DMA1_Stream4 +//#define WS2811_DMA_FLAG DMA_FLAG_TCIF4 +//#define WS2811_DMA_IT DMA_IT_TCIF4 +//#define WS2811_DMA_CHANNEL DMA_CHANNEL_6 +//#define WS2811_DMA_IRQ DMA1_Stream4_IRQn + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define SENSORS_SET (SENSOR_ACC) + +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define SPEKTRUM_BIND +// USART3 Rx, PB11 +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 5 +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8)) \ No newline at end of file diff --git a/src/main/target/FURYF7/target.mk b/src/main/target/FURYF7/target.mk new file mode 100644 index 0000000000..77cfc3e5f7 --- /dev/null +++ b/src/main/target/FURYF7/target.mk @@ -0,0 +1,7 @@ +F7X5XG_TARGETS += $(TARGET) +FEATURES += SDCARD VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_icm20689.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_hal.c \ No newline at end of file From 932ef0d22eb772174cba053359892fa0c0c418a0 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 31 Oct 2016 11:56:32 +1300 Subject: [PATCH 039/188] Cleaned up resource related commands in CLI. --- src/main/io/serial_cli.c | 118 +++++++++++++++++++++++++++------------ 1 file changed, 83 insertions(+), 35 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0af9572e5e..f757a87b25 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -147,7 +147,8 @@ static void cliTasks(char *cmdline); #endif static void cliVersion(char *cmdline); static void cliRxRange(char *cmdline); -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) +static void printResource(uint8_t dumpMask, master_t *defaultConfig); static void cliResource(char *cmdline); #endif #ifdef GPS @@ -202,6 +203,7 @@ typedef enum { DUMP_ALL = (1 << 3), DO_DIFF = (1 << 4), SHOW_DEFAULTS = (1 << 5), + HIDE_UNUSED = (1 << 6), } dumpFlags_e; static const char* const emptyName = "-"; @@ -236,7 +238,7 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } }; -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) // sync this with sensors_e static const char * const sensorTypeNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL @@ -330,7 +332,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), #endif CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), @@ -2738,6 +2740,11 @@ static void printConfig(char *cmdline, bool doDiff) #endif printName(dumpMask); +#ifndef CLI_MINIMAL_VERBOSITY + cliPrint("\r\n# resources\r\n"); +#endif + printResource(dumpMask, &defaultConfig); + #ifndef USE_QUAD_MIXER_ONLY #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# mixer\r\n"); @@ -3720,7 +3727,7 @@ void cliProcess(void) } } -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) typedef struct { const uint8_t owner; @@ -3746,13 +3753,72 @@ const cliResourceValue_t resourceTable[] = { #endif }; +static void printResource(uint8_t dumpMask, master_t *defaultConfig) +{ + for (unsigned int i = 0; i < ARRAYLEN(resourceTable); i++) { + const char* owner; + owner = ownerNames[resourceTable[i].owner]; + + if (resourceTable[i].maxIndex > 0) { + for (int index = 0; index < resourceTable[i].maxIndex; index++) { + ioTag_t ioPtr = *(resourceTable[i].ptr + index); + ioTag_t ioPtrDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + + IO_t io = IOGetByTag(ioPtr); + IO_t ioDefault = IOGetByTag(ioPtrDefault); + bool equalsDefault = io == ioDefault; + const char *format = "resource %s %d %c%02d\r\n"; + const char *formatUnassigned = "resource %s %d NONE\r\n"; + if (DEFIO_TAG_ISEMPTY(ioDefault)) { + cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); + } else { + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault)); + } + if (DEFIO_TAG_ISEMPTY(io)) { + if (!(dumpMask & HIDE_UNUSED)) { + cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); + } + } else { + cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + } + } + } else { + ioTag_t ioPtr = *resourceTable[i].ptr; + ioTag_t ioPtrDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + + IO_t io = IOGetByTag(ioPtr); + IO_t ioDefault = IOGetByTag(ioPtrDefault); + bool equalsDefault = io == ioDefault; + const char *format = "resource %s %c%02d\r\n"; + const char *formatUnassigned = "resource %s NONE\r\n"; + if (DEFIO_TAG_ISEMPTY(ioDefault)) { + cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner); + } else { + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault)); + } + if (DEFIO_TAG_ISEMPTY(io)) { + if (!(dumpMask & HIDE_UNUSED)) { + cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner); + } + } else { + cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + } + } + } +} + static void cliResource(char *cmdline) { - int len; - len = strlen(cmdline); + int len = strlen(cmdline); if (len == 0) { - cliPrintf("IO:\r\n----------------------\r\n"); + printResource(DUMP_MASTER | HIDE_UNUSED, NULL); + + return; + } else if (strncasecmp(cmdline, "list", len) == 0) { +#ifndef CLI_MINIMAL_VERBOSITY + cliPrintf("Currently active IO resource assignments:\r\n(reboot to update)\r\n----------------------\r\n"); +#endif for (uint32_t i = 0; i < DEFIO_IO_USED_COUNT; i++) { const char* owner; owner = ownerNames[ioRecs[i].owner]; @@ -3766,34 +3832,10 @@ static void cliResource(char *cmdline) cliPrintf("%c%02d: %s %s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, resource); } } - cliPrintf("\r\nUse: 'resource list' to see how to change resources.\r\n"); - return; - } else if (strncasecmp(cmdline, "list", len) == 0) { - for (uint8_t i = 0; i < ARRAYLEN(resourceTable); i++) { - const char* owner; - owner = ownerNames[resourceTable[i].owner]; +#ifndef CLI_MINIMAL_VERBOSITY + cliPrintf("\r\nUse: 'resource' to see how to change resources.\r\n"); +#endif - if (resourceTable[i].maxIndex > 0) { - for (int index = 0; index < resourceTable[i].maxIndex; index++) { - - if (DEFIO_TAG_ISEMPTY(*(resourceTable[i].ptr + index))) { - continue; - } - - IO_t io = IOGetByTag(*(resourceTable[i].ptr + index)); - if (!io) { - continue; - } - cliPrintf("resource %s %d %c%02d\r\n", owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); - } - } else { - if (DEFIO_TAG_ISEMPTY(*(resourceTable[i].ptr))) { - continue; - } - IO_t io = IOGetByTag(*resourceTable[i].ptr); - cliPrintf("resource %s %c%02d\r\n", owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); - } - } return; } @@ -3834,7 +3876,11 @@ static void cliResource(char *cmdline) cliPrintf("Resource is freed!"); return; } else { - uint8_t port = (*pch)-'A'; + uint8_t port = (*pch) - 'A'; + if (port >= 8) { + port = (*pch) - 'a'; + } + if (port < 8) { pch++; pin = atoi(pch); @@ -3859,7 +3905,9 @@ static void cliResource(char *cmdline) void cliDfu(char *cmdLine) { UNUSED(cmdLine); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\nRestarting in DFU mode"); +#endif cliRebootEx(true); } From 4c8e08b4028fca4172581b2a869fb20c2d5a96da Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Mon, 24 Oct 2016 21:29:23 +0200 Subject: [PATCH 040/188] smartport / MSP bridge draft implementation implements #1311 --- src/main/telemetry/smartport.c | 295 +++++++++++++++++++++++++++++++-- 1 file changed, 277 insertions(+), 18 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 4be785d479..5d1e800c8f 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -23,6 +23,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/fc_msp.h" #include "io/beeper.h" #include "io/motors.h" @@ -56,6 +57,8 @@ #include "config/config_profile.h" #include "config/feature.h" +#include "msp/msp.h" + extern profile_t *currentProfile; extern controlRateConfig_t *currentControlRateProfile; @@ -70,7 +73,11 @@ enum enum { FSSP_START_STOP = 0x7E, + FSSP_BYTE_STUFF = 0x7D, + FSSP_DATA_FRAME = 0x10, + FSSP_MSPC_FRAME = 0x30, // MSP client frame + FSSP_MSPS_FRAME = 0x32, // MSP server frame // ID of sensor. Must be something that is polled by FrSky RX FSSP_SENSOR_ID1 = 0x1B, @@ -152,22 +159,79 @@ static uint8_t smartPortHasRequest = 0; static uint8_t smartPortIdCnt = 0; static uint32_t smartPortLastRequestTime = 0; +/* uint8_t physicalId */ +/* uint8_t primId */ +/* uint16_t id */ +/* uint32_t data */ +#define SMARTPORT_FRAME_SIZE 8 + +#define SMARTPORT_MSP_VERSION 1 + +static uint8_t smartPortRxBuffer[SMARTPORT_FRAME_SIZE]; +static uint8_t smartPortRxBytes = 0; +static bool smartPortFrameReceived = false; + +static uint8_t smartPortMspTxBuffer[256]; +static mspPacket_t smartPortMspReply; +static bool smartPortMspReplyPending = false; + +// If set, this should be executed once the reply buffer +// has been sent back to the transmitter +static mspPostProcessFnPtr mspPostProcessFn = NULL; + static void smartPortDataReceive(uint16_t c) { + static bool skipUntilStart = true; + static bool byteStuffing = false; + uint32_t now = millis(); - // look for a valid request sequence - static uint8_t lastChar; - if (lastChar == FSSP_START_STOP) { - smartPortState = SPSTATE_WORKING; - if (c == FSSP_SENSOR_ID1 && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { + if (c == FSSP_START_STOP) { + smartPortRxBytes = 0; + smartPortHasRequest = 0; + skipUntilStart = false; + return; + } + else if (skipUntilStart) { + return; + } + + if (smartPortRxBytes == 0) { + if ((c == FSSP_SENSOR_ID1) + && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { + + // our slot is starting... smartPortLastRequestTime = now; smartPortHasRequest = 1; - // we only responde to these IDs - // the X4R-SB does send other IDs, we ignore them, but take note of the time + } + else if (c == FSSP_SENSOR_ID2) { + smartPortRxBuffer[smartPortRxBytes++] = c; + } + else { + skipUntilStart = true; + } + } + else { + + //TODO: add CRC checking + + if (c == FSSP_BYTE_STUFF) { + byteStuffing = true; + return; + } + + if (byteStuffing) { + c ^= 0x20; + byteStuffing = false; + } + + smartPortRxBuffer[smartPortRxBytes++] = c; + + if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) { + smartPortFrameReceived = true; + skipUntilStart = true; } } - lastChar = c; } static void smartPortSendByte(uint8_t c, uint16_t *crcp) @@ -190,21 +254,31 @@ static void smartPortSendByte(uint8_t c, uint16_t *crcp) *crcp = crc; } -static void smartPortSendPackage(uint16_t id, uint32_t val) +static void smartPortSendPackageEx(uint8_t primId, uint8_t* data) { uint16_t crc = 0; - smartPortSendByte(FSSP_DATA_FRAME, &crc); - uint8_t *u8p = (uint8_t*)&id; - smartPortSendByte(u8p[0], &crc); - smartPortSendByte(u8p[1], &crc); - u8p = (uint8_t*)&val; - smartPortSendByte(u8p[0], &crc); - smartPortSendByte(u8p[1], &crc); - smartPortSendByte(u8p[2], &crc); - smartPortSendByte(u8p[3], &crc); + smartPortSendByte(primId, &crc); + for(uint8_t i=0;i<6;i++) { + smartPortSendByte(*(data++), &crc); + } smartPortSendByte(0xFF - (uint8_t)crc, NULL); } +static void smartPortSendPackage(uint16_t id, uint32_t val) +{ + uint8_t packet[6]; + uint8_t *u8p = (uint8_t*)&id; + packet[0] = u8p[0]; + packet[1] = u8p[1]; + u8p = (uint8_t*)&val; + packet[2] = u8p[0]; + packet[3] = u8p[1]; + packet[4] = u8p[2]; + packet[5] = u8p[3]; + + smartPortSendPackageEx(FSSP_DATA_FRAME,packet); +} + void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) { telemetryConfig = initialTelemetryConfig; @@ -270,6 +344,175 @@ void checkSmartPortTelemetryState(void) freeSmartPortTelemetryPort(); } +static void resetMspPacket(mspPacket_t* packet) +{ + packet->buf.ptr = NULL; + packet->buf.end = NULL; + packet->cmd = -1; + packet->result = 0; +} + +static void processMspPacket(mspPacket_t* packet) +{ + smartPortMspReply.buf.ptr = smartPortMspTxBuffer; + smartPortMspReply.buf.end = ARRAYEND(smartPortMspTxBuffer); + + smartPortMspReply.cmd = -1; + smartPortMspReply.result = 0; + + mspPostProcessFn = NULL; + const mspResult_e status = mspFcProcessCommand(packet, &smartPortMspReply, + &mspPostProcessFn); + + if (status != MSP_RESULT_NO_REPLY) { + // change streambuf direction + sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); + + smartPortMspReplyPending = true; + } + else { + //TODO: send ACK reply to avoid re-transmission? + } +} + +/** + * Frame format: + * - Header: 1 byte + * - Version: 3 bits + * - Start-flag: 1 bit + * - CSeq: 4 bits + * + * - MSP payload: + * - Size: 1 Byte + * - Type: 1 Byte + * - payload... + * - CRC + */ +void handleSmartPortMspFrame(uint8_t* frame, uint8_t size) +{ + static uint8_t mspBuffer[64]; + static uint8_t mspStarted = 0; + static uint8_t lastSeq = 0; + static uint8_t checksum = 0; + + static mspPacket_t cmd = { + .buf = { .ptr = mspBuffer, .end = mspBuffer }, + .cmd = -1, + .result = 0 + }; + + //TODO: re-assemble MSP frame & forward to MSP port when complete + uint8_t* p = frame; + uint8_t* end = frame + size; + + uint8_t head = *(p++); + uint8_t seq = head & 0xF; + uint8_t version = (head & 0xE0) >> 5; + + if (version != SMARTPORT_MSP_VERSION) { + // TODO: should a version mismatch error + // be sent back to the transmitter? + resetMspPacket(&cmd); + return; + } + + // check start-flag + if (head & (1 << 4)) { + + //TODO: if (frame[1] > 64) error! + uint8_t p_size = *(p++); + cmd.cmd = *(p++); + cmd.result = 0; + + cmd.buf.ptr = mspBuffer; + cmd.buf.end = mspBuffer + p_size; + + checksum = p_size ^ cmd.cmd; + mspStarted = 1; + } + else if (!mspStarted) { + // no start packet yet, throw this one away + return; + } + else if (((lastSeq + 1) & 0x7) != seq) { + // packet loss detected! + resetMspPacket(&cmd); + return; + } + + // copy payload bytes + while ((p < end) && sbufBytesRemaining(&cmd.buf)) { + checksum ^= *p; + sbufWriteU8(&cmd.buf,*(p++)); + } + + // reached end of smart port frame + if (p == end) { + lastSeq = seq; + return; + } + + // last byte must be the checksum + if (checksum != *p) { + resetMspPacket(&cmd); + return; + } + + // end of MSP packet reached + mspStarted = 0; + sbufSwitchToReader(&cmd.buf,mspBuffer); + + processMspPacket(&cmd); +} + +bool smartPortSendMspReply() +{ + static uint8_t checksum = 0; + static uint8_t seq = 0; + + uint8_t packet[6]; + uint8_t* p = packet; + uint8_t* end = p + 6; + + sbuf_t* txBuf = &smartPortMspReply.buf; + + // detect first reply packet + if (txBuf->ptr == smartPortMspTxBuffer) { + + uint8_t size = sbufBytesRemaining(txBuf); + + // header + *(p++) = (SMARTPORT_MSP_VERSION << 5) | (1 << 4) | (seq++ & 0xF); + + *(p++) = size; + *(p++) = smartPortMspReply.cmd; + + checksum = sbufBytesRemaining(txBuf) ^ smartPortMspReply.cmd; + } + + while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { + *p = sbufReadU8(txBuf); + checksum ^= *(p++); // MSP checksum + } + + // to be continued... + if (p == end) { + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return true; + } + + // nothing left in txBuf, + // append the MSP checksum + *(p++) = checksum; + + // pad with zeros + while (p < end) + *(p++) = 0; + + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return false; +} + void handleSmartPortTelemetry(void) { uint32_t smartPortLastServiceTime = millis(); @@ -295,6 +538,16 @@ void handleSmartPortTelemetry(void) return; } + if(smartPortFrameReceived) { + smartPortFrameReceived = false; + // do not check the physical ID here again + // unless we start receiving other sensors' packets + uint8_t primId = smartPortRxBuffer[1]; + if(primId == FSSP_MSPC_FRAME) { + handleSmartPortMspFrame(smartPortRxBuffer+2,6); + } + } + while (smartPortHasRequest) { // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long. if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) { @@ -302,6 +555,12 @@ void handleSmartPortTelemetry(void) return; } + if(smartPortMspReplyPending) { + smartPortMspReplyPending = smartPortSendMspReply(); + smartPortHasRequest = 0; + return; + } + // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; if (id == 0) { // end of table reached, loop back From 89e810c1b24360061bff55591289e66f4891e3fd Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Wed, 26 Oct 2016 03:19:19 +0200 Subject: [PATCH 041/188] repeat the SPORT/MSP header for each SPORT message --- src/main/telemetry/smartport.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 5d1e800c8f..a808f9a93a 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -489,6 +489,10 @@ bool smartPortSendMspReply() checksum = sbufBytesRemaining(txBuf) ^ smartPortMspReply.cmd; } + else { + // header + *(p++) = (SMARTPORT_MSP_VERSION << 5) | (seq++ & 0xF); + } while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { *p = sbufReadU8(txBuf); From 9eb99048e3e6da6d2a554f00d257e860a424d1ae Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Wed, 26 Oct 2016 03:30:36 +0200 Subject: [PATCH 042/188] remove MSP command from replies --- src/main/telemetry/smartport.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index a808f9a93a..db0956e0a4 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -483,10 +483,7 @@ bool smartPortSendMspReply() // header *(p++) = (SMARTPORT_MSP_VERSION << 5) | (1 << 4) | (seq++ & 0xF); - *(p++) = size; - *(p++) = smartPortMspReply.cmd; - checksum = sbufBytesRemaining(txBuf) ^ smartPortMspReply.cmd; } else { From 9e0efe0a76d6bb93a45d8f979ca9a7176ec99de2 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Wed, 26 Oct 2016 13:57:09 +0200 Subject: [PATCH 043/188] SPORT/MSP bridge: code cleanup --- src/main/telemetry/smartport.c | 128 ++++++++++++++++++--------------- 1 file changed, 70 insertions(+), 58 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index db0956e0a4..52f7f69058 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -73,7 +73,9 @@ enum enum { FSSP_START_STOP = 0x7E, - FSSP_BYTE_STUFF = 0x7D, + + FSSP_DLE = 0x7D, + FSSP_DLE_XOR = 0x20, FSSP_DATA_FRAME = 0x10, FSSP_MSPC_FRAME = 0x30, // MSP client frame @@ -159,26 +161,37 @@ static uint8_t smartPortHasRequest = 0; static uint8_t smartPortIdCnt = 0; static uint32_t smartPortLastRequestTime = 0; -/* uint8_t physicalId */ -/* uint8_t primId */ -/* uint16_t id */ -/* uint32_t data */ -#define SMARTPORT_FRAME_SIZE 8 +typedef struct smartPortFrame_s { + uint8_t sensorId; + uint8_t frameId; + uint16_t valueId; + uint32_t data; +} smartPortFrame_t; -#define SMARTPORT_MSP_VERSION 1 +#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t) +#define SMARTPORT_TX_BUF_SIZE 256 + +#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - 2*sizeof(uint8_t)) +#define SMARTPORT_PAYLOAD_OFFSET 2 static uint8_t smartPortRxBuffer[SMARTPORT_FRAME_SIZE]; static uint8_t smartPortRxBytes = 0; static bool smartPortFrameReceived = false; -static uint8_t smartPortMspTxBuffer[256]; +#define SMARTPORT_MSP_VERSION 1 +#define SMARTPORT_MSP_VER_SHIFT 5 +#define SMARTPORT_MSP_VER_MASK 0xE0 +#define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT) + +#define SMARTPORT_MSP_START_FLAG (1 << 4) +#define SMARTPORT_MSP_SEQ_MASK 0x0F + +#define SMARTPORT_MSP_RX_BUF_SIZE 64 + +static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE]; static mspPacket_t smartPortMspReply; static bool smartPortMspReplyPending = false; -// If set, this should be executed once the reply buffer -// has been sent back to the transmitter -static mspPostProcessFnPtr mspPostProcessFn = NULL; - static void smartPortDataReceive(uint16_t c) { static bool skipUntilStart = true; @@ -197,8 +210,7 @@ static void smartPortDataReceive(uint16_t c) } if (smartPortRxBytes == 0) { - if ((c == FSSP_SENSOR_ID1) - && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { + if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { // our slot is starting... smartPortLastRequestTime = now; @@ -215,13 +227,13 @@ static void smartPortDataReceive(uint16_t c) //TODO: add CRC checking - if (c == FSSP_BYTE_STUFF) { + if (c == FSSP_DLE) { byteStuffing = true; return; } if (byteStuffing) { - c ^= 0x20; + c ^= FSSP_DLE_XOR; byteStuffing = false; } @@ -237,9 +249,9 @@ static void smartPortDataReceive(uint16_t c) static void smartPortSendByte(uint8_t c, uint16_t *crcp) { // smart port escape sequence - if (c == 0x7D || c == 0x7E) { - serialWrite(smartPortSerialPort, 0x7D); - c ^= 0x20; + if (c == FSSP_DLE || c == FSSP_START_STOP) { + serialWrite(smartPortSerialPort, FSSP_DLE); + c ^= FSSP_DLE_XOR; } serialWrite(smartPortSerialPort, c); @@ -254,29 +266,28 @@ static void smartPortSendByte(uint8_t c, uint16_t *crcp) *crcp = crc; } -static void smartPortSendPackageEx(uint8_t primId, uint8_t* data) +static void smartPortSendPackageEx(uint8_t frameId, uint8_t* data) { uint16_t crc = 0; - smartPortSendByte(primId, &crc); - for(uint8_t i=0;i<6;i++) { - smartPortSendByte(*(data++), &crc); + smartPortSendByte(frameId, &crc); + for(unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++) { + smartPortSendByte(*data++, &crc); } smartPortSendByte(0xFF - (uint8_t)crc, NULL); } static void smartPortSendPackage(uint16_t id, uint32_t val) { - uint8_t packet[6]; - uint8_t *u8p = (uint8_t*)&id; - packet[0] = u8p[0]; - packet[1] = u8p[1]; - u8p = (uint8_t*)&val; - packet[2] = u8p[0]; - packet[3] = u8p[1]; - packet[4] = u8p[2]; - packet[5] = u8p[3]; + uint8_t payload[SMARTPORT_PAYLOAD_SIZE]; + uint8_t *dst = payload; + *dst++ = id & 0xFF; + *dst++ = id >> 8; + *dst++ = val & 0xFF; + *dst++ = (val >> 8) & 0xFF; + *dst++ = (val >> 16) & 0xFF; + *dst++ = (val >> 24) & 0xFF; - smartPortSendPackageEx(FSSP_DATA_FRAME,packet); + smartPortSendPackageEx(FSSP_DATA_FRAME,payload); } void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) @@ -360,9 +371,7 @@ static void processMspPacket(mspPacket_t* packet) smartPortMspReply.cmd = -1; smartPortMspReply.result = 0; - mspPostProcessFn = NULL; - const mspResult_e status = mspFcProcessCommand(packet, &smartPortMspReply, - &mspPostProcessFn); + const mspResult_e status = mspFcProcessCommand(packet, &smartPortMspReply, NULL); if (status != MSP_RESULT_NO_REPLY) { // change streambuf direction @@ -390,7 +399,7 @@ static void processMspPacket(mspPacket_t* packet) */ void handleSmartPortMspFrame(uint8_t* frame, uint8_t size) { - static uint8_t mspBuffer[64]; + static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; static uint8_t mspStarted = 0; static uint8_t lastSeq = 0; static uint8_t checksum = 0; @@ -401,13 +410,13 @@ void handleSmartPortMspFrame(uint8_t* frame, uint8_t size) .result = 0 }; - //TODO: re-assemble MSP frame & forward to MSP port when complete + // re-assemble MSP frame & forward to MSP port when complete uint8_t* p = frame; uint8_t* end = frame + size; - uint8_t head = *(p++); - uint8_t seq = head & 0xF; - uint8_t version = (head & 0xE0) >> 5; + uint8_t head = *p++; + uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK; + uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT; if (version != SMARTPORT_MSP_VERSION) { // TODO: should a version mismatch error @@ -417,11 +426,11 @@ void handleSmartPortMspFrame(uint8_t* frame, uint8_t size) } // check start-flag - if (head & (1 << 4)) { + if (head & SMARTPORT_MSP_START_FLAG) { - //TODO: if (frame[1] > 64) error! - uint8_t p_size = *(p++); - cmd.cmd = *(p++); + //TODO: if (p_size > SMARTPORT_MSP_RX_BUF_SIZE) error! + uint8_t p_size = *p++; + cmd.cmd = *p++; cmd.result = 0; cmd.buf.ptr = mspBuffer; @@ -434,7 +443,7 @@ void handleSmartPortMspFrame(uint8_t* frame, uint8_t size) // no start packet yet, throw this one away return; } - else if (((lastSeq + 1) & 0x7) != seq) { + else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) { // packet loss detected! resetMspPacket(&cmd); return; @@ -443,7 +452,7 @@ void handleSmartPortMspFrame(uint8_t* frame, uint8_t size) // copy payload bytes while ((p < end) && sbufBytesRemaining(&cmd.buf)) { checksum ^= *p; - sbufWriteU8(&cmd.buf,*(p++)); + sbufWriteU8(&cmd.buf,*p++); } // reached end of smart port frame @@ -470,9 +479,9 @@ bool smartPortSendMspReply() static uint8_t checksum = 0; static uint8_t seq = 0; - uint8_t packet[6]; + uint8_t packet[SMARTPORT_PAYLOAD_SIZE]; uint8_t* p = packet; - uint8_t* end = p + 6; + uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; sbuf_t* txBuf = &smartPortMspReply.buf; @@ -482,18 +491,19 @@ bool smartPortSendMspReply() uint8_t size = sbufBytesRemaining(txBuf); // header - *(p++) = (SMARTPORT_MSP_VERSION << 5) | (1 << 4) | (seq++ & 0xF); - *(p++) = size; + *p++ = SMARTPORT_MSP_VERSION_S | SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); + + *p++ = size; checksum = sbufBytesRemaining(txBuf) ^ smartPortMspReply.cmd; } else { // header - *(p++) = (SMARTPORT_MSP_VERSION << 5) | (seq++ & 0xF); + *p++ = SMARTPORT_MSP_VERSION_S | (seq++ & SMARTPORT_MSP_SEQ_MASK); } while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { *p = sbufReadU8(txBuf); - checksum ^= *(p++); // MSP checksum + checksum ^= *p++; // MSP checksum } // to be continued... @@ -504,11 +514,11 @@ bool smartPortSendMspReply() // nothing left in txBuf, // append the MSP checksum - *(p++) = checksum; + *p++ = checksum; // pad with zeros while (p < end) - *(p++) = 0; + *p++ = 0; smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); return false; @@ -543,9 +553,11 @@ void handleSmartPortTelemetry(void) smartPortFrameReceived = false; // do not check the physical ID here again // unless we start receiving other sensors' packets - uint8_t primId = smartPortRxBuffer[1]; - if(primId == FSSP_MSPC_FRAME) { - handleSmartPortMspFrame(smartPortRxBuffer+2,6); + smartPortFrame_t* frame = (smartPortFrame_t*)smartPortRxBuffer; + if(frame->frameId == FSSP_MSPC_FRAME) { + + // Pass only the payload: skip sensorId & frameId + handleSmartPortMspFrame(smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET, SMARTPORT_PAYLOAD_SIZE); } } From 1fc6bca61f5b33def160791b7133f66b38c043ae Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 27 Oct 2016 01:48:03 +0200 Subject: [PATCH 044/188] Added LUA test script This LUA script is meant to be run a telemetry script in OpenTX 2.2. --- src/test/SpMsp.lua | 77 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 src/test/SpMsp.lua diff --git a/src/test/SpMsp.lua b/src/test/SpMsp.lua new file mode 100644 index 0000000000..a84e99ce78 --- /dev/null +++ b/src/test/SpMsp.lua @@ -0,0 +1,77 @@ +-- +-- Test script for the MSP/SPORT bridge +-- + +-- Protocol version +SPORT_MSP_VERSION = 1 + +-- Sensor ID used by the local LUA script +LOCAL_SENSOR_ID = 0x0D + +-- Sensor ID used by the MSP server (BF, CF, MW, etc...) +REMOTE_SENSOR_ID = 0x1B + +REQUEST_FRAME_ID = 0x30 +REPLY_FRAME_ID = 0x32 + +-- Sequence number for next MSP/SPORT packet +sportMspSeq = 0 + +-- Stats +requestsSent = 0 +repliesReceived = 0 + +lastReqTS = 0 + +local function pollReply() + local sensorId, frameId, dataId, value = sportTelemetryPop() + if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then + repliesReceived = repliesReceived + 1 + end +end + +local function mspSendRequest(cmd) + + local dataId = 0 + dataId = sportMspSeq -- sequence number + dataId = dataId + bit32.lshift(1,4) -- start flag + dataId = dataId + bit32.lshift(SPORT_MSP_VERSION,5) -- MSP/SPORT version + -- size is 0 for now, no need to add it to dataId + -- dataId = dataId + bit32.lshift(0,8) + sportMspSeq = bit32.band(sportMspSeq + 1, 0x0F) + + local value = 0 + value = bit32.band(cmd,0xFF) -- MSP command + value = value + bit32.lshift(cmd,8) -- CRC + + requestsSent = requestsSent + 1 + return sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value) +end + +local function run(event) + + local now = getTime() + + if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then + requestsSent = 0 + repliesReceived = 0 + end + + lcd.clear() + + lcd.drawText(1,11,"Requests:",0) + lcd.drawNumber(60,11,requestsSent) + + lcd.drawText(1,21,"Replies:",0) + lcd.drawNumber(60,21,repliesReceived) + + -- last request is at least 2s old + if lastReqTS + 200 <= now then + mspSendRequest(117) -- MSP_PIDNAMES + lastReqTS = now + end + + pollReply() +end + +return {run=run} From 083222dcffa6ca107e910e95f1a011ff97f62b69 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 27 Oct 2016 12:43:38 +0200 Subject: [PATCH 045/188] Added checksum checking for MSP/SPORT. --- src/main/telemetry/smartport.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 52f7f69058..e14aaade1a 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -166,12 +166,13 @@ typedef struct smartPortFrame_s { uint8_t frameId; uint16_t valueId; uint32_t data; -} smartPortFrame_t; + uint8_t crc; +} __attribute__((packed)) smartPortFrame_t; #define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t) #define SMARTPORT_TX_BUF_SIZE 256 -#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - 2*sizeof(uint8_t)) +#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - 3*sizeof(uint8_t)) #define SMARTPORT_PAYLOAD_OFFSET 2 static uint8_t smartPortRxBuffer[SMARTPORT_FRAME_SIZE]; @@ -196,6 +197,7 @@ static void smartPortDataReceive(uint16_t c) { static bool skipUntilStart = true; static bool byteStuffing = false; + static uint16_t checksum = 0; uint32_t now = millis(); @@ -218,6 +220,7 @@ static void smartPortDataReceive(uint16_t c) } else if (c == FSSP_SENSOR_ID2) { smartPortRxBuffer[smartPortRxBytes++] = c; + checksum = 0; } else { skipUntilStart = true; @@ -225,8 +228,6 @@ static void smartPortDataReceive(uint16_t c) } else { - //TODO: add CRC checking - if (c == FSSP_DLE) { byteStuffing = true; return; @@ -240,9 +241,16 @@ static void smartPortDataReceive(uint16_t c) smartPortRxBuffer[smartPortRxBytes++] = c; if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) { - smartPortFrameReceived = true; + if (c == (0xFF - checksum)) { + smartPortFrameReceived = true; + } skipUntilStart = true; } + else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) { + checksum += c; + checksum += checksum >> 8; + checksum &= 0x00FF; + } } } From 86c4195b4fe917f0c8cefd09fccebaa6d95b4c0e Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 27 Oct 2016 13:37:24 +0200 Subject: [PATCH 046/188] =?UTF-8?q?Applied=20some=20ledvinap=E2=80=99s=20s?= =?UTF-8?q?uggestions.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/telemetry/smartport.c | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index e14aaade1a..f5c7a8e7d2 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -172,16 +172,16 @@ typedef struct smartPortFrame_s { #define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t) #define SMARTPORT_TX_BUF_SIZE 256 -#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - 3*sizeof(uint8_t)) -#define SMARTPORT_PAYLOAD_OFFSET 2 +#define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId) +#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1) -static uint8_t smartPortRxBuffer[SMARTPORT_FRAME_SIZE]; +static smartPortFrame_t smartPortRxBuffer; static uint8_t smartPortRxBytes = 0; static bool smartPortFrameReceived = false; #define SMARTPORT_MSP_VERSION 1 #define SMARTPORT_MSP_VER_SHIFT 5 -#define SMARTPORT_MSP_VER_MASK 0xE0 +#define SMARTPORT_MSP_VER_MASK (0x7 << SMARTPORT_MSP_VER_SHIFT) #define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT) #define SMARTPORT_MSP_START_FLAG (1 << 4) @@ -211,6 +211,7 @@ static void smartPortDataReceive(uint16_t c) return; } + uint8_t* rxBuffer = (uint8_t*)&smartPortRxBuffer; if (smartPortRxBytes == 0) { if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { @@ -219,7 +220,7 @@ static void smartPortDataReceive(uint16_t c) smartPortHasRequest = 1; } else if (c == FSSP_SENSOR_ID2) { - smartPortRxBuffer[smartPortRxBytes++] = c; + rxBuffer[smartPortRxBytes++] = c; checksum = 0; } else { @@ -238,7 +239,7 @@ static void smartPortDataReceive(uint16_t c) byteStuffing = false; } - smartPortRxBuffer[smartPortRxBytes++] = c; + rxBuffer[smartPortRxBytes++] = c; if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) { if (c == (0xFF - checksum)) { @@ -405,7 +406,7 @@ static void processMspPacket(mspPacket_t* packet) * - payload... * - CRC */ -void handleSmartPortMspFrame(uint8_t* frame, uint8_t size) +void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) { static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; static uint8_t mspStarted = 0; @@ -419,8 +420,8 @@ void handleSmartPortMspFrame(uint8_t* frame, uint8_t size) }; // re-assemble MSP frame & forward to MSP port when complete - uint8_t* p = frame; - uint8_t* end = frame + size; + uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET; + uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; uint8_t head = *p++; uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK; @@ -561,11 +562,10 @@ void handleSmartPortTelemetry(void) smartPortFrameReceived = false; // do not check the physical ID here again // unless we start receiving other sensors' packets - smartPortFrame_t* frame = (smartPortFrame_t*)smartPortRxBuffer; - if(frame->frameId == FSSP_MSPC_FRAME) { + if(smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { // Pass only the payload: skip sensorId & frameId - handleSmartPortMspFrame(smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET, SMARTPORT_PAYLOAD_SIZE); + handleSmartPortMspFrame(&smartPortRxBuffer); } } From b43ce998495201f29bfc1fc160303a984ca758fc Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 27 Oct 2016 15:07:11 +0200 Subject: [PATCH 047/188] Added error checking + SPORT CRC fix --- src/main/telemetry/smartport.c | 188 ++++++++++++++++++++------------- 1 file changed, 113 insertions(+), 75 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index f5c7a8e7d2..b9e046673e 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -184,6 +184,7 @@ static bool smartPortFrameReceived = false; #define SMARTPORT_MSP_VER_MASK (0x7 << SMARTPORT_MSP_VER_SHIFT) #define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT) +#define SMARTPORT_MSP_ERROR_FLAG (1 << 5) #define SMARTPORT_MSP_START_FLAG (1 << 4) #define SMARTPORT_MSP_SEQ_MASK 0x0F @@ -193,6 +194,12 @@ static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE]; static mspPacket_t smartPortMspReply; static bool smartPortMspReplyPending = false; +enum { + SMARTPORT_MSP_VER_MISMATCH=0, + SMARTPORT_MSP_CRC_ERROR=1, + SMARTPORT_MSP_ERROR=2 +}; + static void smartPortDataReceive(uint16_t c) { static bool skipUntilStart = true; @@ -260,10 +267,11 @@ static void smartPortSendByte(uint8_t c, uint16_t *crcp) // smart port escape sequence if (c == FSSP_DLE || c == FSSP_START_STOP) { serialWrite(smartPortSerialPort, FSSP_DLE); - c ^= FSSP_DLE_XOR; + serialWrite(smartPortSerialPort, c ^ FSSP_DLE_XOR); + } + else { + serialWrite(smartPortSerialPort, c); } - - serialWrite(smartPortSerialPort, c); if (crcp == NULL) return; @@ -372,29 +380,114 @@ static void resetMspPacket(mspPacket_t* packet) packet->result = 0; } -static void processMspPacket(mspPacket_t* packet) +static void initSmartPortMspReply(int16_t cmd) { smartPortMspReply.buf.ptr = smartPortMspTxBuffer; smartPortMspReply.buf.end = ARRAYEND(smartPortMspTxBuffer); - smartPortMspReply.cmd = -1; + smartPortMspReply.cmd = cmd; smartPortMspReply.result = 0; +} - const mspResult_e status = mspFcProcessCommand(packet, &smartPortMspReply, NULL); +static void processMspPacket(mspPacket_t* packet) +{ + initSmartPortMspReply(0); - if (status != MSP_RESULT_NO_REPLY) { - // change streambuf direction - sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); + mspFcProcessCommand(packet, &smartPortMspReply, NULL); - smartPortMspReplyPending = true; - } - else { - //TODO: send ACK reply to avoid re-transmission? - } + // change streambuf direction + sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); + smartPortMspReplyPending = true; } /** - * Frame format: + * Request frame format: + * - Header: 1 byte + * - Reserved: 2 bits (future use) + * - Error-flag: 1 bit + * - Start-flag: 1 bit + * - CSeq: 4 bits + * + * - MSP payload: + * - if Error-flag == 0: + * - size: 1 byte + * - payload + * - CRC (request type included) + * - if Error-flag == 1: + * - Error: 1 Byte + * - 0: Version mismatch (type=0) + * - 1: Sequence number error + * - 2: MSP error + * - CRC (request type included) + */ +bool smartPortSendMspReply() +{ + static uint8_t checksum = 0; + static uint8_t seq = 0; + + uint8_t packet[SMARTPORT_PAYLOAD_SIZE]; + uint8_t* p = packet; + uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; + + sbuf_t* txBuf = &smartPortMspReply.buf; + + // detect first reply packet + if (txBuf->ptr == smartPortMspTxBuffer) { + + uint8_t size = sbufBytesRemaining(txBuf); + + // header + + *p = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); + + if (smartPortMspReply.result == MSP_RESULT_ERROR) { + *p++ |= SMARTPORT_MSP_ERROR_FLAG; + *p++ = SMARTPORT_MSP_ERROR; + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return false; + } + + *p++ = size; + checksum = size ^ smartPortMspReply.cmd; + } + else { + // header + *p++ = SMARTPORT_MSP_VERSION_S | (seq++ & SMARTPORT_MSP_SEQ_MASK); + } + + while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { + *p = sbufReadU8(txBuf); + checksum ^= *p++; // MSP checksum + } + + // to be continued... + if (p == end) { + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return true; + } + + // nothing left in txBuf, + // append the MSP checksum + *p++ = checksum; + + // pad with zeros + while (p < end) + *p++ = 0; + + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return false; +} + +void smartPortSendErrorReply(uint8_t error, int16_t cmd) +{ + initSmartPortMspReply(cmd); + sbufWriteU8(&smartPortMspReply.buf,error); + sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); + smartPortMspReplyPending = true; +} + +/** + * Request frame format: * - Header: 1 byte * - Version: 3 bits * - Start-flag: 1 bit @@ -412,12 +505,7 @@ void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) static uint8_t mspStarted = 0; static uint8_t lastSeq = 0; static uint8_t checksum = 0; - - static mspPacket_t cmd = { - .buf = { .ptr = mspBuffer, .end = mspBuffer }, - .cmd = -1, - .result = 0 - }; + static mspPacket_t cmd; // re-assemble MSP frame & forward to MSP port when complete uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET; @@ -428,9 +516,8 @@ void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT; if (version != SMARTPORT_MSP_VERSION) { - // TODO: should a version mismatch error - // be sent back to the transmitter? - resetMspPacket(&cmd); + mspStarted = 0; + smartPortSendErrorReply(SMARTPORT_MSP_VER_MISMATCH,0); return; } @@ -472,7 +559,8 @@ void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) // last byte must be the checksum if (checksum != *p) { - resetMspPacket(&cmd); + mspStarted = 0; + smartPortSendErrorReply(SMARTPORT_MSP_CRC_ERROR,cmd.cmd); return; } @@ -483,56 +571,6 @@ void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) processMspPacket(&cmd); } -bool smartPortSendMspReply() -{ - static uint8_t checksum = 0; - static uint8_t seq = 0; - - uint8_t packet[SMARTPORT_PAYLOAD_SIZE]; - uint8_t* p = packet; - uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; - - sbuf_t* txBuf = &smartPortMspReply.buf; - - // detect first reply packet - if (txBuf->ptr == smartPortMspTxBuffer) { - - uint8_t size = sbufBytesRemaining(txBuf); - - // header - *p++ = SMARTPORT_MSP_VERSION_S | SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); - - *p++ = size; - checksum = sbufBytesRemaining(txBuf) ^ smartPortMspReply.cmd; - } - else { - // header - *p++ = SMARTPORT_MSP_VERSION_S | (seq++ & SMARTPORT_MSP_SEQ_MASK); - } - - while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { - *p = sbufReadU8(txBuf); - checksum ^= *p++; // MSP checksum - } - - // to be continued... - if (p == end) { - smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); - return true; - } - - // nothing left in txBuf, - // append the MSP checksum - *p++ = checksum; - - // pad with zeros - while (p < end) - *p++ = 0; - - smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); - return false; -} - void handleSmartPortTelemetry(void) { uint32_t smartPortLastServiceTime = millis(); From c39201f698f05c770cecf57bb5631a2f1d5d3b7b Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 27 Oct 2016 16:07:04 +0200 Subject: [PATCH 048/188] Pad error reply with 0s + bug fixes. --- src/main/telemetry/smartport.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index b9e046673e..c5ceae9d15 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -443,10 +443,13 @@ bool smartPortSendMspReply() if (smartPortMspReply.result == MSP_RESULT_ERROR) { *p++ |= SMARTPORT_MSP_ERROR_FLAG; *p++ = SMARTPORT_MSP_ERROR; + *p++ = SMARTPORT_MSP_ERROR ^ smartPortMspReply.cmd; // MSP checksum + while (p < end) *p++ = 0; // pad with zeros smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); return false; } + p++; *p++ = size; checksum = size ^ smartPortMspReply.cmd; } From 7fd0a88617738dc5316cb4fed19419e18d4f7376 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 27 Oct 2016 16:57:33 +0200 Subject: [PATCH 049/188] Reworked error replies a bit --- src/main/telemetry/smartport.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index c5ceae9d15..1e34497122 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -194,6 +194,8 @@ static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE]; static mspPacket_t smartPortMspReply; static bool smartPortMspReplyPending = false; +#define SMARTPORT_MSP_RES_ERROR -10 + enum { SMARTPORT_MSP_VER_MISMATCH=0, SMARTPORT_MSP_CRC_ERROR=1, @@ -393,8 +395,10 @@ static void processMspPacket(mspPacket_t* packet) { initSmartPortMspReply(0); - mspFcProcessCommand(packet, &smartPortMspReply, NULL); - + if (mspFcProcessCommand(packet, &smartPortMspReply, NULL) == MSP_RESULT_ERROR) { + sbufWriteU8(&smartPortMspReply.buf, SMARTPORT_MSP_ERROR); + } + // change streambuf direction sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); smartPortMspReplyPending = true; @@ -414,7 +418,8 @@ static void processMspPacket(mspPacket_t* packet) * - payload * - CRC (request type included) * - if Error-flag == 1: - * - Error: 1 Byte + * - size: 1 byte (== 1) + * - error: 1 Byte * - 0: Version mismatch (type=0) * - 1: Sequence number error * - 2: MSP error @@ -434,23 +439,16 @@ bool smartPortSendMspReply() // detect first reply packet if (txBuf->ptr == smartPortMspTxBuffer) { - uint8_t size = sbufBytesRemaining(txBuf); - // header - - *p = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); - - if (smartPortMspReply.result == MSP_RESULT_ERROR) { - *p++ |= SMARTPORT_MSP_ERROR_FLAG; - *p++ = SMARTPORT_MSP_ERROR; - *p++ = SMARTPORT_MSP_ERROR ^ smartPortMspReply.cmd; // MSP checksum - while (p < end) *p++ = 0; // pad with zeros - smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); - return false; + uint8_t head = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); + if (smartPortMspReply.result < 0) { + head |= SMARTPORT_MSP_ERROR_FLAG; } + *p++ = head; - p++; + uint8_t size = sbufBytesRemaining(txBuf); *p++ = size; + checksum = size ^ smartPortMspReply.cmd; } else { @@ -485,6 +483,8 @@ void smartPortSendErrorReply(uint8_t error, int16_t cmd) { initSmartPortMspReply(cmd); sbufWriteU8(&smartPortMspReply.buf,error); + smartPortMspReply.result = SMARTPORT_MSP_RES_ERROR; + sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); smartPortMspReplyPending = true; } From e37594981f1b47991f9caed9041d6c3115f81437 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 27 Oct 2016 19:24:31 +0200 Subject: [PATCH 050/188] Fixed reply header --- src/main/telemetry/smartport.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 1e34497122..cdcd21e571 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -453,7 +453,7 @@ bool smartPortSendMspReply() } else { // header - *p++ = SMARTPORT_MSP_VERSION_S | (seq++ & SMARTPORT_MSP_SEQ_MASK); + *p++ = (seq++ & SMARTPORT_MSP_SEQ_MASK); } while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { From 160bfaf907ec1f74caf200811df2c8c638bffdbc Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 27 Oct 2016 19:25:19 +0200 Subject: [PATCH 051/188] Added frame re-assembly to LUA script --- src/test/SpMsp.lua | 122 ++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 115 insertions(+), 7 deletions(-) diff --git a/src/test/SpMsp.lua b/src/test/SpMsp.lua index a84e99ce78..07ceff1993 100644 --- a/src/test/SpMsp.lua +++ b/src/test/SpMsp.lua @@ -17,18 +17,21 @@ REPLY_FRAME_ID = 0x32 -- Sequence number for next MSP/SPORT packet sportMspSeq = 0 +mspRxBuf = {} +mspRxIdx = 1 +mspRxCRC = 0 +mspStarted = false + -- Stats requestsSent = 0 repliesReceived = 0 -lastReqTS = 0 +mspReceivedReply_cnt = 0 +mspReceivedReply_cnt1 = 0 +mspReceivedReply_cnt2 = 0 +mspReceivedReply_cnt3 = 0 -local function pollReply() - local sensorId, frameId, dataId, value = sportTelemetryPop() - if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then - repliesReceived = repliesReceived + 1 - end -end +lastReqTS = 0 local function mspSendRequest(cmd) @@ -48,6 +51,95 @@ local function mspSendRequest(cmd) return sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value) end +local function mspReceivedReply(payload) + + -- TODO: MSP checksum checking + + mspReceivedReply_cnt = mspReceivedReply_cnt + 1 + + local idx = 1 + local head = payload[idx] + local err_flag = (bit32.band(head,0x20) ~= 0) + idx = idx + 1 + + if err_flag then + -- error flag set + mspStarted = false + + mspReceivedReply_cnt1 = mspReceivedReply_cnt1 + 1 + + -- return error + -- CRC checking missing + + --return bit32.band(payload[idx],0xFF) + return nil + end + + local start = (bit32.band(head,0x10) ~= 0) + local seq = bit32.band(head,0x0F) + + if start then + -- start flag set + mspRxIdx = 1 + mspRxBuf = {} + + mspRxSize = payload[idx] + mspRxCRC = mspRxSize + idx = idx + 1 + mspStarted = true + + mspReceivedReply_cnt2 = mspReceivedReply_cnt2 + 1 + + elseif not mspStarted then + mspReceivedReply_cnt3 = mspReceivedReply_cnt3 + 1 + return nil + -- TODO: add sequence number checking + -- elseif ... + end + + while (idx <= 6) and (mspRxIdx <= mspRxSize) do + mspRxBuf[mspRxIdx] = payload[idx] + mspRxCRC = bit32.bxor(mspRxCRC,payload[idx]) + mspRxIdx = mspRxIdx + 1 + idx = idx + 1 + end + + if idx > 6 then + lastRxSeq = seq + return + end + + -- check CRC + if mspRxCRC ~= payload[idx] then + mspStarted = false + end + + repliesReceived = repliesReceived + 1 + mspStarted = false + return mspRxBuf +end + +local function pollReply() + local sensorId, frameId, dataId, value = sportTelemetryPop() + if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then + + local payload = {} + payload[1] = bit32.band(dataId,0xFF) + dataId = bit32.rshift(dataId,8) + payload[2] = bit32.band(dataId,0xFF) + + payload[3] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[4] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[5] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[6] = bit32.band(value,0xFF) + + return mspReceivedReply(payload) + end +end + local function run(event) local now = getTime() @@ -55,6 +147,10 @@ local function run(event) if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then requestsSent = 0 repliesReceived = 0 + mspReceivedReply_cnt = 0 + mspReceivedReply_cnt1 = 0 + mspReceivedReply_cnt2 = 0 + mspReceivedReply_cnt3 = 0 end lcd.clear() @@ -65,6 +161,18 @@ local function run(event) lcd.drawText(1,21,"Replies:",0) lcd.drawNumber(60,21,repliesReceived) + lcd.drawText(1,31,"cnt:",0) + lcd.drawNumber(30,31,mspReceivedReply_cnt) + + lcd.drawText(1,41,"cnt1:",0) + lcd.drawNumber(30,41,mspReceivedReply_cnt1) + + lcd.drawText(71,31,"cnt2:",0) + lcd.drawNumber(100,31,mspReceivedReply_cnt2) + + lcd.drawText(71,41,"cnt3:",0) + lcd.drawNumber(100,41,mspReceivedReply_cnt3) + -- last request is at least 2s old if lastReqTS + 200 <= now then mspSendRequest(117) -- MSP_PIDNAMES From d38d4949e90614c3337e3b10c59b0505bf598a94 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Fri, 28 Oct 2016 14:19:01 +0200 Subject: [PATCH 052/188] a bit more cleanup --- src/main/telemetry/smartport.c | 10 +--------- src/test/SpMsp.lua | 8 +++++--- 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index cdcd21e571..091946019f 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -374,14 +374,6 @@ void checkSmartPortTelemetryState(void) freeSmartPortTelemetryPort(); } -static void resetMspPacket(mspPacket_t* packet) -{ - packet->buf.ptr = NULL; - packet->buf.end = NULL; - packet->cmd = -1; - packet->result = 0; -} - static void initSmartPortMspReply(int16_t cmd) { smartPortMspReply.buf.ptr = smartPortMspTxBuffer; @@ -544,7 +536,7 @@ void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) } else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) { // packet loss detected! - resetMspPacket(&cmd); + mspStarted = 0; return; } diff --git a/src/test/SpMsp.lua b/src/test/SpMsp.lua index 07ceff1993..056fc9ea8e 100644 --- a/src/test/SpMsp.lua +++ b/src/test/SpMsp.lua @@ -71,7 +71,7 @@ local function mspReceivedReply(payload) -- return error -- CRC checking missing - --return bit32.band(payload[idx],0xFF) + --return payload[idx] return nil end @@ -93,8 +93,10 @@ local function mspReceivedReply(payload) elseif not mspStarted then mspReceivedReply_cnt3 = mspReceivedReply_cnt3 + 1 return nil - -- TODO: add sequence number checking - -- elseif ... + + elseif bit32.band(lastSeq+1,0x0F) ~= seq then + mspStarted = false + return nil end while (idx <= 6) and (mspRxIdx <= mspRxSize) do From ce0dd3536e5bb49ed3f2366199e8d76c2f0d6697 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 30 Oct 2016 13:35:16 +0100 Subject: [PATCH 053/188] LUA improvements (WiP) --- src/test/SpMsp.lua | 96 ++++++++++++++++++++++++++++------------------ 1 file changed, 58 insertions(+), 38 deletions(-) diff --git a/src/test/SpMsp.lua b/src/test/SpMsp.lua index 056fc9ea8e..93b47131be 100644 --- a/src/test/SpMsp.lua +++ b/src/test/SpMsp.lua @@ -15,23 +15,31 @@ REQUEST_FRAME_ID = 0x30 REPLY_FRAME_ID = 0x32 -- Sequence number for next MSP/SPORT packet -sportMspSeq = 0 +local sportMspSeq = 0 -mspRxBuf = {} -mspRxIdx = 1 -mspRxCRC = 0 -mspStarted = false +local mspRxBuf = {} +local mspRxIdx = 1 +local mspRxCRC = 0 +local mspStarted = false -- Stats -requestsSent = 0 -repliesReceived = 0 +mspRequestsSent = 0 +mspRepliesReceived = 0 +mspPkRxed = 0 +mspErrorPk = 0 +mspStartPk = 0 +mspOutOfOrder = 0 +mspCRCErrors = 0 -mspReceivedReply_cnt = 0 -mspReceivedReply_cnt1 = 0 -mspReceivedReply_cnt2 = 0 -mspReceivedReply_cnt3 = 0 - -lastReqTS = 0 +local function mspResetStats() + mspRequestsSent = 0 + mspRepliesReceived = 0 + mspPkRxed = 0 + mspErrorPk = 0 + mspStartPk = 0 + mspOutOfOrderPk = 0 + mspCRCErrors = 0 +end local function mspSendRequest(cmd) @@ -47,15 +55,13 @@ local function mspSendRequest(cmd) value = bit32.band(cmd,0xFF) -- MSP command value = value + bit32.lshift(cmd,8) -- CRC - requestsSent = requestsSent + 1 + mspRequestsSent = requestsSent + 1 return sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value) end local function mspReceivedReply(payload) - -- TODO: MSP checksum checking - - mspReceivedReply_cnt = mspReceivedReply_cnt + 1 + mspPkRxed = mspPkRxed + 1 local idx = 1 local head = payload[idx] @@ -66,7 +72,7 @@ local function mspReceivedReply(payload) -- error flag set mspStarted = false - mspReceivedReply_cnt1 = mspReceivedReply_cnt1 + 1 + mspErrorPk = mspErrorPk + 1 -- return error -- CRC checking missing @@ -88,13 +94,14 @@ local function mspReceivedReply(payload) idx = idx + 1 mspStarted = true - mspReceivedReply_cnt2 = mspReceivedReply_cnt2 + 1 + mspStartPk = mspStartPk + 1 elseif not mspStarted then - mspReceivedReply_cnt3 = mspReceivedReply_cnt3 + 1 + mspOutOfOrder = mspOutOfOrder + 1 return nil elseif bit32.band(lastSeq+1,0x0F) ~= seq then + mspOutOfOrder = mspOutOfOrder + 1 mspStarted = false return nil end @@ -114,14 +121,15 @@ local function mspReceivedReply(payload) -- check CRC if mspRxCRC ~= payload[idx] then mspStarted = false + mspCRCErrors = mspCRCErrors + 1 end - repliesReceived = repliesReceived + 1 + mspRepliesReceived = mspRepliesReceived + 1 mspStarted = false return mspRxBuf end -local function pollReply() +local function mspPollReply() local sensorId, frameId, dataId, value = sportTelemetryPop() if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then @@ -142,6 +150,8 @@ local function pollReply() end end +local lastReqTS = 0 + local function run(event) local now = getTime() @@ -157,28 +167,38 @@ local function run(event) lcd.clear() - lcd.drawText(1,11,"Requests:",0) - lcd.drawNumber(60,11,requestsSent) + -- do we have valid telemetry data? + if getValue("rssi") > 0 then + + -- draw screen + lcd.drawText(1,11,"Requests:",0) + lcd.drawNumber(60,11,mspRequestsSent) - lcd.drawText(1,21,"Replies:",0) - lcd.drawNumber(60,21,repliesReceived) + lcd.drawText(1,21,"Replies:",0) + lcd.drawNumber(60,21,mspRepliesReceived) - lcd.drawText(1,31,"cnt:",0) - lcd.drawNumber(30,31,mspReceivedReply_cnt) + lcd.drawText(1,31,"PkRxed:",0) + lcd.drawNumber(30,31,mspPkRxed) - lcd.drawText(1,41,"cnt1:",0) - lcd.drawNumber(30,41,mspReceivedReply_cnt1) + lcd.drawText(1,41,"ErrorPk:",0) + lcd.drawNumber(30,41,mspErrorPk) - lcd.drawText(71,31,"cnt2:",0) - lcd.drawNumber(100,31,mspReceivedReply_cnt2) + lcd.drawText(71,31,"StartPk:",0) + lcd.drawNumber(100,31,mspStartPk) - lcd.drawText(71,41,"cnt3:",0) - lcd.drawNumber(100,41,mspReceivedReply_cnt3) + lcd.drawText(71,41,"OutOfOrder:",0) + lcd.drawNumber(100,41,mspOutOfOrder) - -- last request is at least 2s old - if lastReqTS + 200 <= now then - mspSendRequest(117) -- MSP_PIDNAMES - lastReqTS = now + lcd.drawText(1,51,"CRCErrors:",0) + lcd.drawNumber(30,51,mspCRCErrors) + + -- last request is at least 2s old + if lastReqTS + 200 <= now then + mspSendRequest(117) -- MSP_PIDNAMES + lastReqTS = now + end + else + lcd.drawText(20,30,"No telemetry signal", XXLSIZE + BLINK) end pollReply() From b42de4f2a41e27f89b36ef5992df550360dbc7f3 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 1 Nov 2016 01:31:09 +0900 Subject: [PATCH 054/188] 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 From 8c779d6cfc7c7a1236dcf83b5f2767d200a4dfae Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Mon, 31 Oct 2016 19:25:50 +0200 Subject: [PATCH 055/188] F7 Fix DSHOT --- src/main/drivers/pwm_output.h | 2 +- src/main/drivers/pwm_output_hal.c | 1 + src/main/drivers/pwm_output_stm32f7xx.c | 31 +++++++++++-------------- src/main/target/ANYFCF7/target.c | 9 +++---- 4 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 53a0a7fc4b..4aa48905ae 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -71,7 +71,7 @@ typedef struct { #endif #if defined(STM32F7) TIM_HandleTypeDef TimHandle; - uint32_t Channel; + DMA_HandleTypeDef hdma_tim; #endif } motorDmaOutput_t; diff --git a/src/main/drivers/pwm_output_hal.c b/src/main/drivers/pwm_output_hal.c index 290b386007..5e3dd4a05c 100644 --- a/src/main/drivers/pwm_output_hal.c +++ b/src/main/drivers/pwm_output_hal.c @@ -202,6 +202,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot break; #ifdef USE_DSHOT case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index 523b993df2..d2b7842435 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -175,23 +175,20 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t } dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource; - - static DMA_HandleTypeDef hdma_tim; - /* Set the parameters to be configured */ - hdma_tim.Init.Channel = timerHardware->dmaChannel; - hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; - hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; - hdma_tim.Init.MemInc = DMA_MINC_ENABLE; - hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ; - hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ; - hdma_tim.Init.Mode = DMA_NORMAL; - hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; - hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; - hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE; - hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; + motor->hdma_tim.Init.Channel = timerHardware->dmaChannel; + motor->hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; + motor->hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; + motor->hdma_tim.Init.MemInc = DMA_MINC_ENABLE; + motor->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ; + motor->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ; + motor->hdma_tim.Init.Mode = DMA_NORMAL; + motor->hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; + motor->hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + motor->hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; + motor->hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE; + motor->hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; /* Set hdma_tim instance */ if(timerHardware->dmaStream == NULL) @@ -199,10 +196,10 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t /* Initialization Error */ return; } - hdma_tim.Instance = timerHardware->dmaStream; + motor->hdma_tim.Instance = timerHardware->dmaStream; /* Link hdma_tim to hdma[x] (channelx) */ - __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], hdma_tim); + __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], motor->hdma_tim); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index d3b944ee70..978118d2eb 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -23,6 +23,7 @@ #include "drivers/timer.h" +#if defined(USE_DSHOT) // DSHOT TEST const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN @@ -32,7 +33,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_5, DMA1_ST7_HANDLER }, // S10_OUT 1 + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_2, DMA1_ST7_HANDLER }, // S10_OUT 1 { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2 { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4 { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT @@ -43,8 +44,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT }; - -/* STANDARD LAYOUT +#else +// STANDARD LAYOUT const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S1_IN { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S2_IN @@ -64,7 +65,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S8_OUT { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S9_OUT }; -*/ +#endif // ALTERNATE LAYOUT //const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { From c19843812c3da38901f88ef9d4a01ee1059ff180 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 1 Nov 2016 03:46:30 +0100 Subject: [PATCH 056/188] fixed missing include --- src/main/telemetry/smartport.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 091946019f..e4b86926a7 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -14,6 +14,7 @@ #include "common/axis.h" #include "common/color.h" #include "common/maths.h" +#include "common/utils.h" #include "drivers/system.h" #include "drivers/sensor.h" From c5bb7cfd2f24413b98d796fcb0a835413016e1c9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 25 Oct 2016 21:02:46 +1100 Subject: [PATCH 057/188] Introduced TIM_USE_ flags to give defaults for reset configuration. --- src/main/drivers/pwm_rx.c | 4 ++-- src/main/drivers/timer.c | 9 +++------ src/main/drivers/timer.h | 11 +++++++++-- src/main/fc/config.c | 14 +++++++++++--- src/main/target/BLUEJAYF4/target.c | 14 +++++++------- src/main/target/NAZE/target.c | 28 ++++++++++++++-------------- 6 files changed, 46 insertions(+), 34 deletions(-) diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index f055683976..a0b8ca2d32 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -408,7 +408,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) IO_t io = IOGetByTag(pwmConfig->ioTags[channel]); IOInit(io, OWNER_PWMINPUT, RESOURCE_INPUT, RESOURCE_INDEX(channel)); - IOConfigGPIO(io, timer->ioMode); + IOConfigGPIO(io, IOCFG_IPD); #if defined(USE_HAL_DRIVER) pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING); @@ -472,7 +472,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) IO_t io = IOGetByTag(ppmConfig->ioTag); IOInit(io, OWNER_PPMINPUT, RESOURCE_INPUT, 0); - IOConfigGPIO(io, timer->ioMode); + IOConfigGPIO(io, IOCFG_IPD); #if defined(USE_HAL_DRIVER) pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 7dfef2033f..168492f8ae 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -693,7 +693,7 @@ void timerInit(void) #if defined(STM32F3) || defined(STM32F4) for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); + IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif @@ -755,14 +755,11 @@ void timerForceOverflow(TIM_TypeDef *tim) } } -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag) +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { - if (flag && (timerHardware[i].output & flag) == flag) { - return &timerHardware[i]; - } else if (!flag && timerHardware[i].output == flag) { - // TODO: shift flag by one so not to be 0 + if (flag && (timerHardware[i].usageFlags & flag) == flag) { return &timerHardware[i]; } } diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index cf4d12b220..bfa9aed546 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -54,6 +54,13 @@ typedef uint32_t timCNT_t; #error "Unknown CPU defined" #endif +typedef enum { + TIM_USE_PPM = 0x1, + TIM_USE_PWM = 0x2, + TIM_USE_MOTOR = 0x4, + TIM_USE_SERVO = 0x8, + TIM_USE_LED = 0x16 +} timerUsageFlag_e; // use different types from capture and overflow - multiple overflow handlers are implemented as linked list struct timerCCHandlerRec_s; @@ -80,8 +87,8 @@ typedef struct timerHardware_s { ioTag_t tag; uint8_t channel; uint8_t irq; + timerUsageFlag_e usageFlags; uint8_t output; - ioConfig_t ioMode; #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) uint8_t alternateFunction; #endif @@ -171,7 +178,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - rccPeriphTag_t timerRCC(TIM_TypeDef *tim); -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag); +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag); #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c6a77b96a3..e17185bfe8 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -243,6 +243,14 @@ void resetServoConfig(servoConfig_t *servoConfig) { servoConfig->servoCenterPulse = 1500; servoConfig->servoPwmRate = 50; + + uint8_t servoIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_SERVOS; i++) { + if ((timerHardware[i].usageFlags & TIM_USE_SERVO) == TIM_USE_SERVO) { + servoConfig->ioTags[servoIndex] = timerHardware[i].tag; + servoIndex++; + } + } } #endif @@ -264,7 +272,7 @@ void resetMotorConfig(motorConfig_t *motorConfig) uint8_t motorIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_MOTORS; i++) { - if ((timerHardware[i].output & TIMER_OUTPUT_ENABLED) == TIMER_OUTPUT_ENABLED) { + if ((timerHardware[i].usageFlags & TIM_USE_MOTOR) == TIM_USE_MOTOR) { motorConfig->ioTags[motorIndex] = timerHardware[i].tag; motorIndex++; } @@ -304,7 +312,7 @@ void resetPpmConfig(ppmConfig_t *ppmConfig) ppmConfig->ioTag = IO_TAG(PPM_PIN); #else for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if ((timerHardware[i].output == TIMER_INPUT_ENABLED)) { + if ((timerHardware[i].usageFlags & TIM_USE_PPM) == TIM_USE_PPM) { ppmConfig->ioTag = timerHardware[i].tag; return; } @@ -318,7 +326,7 @@ void resetPwmConfig(pwmConfig_t *pwmConfig) { uint8_t inputIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && inputIndex < PWM_INPUT_PORT_COUNT; i++) { - if ((timerHardware[i].output == TIMER_INPUT_ENABLED)) { + if ((timerHardware[i].usageFlags & TIM_USE_PWM) == TIM_USE_PWM) { pwmConfig->ioTags[inputIndex] = timerHardware[i].tag; inputIndex++; } diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index c82953b221..3e7a18ac30 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -23,12 +23,12 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT }; diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index af48f988d7..c9bc95fe04 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_IPD } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1 } // PWM14 - OUT6 }; From fa23aa6abff42c4a0bcf1472f72ce879f945a3ae Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 26 Oct 2016 04:31:17 +1100 Subject: [PATCH 058/188] Corrections following review --- src/main/drivers/timer.h | 2 +- src/main/fc/config.c | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index bfa9aed546..9a86a92101 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -59,7 +59,7 @@ typedef enum { TIM_USE_PWM = 0x2, TIM_USE_MOTOR = 0x4, TIM_USE_SERVO = 0x8, - TIM_USE_LED = 0x16 + TIM_USE_LED = 0x10 } timerUsageFlag_e; // use different types from capture and overflow - multiple overflow handlers are implemented as linked list diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e17185bfe8..937745a783 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -245,8 +245,8 @@ void resetServoConfig(servoConfig_t *servoConfig) servoConfig->servoPwmRate = 50; uint8_t servoIndex = 0; - for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_SERVOS; i++) { - if ((timerHardware[i].usageFlags & TIM_USE_SERVO) == TIM_USE_SERVO) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_SERVO) { servoConfig->ioTags[servoIndex] = timerHardware[i].tag; servoIndex++; } @@ -271,8 +271,8 @@ void resetMotorConfig(motorConfig_t *motorConfig) motorConfig->digitalIdleOffset = 40; uint8_t motorIndex = 0; - for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_MOTORS; i++) { - if ((timerHardware[i].usageFlags & TIM_USE_MOTOR) == TIM_USE_MOTOR) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { motorConfig->ioTags[motorIndex] = timerHardware[i].tag; motorIndex++; } @@ -312,7 +312,7 @@ void resetPpmConfig(ppmConfig_t *ppmConfig) ppmConfig->ioTag = IO_TAG(PPM_PIN); #else for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if ((timerHardware[i].usageFlags & TIM_USE_PPM) == TIM_USE_PPM) { + if (timerHardware[i].usageFlags & TIM_USE_PPM) { ppmConfig->ioTag = timerHardware[i].tag; return; } @@ -326,7 +326,7 @@ void resetPwmConfig(pwmConfig_t *pwmConfig) { uint8_t inputIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && inputIndex < PWM_INPUT_PORT_COUNT; i++) { - if ((timerHardware[i].usageFlags & TIM_USE_PWM) == TIM_USE_PWM) { + if (timerHardware[i].usageFlags & TIM_USE_PWM) { pwmConfig->ioTags[inputIndex] = timerHardware[i].tag; inputIndex++; } From 546d8dffa8a1f541fb604216bcd604d15446c9be Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 26 Oct 2016 06:05:41 +1100 Subject: [PATCH 059/188] Simplified check --- src/main/drivers/timer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 168492f8ae..2ad89ca01b 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -759,7 +759,7 @@ const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { - if (flag && (timerHardware[i].usageFlags & flag) == flag) { + if (timerHardware[i].usageFlags & flag) { return &timerHardware[i]; } } From bc5dd3667ce2d7bf600166105c7c3d81fc4304a0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 26 Oct 2016 18:35:27 +1100 Subject: [PATCH 060/188] Started cleanup of other targets --- src/main/target/AIORACERF3/target.c | 24 ++++++++--------- src/main/target/AIR32/target.c | 18 ++++++------- src/main/target/AIRHEROF3/target.c | 29 ++++++++++----------- src/main/target/ALIENFLIGHTF1/target.c | 28 ++++++++++---------- src/main/target/ALIENFLIGHTF3/target.c | 24 ++++++++--------- src/main/target/ALIENFLIGHTF4/target.c | 27 ++++++++++--------- src/main/target/CC3D/target.c | 24 ++++++++--------- src/main/target/CHEBUZZF3/target.c | 36 +++++++++++++------------- src/main/target/CJMCU/target.c | 28 ++++++++++---------- src/main/target/COLIBRI/target.c | 33 ++++++++++++----------- src/main/target/COLIBRI_RACE/target.c | 22 ++++++++-------- src/main/target/DOGE/target.c | 21 +++++++-------- src/main/target/F4BY/target.c | 36 ++++++++++++-------------- src/main/target/FURYF3/target.c | 19 ++++++-------- src/main/target/FURYF4/target.c | 11 ++++---- src/main/target/IMPULSERCF3/target.c | 16 ++++++------ src/main/target/IRCFUSIONF3/target.c | 34 ++++++++++++------------ src/main/target/ISHAPEDF3/target.c | 35 ++++++++++++------------- src/main/target/KISSFC/target.c | 24 ++++++++--------- 19 files changed, 237 insertions(+), 252 deletions(-) diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index acb1586cfe..e94ab4ccea 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -24,16 +24,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, //LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM3 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LEd, 1, GPIO_AF_6 }, //LED_STRIP }; diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 6ead0d6fee..1709455c7d 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -23,13 +23,13 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c index 242a1945f9..d29eac131a 100755 --- a/src/main/target/AIRHEROF3/target.c +++ b/src/main/target/AIRHEROF3/target.c @@ -23,19 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM8 - RC8 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_11}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIME_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1}, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_6}, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_11}, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2} // PWM14 - OUT6 }; diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index e838c785ba..baa453c020 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index fc7861550b..4673d05a46 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -24,18 +24,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // up to 10 Motor Outputs - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N - - // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index dcdce3e43c..1fddc56b96 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -23,19 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 - { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 - { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 - { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 - - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 + { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 + { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 + { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 + { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 }; diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index c0a1dd07c2..71a6462bb4 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -23,17 +23,17 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // S1_IN - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // S4_IN - Current - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // S5_IN - Vbattery - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // S6_IN - PPM IN - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP }, // S1_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP }, // S2_OUT - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP }, // S3_OUT - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP }, // S4_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP } // S6_OUT + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, }, // S1_IN + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, }, // S4_IN - Current + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, }, // S5_IN - Vbattery + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, }, // S6_IN - PPM IN + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S1_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S2_OUT + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S3_OUT + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, }, // S4_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, } // S6_OUT }; diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 8867ca53e6..1986a350ce 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -24,23 +24,23 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // INPUTS CH1-8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM2 - PB8 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM3 - PB9 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM4 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM5 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM6 - PC8 - { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3 }, // PWM7 - PF9 - { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3 }, // PWM8 - PF10 - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PD12 - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM10 - PD13 - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM11 - PD14 - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM12 - PD15 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM13 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM14 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM15 - PA3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM16 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM17 - PB1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 } // PWM18 - PA4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM1 - PA8 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM2 - PB8 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM3 - PB9 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM4 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM5 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM6 - PC8 + { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM7 - PF9 + { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM8 - PF10 + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PD12 + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM10 - PD13 + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM11 - PD14 + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM12 - PD15 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM13 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM14 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM15 - PA3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM16 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM17 - PB1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 } // PWM18 - PA4 }; diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index d784dfcb9f..30f0e1b00e 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index b9cb1b1704..fd5c5d4272 100644 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -24,21 +24,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S1_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S2_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S5_IN - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S6_IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S7_IN - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S8_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_OUT - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S4_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S6_OUT - { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM10 }, // S7_OUT - { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM11 }, // S8_OUT + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // S1_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S2_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S5_IN + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S6_IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 }, // S7_IN + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 }, // S8_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S3_OUT + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12 }, // S4_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S5_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12 }, // S6_OUT + { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM10 }, // S7_OUT + { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM11 }, // S8_OUT }; diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 9bf7e986c0..08fd1255e1 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -23,16 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PC6 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PC7 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PMW4 - PC8 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM11 - PB15 }; diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index b75f0e7dd8..58f98c9a5c 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -23,17 +23,14 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB9 - - { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PMW4 - PA10 - { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM5 - PA9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM8 - PB1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM9 - PB0 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB9 + { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PMW4 - PA10 + { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM5 - PA9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM8 - PB1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM9 - PB0 }; diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index 42608606e9..cf7aab2df1 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -6,24 +6,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN - - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT - - { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S8_IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S8_OUT + { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 504e3b6c2d..4f50ab5d01 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -25,16 +25,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PPM IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM4 - S1 - { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 - { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM8_CC_IRQn, (1 | TIMER_OUTPUT_N_CHANNEL), IOCFG_AF_PP, GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO TIMER - LED_STRIP - + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_6, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM4 - S1 + { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 + { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, (1 | TIMER_OUTPUT_N_CHANNEL), GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO TIMER - LED_STRIP }; diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index af6feb53c6..322753dd75 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -24,12 +24,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM_IN - - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM_IN + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT // { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5 }, // LED Strip }; diff --git a/src/main/target/IMPULSERCF3/target.c b/src/main/target/IMPULSERCF3/target.c index 04200bc0b4..6e8a09cba2 100644 --- a/src/main/target/IMPULSERCF3/target.c +++ b/src/main/target/IMPULSERCF3/target.c @@ -22,13 +22,13 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1}, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM2 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM6 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6}, // LED_STRIP }; diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index 370f9eb358..5add051e06 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -23,22 +23,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/ISHAPEDF3/target.c b/src/main/target/ISHAPEDF3/target.c index d998cdba92..5a4f973de0 100644 --- a/src/main/target/ISHAPEDF3/target.c +++ b/src/main/target/ISHAPEDF3/target.c @@ -23,25 +23,24 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM |TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index d36fa9903a..08ca12f3e3 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -24,18 +24,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_6, DMA1_Channel5, DMA1_CH5_HANDLER }, - { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_2, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, + { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_6, DMA1_Channel5, DMA1_CH5_HANDLER }, + { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_2, DMA2_Channel5, DMA2_CH5_HANDLER }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel2, DMA1_CH2_HANDLER }, + { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_10, NULL, 0}, - { TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_5, NULL, 0}, + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_10, NULL, 0}, + { TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_5, NULL, 0}, }; From 801ebd4a586c187076d0f01a9ff3a87298ec0d60 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 26 Oct 2016 21:01:57 +1100 Subject: [PATCH 061/188] Completed clean up of other targets --- src/main/drivers/timer_hal.c | 11 +++---- src/main/target/LUX_RACE/target.c | 22 +++++++------- src/main/target/MICROSCISKY/target.c | 28 +++++++++--------- src/main/target/MOTOLAB/target.c | 18 ++++++------ src/main/target/OMNIBUS/target.c | 20 ++++++------- src/main/target/OMNIBUSF4/target.c | 24 +++++++-------- src/main/target/PIKOBLX/target.c | 18 ++++++------ src/main/target/RACEBASE/target.c | 11 ++++--- src/main/target/RCEXPLORERF3/target.c | 12 ++++---- src/main/target/REVO/target.c | 24 +++++++-------- src/main/target/REVONANO/target.c | 24 +++++++-------- src/main/target/RMDO/target.c | 34 ++++++++++----------- src/main/target/SINGULARITY/target.c | 20 ++++++------- src/main/target/SIRINFPV/target.c | 16 +++++----- src/main/target/SOULF4/target.c | 24 +++++++-------- src/main/target/SPARKY/target.c | 22 +++++++------- src/main/target/SPARKY2/target.c | 23 +++++++-------- src/main/target/SPRACINGF3/target.c | 36 +++++++++++------------ src/main/target/SPRACINGF3EVO/target.c | 24 +++++++-------- src/main/target/SPRACINGF3MINI/target.c | 26 ++++++++-------- src/main/target/STM32F3DISCOVERY/target.c | 28 +++++++++--------- src/main/target/VRRACE/target.c | 25 ++++++++-------- src/main/target/X_RACERSPI/target.c | 32 ++++++++++---------- src/main/target/YUPIF4/target.c | 14 ++++----- src/main/target/ZCOREF3/target.c | 36 +++++++++++------------ 25 files changed, 280 insertions(+), 292 deletions(-) diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 05c01a5210..c71743def3 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -794,7 +794,7 @@ void timerInit(void) #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); + IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif @@ -856,16 +856,13 @@ void timerForceOverflow(TIM_TypeDef *tim) } } -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag) +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { - if (flag && (timerHardware[i].output & flag) == flag) { + if (timerHardware[i].output & flag) { return &timerHardware[i]; - } else if (!flag && timerHardware[i].output == flag) { - // TODO: shift flag by one so not to be 0 - return &timerHardware[i]; - } + } } } return NULL; diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 07c5716f90..9352353f82 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -23,16 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PC6 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PC7 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PMW4 - PC8 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM11 - PB15 }; diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index e838c785ba..4319f6fb45 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index 43db149492..10cb9ad654 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -23,14 +23,14 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 1f8269072b..0e15861a61 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -25,18 +25,18 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 // UART3 RX/TX - //{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) - //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 - //{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + //{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) + //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 + //{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 5e63b90590..e088371565 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -24,17 +24,17 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT }; diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index 43db149492..10cb9ad654 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -23,14 +23,14 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/RACEBASE/target.c b/src/main/target/RACEBASE/target.c index cc2c688932..dc726133e3 100755 --- a/src/main/target/RACEBASE/target.c +++ b/src/main/target/RACEBASE/target.c @@ -23,12 +23,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, - - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 - PC6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 - PC7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PMW4 - PC8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 1, GPIO_AF_1}, + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM2 - PC6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM3 - PC7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PMW4 - PC8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM5 - PC9 }; diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index 8ac51e6713..e531b55ab0 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -24,10 +24,10 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 - PA4 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - PA7 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM3 - PA8 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PB1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 - PPM + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM1 - PA4 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_1}, // PWM2 - PA7 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_6}, // PWM3 - PA8 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM4 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM5 - PB1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIME_USE_PPM, 0, GPIO_AF_1}, // PWM6 - PPM }; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index f8be6e2eab..66da4ce854 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -24,16 +24,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT }; diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 9349b40100..e8d2132393 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -23,18 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_IN - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S4_IN - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_IN - { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S6_IN - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S1_OUT - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S2_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // S3_OUT - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM | TIME_USE_PPM, 0, GPIO_AF_TIM2 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S5_IN + { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S6_IN + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S1_OUT + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S3_OUT + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 }, // S5_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index 61603c7b4e..72523ef2b9 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -23,22 +23,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 0926431f19..7176e7bc7d 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -23,15 +23,15 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // SOFTSERIAL1 RX (NC) - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // SOFTSERIAL1 TX - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // SOFTSERIAL1 RX (NC) + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // SOFTSERIAL1 TX + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // LED_STRIP }; diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index be26986a86..2927c0b960 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -24,15 +24,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB6 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 - - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PB6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PB6 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB9 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y }; diff --git a/src/main/target/SOULF4/target.c b/src/main/target/SOULF4/target.c index f0673d116c..0b8819b4a3 100644 --- a/src/main/target/SOULF4/target.c +++ b/src/main/target/SOULF4/target.c @@ -22,16 +22,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 44aa07090a..f7a44f00d7 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -24,18 +24,18 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // 6 3-pin headers - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 // PWM7 - PMW10 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PWM, 1, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c index 87abe2834b..359c09910c 100644 --- a/src/main/target/SPARKY2/target.c +++ b/src/main/target/SPARKY2/target.c @@ -23,17 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM8 }, // PPM IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S2_IN + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S5_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index b96e424066..fe9518d8a9 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -24,25 +24,25 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + // Production boards swapped RC_CH3/4 swapped to make it easierTIM_USE_MOTOR, to using supplied cables - compared to first prototype. + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 16541c9aeb..3aeac1bd84 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -24,17 +24,17 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM3 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index 13e7ee222c..6d40631b0a 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -25,27 +25,27 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB5 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM5 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM6 - PA3 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA1 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, 1, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, 1, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) // LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 8cd9a3584f..54a5dbded7 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -24,19 +24,19 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, NULL, 0 }, - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, NULL, 0 }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, NULL, 0 }, - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 } + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, 0, GPIO_AF_1, NULL, 0 }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, 0, GPIO_AF_1, NULL, 0 }, + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, 0, GPIO_AF_1, NULL, 0 } }; diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index af85c900d0..c5fc07b418 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -23,17 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PPM - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S2_IN - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S3_IN - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S4_IN - { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S5_IN - { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S6_IN - - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S1_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S4_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PPM + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S2_IN + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S3_IN + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S4_IN + { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM1_BRK_TIM9_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S5_IN + { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S6_IN + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S4_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S5_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S6_OUT }; diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index b59d0ba6a3..d83ee38e58 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -8,22 +8,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 951a147791..20dbd87572 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -22,11 +22,11 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // MS5 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // MS6 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8 }, // PPM IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // MS5 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // MS6 }; diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index d6197448da..9f99cf2be4 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -8,24 +8,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; From 25b3f5b5ea1db56550b9e29ec170463b09ca5c60 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 26 Oct 2016 21:15:22 +1100 Subject: [PATCH 062/188] Tidy up ints. --- src/main/drivers/timer.c | 8 ++++---- src/main/drivers/timer_hal.c | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 2ad89ca01b..77096a1cb7 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -199,7 +199,7 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) rccPeriphTag_t timerRCC(TIM_TypeDef *tim) { - for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { return timerDefinitions[i].rcc; } @@ -686,12 +686,12 @@ void timerInit(void) #endif /* enable the timer peripherals */ - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { RCC_ClockCmd(timerRCC(timerHardware[i].tim), ENABLE); } #if defined(STM32F3) || defined(STM32F4) - for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { + for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } @@ -757,7 +757,7 @@ void timerForceOverflow(TIM_TypeDef *tim) const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { if (timerHardware[i].usageFlags & flag) { return &timerHardware[i]; diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index c71743def3..8268bd3497 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -208,7 +208,7 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) rccPeriphTag_t timerRCC(TIM_TypeDef *tim) { - for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { return timerDefinitions[i].rcc; } @@ -787,12 +787,12 @@ void timerInit(void) #endif /* enable the timer peripherals */ - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { RCC_ClockCmd(timerRCC(timerHardware[i].tim), ENABLE); } #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) - for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { + for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } @@ -858,7 +858,7 @@ void timerForceOverflow(TIM_TypeDef *tim) const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { if (timerHardware[i].output & flag) { return &timerHardware[i]; From a57fbce95d6db1ef62834c7acf636945105232f9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 28 Oct 2016 09:36:57 +1100 Subject: [PATCH 063/188] uint8_t to int --- src/main/fc/config.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 937745a783..0d8fe7f949 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -244,7 +244,7 @@ void resetServoConfig(servoConfig_t *servoConfig) servoConfig->servoCenterPulse = 1500; servoConfig->servoPwmRate = 50; - uint8_t servoIndex = 0; + int servoIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) { if (timerHardware[i].usageFlags & TIM_USE_SERVO) { servoConfig->ioTags[servoIndex] = timerHardware[i].tag; @@ -270,7 +270,7 @@ void resetMotorConfig(motorConfig_t *motorConfig) motorConfig->mincommand = 1000; motorConfig->digitalIdleOffset = 40; - uint8_t motorIndex = 0; + int motorIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { motorConfig->ioTags[motorIndex] = timerHardware[i].tag; @@ -324,7 +324,7 @@ void resetPpmConfig(ppmConfig_t *ppmConfig) void resetPwmConfig(pwmConfig_t *pwmConfig) { - uint8_t inputIndex = 0; + int inputIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && inputIndex < PWM_INPUT_PORT_COUNT; i++) { if (timerHardware[i].usageFlags & TIM_USE_PWM) { pwmConfig->ioTags[inputIndex] = timerHardware[i].tag; From 9fdd3aa2e4237137989c37c85d32aa48de0c9108 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 1 Nov 2016 14:34:06 +1100 Subject: [PATCH 064/188] Fixes for hardfault, target changes for ANYFCF7 and BFF3 --- src/main/drivers/pwm_output.c | 4 +- src/main/drivers/pwm_rx.c | 4 +- src/main/target/ANYFCF7/target.c | 64 +++++++++++++-------------- src/main/target/BETAFLIGHTF3/target.c | 20 ++++----- 4 files changed, 46 insertions(+), 46 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 46f082a1dc..59f01726bb 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -208,7 +208,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot break; } - const timerHardware_t *timerHardware = timerGetByTag(tag, TIMER_OUTPUT_ENABLED); + const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_MOTOR); if (timerHardware == NULL) { /* flag failure and disable ability to arm */ @@ -271,7 +271,7 @@ void servoInit(const servoConfig_t *servoConfig) IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_OUTPUT, RESOURCE_INDEX(servoIndex)); IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP); - const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED); + const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_SERVO); if (timer == NULL) { /* flag failure and disable ability to arm */ diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index a0b8ca2d32..13f074fad1 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -393,7 +393,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) pwmInputPort_t *port = &pwmInputPorts[channel]; - const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIMER_INPUT_ENABLED); + const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_PWM); if (!timer) { /* TODO: maybe fail here if not enough channels? */ @@ -459,7 +459,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT]; - const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIMER_INPUT_ENABLED); + const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIM_USE_PPM); if (!timer) { /* TODO: fail here? */ return; diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index 978118d2eb..e2f55aa290 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -26,44 +26,44 @@ #if defined(USE_DSHOT) // DSHOT TEST const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_2, DMA1_ST7_HANDLER }, // S10_OUT 1 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3, DMA1_Stream5, DMA_CHANNEL_5, DMA1_ST5_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5, DMA1_Stream2, DMA_CHANNEL_6, DMA1_ST2_HANDLER }, // S7_OUT - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 - { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT - { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_2, DMA1_ST7_HANDLER }, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream5, DMA_CHANNEL_5, DMA1_ST5_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream2, DMA_CHANNEL_6, DMA1_ST2_HANDLER }, // S7_OUT + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT }; #else // STANDARD LAYOUT const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S1_IN - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S2_IN - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S3_IN - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S4_IN - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S5_IN - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S6_IN + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF9_TIM12}, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF9_TIM12}, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S6_IN - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4}, // S10_OUT 1 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S6_OUT 2 - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4}, // S5_OUT 3 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S1_OUT 4 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5}, // S2_OUT - { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM9}, // S3_OUT - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5}, // S7_OUT - { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S8_OUT - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S9_OUT + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4}, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S6_OUT 2 + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4}, // S5_OUT 3 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5}, // S2_OUT + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9}, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3}, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5}, // S7_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3}, // S9_OUT }; #endif diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 82adbb72fa..d67ff5f6b1 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -24,17 +24,17 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel1, DMA1_CH1_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10,DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM4 - PB9 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel1, DMA1_CH1_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM4 - PB9 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP }; From ec74dff10e0c3a5790f955cc95fe8d168630b359 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 1 Nov 2016 16:31:03 +1100 Subject: [PATCH 065/188] KISSFC fixed up for dshot --- src/main/drivers/pwm_output_stm32f3xx.c | 37 ++++++++++------------- src/main/drivers/pwm_output_stm32f4xx.c | 39 +++++++++++++------------ src/main/target/KISSFC/target.c | 12 ++++---- src/main/target/KISSFC/target.h | 2 +- 4 files changed, 44 insertions(+), 46 deletions(-) diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 80903ce19f..7c76fdd15b 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -123,18 +123,18 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - uint32_t hz; - switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): - hz = MOTOR_DSHOT600_MHZ * 1000000; - break; - case(PWM_TYPE_DSHOT300): - hz = MOTOR_DSHOT300_MHZ * 1000000; - break; - default: - case(PWM_TYPE_DSHOT150): - hz = MOTOR_DSHOT150_MHZ * 1000000; - } + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / hz) - 1); TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; @@ -146,20 +146,15 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_Low; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_Low; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } + TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 7e60c62f1d..ccf262aa41 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -124,18 +124,18 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - uint32_t hz; - switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): - hz = MOTOR_DSHOT600_MHZ * 1000000; - break; - case(PWM_TYPE_DSHOT300): - hz = MOTOR_DSHOT300_MHZ * 1000000; - break; - default: - case(PWM_TYPE_DSHOT150): - hz = MOTOR_DSHOT150_MHZ * 1000000; - } + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1; TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; @@ -145,12 +145,15 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t } TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + } TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 08ca12f3e3..03bfa78733 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -24,12 +24,12 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_6, DMA1_Channel5, DMA1_CH5_HANDLER }, - { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_2, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, + { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, + { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel3, DMA1_CH3_HANDLER }, + { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel5, DMA1_CH5_HANDLER }, + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index f8a6062776..fcb2c38a03 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -84,5 +84,5 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) From ed3ed31ac669fea321ed3b0296fe26daef1e5d33 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 1 Nov 2016 16:37:37 +1100 Subject: [PATCH 066/188] Removed excess element warnings (leaving with AUX1/2 as SWD) --- src/main/target/KISSFC/target.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 03bfa78733..6c27661b0d 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -32,10 +32,10 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, - { TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_10, NULL, 0}, - { TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_5, NULL, 0}, + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + //{ TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_10, NULL, 0}, + //{ TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_5, NULL, 0}, }; From 26a00cb66069d6015094b6889da0725979573e7f Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 1 Nov 2016 17:05:52 +1100 Subject: [PATCH 067/188] Small fix for F4 hanging on dump all --- src/main/vcpf4/usbd_cdc_vcp.c | 7 +------ src/main/vcpf4/usbd_conf.h | 18 ++++++++++-------- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index dbddd6850c..95e62d752f 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -48,7 +48,6 @@ extern uint32_t APP_Rx_ptr_in; APP TX is the circular buffer for data that is transmitted from the APP (host) to the USB device (flight controller). */ -#define APP_TX_DATA_SIZE 1024 static uint8_t APP_Tx_Buffer[APP_TX_DATA_SIZE]; static uint32_t APP_Tx_ptr_out = 0; static uint32_t APP_Tx_ptr_in = 0; @@ -195,7 +194,7 @@ static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; - while (CDC_Send_FreeBytes() <= 0); + while (CDC_Send_FreeBytes() == 0); } return USBD_OK; @@ -247,15 +246,11 @@ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) return USBD_FAIL; } - __disable_irq(); - for (uint32_t i = 0; i < Len; i++) { APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % APP_TX_DATA_SIZE; } - __enable_irq(); - return USBD_OK; } diff --git a/src/main/vcpf4/usbd_conf.h b/src/main/vcpf4/usbd_conf.h index 87481c2ef1..d062c1e431 100644 --- a/src/main/vcpf4/usbd_conf.h +++ b/src/main/vcpf4/usbd_conf.h @@ -42,19 +42,21 @@ /* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */ #ifdef USE_USB_OTG_HS - #define CDC_DATA_MAX_PACKET_SIZE 512 /* Endpoint IN & OUT Packet size */ - #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ +#define CDC_DATA_MAX_PACKET_SIZE 512 /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ - #define CDC_IN_FRAME_INTERVAL 40 /* Number of micro-frames between IN transfers */ - #define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer: +#define CDC_IN_FRAME_INTERVAL 40 /* Number of micro-frames between IN transfers */ +#define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL*8 */ +#define APP_TX_DATA_SIZE 2048 /* total size of the OUT (inbound to FC) buffer */ #else - #define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ - #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ +#define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ - #define CDC_IN_FRAME_INTERVAL 15 /* Number of frames between IN transfers */ - #define APP_RX_DATA_SIZE 1024 /* Total size of IN buffer: +#define CDC_IN_FRAME_INTERVAL 15 /* Number of frames between IN transfers */ +#define APP_RX_DATA_SIZE 2048 /* Total size of IN (outbound from FC) buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */ +#define APP_TX_DATA_SIZE 2048 /* total size of the OUT (inbound to FC) buffer */ #endif /* USE_USB_OTG_HS */ #define APP_FOPS VCP_fops From 680a66ed2a3fd05a3dc1fc1f13165d7837982462 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 1 Nov 2016 18:10:09 +1100 Subject: [PATCH 068/188] Added small delay when buffer is full --- src/main/vcpf4/usbd_cdc_vcp.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 95e62d752f..3723a7bc65 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -194,7 +194,9 @@ static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; - while (CDC_Send_FreeBytes() == 0); + while (CDC_Send_FreeBytes() == 0) { + delay(1); + } } return USBD_OK; From 00014daa679759094dba703053fb5458ee5619f0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 1 Nov 2016 18:46:49 +1100 Subject: [PATCH 069/188] More uints to ints --- src/main/drivers/pwm_output_stm32f3xx.c | 2 +- src/main/drivers/pwm_output_stm32f4xx.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 7c76fdd15b..e72e03234c 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -84,7 +84,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); - for (uint8_t i = 0; i < dmaMotorTimerCount; i++) { + for (int i = 0; i < dmaMotorTimerCount; i++) { TIM_SetCounter(dmaMotorTimers[i].timer, 0); TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE); } diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index ccf262aa41..d2bf798187 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -85,7 +85,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); - for (uint8_t i = 0; i < dmaMotorTimerCount; i++) { + for (int i = 0; i < dmaMotorTimerCount; i++) { TIM_SetCounter(dmaMotorTimers[i].timer, 0); TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE); } From 042096fbb77fec37f191f93496d0964e00c96dc9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 2 Nov 2016 00:30:54 +0900 Subject: [PATCH 070/188] Tx buffer availability hack to handle slow devices --- src/main/io/canvas.c | 10 ++++--- src/main/io/cms.c | 55 ++++++++++++++++++++++----------------- src/main/io/cms.h | 4 +-- src/main/io/display.c | 3 ++- src/main/io/osd.c | 5 ++-- src/main/msp/msp_serial.c | 25 ++++++++++++++++++ src/main/msp/msp_serial.h | 1 + 7 files changed, 70 insertions(+), 33 deletions(-) diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index 97b12f03d9..5b0973ed90 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -67,6 +67,11 @@ int canvasWrite(uint8_t col, uint8_t row, char *string) return canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4); } +uint16_t canvasTxRoom() +{ + return mspSerialPushTxRoom(); +} + screenFnVTable_t canvasVTable = { canvasBegin, canvasEnd, @@ -74,15 +79,14 @@ screenFnVTable_t canvasVTable = { canvasWrite, canvasHeartBeat, NULL, + canvasTxRoom, }; 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; + pPort->vTable = &canvasVTable; } void canvasInit() diff --git a/src/main/io/cms.c b/src/main/io/cms.c index c83a71d286..be7e5186e8 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -107,37 +107,45 @@ int8_t lastCursorPos; void cmsScreenClear(displayPort_t *instance) { - instance->VTable->clear(); + instance->vTable->clear(); instance->cleared = true; lastCursorPos = -1; // XXX Here } void cmsScreenBegin(displayPort_t *instance) { - instance->VTable->begin(); - instance->VTable->clear(); + instance->vTable->begin(); + instance->vTable->clear(); } void cmsScreenEnd(displayPort_t *instance) { - instance->VTable->end(); + instance->vTable->end(); } int cmsScreenWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s) { - return instance->VTable->write(x, y, s); + return instance->vTable->write(x, y, s); } void cmsScreenHeartBeat(displayPort_t *instance) { - if (instance->VTable->heartbeat) - instance->VTable->heartbeat(); + if (instance->vTable->heartbeat) + instance->vTable->heartbeat(); } void cmsScreenResync(displayPort_t *instance) { - if (instance->VTable->resync) - instance->VTable->resync(); + if (instance->vTable->resync) + instance->vTable->resync(); +} + +uint16_t cmsScreenTxRoom(displayPort_t *instance) +{ + if (instance->vTable->txroom) + return instance->vTable->txroom(); + else + return 10000; } void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) @@ -377,7 +385,7 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) static uint8_t pollDenom = 0; bool drawPolled = (++pollDenom % 8 == 0); - int16_t cnt = 0; + int room = cmsScreenTxRoom(pDisplay); if (!currentMenu) return; @@ -404,20 +412,26 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) currentCursorPos++; if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) { - cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, lastCursorPos + top, " "); + room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, lastCursorPos + top, " "); } + if (room < 30) + return; + if (lastCursorPos != currentCursorPos) { - cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >"); + room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >"); lastCursorPos = currentCursorPos; } + if (room < 30) + return; + // 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); + room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text); CLR_PRINTLABEL(p); - if (cnt > pDisplay->batchsize) + if (room < 30) return; } } @@ -425,12 +439,12 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) // 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. + // XXX printed if not enough room 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) + room -= cmsDrawMenuEntry(pDisplay, p, top + i, drawPolled); + if (room < 30) return; } } @@ -494,12 +508,6 @@ long cmsMenuBack(displayPort_t *pDisplay) return 0; } -// 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); @@ -528,7 +536,6 @@ void cmsMenuOpen(void) return; cmsScreenInit(¤tDisplay, initfunc); - cmsComputeBatchsize(¤tDisplay); cmsScreenBegin(¤tDisplay); cmsMenuChange(¤tDisplay, currentMenu); } diff --git a/src/main/io/cms.h b/src/main/io/cms.h index a9ab352cd3..76d711a03d 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -7,6 +7,7 @@ typedef struct screenFnVTable_s { int (*write)(uint8_t, uint8_t, char *); int (*heartbeat)(void); void (*resync)(void); + uint16_t (*txroom)(void); } screenFnVTable_t; typedef struct displayPort_s { @@ -14,8 +15,7 @@ typedef struct displayPort_s { uint8_t cols; uint16_t buftime; uint16_t bufsize; - uint16_t batchsize; // Computed by CMS - screenFnVTable_t *VTable; + screenFnVTable_t *vTable; // CMS state bool cleared; diff --git a/src/main/io/display.c b/src/main/io/display.c index d8b5a8b82e..a9fb5c2d3f 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -783,6 +783,7 @@ screenFnVTable_t displayCmsVTable = { displayCmsWrite, NULL, NULL, + NULL, }; void displayCmsInit(displayPort_t *pPort) @@ -791,7 +792,7 @@ void displayCmsInit(displayPort_t *pPort) pPort->cols = 21; pPort->buftime = 1; pPort->bufsize = 50000; - pPort->VTable = &displayCmsVTable; + pPort->vTable = &displayCmsVTable; } #endif // OLEDCMS diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c67c07ce64..7c37b43db8 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -702,6 +702,7 @@ screenFnVTable_t osdVTable = { osdWrite, NULL, max7456RefreshAll, + NULL, }; void osdCmsInit(displayPort_t *pPort) @@ -709,8 +710,6 @@ void osdCmsInit(displayPort_t *pPort) shiftdown = masterConfig.osdProfile.row_shiftdown; pPort->rows = max7456GetRowsCount() - shiftdown; pPort->cols = 30; - pPort->buftime = 1; // Very fast - pPort->bufsize = 50000; // Very large - pPort->VTable = &osdVTable; + pPort->vTable = &osdVTable; } #endif // OSD diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index ef39598c96..c8d79ecbc9 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -246,3 +246,28 @@ void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFnToUse) { mspPushCommandFn = mspPushCommandFnToUse; } + +uint16_t mspSerialPushTxRoom() +{ + uint16_t minroom = 50000; + + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) + if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { + continue; + } + + uint32_t room = mspPort->port->vTable->serialTotalTxFree(mspPort->port); + + if (room < minroom) { + minroom = room; + } + } + + return minroom; +} diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 5d92114736..11f0d721cf 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -68,3 +68,4 @@ void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFn); void mspSerialPush(uint8_t, uint8_t *, int); +uint16_t mspSerialPushTxRoom(); From a88ef7e1606a74e00fc45e089d8d8fa658751a45 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 1 Nov 2016 22:35:50 +0100 Subject: [PATCH 071/188] Remap some timers --- src/main/drivers/exti.c | 6 ++++++ src/main/target/BETAFLIGHTF3/target.c | 15 +++++++-------- src/main/target/BETAFLIGHTF3/target.h | 9 +++------ 3 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index b4d4cfaa49..db41a01c7a 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -56,6 +56,12 @@ void EXTIInit(void) #if defined(STM32F3) || defined(STM32F4) /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); +#ifdef REMAP_TIM16_DMA + SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM16, ENABLE); +#endif +#ifdef REMAP_TIM17_DMA + SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM17, ENABLE); +#endif #endif memset(extiChannelRecs, 0, sizeof(extiChannelRecs)); memset(extiGroupPriority, 0xff, sizeof(extiGroupPriority)); diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index d67ff5f6b1..78d034bf84 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -26,14 +26,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel1, DMA1_CH1_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM4 - PB9 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - TIM2_CH4 AF1, TIM15_CH2 AF9 { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index f7d6db277d..bdfde547d7 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -25,7 +25,7 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 10 +#define USABLE_TIMER_CHANNEL_COUNT 9 #define MPU6000_CS_PIN PA15 #define MPU6000_SPI_INSTANCE SPI1 @@ -45,11 +45,8 @@ #define USE_EXTI #define USE_DSHOT - -// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4 -#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT) -#undef USE_UART1_TX_DMA -#endif +#define REMAP_TIM16_DMA +#define REMAP_TIM17_DMA #define USB_IO From cf4d764a5874940bef77bc2e2826b6d607af84ec Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 2 Nov 2016 00:01:24 +0100 Subject: [PATCH 072/188] AIORACERF3. Fix typo. --- src/main/target/AIORACERF3/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index e94ab4ccea..591219e747 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -35,5 +35,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // UART3_TX (AF7) { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LEd, 1, GPIO_AF_6 }, //LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, //LED_STRIP }; From 41b36bfae190c5563f54833a14032cf0dbc42df6 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 2 Nov 2016 00:26:13 +0100 Subject: [PATCH 073/188] AIRHEROF3. More typos fixed. --- src/main/target/AIRHEROF3/target.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c index d29eac131a..e7ca5a7261 100755 --- a/src/main/target/AIRHEROF3/target.c +++ b/src/main/target/AIRHEROF3/target.c @@ -23,7 +23,7 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIME_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1}, // PWM1 - RC1 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1}, // PWM1 - RC1 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM2 - RC2 { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM3 - RC3 { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM4 - RC4 @@ -31,10 +31,10 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM6 - RC6 { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM7 - RC7 { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_6}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_11}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2} // PWM14 - OUT6 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6}, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_11}, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2} // PWM14 - OUT6 }; From 58d12a11cfb3f4f691b0b4fd53b02874149eee8c Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 2 Nov 2016 00:36:12 +0100 Subject: [PATCH 074/188] RCEXPLORERF3. Typos fixed. --- src/main/target/RCEXPLORERF3/target.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index e531b55ab0..730d71b682 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -24,10 +24,10 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM1 - PA4 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_1}, // PWM2 - PA7 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_6}, // PWM3 - PA8 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM4 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIME_USE_MOTOR, 1, GPIO_AF_2}, // PWM5 - PB1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIME_USE_PPM, 0, GPIO_AF_1}, // PWM6 - PPM + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM1 - PA4 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM2 - PA7 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6}, // PWM3 - PA8 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM4 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM5 - PB1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1}, // PWM6 - PPM }; From f58f7f65b1ad8614c395aabb567ab7743313fe9c Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 2 Nov 2016 09:12:37 +0900 Subject: [PATCH 075/188] Cleanups --- src/main/fc/fc_msp.c | 14 ------------- src/main/fc/fc_msp.h | 2 -- src/main/io/.canvas.c.swo | Bin 0 -> 12288 bytes src/main/io/canvas.c | 20 +++++++++++------- src/main/io/cms.c | 30 ++++++++------------------ src/main/io/cms.h | 29 +++++++++++++------------- src/main/io/display.c | 31 ++++++++++++++++++++------- src/main/io/display.h | 2 +- src/main/io/osd.c | 37 ++++++++++++++++++++++++--------- src/main/msp/.msp_serial.h.swo | Bin 0 -> 12288 bytes src/main/msp/msp_serial.c | 15 ++++--------- src/main/msp/msp_serial.h | 5 ++--- 12 files changed, 94 insertions(+), 91 deletions(-) create mode 100644 src/main/io/.canvas.c.swo create mode 100644 src/main/msp/.msp_serial.h.swo diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3119e27bc9..b92507b1af 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1778,17 +1778,3 @@ void mspFcInit(void) { initActiveBoxIds(); } - -void mspServerPush(mspPacket_t *push, uint8_t *data, int len) -{ - sbuf_t *dst = &push->buf; - - while (len--) { - sbufWriteU8(dst, *data++); - } -} - -mspPushCommandFnPtr mspFcPushInit(void) -{ - return mspServerPush; -} diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index df317ab3a1..53edbd8ed1 100644 --- a/src/main/fc/fc_msp.h +++ b/src/main/fc/fc_msp.h @@ -21,5 +21,3 @@ void mspFcInit(void); mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); - -mspPushCommandFnPtr mspFcPushInit(void); diff --git a/src/main/io/.canvas.c.swo b/src/main/io/.canvas.c.swo new file mode 100644 index 0000000000000000000000000000000000000000..0a4fbf80661581b5a542e89ad234e54cad2ff866 GIT binary patch literal 12288 zcmeI2O>7%Q7=|b104=mZpt%`@LP ziR@-|@zQ1b#AuD+dYBNua$X7mhb;ck1IcE*4)UX8(fHCktFd*9l3Q za2}inW$-9?0Nj2MbA!*or(g?gf(=jy^I#DCeuR+Uz-{mY_#S)$J_a9wx4|_Kf)~NF zU=lnAj)OsP4EzNDKZ5tbTY!T#;DZLJg9<1EwO6Nrxr_m0z!)$Fi~(cdUo$YwB9HrI zi%?u{m|S2T?y@>ty3t~GKUHkv#zBGxwd$Uzh+k3BvDy^1ZMwX?OqX~NP{I6wxsqOKEX>*SGtW^T zQQ2bDPh~3D-h}czEsrPd!0D=lRwXTGnW;SWd^)ejdOeQAN|u+7)LGJv++ySz<_I~< z99ax52`*VNFo*AWielhmiZQY$LL+BbVH1;gSZ8Yb-pCb$zc)@Qqt zB~J;)tZWxj9?9DH&Io!jmfD;C{m8Zc^Rl(BeO+f_^L6NDKA*~VDy_>4^Q&_+R~Ba$ zD%4WzIIH%k)J**f4Om1^(+Mmw>mIVuZFeopZ8-u>M$qQ!#7c!GQt)UkYmTNAmwHlL zfx^6|)cv@+N3HF)7*i1l*^N|Hq?S*o22vU9mL~V9W$6&+#1EF9x>Tamsug6bPFa3g z3RRWnE#m^3>&y5e^g?wra4I^Lny?jhT1 zf3`ZGJ;yAUG>MJRHajbbor61{CM_%58i@w3nxw6}4qpj)P4A1E^(I4}m N=P$8ltd>1Z{sbQ4hUWkP literal 0 HcmV?d00001 diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index 5b0973ed90..e57e641eb9 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -9,6 +9,8 @@ #ifdef CANVAS +#include "common/utils.h" + #include "drivers/system.h" #include "io/cms.h" @@ -67,32 +69,36 @@ int canvasWrite(uint8_t col, uint8_t row, char *string) return canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4); } -uint16_t canvasTxRoom() +void canvasResync(displayPort_t *pPort) { - return mspSerialPushTxRoom(); + pPort->rows = 13; // XXX Will reflect NTSC/PAL in the future + pPort->rows = 30; } -screenFnVTable_t canvasVTable = { +uint32_t canvasTxRoom(void) +{ + return mspSerialTxBytesFree(); +} + +displayPortVTable_t canvasVTable = { canvasBegin, canvasEnd, canvasClear, canvasWrite, canvasHeartBeat, - NULL, + canvasResync, canvasTxRoom, }; void canvasCmsInit(displayPort_t *pPort) { - pPort->rows = 13; + pPort->rows = 13; // XXX Will reflect NTSC/PAL in the future pPort->cols = 30; pPort->vTable = &canvasVTable; } void canvasInit() { - mspSerialPushInit(mspFcPushInit()); // Called once at startup to initialize push function in msp - cmsDeviceRegister(canvasCmsInit); } #endif diff --git a/src/main/io/cms.c b/src/main/io/cms.c index be7e5186e8..706b1ccec6 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -37,9 +37,7 @@ #include "io/cms.h" #include "io/cms_types.h" -#ifdef CANVAS #include "io/canvas.h" -#endif #include "io/flashfs.h" #include "io/osd.h" @@ -130,22 +128,17 @@ int cmsScreenWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s) void cmsScreenHeartBeat(displayPort_t *instance) { - if (instance->vTable->heartbeat) - instance->vTable->heartbeat(); + instance->vTable->heartbeat(); } void cmsScreenResync(displayPort_t *instance) { - if (instance->vTable->resync) - instance->vTable->resync(); + instance->vTable->resync(instance); } uint16_t cmsScreenTxRoom(displayPort_t *instance) { - if (instance->vTable->txroom) - return instance->vTable->txroom(); - else - return 10000; + return instance->vTable->txroom(); } void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) @@ -372,10 +365,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr return cnt; } -void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) +void cmsDrawMenu(displayPort_t *pDisplay) { - UNUSED(currentTime); - uint8_t i; OSD_Entry *p; uint8_t top = (pDisplay->rows - currentMenuIdx) / 2 - 1; @@ -385,7 +376,7 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) static uint8_t pollDenom = 0; bool drawPolled = (++pollDenom % 8 == 0); - int room = cmsScreenTxRoom(pDisplay); + uint32_t room = cmsScreenTxRoom(pDisplay); if (!currentMenu) return; @@ -799,7 +790,7 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) return; } - cmsDrawMenu(pDisplay, currentTime); + cmsDrawMenu(pDisplay); if (currentTime > lastCmsHeartBeat + 500) { // Heart beat for external CMS display device @ 500msec @@ -811,15 +802,13 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) lastCalled = currentTime; } -void cmsHandler(uint32_t unusedTime) +void cmsHandler(uint32_t currentTime) { - UNUSED(unusedTime); - if (cmsDeviceCount < 0) return; static uint32_t lastCalled = 0; - uint32_t now = millis(); + const uint32_t now = currentTime / 1000; if (now - lastCalled >= CMS_UPDATE_INTERVAL) { cmsUpdate(¤tDisplay, now); @@ -1243,8 +1232,7 @@ OSD_Entry menuInfo[] = { void cmsx_InfoInit(void) { - int i; - for (i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { + for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; else diff --git a/src/main/io/cms.h b/src/main/io/cms.h index 76d711a03d..e90f228278 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -1,40 +1,41 @@ #pragma once -typedef struct screenFnVTable_s { +struct displayPort_s; + +typedef struct displayPortVTable_s { int (*begin)(void); int (*end)(void); int (*clear)(void); - int (*write)(uint8_t, uint8_t, char *); + int (*write)(uint8_t col, uint8_t row, char *text); int (*heartbeat)(void); - void (*resync)(void); - uint16_t (*txroom)(void); -} screenFnVTable_t; + void (*resync)(struct displayPort_s *pPort); + uint32_t (*txroom)(void); +} displayPortVTable_t; typedef struct displayPort_s { + displayPortVTable_t *vTable; uint8_t rows; uint8_t cols; uint16_t buftime; uint16_t bufsize; - screenFnVTable_t *vTable; // CMS state bool cleared; } displayPort_t; // Device management -typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *); +typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *pPort); bool cmsDeviceRegister(cmsDeviceInitFuncPtr); // For main.c and scheduler void cmsInit(void); -void cmsHandler(uint32_t); +void cmsHandler(uint32_t currentTime); // Required for external CMS tables -long cmsChangeScreen(displayPort_t *, void *); -long cmsExitMenu(displayPort_t *, void *); - -#define STARTUP_HELP_TEXT1 "MENU: THR MID" -#define STARTUP_HELP_TEXT2 "+ YAW LEFT" -#define STARTUP_HELP_TEXT3 "+ PITCH UP" +long cmsChangeScreen(displayPort_t *pPort, void *ptr); +long cmsExitMenu(displayPort_t *pPort, void *ptr); +#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" +#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" +#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" diff --git a/src/main/io/display.c b/src/main/io/display.c index a9fb5c2d3f..d9b651b69d 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -23,6 +23,8 @@ #ifdef DISPLAY +#include "common/utils.h" + #include "build/version.h" #include "build/debug.h" @@ -776,22 +778,35 @@ int displayCmsWrite(uint8_t x, uint8_t y, char *s) return 0; } -screenFnVTable_t displayCmsVTable = { +int displayCmsHeartbeat(void) +{ + return 0; +} + +void displayCmsResync(displayPort_t *pPort) +{ + UNUSED(pPort); +} + +uint32_t displayCmsTxroom(void) +{ + return UINT32_MAX; +} + +displayPortVTable_t displayCmsVTable = { displayCmsBegin, displayCmsEnd, displayCmsClear, displayCmsWrite, - NULL, - NULL, - NULL, + displayCmsHeartbeat, + displayCmsResync, + displayCmsTxroom, }; void displayCmsInit(displayPort_t *pPort) { - pPort->rows = 8; - pPort->cols = 21; - pPort->buftime = 1; - pPort->bufsize = 50000; + pPort->rows = SCREEN_CHARACTER_ROW_COUNT; + pPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; pPort->vTable = &displayCmsVTable; } #endif // OLEDCMS diff --git a/src/main/io/display.h b/src/main/io/display.h index e2c6c2b967..f2cf782bd1 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -47,5 +47,5 @@ void displayResetPageCycling(void); void displaySetNextPageChangeAt(uint32_t futureMicros); #ifdef CMS -void displayCmsInit(displayPort_t *); +void displayCmsInit(displayPort_t *pPort); #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7c37b43db8..472dacd428 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -392,9 +392,11 @@ void osdInit(void) sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); max7456Write(5, 6, string_buffer); - max7456Write(7, 7, STARTUP_HELP_TEXT1); - max7456Write(11, 8, STARTUP_HELP_TEXT2); - max7456Write(11, 9, STARTUP_HELP_TEXT3); +#ifdef CMS + max7456Write(7, 7, CMS_STARTUP_HELP_TEXT1); + max7456Write(11, 8, CMS_STARTUP_HELP_TEXT2); + max7456Write(11, 9, CMS_STARTUP_HELP_TEXT3); +#endif max7456RefreshAll(); @@ -669,6 +671,23 @@ int osdWrite(uint8_t x, uint8_t y, char *s) return 0; } +void osdResync(displayPort_t *pPort) +{ + max7456RefreshAll(); + pPort->rows = max7456GetRowsCount() - masterConfig.osdProfile.row_shiftdown; + pPort->cols = 30; +} + +int osdHeartbeat(void) +{ + return 0; +} + +uint32_t osdTxroom(void) +{ + return UINT32_MAX; +} + #ifdef EDIT_ELEMENT_SUPPORT void osdEditElement(void *ptr) { @@ -695,21 +714,19 @@ void osdDrawElementPositioningHelp(void) } #endif -screenFnVTable_t osdVTable = { +displayPortVTable_t osdVTable = { osdMenuBegin, osdMenuEnd, osdClearScreen, osdWrite, - NULL, - max7456RefreshAll, - NULL, + osdHeartbeat, + osdResync, + osdTxroom, }; void osdCmsInit(displayPort_t *pPort) { - shiftdown = masterConfig.osdProfile.row_shiftdown; - pPort->rows = max7456GetRowsCount() - shiftdown; - pPort->cols = 30; + osdResync(pPort); pPort->vTable = &osdVTable; } #endif // OSD diff --git a/src/main/msp/.msp_serial.h.swo b/src/main/msp/.msp_serial.h.swo new file mode 100644 index 0000000000000000000000000000000000000000..fd05903527a791eceeaf20b9ad5b46eb7f633ef1 GIT binary patch literal 12288 zcmeI2%WvFN6vl5sqPz;IbOEeRlTDIN$H}CHw&CT>qaBfXXgrfNJT!9b>)68MYkcjb zp*$qSf^OI$m9F^%SRt`t#SSD^2!w9p?Q*W9l1P0j2t@}a$#vvS z8D5VZxDJQPD+ELd+KIa(!zK&5oYA6OywbICc@!T z&Y`gKjvZ!rPF+Mb8JCb+mlL0u%&8ii1Gl2+_3X&4M9nmrHa(G1H44&EB86jNnN zy2_@+TfuKgJaPk%&B7^&IPI6-=(I<*Q{n9MunQ%>v)SCfbIEL*l{M44)J2R+xz(4D zPK~3F4XX7@qknsTGWZlLT2iS@PkC&9>r>AxoYoi7Z#Y+7>y3I>4OBM$Zd8Nvy1CXd+vVHt=JwZ@ zM_CVRI_TtKOOC@98?~BQFLhhxV!66rF6~FXR5nXkj$*BpJ2?}+vK>Ldx7ii~*b5~MNBpfDoM1}qMGLPO5 zA}7pTPBVB<)Zyg3KZDsgvA26RiOAuM(SiZU4n?$0yFuzxaeTTtPgM0N5@`;5Bc~Vr zG>tD9#_sNJemlzYBH1>=z~PbP(s((q;%thHq%X42CErFN!-7<8j)?b2cu17f#d?P> z@`xvPNX={uL1=9d2tibw!wtw2*b-I6E>Gwl85{AhW(n8)yy0+g$03B5;@Q zU^7z52fa89xJ$cslGstIic%47(T&t<1=x@RgoR}=aP&poUneYKim=$=wm4y5^Rf`V z)vtTbPH9hM35hrjL^O}S*rN~)lF$xMB&Pc1sx;J(NV0^NA`gs27GckYL5?_JE(6U65F#1qaw9cn|nbBjqw;8`#$>R7G3kqSYZDH&JFt0 literal 0 HcmV?d00001 diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index c8d79ecbc9..409e60afc8 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -211,8 +211,6 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } -mspPushCommandFnPtr mspPushCommandFn; - void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) { static uint8_t pushBuf[30]; @@ -234,7 +232,7 @@ void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) continue; } - mspPushCommandFn(&push, data, datalen); + sbufWriteData(&push.buf, data, datalen); sbufSwitchToReader(&push.buf, pushBuf); @@ -242,14 +240,9 @@ void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) } } -void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFnToUse) +uint32_t mspSerialTxBytesFree() { - mspPushCommandFn = mspPushCommandFnToUse; -} - -uint16_t mspSerialPushTxRoom() -{ - uint16_t minroom = 50000; + uint32_t minroom = UINT16_MAX; for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { mspPort_t * const mspPort = &mspPorts[portIndex]; @@ -262,7 +255,7 @@ uint16_t mspSerialPushTxRoom() continue; } - uint32_t room = mspPort->port->vTable->serialTotalTxFree(mspPort->port); + uint32_t room = serialTxBytesFree(mspPort->port); if (room < minroom) { minroom = room; diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 11f0d721cf..0f1c8a2fff 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -66,6 +66,5 @@ void mspSerialInit(void); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); -void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFn); -void mspSerialPush(uint8_t, uint8_t *, int); -uint16_t mspSerialPushTxRoom(); +void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen); +uint32_t mspSerialTxBytesFree(void); From 567c5cded51344f4261434c3779b284c38f19aaa Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 2 Nov 2016 09:13:57 +0900 Subject: [PATCH 076/188] Delete vi swap files... --- src/main/io/.canvas.c.swo | Bin 12288 -> 0 bytes src/main/msp/.msp_serial.h.swo | Bin 12288 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/main/io/.canvas.c.swo delete mode 100644 src/main/msp/.msp_serial.h.swo diff --git a/src/main/io/.canvas.c.swo b/src/main/io/.canvas.c.swo deleted file mode 100644 index 0a4fbf80661581b5a542e89ad234e54cad2ff866..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12288 zcmeI2O>7%Q7=|b104=mZpt%`@LP ziR@-|@zQ1b#AuD+dYBNua$X7mhb;ck1IcE*4)UX8(fHCktFd*9l3Q za2}inW$-9?0Nj2MbA!*or(g?gf(=jy^I#DCeuR+Uz-{mY_#S)$J_a9wx4|_Kf)~NF zU=lnAj)OsP4EzNDKZ5tbTY!T#;DZLJg9<1EwO6Nrxr_m0z!)$Fi~(cdUo$YwB9HrI zi%?u{m|S2T?y@>ty3t~GKUHkv#zBGxwd$Uzh+k3BvDy^1ZMwX?OqX~NP{I6wxsqOKEX>*SGtW^T zQQ2bDPh~3D-h}czEsrPd!0D=lRwXTGnW;SWd^)ejdOeQAN|u+7)LGJv++ySz<_I~< z99ax52`*VNFo*AWielhmiZQY$LL+BbVH1;gSZ8Yb-pCb$zc)@Qqt zB~J;)tZWxj9?9DH&Io!jmfD;C{m8Zc^Rl(BeO+f_^L6NDKA*~VDy_>4^Q&_+R~Ba$ zD%4WzIIH%k)J**f4Om1^(+Mmw>mIVuZFeopZ8-u>M$qQ!#7c!GQt)UkYmTNAmwHlL zfx^6|)cv@+N3HF)7*i1l*^N|Hq?S*o22vU9mL~V9W$6&+#1EF9x>Tamsug6bPFa3g z3RRWnE#m^3>&y5e^g?wra4I^Lny?jhT1 zf3`ZGJ;yAUG>MJRHajbbor61{CM_%58i@w3nxw6}4qpj)P4A1E^(I4}m N=P$8ltd>1Z{sbQ4hUWkP diff --git a/src/main/msp/.msp_serial.h.swo b/src/main/msp/.msp_serial.h.swo deleted file mode 100644 index fd05903527a791eceeaf20b9ad5b46eb7f633ef1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12288 zcmeI2%WvFN6vl5sqPz;IbOEeRlTDIN$H}CHw&CT>qaBfXXgrfNJT!9b>)68MYkcjb zp*$qSf^OI$m9F^%SRt`t#SSD^2!w9p?Q*W9l1P0j2t@}a$#vvS z8D5VZxDJQPD+ELd+KIa(!zK&5oYA6OywbICc@!T z&Y`gKjvZ!rPF+Mb8JCb+mlL0u%&8ii1Gl2+_3X&4M9nmrHa(G1H44&EB86jNnN zy2_@+TfuKgJaPk%&B7^&IPI6-=(I<*Q{n9MunQ%>v)SCfbIEL*l{M44)J2R+xz(4D zPK~3F4XX7@qknsTGWZlLT2iS@PkC&9>r>AxoYoi7Z#Y+7>y3I>4OBM$Zd8Nvy1CXd+vVHt=JwZ@ zM_CVRI_TtKOOC@98?~BQFLhhxV!66rF6~FXR5nXkj$*BpJ2?}+vK>Ldx7ii~*b5~MNBpfDoM1}qMGLPO5 zA}7pTPBVB<)Zyg3KZDsgvA26RiOAuM(SiZU4n?$0yFuzxaeTTtPgM0N5@`;5Bc~Vr zG>tD9#_sNJemlzYBH1>=z~PbP(s((q;%thHq%X42CErFN!-7<8j)?b2cu17f#d?P> z@`xvPNX={uL1=9d2tibw!wtw2*b-I6E>Gwl85{AhW(n8)yy0+g$03B5;@Q zU^7z52fa89xJ$cslGstIic%47(T&t<1=x@RgoR}=aP&poUneYKim=$=wm4y5^Rf`V z)vtTbPH9hM35hrjL^O}S*rN~)lF$xMB&Pc1sx;J(NV0^NA`gs27GckYL5?_JE(6U65F#1qaw9cn|nbBjqw;8`#$>R7G3kqSYZDH&JFt0 From 229d47943ef7d8ac59c4092a5d336c8eeda6e65f Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 2 Nov 2016 01:18:25 +0100 Subject: [PATCH 077/188] REVONANO. Typos typos typos..... --- src/main/target/REVONANO/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index e8d2132393..1ddf9641cc 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -23,7 +23,7 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM | TIME_USE_PPM, 0, GPIO_AF_TIM2 }, // PPM + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM2 }, // PPM { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN From 5bc11058715a6ce192b8406ca0f3b11a3e699118 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 2 Nov 2016 05:00:09 +0000 Subject: [PATCH 078/188] Renamed display to dashboard --- Makefile | 2 +- src/main/fc/config.c | 4 +-- src/main/fc/config.h | 2 +- src/main/fc/fc_tasks.c | 22 ++++++------ src/main/fc/rc_controls.c | 8 ++--- src/main/io/{display.c => dashboard.c} | 46 +++++++++++++------------- src/main/io/{display.h => dashboard.h} | 18 +++++----- src/main/io/gps.c | 14 ++++---- src/main/main.c | 18 +++++----- src/main/scheduler/scheduler.h | 4 +-- src/main/target/AIR32/target.h | 1 - src/main/target/MICROSCISKY/target.h | 1 - src/main/target/MOTOLAB/target.h | 1 - src/main/target/NAZE/target.h | 2 -- src/main/target/RCEXPLORERF3/target.h | 1 - src/main/target/common.h | 2 +- 16 files changed, 70 insertions(+), 76 deletions(-) rename src/main/io/{display.c => dashboard.c} (95%) rename src/main/io/{display.h => dashboard.h} (70%) diff --git a/Makefile b/Makefile index 983c51c904..fa5bed9106 100644 --- a/Makefile +++ b/Makefile @@ -561,7 +561,7 @@ HIGHEND_SRC = \ flight/gps_conversion.c \ io/gps.c \ io/ledstrip.c \ - io/display.c \ + io/dashboard.c \ sensors/sonar.c \ sensors/barometer.c \ telemetry/telemetry.c \ diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 0d8fe7f949..027a80b0f6 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -903,8 +903,8 @@ void validateAndFixConfig(void) #endif #if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3) - if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) { - featureClear(FEATURE_DISPLAY); + if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DASHBOARD)) { + featureClear(FEATURE_DASHBOARD); } #endif diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 1432b6882b..e6cc491229 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -45,7 +45,7 @@ typedef enum { FEATURE_RX_MSP = 1 << 14, FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, - FEATURE_DISPLAY = 1 << 17, + FEATURE_DASHBOARD = 1 << 17, FEATURE_OSD = 1 << 18, FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b81d36ce01..7466c1adac 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -41,7 +41,7 @@ #include "flight/altitudehold.h" #include "io/beeper.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" @@ -222,11 +222,11 @@ static void taskCalculateAltitude(uint32_t currentTime) }} #endif -#ifdef DISPLAY -static void taskUpdateDisplay(uint32_t currentTime) +#ifdef USE_DASHBOARD +static void taskUpdateDashboard(uint32_t currentTime) { - if (feature(FEATURE_DISPLAY)) { - displayUpdate(currentTime); + if (feature(FEATURE_DASHBOARD)) { + dashboardUpdate(currentTime); } } #endif @@ -307,8 +307,8 @@ void fcTasksInit(void) #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif -#ifdef DISPLAY - setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); +#ifdef USE_DASHBOARD + setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); @@ -446,10 +446,10 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef DISPLAY - [TASK_DISPLAY] = { - .taskName = "DISPLAY", - .taskFunc = taskUpdateDisplay, +#ifdef USE_DASHBOARD + [TASK_DASHBOARD] = { + .taskName = "DASHBOARD", + .taskFunc = taskUpdateDashboard, .desiredPeriod = 1000000 / 10, .staticPriority = TASK_PRIORITY_LOW, }, diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 316d5178b2..91123768e3 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -44,7 +44,7 @@ #include "io/beeper.h" #include "io/motors.h" #include "io/vtx.h" -#include "io/display.h" +#include "io/dashboard.h" #include "sensors/barometer.h" #include "sensors/battery.h" @@ -291,13 +291,13 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } -#ifdef DISPLAY +#ifdef USE_DASHBOARD if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { - displayDisablePageCycling(); + dashboardDisablePageCycling(); } if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { - displayEnablePageCycling(); + dashboardEnablePageCycling(); } #endif diff --git a/src/main/io/display.c b/src/main/io/dashboard.c similarity index 95% rename from src/main/io/display.c rename to src/main/io/dashboard.c index 1d7c88199a..972b14a835 100644 --- a/src/main/io/display.c +++ b/src/main/io/dashboard.c @@ -21,7 +21,7 @@ #include "platform.h" -#ifdef DISPLAY +#ifdef USE_DASHBOARD #include "build/version.h" #include "build/debug.h" @@ -58,7 +58,7 @@ #include "config/feature.h" #include "config/config_profile.h" -#include "io/display.h" +#include "io/dashboard.h" #include "rx/rx.h" @@ -74,7 +74,7 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex); #define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5) static uint32_t nextDisplayUpdateAt = 0; -static bool displayPresent = false; +static bool dashboardPresent = false; static rxConfig_t *rxConfig; @@ -98,7 +98,7 @@ static const char* const pageTitles[] = { #ifdef GPS ,"GPS" #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE ,"DEBUG" #endif }; @@ -116,7 +116,7 @@ const pageId_e cyclePageIds[] = { #ifndef SKIP_TASK_STATISTICS ,PAGE_TASKS #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE ,PAGE_DEBUG, #endif }; @@ -144,7 +144,7 @@ typedef struct pageState_s { static pageState_t pageState; void resetDisplay(void) { - displayPresent = ug2864hsweg01InitI2C(); + dashboardPresent = ug2864hsweg01InitI2C(); } void LCDprint(uint8_t i) { @@ -562,7 +562,7 @@ void showTasksPage(void) } #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE void showDebugPage(void) { @@ -577,7 +577,7 @@ void showDebugPage(void) } #endif -void displayUpdate(uint32_t currentTime) +void dashboardUpdate(uint32_t currentTime) { static uint8_t previousArmedState = 0; @@ -623,13 +623,13 @@ void displayUpdate(uint32_t currentTime) // user to power off/on the display or connect it while powered. resetDisplay(); - if (!displayPresent) { + if (!dashboardPresent) { return; } handlePageChange(); } - if (!displayPresent) { + if (!dashboardPresent) { return; } @@ -666,7 +666,7 @@ void displayUpdate(uint32_t currentTime) } break; #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE case PAGE_DEBUG: showDebugPage(); break; @@ -680,13 +680,13 @@ void displayUpdate(uint32_t currentTime) } -void displaySetPage(pageId_e pageId) +void dashboardSetPage(pageId_e pageId) { pageState.pageId = pageId; pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; } -void displayInit(rxConfig_t *rxConfigToUse) +void dashboardInit(rxConfig_t *rxConfigToUse) { delay(200); resetDisplay(); @@ -695,36 +695,36 @@ void displayInit(rxConfig_t *rxConfigToUse) rxConfig = rxConfigToUse; memset(&pageState, 0, sizeof(pageState)); - displaySetPage(PAGE_WELCOME); + dashboardSetPage(PAGE_WELCOME); - displayUpdate(micros()); + dashboardUpdate(micros()); - displaySetNextPageChangeAt(micros() + (1000 * 1000 * 5)); + dashboardSetNextPageChangeAt(micros() + (1000 * 1000 * 5)); } -void displayShowFixedPage(pageId_e pageId) +void dashboardShowFixedPage(pageId_e pageId) { - displaySetPage(pageId); - displayDisablePageCycling(); + dashboardSetPage(pageId); + dashboardDisablePageCycling(); } -void displaySetNextPageChangeAt(uint32_t futureMicros) +void dashboardSetNextPageChangeAt(uint32_t futureMicros) { pageState.nextPageAt = futureMicros; } -void displayEnablePageCycling(void) +void dashboardEnablePageCycling(void) { pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED; } -void displayResetPageCycling(void) +void dashboardResetPageCycling(void) { pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page } -void displayDisablePageCycling(void) +void dashboardDisablePageCycling(void) { pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED; } diff --git a/src/main/io/display.h b/src/main/io/dashboard.h similarity index 70% rename from src/main/io/display.h rename to src/main/io/dashboard.h index 75abffbe3c..cca02effdf 100644 --- a/src/main/io/display.h +++ b/src/main/io/dashboard.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -#define ENABLE_DEBUG_OLED_PAGE +#define ENABLE_DEBUG_DASHBOARD_PAGE typedef enum { PAGE_WELCOME, @@ -30,18 +30,18 @@ typedef enum { #ifdef GPS PAGE_GPS, #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE PAGE_DEBUG, #endif } pageId_e; struct rxConfig_s; -void displayInit(struct rxConfig_s *intialRxConfig); -void displayUpdate(uint32_t currentTime); +void dashboardInit(struct rxConfig_s *intialRxConfig); +void dashboardUpdate(uint32_t currentTime); -void displayShowFixedPage(pageId_e pageId); +void dashboardShowFixedPage(pageId_e pageId); -void displayEnablePageCycling(void); -void displayDisablePageCycling(void); -void displayResetPageCycling(void); -void displaySetNextPageChangeAt(uint32_t futureMicros); +void dashboardEnablePageCycling(void); +void dashboardDisablePageCycling(void); +void dashboardResetPageCycling(void); +void dashboardSetNextPageChangeAt(uint32_t futureMicros); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index f8e9d880f0..2cd9873baf 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -39,7 +39,7 @@ #include "sensors/sensors.h" #include "io/serial.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/gps.h" #include "flight/gps_conversion.h" @@ -1072,9 +1072,9 @@ static bool gpsNewFrameUBLOX(uint8_t data) static void gpsHandlePassthrough(uint8_t data) { gpsNewData(data); - #ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayUpdate(micros()); + #ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardUpdate(micros()); } #endif @@ -1088,9 +1088,9 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) if(!(gpsPort->mode & MODE_TX)) serialSetMode(gpsPort, gpsPort->mode | MODE_TX); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayShowFixedPage(PAGE_GPS); +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardShowFixedPage(PAGE_GPS); } #endif diff --git a/src/main/main.c b/src/main/main.c index 442964e9c1..a6fccd0ebe 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -82,7 +82,7 @@ #include "io/servos.h" #include "io/gimbal.h" #include "io/ledstrip.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" @@ -395,9 +395,9 @@ void init(void) initBoardAlignment(&masterConfig.boardAlignment); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayInit(&masterConfig.rxConfig); +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardInit(&masterConfig.rxConfig); } #endif @@ -585,13 +585,13 @@ void init(void) if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY - displayShowFixedPage(PAGE_GPS); + dashboardShowFixedPage(PAGE_GPS); #else - displayResetPageCycling(); - displayEnablePageCycling(); + dashboardResetPageCycling(); + dashboardEnablePageCycling(); #endif } #endif diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index f44cd497f9..ed1b50f0a5 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -67,8 +67,8 @@ typedef enum { #if defined(BARO) || defined(SONAR) TASK_ALTITUDE, #endif -#ifdef DISPLAY - TASK_DISPLAY, +#ifdef USE_DASHBOARD + TASK_DASHBOARD, #endif #ifdef TELEMETRY TASK_TELEMETRY, diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 1b8f6c2111..0cff2b6aab 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -89,7 +89,6 @@ #define SENSORS_SET (SENSOR_ACC) #undef GPS -#define DISPLAY #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 2628264522..f61806f3b7 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -91,7 +91,6 @@ #undef GPS #undef USE_SERVOS #define USE_QUAD_MIXER_ONLY -#define DISPLAY // IO - assuming all IOs on 48pin package diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 6a7ae42a0b..93e92d61cb 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -91,7 +91,6 @@ #define SENSORS_SET (SENSOR_ACC) #undef GPS -#define DISPLAY #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index eb8ab5503a..d7586fe858 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -118,8 +118,6 @@ //#define SONAR_TRIGGER_PIN_PWM PB8 //#define SONAR_ECHO_PIN_PWM PB9 -//#define DISPLAY - #define USE_UART1 #define USE_UART2 /* only 2 uarts available on the NAZE, add ifdef here if present on other boards */ diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index a1f5f8527d..2f19300b0d 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -130,7 +130,6 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE -#define DISPLAY #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/common.h b/src/main/target/common.h index 8d0ea828d2..42dfed36de 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -55,7 +55,7 @@ #endif #if (FLASH_SIZE > 128) -#define DISPLAY +#define USE_DASHBOARD #define TELEMETRY_MAVLINK #else #define SKIP_CLI_COMMAND_HELP From 9363f53945bb933638f38cba5fea1e459a1def58 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 2 Nov 2016 15:55:29 +0900 Subject: [PATCH 079/188] Consistent naming for TxBytesFree --- src/main/io/canvas.c | 4 ++-- src/main/io/cms.c | 6 +++--- src/main/io/cms.h | 2 +- src/main/io/display.c | 4 ++-- src/main/io/osd.c | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index e57e641eb9..f2d0eeed15 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -75,7 +75,7 @@ void canvasResync(displayPort_t *pPort) pPort->rows = 30; } -uint32_t canvasTxRoom(void) +uint32_t canvasTxBytesFree(void) { return mspSerialTxBytesFree(); } @@ -87,7 +87,7 @@ displayPortVTable_t canvasVTable = { canvasWrite, canvasHeartBeat, canvasResync, - canvasTxRoom, + canvasTxBytesFree, }; void canvasCmsInit(displayPort_t *pPort) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 706b1ccec6..d65ac12d94 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -136,9 +136,9 @@ void cmsScreenResync(displayPort_t *instance) instance->vTable->resync(instance); } -uint16_t cmsScreenTxRoom(displayPort_t *instance) +uint16_t cmsScreenTxBytesFree(displayPort_t *instance) { - return instance->vTable->txroom(); + return instance->vTable->txBytesFree(); } void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) @@ -376,7 +376,7 @@ void cmsDrawMenu(displayPort_t *pDisplay) static uint8_t pollDenom = 0; bool drawPolled = (++pollDenom % 8 == 0); - uint32_t room = cmsScreenTxRoom(pDisplay); + uint32_t room = cmsScreenTxBytesFree(pDisplay); if (!currentMenu) return; diff --git a/src/main/io/cms.h b/src/main/io/cms.h index e90f228278..bef66c62e2 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -9,7 +9,7 @@ typedef struct displayPortVTable_s { int (*write)(uint8_t col, uint8_t row, char *text); int (*heartbeat)(void); void (*resync)(struct displayPort_s *pPort); - uint32_t (*txroom)(void); + uint32_t (*txBytesFree)(void); } displayPortVTable_t; typedef struct displayPort_s { diff --git a/src/main/io/display.c b/src/main/io/display.c index d9b651b69d..bc44e6552d 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -788,7 +788,7 @@ void displayCmsResync(displayPort_t *pPort) UNUSED(pPort); } -uint32_t displayCmsTxroom(void) +uint32_t displayCmsTxBytesFree(void) { return UINT32_MAX; } @@ -800,7 +800,7 @@ displayPortVTable_t displayCmsVTable = { displayCmsWrite, displayCmsHeartbeat, displayCmsResync, - displayCmsTxroom, + displayCmsTxBytesFree, }; void displayCmsInit(displayPort_t *pPort) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 472dacd428..6ba590020c 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -683,7 +683,7 @@ int osdHeartbeat(void) return 0; } -uint32_t osdTxroom(void) +uint32_t osdTxBytesFree(void) { return UINT32_MAX; } @@ -721,7 +721,7 @@ displayPortVTable_t osdVTable = { osdWrite, osdHeartbeat, osdResync, - osdTxroom, + osdTxBytesFree, }; void osdCmsInit(displayPort_t *pPort) From 15c83c184c7919afcf4e0d9ff87201213b880166 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 2 Nov 2016 17:57:53 +0900 Subject: [PATCH 080/188] Add OME_String for simple string entry --- src/main/io/canvas.h | 2 +- src/main/io/cms.c | 21 ++++++++++++++++++--- src/main/io/cms_types.h | 9 +++++++++ 3 files changed, 28 insertions(+), 4 deletions(-) diff --git a/src/main/io/canvas.h b/src/main/io/canvas.h index c7f96207ca..d288656f6a 100644 --- a/src/main/io/canvas.h +++ b/src/main/io/canvas.h @@ -1,3 +1,3 @@ #pragma once void canvasInit(void); -void canvasCmsInit(displayPort_t *); +void canvasCmsInit(displayPort_t *dPort); diff --git a/src/main/io/cms.c b/src/main/io/cms.c index d65ac12d94..ab0e24cbda 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -258,6 +258,12 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr int cnt = 0; switch (p->type) { + case OME_String: + if (IS_PRINTVALUE(p) && p->data) { + cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data); + CLR_PRINTVALUE(p); + } + break; case OME_Submenu: if (IS_PRINTVALUE(p)) { cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); @@ -728,6 +734,8 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) SET_PRINTVALUE(p); } break; + case OME_String: + break; case OME_Poll_INT16: case OME_Label: case OME_END: @@ -976,7 +984,7 @@ OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; -OSD_Entry menuMisc[]= +OSD_Entry menuImuMisc[]= { {"--- MISC ---", OME_Label, NULL, NULL, 0}, {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, @@ -992,12 +1000,12 @@ OSD_Entry menuMisc[]= OSD_Entry menuImu[] = { - {"--- CFG. IMU ---", OME_Label, NULL, NULL, 0}, + {"--- 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}, + {"MISC", OME_Submenu, cmsMenuChange, &menuImuMisc[0], 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; @@ -1222,10 +1230,17 @@ static char infoTargetName[] = __TARGET__; OSD_Entry menuInfo[] = { { "--- INFO ---", OME_Label, NULL, NULL, 0 }, + { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 }, + { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, + { "GITREV", OME_String, NULL, infoGitRev, 0 }, + { "TARGET", OME_String, NULL, infoTargetName, 0 }, + +#if 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 }, +#endif { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index df68379ebf..973b6610b7 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -8,6 +8,7 @@ typedef long (*OSDMenuFuncPtr)(displayPort_t *, void *); //type of elements + typedef enum { OME_Label, @@ -20,9 +21,12 @@ typedef enum OME_UINT16, OME_INT16, OME_Poll_INT16, + OME_String, OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's //wlasciwosci elementow +#ifdef OSD OME_VISIBLE, +#endif OME_TAB, OME_END, } OSD_MenuElement; @@ -95,3 +99,8 @@ typedef struct uint8_t max; const char * const *names; } OSD_TAB_t; + +typedef struct +{ + char *val; +} OSD_String_t; From cc69964d6b9a24bc9c0e135154b7f6342a878a0a Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 2 Nov 2016 19:12:50 +0900 Subject: [PATCH 081/188] Use max7456WriteChar instead of direct access to screenBuffer (osd.c) Some fix-ups for different targets. --- src/main/io/cms.c | 12 +++++----- src/main/io/osd.c | 56 +++++++++++++++++++++++------------------------ 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index ab0e24cbda..13bed7f3f8 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -289,8 +289,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr } break; } - case OME_VISIBLE: #ifdef OSD + case OME_VISIBLE: if (IS_PRINTVALUE(p) && p->data) { uint32_t address = (uint32_t)p->data; uint16_t *val; @@ -304,8 +304,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr } CLR_PRINTVALUE(p); } -#endif break; +#endif case OME_UINT8: if (IS_PRINTVALUE(p) && p->data) { OSD_UINT8_t *ptr = p->data; @@ -644,8 +644,8 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) SET_PRINTVALUE(p); } break; - case OME_VISIBLE: #ifdef OSD + case OME_VISIBLE: if (p->data) { uint32_t address = (uint32_t)p->data; uint16_t *val; @@ -658,8 +658,8 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) *val %= ~VISIBLE_FLAG; SET_PRINTVALUE(p); } -#endif break; +#endif case OME_UINT8: case OME_FLOAT: if (p->data) { @@ -1018,7 +1018,7 @@ OSD_Entry menuImu[] = // Should goto flashfs eventually. // #ifdef USE_FLASHFS -void cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) +long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) { UNUSED(ptr); @@ -1033,6 +1033,8 @@ void cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) cmsScreenClear(pDisplay); cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? + + return 0; } #endif // USE_FLASHFS diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6ba590020c..e105a6f07c 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -266,30 +266,26 @@ void osdDrawSingleElement(uint8_t item) #endif // VTX case OSD_CROSSHAIRS: - { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; - + elemPosX = 14 - 1; // Offset for 1 char to the left + elemPosY = 6; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; - - screenBuffer[position - 1] = (SYM_AH_CENTER_LINE); - screenBuffer[position + 1] = (SYM_AH_CENTER_LINE_RIGHT); - screenBuffer[position] = (SYM_AH_CENTER); - - return; - } + ++elemPosY; + buff[0] = SYM_AH_CENTER_LINE; + buff[1] = SYM_AH_CENTER; + buff[2] = SYM_AH_CENTER_LINE_RIGHT; + buff[3] = 0; + break; case OSD_ARTIFICIAL_HORIZON: { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; + elemPosX = 14; + elemPosY = 6 - 4; // Top center of the AH area int rollAngle = attitude.values.roll; int pitchAngle = attitude.values.pitch; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; + ++elemPosY; if (pitchAngle > AH_MAX_PITCH) pitchAngle = AH_MAX_PITCH; @@ -300,13 +296,15 @@ void osdDrawSingleElement(uint8_t item) if (rollAngle < -AH_MAX_ROLL) rollAngle = -AH_MAX_ROLL; - for (uint8_t x = 0; x <= 8; x++) { - int y = (rollAngle * (4 - x)) / 64; - y -= pitchAngle / 8; - y += 41; + // Convert pitchAngle to y compensation value + pitchAngle = (pitchAngle / 8) - 41; // 41 = 4 * 9 + 5 + + for (int8_t x = -4; x <= 4; x++) { + int y = (rollAngle * x) / 64; + y -= pitchAngle; + // y += 41; // == 4 * 9 + 5 if (y >= 0 && y <= 81) { - uint16_t pos = position - 7 + LINE * (y / 9) + 3 - 4 * LINE + x; - screenBuffer[pos] = (SYM_AH_BAR9_0 + (y % 9)); + max7456WriteChar(elemPosX + x, elemPosY + (y / 9), (SYM_AH_BAR9_0 + (y % 9))); } } @@ -317,23 +315,23 @@ void osdDrawSingleElement(uint8_t item) case OSD_HORIZON_SIDEBARS: { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; + elemPosX = 14; + elemPosY = 6; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; + ++elemPosY; // Draw AH sides int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; - for (int8_t x = -hudheight; x <= hudheight; x++) { - screenBuffer[position - hudwidth + (x * LINE)] = (SYM_AH_DECORATION); - screenBuffer[position + hudwidth + (x * LINE)] = (SYM_AH_DECORATION); + for (int8_t y = -hudheight; y <= hudheight; y++) { + max7456WriteChar(elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION); + max7456WriteChar(elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION); } // AH level indicators - screenBuffer[position - hudwidth + 1] = (SYM_AH_LEFT); - screenBuffer[position + hudwidth - 1] = (SYM_AH_RIGHT); + max7456WriteChar(elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT); + max7456WriteChar(elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT); return; } From 97c2776644b3b2c63ac8af28eeee786fa2fc0370 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Wed, 2 Nov 2016 15:29:23 +0100 Subject: [PATCH 082/188] small fix + coding style --- src/main/telemetry/smartport.c | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index e4b86926a7..02fdbac1ae 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -195,7 +195,7 @@ static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE]; static mspPacket_t smartPortMspReply; static bool smartPortMspReplyPending = false; -#define SMARTPORT_MSP_RES_ERROR -10 +#define SMARTPORT_MSP_RES_ERROR (-10) enum { SMARTPORT_MSP_VER_MISMATCH=0, @@ -216,8 +216,7 @@ static void smartPortDataReceive(uint16_t c) smartPortHasRequest = 0; skipUntilStart = false; return; - } - else if (skipUntilStart) { + } else if (skipUntilStart) { return; } @@ -228,8 +227,7 @@ static void smartPortDataReceive(uint16_t c) // our slot is starting... smartPortLastRequestTime = now; smartPortHasRequest = 1; - } - else if (c == FSSP_SENSOR_ID2) { + } else if (c == FSSP_SENSOR_ID2) { rxBuffer[smartPortRxBytes++] = c; checksum = 0; } @@ -256,8 +254,7 @@ static void smartPortDataReceive(uint16_t c) smartPortFrameReceived = true; } skipUntilStart = true; - } - else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) { + } else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) { checksum += c; checksum += checksum >> 8; checksum &= 0x00FF; @@ -530,12 +527,10 @@ void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) checksum = p_size ^ cmd.cmd; mspStarted = 1; - } - else if (!mspStarted) { + } else if (!mspStarted) { // no start packet yet, throw this one away return; - } - else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) { + } else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) { // packet loss detected! mspStarted = 0; return; @@ -772,13 +767,10 @@ void handleSmartPortTelemetry(void) smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; #endif - } - else if (feature(FEATURE_GPS)) { + } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; - } - - else if (telemetryConfig->pidValuesAsTelemetry){ + } else if (telemetryConfig->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: tmp2 = currentProfile->pidProfile.P8[ROLL]; From f8c48f569aa43df8adadb34c5e475f55e2ec07cf Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 3 Nov 2016 03:35:33 +0900 Subject: [PATCH 083/188] Menu contents separation --- src/main/io/cms.c | 518 ++------------------- src/main/io/cms.h | 7 +- src/main/io/cms_blackbox.c | 77 +++ src/main/io/cms_blackbox.h | 4 + src/main/io/cms_imu.c | 206 ++++++++ src/main/io/cms_imu.h | 13 + src/main/io/cms_ledstrip.c | 107 +++++ src/main/io/cms_ledstrip.h | 6 + src/main/io/cms_osd.h | 2 + src/main/io/cms_vtx.c | 96 ++++ src/main/io/cms_vtx.h | 7 + src/main/io/osd.c | 52 +++ src/main/target/OMNIBUS/target.mk | 6 +- src/main/target/SIRINFPV/target.mk | 6 +- src/main/target/SPRACINGF3/target.mk | 6 +- src/main/target/STM32F3DISCOVERY/target.mk | 8 +- 16 files changed, 646 insertions(+), 475 deletions(-) create mode 100644 src/main/io/cms_blackbox.c create mode 100644 src/main/io/cms_blackbox.h create mode 100644 src/main/io/cms_imu.c create mode 100644 src/main/io/cms_imu.h create mode 100644 src/main/io/cms_ledstrip.c create mode 100644 src/main/io/cms_ledstrip.h create mode 100644 src/main/io/cms_osd.h create mode 100644 src/main/io/cms_vtx.c create mode 100644 src/main/io/cms_vtx.h diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 13bed7f3f8..7c74a777ea 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -57,6 +57,20 @@ #include "build/debug.h" +// External menu contents +#include "io/cms_imu.h" +#include "io/cms_blackbox.h" +#include "io/cms_vtx.h" +#ifdef OSD +#include "io/cms_osd.h" +#endif // OSD +#include "io/cms_ledstrip.h" + +// Forwards +void cmsx_InfoInit(void); +void cmsx_FeatureRead(void); +void cmsx_FeatureWriteback(void); + // Device management #define CMS_MAX_DEVICE 4 @@ -164,9 +178,11 @@ void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) // #define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN(p) ((p)->cols - 7) +#define RIGHT_MENU_COLUMN(p) ((p)->cols - 10) #define MAX_MENU_ITEMS(p) ((p)->rows - 2) +displayPort_t currentDisplay; + bool cmsInMenu = false; OSD_Entry *menuStack[10]; //tab to save menu stack @@ -210,7 +226,7 @@ void cmsUpdateMaxRows(displayPort_t *instance) currentMenuIdx--; } -static void cmsFtoa(int32_t value, char *floatString) +static void cmsFormatFloat(int32_t value, char *floatString) { uint8_t k; // np. 3450 @@ -225,7 +241,8 @@ static void cmsFtoa(int32_t value, char *floatString) // 03.450 // usuwam koncowe zera i kropke - for (k = 5; k > 1; k--) + // Keep the first decimal place + for (k = 5; k > 3; k--) if (floatString[k] == '0' || floatString[k] == '.') floatString[k] = 0; else @@ -355,7 +372,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr case OME_FLOAT: if (IS_PRINTVALUE(p) && p->data) { OSD_FLOAT_t *ptr = p->data; - cmsFtoa(*ptr->val * ptr->multipler, buff); + cmsFormatFloat(*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); @@ -447,22 +464,14 @@ void cmsDrawMenu(displayPort_t *pDisplay) } } -// XXX Needs separation -OSD_Entry menuPid[]; -void cmsx_PidRead(void); -void cmsx_PidWriteback(void); -OSD_Entry menuRateExpo[]; -void cmsx_RateExpoRead(void); -void cmsx_RateExpoWriteback(void); - long cmsMenuChange(displayPort_t *pDisplay, void *ptr) { if (ptr) { // XXX (jflyper): This can be avoided by adding pre- and post- // XXX (or onEnter and onExit) functions? - if (ptr == &menuPid[0]) + if (ptr == cmsx_menuPid) cmsx_PidRead(); - if (ptr == &menuRateExpo[0]) + if (ptr == cmsx_menuRateExpo) cmsx_RateExpoRead(); if ((OSD_Entry *)ptr != currentMenu) { @@ -485,11 +494,11 @@ 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 - if (currentMenu == &menuPid[0]) + if (currentMenu == cmsx_menuPid) cmsx_PidWriteback(); // hack - save rate config for current profile - if (currentMenu == &menuRateExpo[0]) + if (currentMenu == cmsx_menuRateExpo) cmsx_RateExpoWriteback(); if (menuStackIdx) { @@ -505,13 +514,6 @@ long cmsMenuBack(displayPort_t *pDisplay) return 0; } -// XXX Separation -void cmsx_FeatureRead(void); -void cmsx_FeatureWriteback(void); -void cmsx_InfoInit(void); - -displayPort_t currentDisplay; - void cmsMenuOpen(void) { cmsDeviceInitFuncPtr initfunc; @@ -744,8 +746,6 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) return res; } -OSD_Entry menuRc[]; - void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) { static int16_t rcDelay = BUTTON_TIME; @@ -785,7 +785,7 @@ void cmsUpdate(displayPort_t *pDisplay, 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't exit using YAW + else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != cmsx_menuRc) // this menu is used to check transmitter signals so can't exit using YAW { key = KEY_ESC; rcDelay = BUTTON_TIME; @@ -830,401 +830,11 @@ void cmsInit(void) } // -// Menu tables, should eventually be all GONE!? +// Menu contents // -// -// IMU -// - -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}; - -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}; - -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}, - {"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} -}; - -// -// 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}; -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}; - -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}, - {"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 menuImuMisc[]= -{ - {"--- 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, &menuImuMisc[0], 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// Black box -// - -// -// Should goto flashfs eventually. -// -#ifdef USE_FLASHFS -long 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? - - return 0; -} -#endif // USE_FLASHFS - -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 - {"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 long 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); - } - - return 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 - -#ifdef OSD -// -// OSD specific menu pages and items -// XXX Should be part of the osd.c, or new osd_cms.c. -// - -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, 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} -}; - -OSD_Entry menuOsdActiveElems[] = -{ - {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, - {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, - {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0}, - {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0}, - {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0}, - {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0}, - {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0}, - {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0}, - {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0}, - {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0}, -#ifdef VTX - {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, -#endif // VTX - {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0}, - {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0}, -#ifdef GPS - {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0}, - {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0}, -#endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -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__; @@ -1236,13 +846,6 @@ OSD_Entry menuInfo[] = { { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, { "GITREV", OME_String, NULL, infoGitRev, 0 }, { "TARGET", OME_String, NULL, infoTargetName, 0 }, - -#if 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 }, -#endif { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; @@ -1257,30 +860,34 @@ void cmsx_InfoInit(void) } } +// Features + OSD_Entry menuFeatures[] = { {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, - {"BLACKBOX", OME_Submenu, cmsMenuChange, &menuBlackbox[0], 0}, + {"BLACKBOX", OME_Submenu, cmsMenuChange, cmsx_menuBlackbox, 0}, #if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, cmsMenuChange, &menu_vtx[0], 0}, + {"VTX", OME_Submenu, cmsMenuChange, cmsx_menuVtx, 0}, #endif // VTX || USE_RTC6705 #ifdef LED_STRIP - {"LED STRIP", OME_Submenu, cmsMenuChange, &menuLedstrip[0], 0}, + {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, #endif // LED_STRIP {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; +// Main + 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}, + {"CFG&IMU", OME_Submenu, cmsMenuChange, cmsx_menuImu, 0}, + {"FEATURES", OME_Submenu, cmsMenuChange, menuFeatures, 0}, #ifdef OSD - {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &menuOsdLayout[0], 0}, - {"ALARMS", OME_Submenu, cmsMenuChange, &menuAlarms[0], 0}, + {"SCR LAYOUT", OME_Submenu, cmsMenuChange, cmsx_menuOsdLayout, 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, cmsx_menuAlarms, 0}, #endif - {"INFO", OME_Submenu, cmsMenuChange, &menuInfo[0], 0}, + {"FC&FW INFO", OME_Submenu, cmsMenuChange, menuInfo, 0}, {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, {NULL,OME_END, NULL, NULL, 0} @@ -1288,60 +895,33 @@ OSD_Entry menuMain[] = void cmsx_FeatureRead(void) { - featureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; + cmsx_Blackbox_FeatureRead(); #ifdef LED_STRIP - featureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; - getLedColor(); + cmsx_Ledstrip_FeatureRead(); + cmsx_Ledstrip_ConfigRead(); #endif #if defined(VTX) || defined(USE_RTC6705) - featureVtx = feature(FEATURE_VTX) ? 1 : 0; + cmsx_Vtx_FeatureRead(); + cmsx_Vtx_ConfigRead(); #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); + cmsx_Blackbox_FeatureWriteback(); #ifdef LED_STRIP - if (featureLedstrip) - featureSet(FEATURE_LED_STRIP); - else - featureClear(FEATURE_LED_STRIP); + cmsx_Ledstrip_FeatureWriteback(); #endif #if defined(VTX) || defined(USE_RTC6705) - if (featureVtx) - featureSet(FEATURE_VTX); - else - featureClear(FEATURE_VTX); + cmsx_Vtx_FeatureWriteback(); + cmsx_Vtx_ConfigWriteback(); #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 bef66c62e2..a7d0d21e08 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -32,9 +32,12 @@ void cmsInit(void); void cmsHandler(uint32_t currentTime); // Required for external CMS tables +void cmsScreenClear(displayPort_t *pPort); +void cmsScreenResync(displayPort_t *pPort); +int cmsScreenWrite(displayPort_t *pPort, uint8_t x, uint8_t y, char *s); -long cmsChangeScreen(displayPort_t *pPort, void *ptr); -long cmsExitMenu(displayPort_t *pPort, void *ptr); +long cmsMenuChange(displayPort_t *pPort, void *ptr); +long cmsMenuExit(displayPort_t *pPort, void *ptr); #define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" #define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c new file mode 100644 index 0000000000..3f118f87ee --- /dev/null +++ b/src/main/io/cms_blackbox.c @@ -0,0 +1,77 @@ +// +// CMS things for blackbox and flashfs. +// Should be part of blackbox.c (or new blackbox/blackbox_cms.c) and io/flashfs.c +// +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_blackbox.h" + +#include "io/flashfs.h" + +#ifdef USE_FLASHFS +long 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? + + return 0; +} +#endif // USE_FLASHFS + +uint8_t cmsx_FeatureBlackbox; + +OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; + +OSD_Entry cmsx_menuBlackbox[] = +{ + {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0}, + {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0}, +#ifdef USE_FLASHFS + {"ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0}, +#endif // USE_FLASHFS + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +void cmsx_Blackbox_FeatureRead(void) +{ + cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; +} + +void cmsx_Blackbox_FeatureWriteback(void) +{ + if (cmsx_FeatureBlackbox) + featureSet(FEATURE_BLACKBOX); + else + featureClear(FEATURE_BLACKBOX); +} +#endif diff --git a/src/main/io/cms_blackbox.h b/src/main/io/cms_blackbox.h new file mode 100644 index 0000000000..6e7d02eff2 --- /dev/null +++ b/src/main/io/cms_blackbox.h @@ -0,0 +1,4 @@ +extern OSD_Entry cmsx_menuBlackbox[]; + +void cmsx_Blackbox_FeatureRead(void); +void cmsx_Blackbox_FeatureWriteback(void); diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c new file mode 100644 index 0000000000..9428d993d6 --- /dev/null +++ b/src/main/io/cms_imu.c @@ -0,0 +1,206 @@ + +// Menu contents for PID, RATES, RC preview, misc +// Should be part of the relevant .c file. + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +//#include "common/typeconversion.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_imu.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/pid.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +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}; + +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}; + +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 cmsx_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} +}; + +// +// 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}; +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}; + +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 cmsx_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 cmsx_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 menuImuMisc[]= +{ + {"--- 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 cmsx_menuImu[] = +{ + {"--- CFG.IMU ---", OME_Label, NULL, NULL, 0}, + {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, + {"PID", OME_Submenu, cmsMenuChange, cmsx_menuPid, 0}, + {"RATE&RXPO", OME_Submenu, cmsMenuChange, cmsx_menuRateExpo, 0}, + {"RC PREV", OME_Submenu, cmsMenuChange, cmsx_menuRc, 0}, + {"MISC", OME_Submenu, cmsMenuChange, menuImuMisc, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; +#endif // CMS diff --git a/src/main/io/cms_imu.h b/src/main/io/cms_imu.h new file mode 100644 index 0000000000..b78c6ddb4b --- /dev/null +++ b/src/main/io/cms_imu.h @@ -0,0 +1,13 @@ +extern OSD_Entry cmsx_menuImu[]; + +// All of below should be gone. + +extern OSD_Entry cmsx_menuPid[]; +extern OSD_Entry cmsx_menuRc[]; +extern OSD_Entry cmsx_menuRateExpo[]; + +void cmsx_PidRead(void); +void cmsx_PidWriteback(void); +void cmsx_RateExpoRead(void); +void cmsx_RateExpoWriteback(void); + diff --git a/src/main/io/cms_ledstrip.c b/src/main/io/cms_ledstrip.c new file mode 100644 index 0000000000..63e15d74e6 --- /dev/null +++ b/src/main/io/cms_ledstrip.c @@ -0,0 +1,107 @@ +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_blackbox.h" + +#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 +void cmsx_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 long 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); + } + + return 0; +} + +uint8_t cmsx_FeatureLedstrip; + +OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; + +OSD_Entry cmsx_menuLedstrip[] = +{ + {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0}, + {"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +void cmsx_Ledstrip_FeatureRead(void) +{ + cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; +} + +void cmsx_Ledstrip_FeatureWriteback(void) +{ + if (cmsx_FeatureLedstrip) + featureSet(FEATURE_LED_STRIP); + else + featureClear(FEATURE_LED_STRIP); +} + +void cmsx_Ledstrip_ConfigRead(void) +{ + cmsx_GetLedColor(); +} + +#endif // LED_STRIP +#endif // CMS diff --git a/src/main/io/cms_ledstrip.h b/src/main/io/cms_ledstrip.h new file mode 100644 index 0000000000..1ce1d5009f --- /dev/null +++ b/src/main/io/cms_ledstrip.h @@ -0,0 +1,6 @@ +extern OSD_Entry cmsx_menuLedstrip[]; + +void cmsx_Ledstrip_FeatureRead(void); +void cmsx_Ledstrip_FeatureWriteback(void); + +void cmsx_Ledstrip_ConfigRead(void); diff --git a/src/main/io/cms_osd.h b/src/main/io/cms_osd.h new file mode 100644 index 0000000000..5c93a65215 --- /dev/null +++ b/src/main/io/cms_osd.h @@ -0,0 +1,2 @@ +extern OSD_Entry cmsx_menuAlarms[]; +extern OSD_Entry cmsx_menuOsdLayout[]; diff --git a/src/main/io/cms_vtx.c b/src/main/io/cms_vtx.c new file mode 100644 index 0000000000..61e0af60d0 --- /dev/null +++ b/src/main/io/cms_vtx.c @@ -0,0 +1,96 @@ +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_vtx.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#ifdef CMS + +#if defined(VTX) || defined(USE_RTC6705) + +uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; + +static const char * const vtxBandNames[] = { + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; +OSD_UINT8_t entryVtxChannel = {&cmsx_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 cmsx_menuVtx[] = +{ + {"--- 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} +}; + +void cmsx_Vtx_FeatureRead(void) +{ + cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; +} + +void cmsx_Vtx_FeatureWriteback(void) +{ + if (cmsx_featureVtx) + featureSet(FEATURE_VTX); + else + featureClear(FEATURE_VTX); +} + +void cmsx_Vtx_ConfigRead(void) +{ +#ifdef VTX + cmsx_vtxBand = masterConfig.vtxBand; + cmsx_vtxChannel = masterConfig.vtx_channel + 1; +#endif // VTX + +#ifdef USE_RTC6705 + cmsx_vtxBand = masterConfig.vtx_channel / 8; + cmsx_vtxChannel = masterConfig.vtx_channel % 8 + 1; +#endif // USE_RTC6705 +} + +void cmsx_Vtx_ConfigRead(void) +{ +#ifdef VTX + masterConfig.vtxBand = cmsx_vtxBand; + masterConfig.vtx_channel = cmsx_vtxChannel - 1; +#endif // VTX + +#ifdef USE_RTC6705 + masterConfig.vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1; +#endif // USE_RTC6705 +} + +#endif // VTX || USE_RTC6705 + +#endif // CMS diff --git a/src/main/io/cms_vtx.h b/src/main/io/cms_vtx.h new file mode 100644 index 0000000000..412a9e4f73 --- /dev/null +++ b/src/main/io/cms_vtx.h @@ -0,0 +1,7 @@ +extern OSD_Entry cmsx_menuVtx[]; + +void cmsx_Vtx_FeatureRead(void); +void cmsx_Vtx_FeatureWriteback(void); + +void cmsx_Vtx_ConfigRead(void); +void cmsx_Vtx_ConfigWriteback(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e105a6f07c..c271d4339f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -636,6 +636,7 @@ void osdUpdate(uint32_t currentTime) // // OSD specific CMS functions // +#include "io/cms_osd.h" uint8_t shiftdown; @@ -727,4 +728,55 @@ void osdCmsInit(displayPort_t *pPort) osdResync(pPort); pPort->vTable = &osdVTable; } + +OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; +OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; +OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1}; +OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1}; + +OSD_Entry cmsx_menuAlarms[] = +{ + {"--- 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} +}; + +OSD_Entry menuOsdActiveElems[] = +{ + {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0}, + {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0}, + {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0}, + {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0}, + {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0}, + {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0}, + {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0}, +#ifdef VTX + {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, +#endif // VTX + {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0}, + {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0}, +#ifdef GPS + {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0}, + {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0}, +#endif // GPS + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +OSD_Entry cmsx_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 diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 4ec2d2df37..5a7e1a2382 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -15,5 +15,9 @@ TARGET_SRC = \ io/transponder_ir.c \ drivers/max7456.c \ io/osd.c \ + io/canvas.c \ io/cms.c \ - io/canvas.c + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 6481f94182..e64c9781b3 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -13,5 +13,9 @@ TARGET_SRC = \ drivers/vtx_soft_spi_rtc6705.c \ drivers/max7456.c \ io/osd.c \ + io/canvas.c \ io/cms.c \ - io/canvas.c + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index dd7702375e..f2a3df7442 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -9,6 +9,10 @@ TARGET_SRC = \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ + io/canvas.c \ io/cms.c \ - io/canvas.c + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 6b718b5b0a..4142776c47 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -27,4 +27,10 @@ TARGET_SRC = \ drivers/flash_m25p16.c \ drivers/max7456.c \ io/osd.c \ - io/cms.c + io/canvas.c \ + io/cms.c \ + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c + From 617631e64f49c4c61ce793a4d34f72a0e96e4eb7 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 3 Nov 2016 04:05:41 +0900 Subject: [PATCH 084/188] Fix VTX case --- src/main/io/cms_vtx.c | 4 ++-- src/main/target/REVO/target.h | 3 --- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/io/cms_vtx.c b/src/main/io/cms_vtx.c index 61e0af60d0..f133f4e7f1 100644 --- a/src/main/io/cms_vtx.c +++ b/src/main/io/cms_vtx.c @@ -39,7 +39,7 @@ OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; OSD_Entry cmsx_menuVtx[] = { {"--- VTX ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &featureVtx, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0}, #ifdef VTX {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, @@ -79,7 +79,7 @@ void cmsx_Vtx_ConfigRead(void) #endif // USE_RTC6705 } -void cmsx_Vtx_ConfigRead(void) +void cmsx_Vtx_ConfigWriteback(void) { #ifdef VTX masterConfig.vtxBand = cmsx_vtxBand; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index eed70b40de..4d73c99355 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -144,6 +144,3 @@ #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 From 7b8ebfc8f995ce6cb2f1474241ba5d2bf4ca7d7f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 2 Nov 2016 22:22:51 +0100 Subject: [PATCH 085/188] Disable -flto optimiser option for F3, F4 and F7 --- Makefile | 4 ++++ src/main/common/filter.c | 2 +- src/main/drivers/pwm_output.c | 2 +- src/main/telemetry/smartport.c | 2 +- 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index fa5bed9106..c6ccb2b20f 100644 --- a/Makefile +++ b/Makefile @@ -734,7 +734,11 @@ OPTIMIZE = -O0 LTO_FLAGS = $(OPTIMIZE) else OPTIMIZE = -Os +ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) +else +LTO_FLAGS = -fuse-linker-plugin $(OPTIMIZE) +endif endif DEBUG_FLAGS = -ggdb3 -DDEBUG diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 98d82919e4..6fdf6e5c59 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -75,7 +75,7 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh const float cs = cosf(omega); const float alpha = sn / (2 * Q); - float b0, b1, b2, a0, a1, a2; + float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0; switch (filterType) { case FILTER_LPF: diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 59f01726bb..ea28c3e48e 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -156,7 +156,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) { - uint32_t timerMhzCounter; + uint32_t timerMhzCounter = 0; pwmWriteFuncPtr pwmWritePtr; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool isDigital = false; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 02fdbac1ae..c9be67089c 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -620,7 +620,7 @@ void handleSmartPortTelemetry(void) smartPortIdCnt++; int32_t tmpi; - uint32_t tmp2; + uint32_t tmp2 = 0; static uint8_t t1Cnt = 0; static uint8_t t2Cnt = 0; From e2e44b28f56301c9a3fc4b5d265f995b3a7dbc74 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 2 Nov 2016 23:28:20 +0100 Subject: [PATCH 086/188] Cleanup target code BFF3 --- src/main/target/BETAFLIGHTF3/target.c | 5 +++-- src/main/target/BETAFLIGHTF3/target.h | 9 ++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 78d034bf84..e88dce7400 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -24,15 +24,16 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // RC PPM - PB7 - TIM17_CH1N AF1, TIM4_CH2 AF2, TIM8_BKIN AF5, TIM3_CH4 AF10 { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - TIM2_CH4 AF1, TIM15_CH2 AF9 { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index bdfde547d7..4806240672 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -25,7 +25,7 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define MPU6000_CS_PIN PA15 #define MPU6000_SPI_INSTANCE SPI1 @@ -63,11 +63,11 @@ #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_TX_PIN PA2 #define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 5 @@ -78,7 +78,6 @@ #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 -//GPIO_AF_1 #define SPI1_NSS_PIN PA15 #define SPI1_SCK_PIN PB3 From 3e8540a07f5afe42292367e58d05a7154467ca53 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 3 Nov 2016 12:13:07 +0900 Subject: [PATCH 087/188] Remove .orig files --- src/main/io/dashboard.c.orig | 818 ----------------------------------- src/main/io/dashboard.h.orig | 58 --- src/main/main.c.orig | 750 -------------------------------- 3 files changed, 1626 deletions(-) delete mode 100644 src/main/io/dashboard.c.orig delete mode 100644 src/main/io/dashboard.h.orig delete mode 100644 src/main/main.c.orig diff --git a/src/main/io/dashboard.c.orig b/src/main/io/dashboard.c.orig deleted file mode 100644 index 4bb7b63774..0000000000 --- a/src/main/io/dashboard.c.orig +++ /dev/null @@ -1,818 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include - -#include "platform.h" - -#ifdef USE_DASHBOARD - -#include "common/utils.h" - -#include "build/version.h" -#include "build/debug.h" - -#include "build/build_config.h" - -#include "drivers/system.h" -#include "drivers/display_ug2864hsweg01.h" - -#include "common/printf.h" -#include "common/maths.h" -#include "common/axis.h" -#include "common/typeconversion.h" - -#include "sensors/battery.h" -#include "sensors/sensors.h" -#include "sensors/compass.h" -#include "sensors/acceleration.h" -#include "sensors/gyro.h" - -#include "fc/config.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/failsafe.h" - -#include "io/cms.h" - -#ifdef GPS -#include "io/gps.h" -#include "flight/navigation.h" -#endif - -#include "config/feature.h" -#include "config/config_profile.h" - -#include "io/dashboard.h" - -#include "rx/rx.h" - -#include "scheduler/scheduler.h" - -extern profile_t *currentProfile; - -controlRateConfig_t *getControlRateConfig(uint8_t profileIndex); - -#define MICROSECONDS_IN_A_SECOND (1000 * 1000) - -#define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5) -#define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5) - -static uint32_t nextDisplayUpdateAt = 0; -static bool dashboardPresent = false; - -static rxConfig_t *rxConfig; - -#define PAGE_TITLE_LINE_COUNT 1 - -static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1]; - -#define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2) -#define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1) - -static const char* const pageTitles[] = { - "CLEANFLIGHT", - "ARMED", - "BATTERY", - "SENSORS", - "RX", - "PROFILE" -#ifndef SKIP_TASK_STATISTICS - ,"TASKS" -#endif -#ifdef GPS - ,"GPS" -#endif -#ifdef ENABLE_DEBUG_DASHBOARD_PAGE - ,"DEBUG" -#endif -}; - -#define PAGE_COUNT (PAGE_RX + 1) - -const pageId_e cyclePageIds[] = { - PAGE_PROFILE, -#ifdef GPS - PAGE_GPS, -#endif - PAGE_RX, - PAGE_BATTERY, - PAGE_SENSORS -#ifndef SKIP_TASK_STATISTICS - ,PAGE_TASKS -#endif -#ifdef ENABLE_DEBUG_DASHBOARD_PAGE - ,PAGE_DEBUG, -#endif -}; - -#define CYCLE_PAGE_ID_COUNT (sizeof(cyclePageIds) / sizeof(cyclePageIds[0])) - -static const char* tickerCharacters = "|/-\\"; // use 2/4/8 characters so that the divide is optimal. -#define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char)) - -typedef enum { - PAGE_STATE_FLAG_NONE = 0, - PAGE_STATE_FLAG_CYCLE_ENABLED = (1 << 0), - PAGE_STATE_FLAG_FORCE_PAGE_CHANGE = (1 << 1) -} pageFlags_e; - -typedef struct pageState_s { - bool pageChanging; - pageId_e pageId; - pageId_e pageIdBeforeArming; - uint8_t pageFlags; - uint8_t cycleIndex; - uint32_t nextPageAt; -} pageState_t; - -static pageState_t pageState; - -void resetDisplay(void) { - dashboardPresent = ug2864hsweg01InitI2C(); -} - -void LCDprint(uint8_t i) { - i2c_OLED_send_char(i); -} - -void padLineBuffer(void) -{ - uint8_t length = strlen(lineBuffer); - while (length < sizeof(lineBuffer) - 1) { - lineBuffer[length++] = ' '; - } - lineBuffer[length] = 0; -} - -void padHalfLineBuffer(void) -{ - uint8_t halfLineIndex = sizeof(lineBuffer) / 2; - uint8_t length = strlen(lineBuffer); - while (length < halfLineIndex - 1) { - lineBuffer[length++] = ' '; - } - lineBuffer[length] = 0; -} - -// LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display -void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) { - uint8_t i, j; - - if (percent > 100) - percent = 100; - - j = (width * percent) / 100; - - for (i = 0; i < j; i++) - LCDprint(159); // full - - if (j < width) - LCDprint(154 + (percent * width * 5 / 100 - 5 * j)); // partial fill - - for (i = j + 1; i < width; i++) - LCDprint(154); // empty -} - -#if 0 -void fillScreenWithCharacters() -{ - for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) { - for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) { - i2c_OLED_set_xy(column, row); - i2c_OLED_send_char('A' + column); - } - } -} -#endif - - -void updateTicker(void) -{ - static uint8_t tickerIndex = 0; - i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 1, 0); - i2c_OLED_send_char(tickerCharacters[tickerIndex]); - tickerIndex++; - tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT; -} - -void updateRxStatus(void) -{ - i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 2, 0); - char rxStatus = '!'; - if (rxIsReceivingSignal()) { - rxStatus = 'r'; - } if (rxAreFlightChannelsValid()) { - rxStatus = 'R'; - } - i2c_OLED_send_char(rxStatus); -} - -void updateFailsafeStatus(void) -{ - char failsafeIndicator = '?'; - switch (failsafePhase()) { - case FAILSAFE_IDLE: - failsafeIndicator = '-'; - break; - case FAILSAFE_RX_LOSS_DETECTED: - failsafeIndicator = 'R'; - break; - case FAILSAFE_LANDING: - failsafeIndicator = 'l'; - break; - case FAILSAFE_LANDED: - failsafeIndicator = 'L'; - break; - case FAILSAFE_RX_LOSS_MONITORING: - failsafeIndicator = 'M'; - break; - case FAILSAFE_RX_LOSS_RECOVERED: - failsafeIndicator = 'r'; - break; - } - i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0); - i2c_OLED_send_char(failsafeIndicator); -} - -void showTitle() -{ - i2c_OLED_set_line(0); - i2c_OLED_send_string(pageTitles[pageState.pageId]); -} - -void handlePageChange(void) -{ - i2c_OLED_clear_display_quick(); - showTitle(); -} - -void drawRxChannel(uint8_t channelIndex, uint8_t width) -{ - uint32_t percentage; - - LCDprint(rcChannelLetters[channelIndex]); - - percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); - drawHorizonalPercentageBar(width - 1, percentage); -} - -#define RX_CHANNELS_PER_PAGE_COUNT 14 -void showRxPage(void) -{ - - for (uint8_t channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) { - i2c_OLED_set_line((channelIndex / 2) + PAGE_TITLE_LINE_COUNT); - - drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT); - - if (channelIndex >= rxRuntimeConfig.channelCount) { - continue; - } - - if (IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD) { - LCDprint(' '); - } - - drawRxChannel(channelIndex + PAGE_TITLE_LINE_COUNT, HALF_SCREEN_CHARACTER_COLUMN_COUNT); - } -} - -void showWelcomePage(void) -{ - uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - - tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(targetName); -} - -void showArmedPage(void) -{ -} - -void showProfilePage(void) -{ - uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - - tfp_sprintf(lineBuffer, "Profile: %d", getCurrentProfile()); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"}; - const pidProfile_t *pidProfile = ¤tProfile->pidProfile; - for (int axis = 0; axis < 3; ++axis) { - tfp_sprintf(lineBuffer, "%s P:%3d I:%3d D:%3d", - axisTitles[axis], - pidProfile->P8[axis], - pidProfile->I8[axis], - pidProfile->D8[axis] - ); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - } - - const uint8_t currentRateProfileIndex = getCurrentControlRateProfile(); - tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - const controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex); - tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d", - controlRateConfig->rcExpo8, - controlRateConfig->rcRate8 - ); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "RR:%d PR:%d YR:%d", - controlRateConfig->rates[FD_ROLL], - controlRateConfig->rates[FD_PITCH], - controlRateConfig->rates[FD_YAW] - ); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); -} -#define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0])) -#define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2) - -#ifdef GPS -void showGpsPage() { - uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - - static uint8_t gpsTicker = 0; - static uint32_t lastGPSSvInfoReceivedCount = 0; - if (GPS_svInfoReceivedCount != lastGPSSvInfoReceivedCount) { - lastGPSSvInfoReceivedCount = GPS_svInfoReceivedCount; - gpsTicker++; - gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT; - } - - i2c_OLED_set_xy(0, rowIndex); - i2c_OLED_send_char(tickerCharacters[gpsTicker]); - - i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++); - - uint32_t index; - for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) { - uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1); - bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); - i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset); - } - - - char fixChar = STATE(GPS_FIX) ? 'Y' : 'N'; - tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed); - padHalfLineBuffer(); - i2c_OLED_set_line(rowIndex); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course); - padHalfLineBuffer(); - i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount); - padHalfLineBuffer(); - i2c_OLED_set_line(rowIndex); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors, gpsData.timeouts); - padHalfLineBuffer(); - i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage); - padHalfLineBuffer(); - i2c_OLED_set_line(rowIndex); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts); - padHalfLineBuffer(); - i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); - i2c_OLED_send_string(lineBuffer); - - strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT); - padHalfLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - -#ifdef GPS_PH_DEBUG - tfp_sprintf(lineBuffer, "Angles: P:%d R:%d", GPS_angle[PITCH], GPS_angle[ROLL]); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); -#endif - -#if 0 - tfp_sprintf(lineBuffer, "%d %d %d %d", debug[0], debug[1], debug[2], debug[3]); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); -#endif -} -#endif - -void showBatteryPage(void) -{ - uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - - if (feature(FEATURE_VBAT)) { - tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", vbat / 10, vbat % 10, batteryCellCount); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - uint8_t batteryPercentage = calculateBatteryPercentage(); - i2c_OLED_set_line(rowIndex++); - drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage); - } - - if (feature(FEATURE_CURRENT_METER)) { - tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, mAhDrawn); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - uint8_t capacityPercentage = calculateBatteryCapacityRemainingPercentage(); - i2c_OLED_set_line(rowIndex++); - drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage); - } -} - -void showSensorsPage(void) -{ - uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - static const char *format = "%s %5d %5d %5d"; - - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(" X Y Z"); - - if (sensors(SENSOR_ACC)) { - tfp_sprintf(lineBuffer, format, "ACC", accSmooth[X], accSmooth[Y], accSmooth[Z]); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - } - - if (sensors(SENSOR_GYRO)) { - tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - } - -#ifdef MAG - if (sensors(SENSOR_MAG)) { - tfp_sprintf(lineBuffer, format, "MAG", magADC[X], magADC[Y], magADC[Z]); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - } -#endif - - tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - /* - uint8_t length; - - ftoa(EstG.A[X], lineBuffer); - length = strlen(lineBuffer); - while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) { - lineBuffer[length++] = ' '; - lineBuffer[length+1] = 0; - } - ftoa(EstG.A[Y], lineBuffer + length); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - ftoa(EstG.A[Z], lineBuffer); - length = strlen(lineBuffer); - while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) { - lineBuffer[length++] = ' '; - lineBuffer[length+1] = 0; - } - ftoa(smallAngle, lineBuffer + length); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - */ - -} - -#ifndef SKIP_TASK_STATISTICS -void showTasksPage(void) -{ - uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - static const char *format = "%2d%6d%5d%4d%4d"; - - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string("Task max avg mx% av%"); - cfTaskInfo_t taskInfo; - for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) { - getTaskInfo(taskId, &taskInfo); - if (taskInfo.isEnabled && taskId != TASK_SERIAL) {// don't waste a line of the display showing serial taskInfo - const int taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); - const int maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 10000; - const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 10000; - tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad, averageLoad); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) { - break; - } - } - } -} -#endif - -#ifdef ENABLE_DEBUG_DASHBOARD_PAGE - -void showDebugPage(void) -{ - uint8_t rowIndex; - - for (rowIndex = 0; rowIndex < 4; rowIndex++) { - tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]); - padLineBuffer(); - i2c_OLED_set_line(rowIndex + PAGE_TITLE_LINE_COUNT); - i2c_OLED_send_string(lineBuffer); - } -} -#endif - -<<<<<<< HEAD:src/main/io/display.c -#ifdef OLEDCMS -static bool displayInCMS = false; -#endif - -void displayUpdate(uint32_t currentTime) -======= -void dashboardUpdate(uint32_t currentTime) ->>>>>>> betaflight/master:src/main/io/dashboard.c -{ - static uint8_t previousArmedState = 0; - -#ifdef OLEDCMS - if (displayInCMS) - return; -#endif - - const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L; - if (!updateNow) { - return; - } - - nextDisplayUpdateAt = currentTime + DISPLAY_UPDATE_FREQUENCY; - - bool armedState = ARMING_FLAG(ARMED) ? true : false; - bool armedStateChanged = armedState != previousArmedState; - previousArmedState = armedState; - - if (armedState) { - if (!armedStateChanged) { - return; - } - pageState.pageIdBeforeArming = pageState.pageId; - pageState.pageId = PAGE_ARMED; - pageState.pageChanging = true; - } else { - if (armedStateChanged) { - pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; - pageState.pageId = pageState.pageIdBeforeArming; - } - - pageState.pageChanging = (pageState.pageFlags & PAGE_STATE_FLAG_FORCE_PAGE_CHANGE) || - (((int32_t)(currentTime - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED))); - if (pageState.pageChanging && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)) { - pageState.cycleIndex++; - pageState.cycleIndex = pageState.cycleIndex % CYCLE_PAGE_ID_COUNT; - pageState.pageId = cyclePageIds[pageState.cycleIndex]; - } - } - - if (pageState.pageChanging) { - pageState.pageFlags &= ~PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; - pageState.nextPageAt = currentTime + PAGE_CYCLE_FREQUENCY; - - // Some OLED displays do not respond on the first initialisation so refresh the display - // when the page changes in the hopes the hardware responds. This also allows the - // user to power off/on the display or connect it while powered. - resetDisplay(); - - if (!dashboardPresent) { - return; - } - handlePageChange(); - } - - if (!dashboardPresent) { - return; - } - - switch(pageState.pageId) { - case PAGE_WELCOME: - showWelcomePage(); - break; - case PAGE_ARMED: - showArmedPage(); - break; - case PAGE_BATTERY: - showBatteryPage(); - break; - case PAGE_SENSORS: - showSensorsPage(); - break; - case PAGE_RX: - showRxPage(); - break; - case PAGE_PROFILE: - showProfilePage(); - break; -#ifndef SKIP_TASK_STATISTICS - case PAGE_TASKS: - showTasksPage(); - break; -#endif -#ifdef GPS - case PAGE_GPS: - if (feature(FEATURE_GPS)) { - showGpsPage(); - } else { - pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; - } - break; -#endif -#ifdef ENABLE_DEBUG_DASHBOARD_PAGE - case PAGE_DEBUG: - showDebugPage(); - break; -#endif - } - if (!armedState) { - updateFailsafeStatus(); - updateRxStatus(); - updateTicker(); - } - -} - -void dashboardSetPage(pageId_e pageId) -{ - pageState.pageId = pageId; - pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; -} - -void dashboardInit(rxConfig_t *rxConfigToUse) -{ - delay(200); - resetDisplay(); - delay(200); - -#if defined(CMS) && defined(OLEDCMS) - cmsDeviceRegister(displayCmsInit); -#endif - - rxConfig = rxConfigToUse; - - memset(&pageState, 0, sizeof(pageState)); - dashboardSetPage(PAGE_WELCOME); - - dashboardUpdate(micros()); - - dashboardSetNextPageChangeAt(micros() + (1000 * 1000 * 5)); -} - -void dashboardShowFixedPage(pageId_e pageId) -{ - dashboardSetPage(pageId); - dashboardDisablePageCycling(); -} - -void dashboardSetNextPageChangeAt(uint32_t futureMicros) -{ - pageState.nextPageAt = futureMicros; -} - -void dashboardEnablePageCycling(void) -{ - pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED; -} - -void dashboardResetPageCycling(void) -{ - pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page - -} - -void dashboardDisablePageCycling(void) -{ - pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED; -} - -#ifdef OLEDCMS -#include "io/cms.h" - -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; -} - -int displayCmsHeartbeat(void) -{ - return 0; -} - -void displayCmsResync(displayPort_t *pPort) -{ - UNUSED(pPort); -} - -uint32_t displayCmsTxBytesFree(void) -{ - return UINT32_MAX; -} - -displayPortVTable_t displayCmsVTable = { - displayCmsBegin, - displayCmsEnd, - displayCmsClear, - displayCmsWrite, - displayCmsHeartbeat, - displayCmsResync, - displayCmsTxBytesFree, -}; - -void displayCmsInit(displayPort_t *pPort) -{ - pPort->rows = SCREEN_CHARACTER_ROW_COUNT; - pPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; - pPort->vTable = &displayCmsVTable; -} -#endif // OLEDCMS - -#endif diff --git a/src/main/io/dashboard.h.orig b/src/main/io/dashboard.h.orig deleted file mode 100644 index 9a8c9971b5..0000000000 --- a/src/main/io/dashboard.h.orig +++ /dev/null @@ -1,58 +0,0 @@ -/* - * 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 . - */ - -#define ENABLE_DEBUG_DASHBOARD_PAGE - -typedef enum { - PAGE_WELCOME, - PAGE_ARMED, - PAGE_BATTERY, - PAGE_SENSORS, - PAGE_RX, - PAGE_PROFILE, -#ifndef SKIP_TASK_STATISTICS - PAGE_TASKS, -#endif -#ifdef GPS - PAGE_GPS, -#endif -#ifdef ENABLE_DEBUG_DASHBOARD_PAGE - PAGE_DEBUG, -#endif -} pageId_e; - -struct rxConfig_s; -void dashboardInit(struct rxConfig_s *intialRxConfig); -void dashboardUpdate(uint32_t currentTime); - -void dashboardShowFixedPage(pageId_e pageId); - -<<<<<<< HEAD:src/main/io/display.h -void displayEnablePageCycling(void); -void displayDisablePageCycling(void); -void displayResetPageCycling(void); -void displaySetNextPageChangeAt(uint32_t futureMicros); - -#ifdef CMS -void displayCmsInit(displayPort_t *pPort); -#endif -======= -void dashboardEnablePageCycling(void); -void dashboardDisablePageCycling(void); -void dashboardResetPageCycling(void); -void dashboardSetNextPageChangeAt(uint32_t futureMicros); ->>>>>>> betaflight/master:src/main/io/dashboard.h diff --git a/src/main/main.c.orig b/src/main/main.c.orig deleted file mode 100644 index 5fc05be537..0000000000 --- a/src/main/main.c.orig +++ /dev/null @@ -1,750 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include -#include - -#include "platform.h" - -#include "blackbox/blackbox.h" - -#include "common/axis.h" -#include "common/color.h" -#include "common/maths.h" -#include "common/printf.h" - -#include "drivers/nvic.h" - -#include "drivers/sensor.h" -#include "drivers/system.h" -#include "drivers/dma.h" -#include "drivers/gpio.h" -#include "drivers/io.h" -#include "drivers/light_led.h" -#include "drivers/sound_beeper.h" -#include "drivers/timer.h" -#include "drivers/serial.h" -#include "drivers/serial_softserial.h" -#include "drivers/serial_uart.h" -#include "drivers/accgyro.h" -#include "drivers/compass.h" -#include "drivers/pwm_rx.h" -#include "drivers/pwm_output.h" -#include "drivers/adc.h" -#include "drivers/bus_i2c.h" -#include "drivers/bus_spi.h" -#include "drivers/inverter.h" -#include "drivers/flash_m25p16.h" -#include "drivers/sonar_hcsr04.h" -#include "drivers/sdcard.h" -#include "drivers/usb_io.h" -#include "drivers/transponder_ir.h" -#include "drivers/io.h" -#include "drivers/exti.h" -#include "drivers/vtx_soft_spi_rtc6705.h" - -#ifdef USE_BST -#include "bus_bst.h" -#endif - -#include "fc/config.h" -#include "fc/fc_tasks.h" -#include "fc/fc_msp.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "msp/msp_serial.h" - -#include "rx/rx.h" -#include "rx/spektrum.h" - -#include "io/cms.h" - -#include "io/beeper.h" -#include "io/serial.h" -#include "io/flashfs.h" -#include "io/gps.h" -#include "io/motors.h" -#include "io/servos.h" -#include "io/gimbal.h" -#include "io/ledstrip.h" -#include "io/dashboard.h" -#include "io/asyncfatfs/asyncfatfs.h" -#include "io/serial_cli.h" -#include "io/transponder_ir.h" -#include "io/cms.h" -#include "io/osd.h" -#include "io/vtx.h" -#include "io/canvas.h" - -#include "scheduler/scheduler.h" - -#include "sensors/sensors.h" -#include "sensors/sonar.h" -#include "sensors/barometer.h" -#include "sensors/compass.h" -#include "sensors/acceleration.h" -#include "sensors/gyro.h" -#include "sensors/battery.h" -#include "sensors/boardalignment.h" -#include "sensors/initialisation.h" - -#include "telemetry/telemetry.h" - -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/mixer.h" -#include "flight/failsafe.h" -#include "flight/navigation.h" - -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -#ifdef USE_HARDWARE_REVISION_DETECTION -#include "hardware_revision.h" -#endif - -#include "build/build_config.h" -#include "build/debug.h" - -extern uint8_t motorControlEnable; - -#ifdef SOFTSERIAL_LOOPBACK -serialPort_t *loopbackPort; -#endif - -typedef enum { - SYSTEM_STATE_INITIALISING = 0, - SYSTEM_STATE_CONFIG_LOADED = (1 << 0), - SYSTEM_STATE_SENSORS_READY = (1 << 1), - SYSTEM_STATE_MOTORS_READY = (1 << 2), - SYSTEM_STATE_TRANSPONDER_ENABLED = (1 << 3), - SYSTEM_STATE_READY = (1 << 7) -} systemState_e; - -static uint8_t systemState = SYSTEM_STATE_INITIALISING; - -void init(void) -{ -#ifdef USE_HAL_DRIVER - HAL_Init(); -#endif - - printfSupportInit(); - - initEEPROM(); - - ensureEEPROMContainsValidData(); - readEEPROM(); - - systemState |= SYSTEM_STATE_CONFIG_LOADED; - - systemInit(); - - //i2cSetOverclock(masterConfig.i2c_overclock); - - // initialize IO (needed for all IO operations) - IOInitGlobal(); - - debugMode = masterConfig.debug_mode; - -#ifdef USE_HARDWARE_REVISION_DETECTION - detectHardwareRevision(); -#endif - - // Latch active features to be used for feature() in the remainder of init(). - latchActiveFeatures(); - -#ifdef ALIENFLIGHTF3 - ledInit(hardwareRevision == AFF3_REV_1 ? false : true); -#else - ledInit(false); -#endif - LED2_ON; - -#ifdef USE_EXTI - EXTIInit(); -#endif - -#if defined(BUTTONS) - gpio_config_t buttonAGpioConfig = { - BUTTON_A_PIN, - Mode_IPU, - Speed_2MHz - }; - gpioInit(BUTTON_A_PORT, &buttonAGpioConfig); - - gpio_config_t buttonBGpioConfig = { - BUTTON_B_PIN, - Mode_IPU, - Speed_2MHz - }; - gpioInit(BUTTON_B_PORT, &buttonBGpioConfig); - - // Check status of bind plug and exit if not active - delayMicroseconds(10); // allow GPIO configuration to settle - - if (!isMPUSoftReset()) { - uint8_t secondsRemaining = 5; - bool bothButtonsHeld; - do { - bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN); - if (bothButtonsHeld) { - if (--secondsRemaining == 0) { - resetEEPROM(); - systemReset(); - } - delay(1000); - LED0_TOGGLE; - } - } while (bothButtonsHeld); - } -#endif - -#ifdef SPEKTRUM_BIND - if (feature(FEATURE_RX_SERIAL)) { - switch (masterConfig.rxConfig.serialrx_provider) { - case SERIALRX_SPEKTRUM1024: - case SERIALRX_SPEKTRUM2048: - // Spektrum satellite binding if enabled on startup. - // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. - // The rest of Spektrum initialization will happen later - via spektrumInit() - spektrumBind(&masterConfig.rxConfig); - break; - } - } -#endif - - delay(100); - - timerInit(); // timer must be initialized before any channel is allocated - -#if !defined(USE_HAL_DRIVER) - dmaInit(); -#endif - -#if defined(AVOID_UART1_FOR_PWM_PPM) - serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), - feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); -#elif defined(AVOID_UART2_FOR_PWM_PPM) - serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), - feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); -#elif defined(AVOID_UART3_FOR_PWM_PPM) - serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), - feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); -#else - serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); -#endif - - mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); -#ifdef USE_SERVOS - servoMixerInit(masterConfig.customServoMixer); -#endif - - uint16_t idlePulse = masterConfig.motorConfig.mincommand; - if (feature(FEATURE_3D)) { - idlePulse = masterConfig.flight3DConfig.neutral3d; - } - - if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) { - featureClear(FEATURE_3D); - idlePulse = 0; // brushed motors - } - -#ifdef USE_QUAD_MIXER_ONLY - motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT); -#else - motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount); -#endif - -#ifdef USE_SERVOS - if (isMixerUsingServos()) { - //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); - servoInit(&masterConfig.servoConfig); - } -#endif - -#ifndef SKIP_RX_PWM_PPM - if (feature(FEATURE_RX_PPM)) { - ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol); - } else if (feature(FEATURE_RX_PARALLEL_PWM)) { - pwmRxInit(&masterConfig.pwmConfig); - } - pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode); -#endif - - mixerConfigureOutput(); -#ifdef USE_SERVOS - servoConfigureOutput(); -#endif - systemState |= SYSTEM_STATE_MOTORS_READY; - -#ifdef BEEPER - beeperInit(&masterConfig.beeperConfig); -#endif -/* temp until PGs are implemented. */ -#ifdef INVERTER - initInverter(); -#endif - -#ifdef USE_BST - bstInit(BST_DEVICE); -#endif - -#ifdef USE_SPI -#ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1); -#endif -#ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2); -#endif -#ifdef USE_SPI_DEVICE_3 -#ifdef ALIENFLIGHTF3 - if (hardwareRevision == AFF3_REV_2) { - spiInit(SPIDEV_3); - } -#else - spiInit(SPIDEV_3); -#endif -#endif -#ifdef USE_SPI_DEVICE_4 - spiInit(SPIDEV_4); -#endif -#endif - -#ifdef VTX - vtxInit(); -#endif - -#ifdef USE_HARDWARE_REVISION_DETECTION - updateHardwareRevision(); -#endif - -#if defined(NAZE) - if (hardwareRevision == NAZE32_SP) { - serialRemovePort(SERIAL_PORT_SOFTSERIAL2); - } else { - serialRemovePort(SERIAL_PORT_USART3); - } -#endif - -#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) - if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { - serialRemovePort(SERIAL_PORT_SOFTSERIAL2); - } -#endif - -#if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI) -#if defined(SONAR) && defined(USE_SOFTSERIAL1) - if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { - serialRemovePort(SERIAL_PORT_SOFTSERIAL1); - } -#endif -#endif - -#ifdef USE_I2C -#if defined(NAZE) - if (hardwareRevision != NAZE32_SP) { - i2cInit(I2C_DEVICE); - } else { - if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { - i2cInit(I2C_DEVICE); - } - } -#elif defined(CC3D) - if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { - i2cInit(I2C_DEVICE); - } -#else - i2cInit(I2C_DEVICE); -#endif -#endif - -#ifdef USE_ADC - drv_adc_config_t adc_params; - - adc_params.enableVBat = feature(FEATURE_VBAT); - adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); - adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); - adc_params.enableExternal1 = false; -#ifdef OLIMEXINO - adc_params.enableExternal1 = true; -#endif -#ifdef NAZE - // optional ADC5 input on rev.5 hardware - adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); -#endif - - adcInit(&adc_params); -#endif - - - initBoardAlignment(&masterConfig.boardAlignment); - -<<<<<<< HEAD -#ifdef CMS - cmsInit(); -#endif - -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayInit(&masterConfig.rxConfig); -======= -#ifdef USE_DASHBOARD - if (feature(FEATURE_DASHBOARD)) { - dashboardInit(&masterConfig.rxConfig); ->>>>>>> betaflight/master - } -#endif - -#ifdef USE_RTC6705 - if (feature(FEATURE_VTX)) { - rtc6705_soft_spi_init(); - current_vtx_channel = masterConfig.vtx_channel; - rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); - rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); - } -#endif - -#ifdef OSD - if (feature(FEATURE_OSD)) { - osdInit(); - } -#endif - - if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, - masterConfig.acc_hardware, - masterConfig.mag_hardware, - masterConfig.baro_hardware, - masterConfig.mag_declination, - masterConfig.gyro_lpf, - masterConfig.gyro_sync_denom)) { - // if gyro was not detected due to whatever reason, we give up now. - failureMode(FAILURE_MISSING_ACC); - } - - systemState |= SYSTEM_STATE_SENSORS_READY; - - LED1_ON; - LED0_OFF; - LED2_OFF; - - for (int i = 0; i < 10; i++) { - LED1_TOGGLE; - LED0_TOGGLE; - delay(25); - if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON; - delay(25); - BEEP_OFF; - } - LED0_OFF; - LED1_OFF; - -#ifdef MAG - if (sensors(SENSOR_MAG)) - compassInit(); -#endif - - imuInit(); - - mspFcInit(); - mspSerialInit(); - -#ifdef CANVAS - if (feature(FEATURE_CANVAS)) { - canvasInit(); - } -#endif - -#ifdef USE_CLI - cliInit(&masterConfig.serialConfig); -#endif - - failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - - rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions); - -#ifdef GPS - if (feature(FEATURE_GPS)) { - gpsInit( - &masterConfig.serialConfig, - &masterConfig.gpsConfig - ); - navigationInit( - &masterConfig.gpsProfile, - ¤tProfile->pidProfile - ); - } -#endif - -#ifdef SONAR - if (feature(FEATURE_SONAR)) { - sonarInit(&masterConfig.sonarConfig); - } -#endif - -#ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); - - if (feature(FEATURE_LED_STRIP)) { - ledStripEnable(); - } -#endif - -#ifdef TELEMETRY - if (feature(FEATURE_TELEMETRY)) { - telemetryInit(); - } -#endif - -#ifdef USB_CABLE_DETECTION - usbCableDetectInit(); -#endif - -#ifdef TRANSPONDER - if (feature(FEATURE_TRANSPONDER)) { - transponderInit(masterConfig.transponderData); - transponderEnable(); - transponderStartRepeating(); - systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; - } -#endif - -#ifdef USE_FLASHFS -#ifdef NAZE - if (hardwareRevision == NAZE32_REV5) { - m25p16_init(IO_TAG_NONE); - } -#elif defined(USE_FLASH_M25P16) - m25p16_init(IO_TAG_NONE); -#endif - - flashfsInit(); -#endif - -#ifdef USE_SDCARD - bool sdcardUseDMA = false; - - sdcardInsertionDetectInit(); - -#ifdef SDCARD_DMA_CHANNEL_TX - -#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) - // Ensure the SPI Tx DMA doesn't overlap with the led strip -#if defined(STM32F4) || defined(STM32F7) - sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM; -#else - sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; -#endif -#else - sdcardUseDMA = true; -#endif - -#endif - - sdcard_init(sdcardUseDMA); - - afatfs_init(); -#endif - - if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) { - masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - masterConfig.gyro_sync_denom = 1; - } - - setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime - -#ifdef BLACKBOX - initBlackbox(); -#endif - - if (masterConfig.mixerMode == MIXER_GIMBAL) { - accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); - } - gyroSetCalibrationCycles(); -#ifdef BARO - baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); -#endif - - // start all timers - // TODO - not implemented yet - timerStart(); - - ENABLE_STATE(SMALL_ANGLE); - DISABLE_ARMING_FLAG(PREVENT_ARMING); - -#ifdef SOFTSERIAL_LOOPBACK - // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly - loopbackPort = (serialPort_t*)&(softSerialPorts[0]); - if (!loopbackPort->vTable) { - loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); - } - serialPrint(loopbackPort, "LOOPBACK\r\n"); -#endif - - // Now that everything has powered up the voltage and cell count be determined. - - if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) - batteryInit(&masterConfig.batteryConfig); - -#ifdef USE_DASHBOARD - if (feature(FEATURE_DASHBOARD)) { -#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY - dashboardShowFixedPage(PAGE_GPS); -#else - dashboardResetPageCycling(); - dashboardEnablePageCycling(); -#endif - } -#endif - -#ifdef CJMCU - LED2_ON; -#endif - - // Latch active features AGAIN since some may be modified by init(). - latchActiveFeatures(); - motorControlEnable = true; - - fcTasksInit(); - systemState |= SYSTEM_STATE_READY; -} - -#ifdef SOFTSERIAL_LOOPBACK -void processLoopback(void) { - if (loopbackPort) { - uint8_t bytesWaiting; - while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { - uint8_t b = serialRead(loopbackPort); - serialWrite(loopbackPort, b); - }; - } -} -#else -#define processLoopback() -#endif - - -void main_step(void) -{ - scheduler(); - processLoopback(); -} - -#ifndef NOMAIN -int main(void) -{ - init(); - while (true) { - main_step(); - } -} -#endif - -#ifdef DEBUG_HARDFAULTS -//from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/ -/** - * hard_fault_handler_c: - * This is called from the HardFault_HandlerAsm with a pointer the Fault stack - * as the parameter. We can then read the values from the stack and place them - * into local variables for ease of reading. - * We then read the various Fault Status and Address Registers to help decode - * cause of the fault. - * The function ends with a BKPT instruction to force control back into the debugger - */ -void hard_fault_handler_c(unsigned long *hardfault_args) -{ - volatile unsigned long stacked_r0 ; - volatile unsigned long stacked_r1 ; - volatile unsigned long stacked_r2 ; - volatile unsigned long stacked_r3 ; - volatile unsigned long stacked_r12 ; - volatile unsigned long stacked_lr ; - volatile unsigned long stacked_pc ; - volatile unsigned long stacked_psr ; - volatile unsigned long _CFSR ; - volatile unsigned long _HFSR ; - volatile unsigned long _DFSR ; - volatile unsigned long _AFSR ; - volatile unsigned long _BFAR ; - volatile unsigned long _MMAR ; - - stacked_r0 = ((unsigned long)hardfault_args[0]) ; - stacked_r1 = ((unsigned long)hardfault_args[1]) ; - stacked_r2 = ((unsigned long)hardfault_args[2]) ; - stacked_r3 = ((unsigned long)hardfault_args[3]) ; - stacked_r12 = ((unsigned long)hardfault_args[4]) ; - stacked_lr = ((unsigned long)hardfault_args[5]) ; - stacked_pc = ((unsigned long)hardfault_args[6]) ; - stacked_psr = ((unsigned long)hardfault_args[7]) ; - - // Configurable Fault Status Register - // Consists of MMSR, BFSR and UFSR - _CFSR = (*((volatile unsigned long *)(0xE000ED28))) ; - - // Hard Fault Status Register - _HFSR = (*((volatile unsigned long *)(0xE000ED2C))) ; - - // Debug Fault Status Register - _DFSR = (*((volatile unsigned long *)(0xE000ED30))) ; - - // Auxiliary Fault Status Register - _AFSR = (*((volatile unsigned long *)(0xE000ED3C))) ; - - // Read the Fault Address Registers. These may not contain valid values. - // Check BFARVALID/MMARVALID to see if they are valid values - // MemManage Fault Address Register - _MMAR = (*((volatile unsigned long *)(0xE000ED34))) ; - // Bus Fault Address Register - _BFAR = (*((volatile unsigned long *)(0xE000ED38))) ; - - __asm("BKPT #0\n") ; // Break into the debugger -} - -#else -void HardFault_Handler(void) -{ - LED2_ON; - - // fall out of the sky - uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; - if ((systemState & requiredStateForMotors) == requiredStateForMotors) { - stopMotors(); - } -#ifdef TRANSPONDER - // prevent IR LEDs from burning out. - uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED; - if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) { - transponderIrDisable(); - } -#endif - - LED1_OFF; - LED0_OFF; - - while (1) { -#ifdef LED2 - delay(50); - LED2_TOGGLE; -#endif - } -} -#endif From 82d85181f4e757441555f6572d32019134c4b99c Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 3 Nov 2016 12:19:14 +0900 Subject: [PATCH 088/188] Remove debugs from dashboard.c --- src/main/io/dashboard.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index eb34084ca5..c63b6d90c0 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -757,9 +757,6 @@ int dashboardCmsBegin(void) { dashboardInCMS = true; - debug[1]++; - delay(300); - return 0; } @@ -812,8 +809,6 @@ displayPortVTable_t dashboardCmsVTable = { void dashboardCmsInit(displayPort_t *pPort) { -debug[1]++; -delay(300); pPort->rows = SCREEN_CHARACTER_ROW_COUNT; pPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; pPort->vTable = &dashboardCmsVTable; From b8cb115a0ba12e2b2b88849ea3275c607337fc74 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Thu, 3 Nov 2016 14:56:18 +0100 Subject: [PATCH 089/188] MSP/SPORT: fixed LUA script --- src/test/SpMsp.lua | 42 +++++++++++++++++++++--------------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/test/SpMsp.lua b/src/test/SpMsp.lua index 93b47131be..40d49d6cce 100644 --- a/src/test/SpMsp.lua +++ b/src/test/SpMsp.lua @@ -16,11 +16,13 @@ REPLY_FRAME_ID = 0x32 -- Sequence number for next MSP/SPORT packet local sportMspSeq = 0 +local sportMspRemoteSeq = 0 local mspRxBuf = {} local mspRxIdx = 1 local mspRxCRC = 0 local mspStarted = false +local mspLastReq = 0 -- Stats mspRequestsSent = 0 @@ -55,7 +57,8 @@ local function mspSendRequest(cmd) value = bit32.band(cmd,0xFF) -- MSP command value = value + bit32.lshift(cmd,8) -- CRC - mspRequestsSent = requestsSent + 1 + mspLastReq = cmd + mspRequestsSent = mspRequestsSent + 1 return sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value) end @@ -90,7 +93,7 @@ local function mspReceivedReply(payload) mspRxBuf = {} mspRxSize = payload[idx] - mspRxCRC = mspRxSize + mspRxCRC = bit32.bxor(mspRxSize,mspLastReq) idx = idx + 1 mspStarted = true @@ -100,7 +103,7 @@ local function mspReceivedReply(payload) mspOutOfOrder = mspOutOfOrder + 1 return nil - elseif bit32.band(lastSeq+1,0x0F) ~= seq then + elseif bit32.band(sportMspRemoteSeq + 1, 0x0F) ~= seq then mspOutOfOrder = mspOutOfOrder + 1 mspStarted = false return nil @@ -114,7 +117,7 @@ local function mspReceivedReply(payload) end if idx > 6 then - lastRxSeq = seq + sportMspRemoteSeq = seq return end @@ -122,6 +125,7 @@ local function mspReceivedReply(payload) if mspRxCRC ~= payload[idx] then mspStarted = false mspCRCErrors = mspCRCErrors + 1 + return nil end mspRepliesReceived = mspRepliesReceived + 1 @@ -157,18 +161,14 @@ local function run(event) local now = getTime() if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then - requestsSent = 0 - repliesReceived = 0 - mspReceivedReply_cnt = 0 - mspReceivedReply_cnt1 = 0 - mspReceivedReply_cnt2 = 0 - mspReceivedReply_cnt3 = 0 + mspResetStats() end lcd.clear() - + lcd.drawText(41,1,"MSP/SPORT test script",INVERS) + -- do we have valid telemetry data? - if getValue("rssi") > 0 then + if getValue("RSSI") > 0 then -- draw screen lcd.drawText(1,11,"Requests:",0) @@ -178,19 +178,19 @@ local function run(event) lcd.drawNumber(60,21,mspRepliesReceived) lcd.drawText(1,31,"PkRxed:",0) - lcd.drawNumber(30,31,mspPkRxed) + lcd.drawNumber(60,31,mspPkRxed) lcd.drawText(1,41,"ErrorPk:",0) - lcd.drawNumber(30,41,mspErrorPk) + lcd.drawNumber(60,41,mspErrorPk) - lcd.drawText(71,31,"StartPk:",0) - lcd.drawNumber(100,31,mspStartPk) + lcd.drawText(91,31,"StartPk:",0) + lcd.drawNumber(160,31,mspStartPk) - lcd.drawText(71,41,"OutOfOrder:",0) - lcd.drawNumber(100,41,mspOutOfOrder) + lcd.drawText(91,41,"OutOfOrder:",0) + lcd.drawNumber(160,41,mspOutOfOrder) lcd.drawText(1,51,"CRCErrors:",0) - lcd.drawNumber(30,51,mspCRCErrors) + lcd.drawNumber(60,51,mspCRCErrors) -- last request is at least 2s old if lastReqTS + 200 <= now then @@ -198,10 +198,10 @@ local function run(event) lastReqTS = now end else - lcd.drawText(20,30,"No telemetry signal", XXLSIZE + BLINK) + lcd.drawText(15,20,"No telemetry signal", BLINK + DBLSIZE) end - pollReply() + mspPollReply() end return {run=run} From 1f5e5931204522e94c1a0aa248fb3eb2e2cc8293 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 3 Nov 2016 21:24:31 +0000 Subject: [PATCH 090/188] Abstracted displayPort from CMS --- Makefile | 1 + src/main/drivers/display.c | 68 +++++++++++++++++ src/main/drivers/display.h | 49 ++++++++++++ src/main/io/canvas.c | 50 +++++++------ src/main/io/cms.c | 111 +++++++++------------------- src/main/io/cms.h | 28 +------ src/main/io/cms_blackbox.c | 10 +-- src/main/io/dashboard.c | 50 +++++++------ src/main/io/osd.c | 77 +------------------ src/main/io/osd_max7456.c | 90 ++++++++++++++++++++++ src/main/io/osd_max7456.h | 3 + src/main/target/OMNIBUS/target.mk | 1 + src/main/target/OMNIBUSF4/target.mk | 4 + 13 files changed, 313 insertions(+), 229 deletions(-) create mode 100644 src/main/drivers/display.c create mode 100644 src/main/drivers/display.h create mode 100644 src/main/io/osd_max7456.c create mode 100644 src/main/io/osd_max7456.h diff --git a/Makefile b/Makefile index c6ccb2b20f..5696fd6bf6 100644 --- a/Makefile +++ b/Makefile @@ -485,6 +485,7 @@ COMMON_SRC = \ drivers/bus_i2c_soft.c \ drivers/bus_spi.c \ drivers/bus_spi_soft.c \ + drivers/display.c \ drivers/exti.c \ drivers/gyro_sync.c \ drivers/io.c \ diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c new file mode 100644 index 0000000000..2587abb702 --- /dev/null +++ b/src/main/drivers/display.c @@ -0,0 +1,68 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "common/utils.h" + +#include "display.h" + +// XXX Why is this here? Something wrong? +// XXX Something like Drawing Context that holds all state variables would be the way... +int8_t lastCursorPos; + +void displayClear(displayPort_t *instance) +{ + instance->vTable->clear(instance); + instance->cleared = true; + instance->lastCursorPos = -1; +} + +void displayBegin(displayPort_t *instance) +{ + instance->vTable->begin(instance); + instance->vTable->clear(instance); +} + +void displayEnd(displayPort_t *instance) +{ + instance->vTable->end(instance); +} + +int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s) +{ + return instance->vTable->write(instance, x, y, s); +} + +void displayHeartbeat(displayPort_t *instance) +{ + instance->vTable->heartbeat(instance); +} + +void displayResync(displayPort_t *instance) +{ + instance->vTable->resync(instance); +} + +uint16_t displayTxBytesFree(displayPort_t *instance) +{ + return instance->vTable->txBytesFree(instance); +} + diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h new file mode 100644 index 0000000000..2f6f261659 --- /dev/null +++ b/src/main/drivers/display.h @@ -0,0 +1,49 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +struct displayPortVTable_s; +typedef struct displayPort_s { + const struct displayPortVTable_s *vTable; + uint8_t rows; + uint8_t cols; + uint16_t buftime; + uint16_t bufsize; + + // CMS state + bool cleared; + int8_t lastCursorPos; +} displayPort_t; + +typedef struct displayPortVTable_s { + int (*begin)(displayPort_t *displayPort); + int (*end)(displayPort_t *displayPort); + int (*clear)(displayPort_t *displayPort); + int (*write)(displayPort_t *displayPort, uint8_t col, uint8_t row, char *text); + int (*heartbeat)(displayPort_t *displayPort); + void (*resync)(displayPort_t *displayPort); + uint32_t (*txBytesFree)(displayPort_t *displayPort); +} displayPortVTable_t; + +void displayClear(displayPort_t *instance); +void displayBegin(displayPort_t *instance); +void displayEnd(displayPort_t *instance); +int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s); +void displayHeartbeat(displayPort_t *instance); +void displayResync(displayPort_t *instance); +uint16_t displayTxBytesFree(displayPort_t *instance); diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index f2d0eeed15..bc76e09e4c 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -5,10 +5,10 @@ #include "platform.h" -#include "build/version.h" - #ifdef CANVAS +#include "build/version.h" + #include "common/utils.h" #include "drivers/system.h" @@ -16,46 +16,48 @@ #include "io/cms.h" #include "fc/fc_msp.h" + #include "msp/msp_protocol.h" #include "msp/msp_serial.h" -int canvasOutput(uint8_t cmd, uint8_t *buf, int len) +static int canvasOutput(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len) { + UNUSED(displayPort); mspSerialPush(cmd, buf, len); return 6 + len; } -int canvasBegin(void) +static int canvasBegin(displayPort_t *displayPort) { uint8_t subcmd[] = { 0 }; - return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); + return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); } -int canvasHeartBeat(void) +static int canvasHeartBeat(displayPort_t *displayPort) { - return canvasBegin(); + return canvasBegin(displayPort); } -int canvasEnd(void) +static int canvasEnd(displayPort_t *displayPort) { uint8_t subcmd[] = { 1 }; - return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); + return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); } -int canvasClear(void) +static int canvasClear(displayPort_t *displayPort) { uint8_t subcmd[] = { 2 }; - return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); + return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); } -int canvasWrite(uint8_t col, uint8_t row, char *string) +static int canvasWrite(displayPort_t *displayPort, uint8_t col, uint8_t row, char *string) { int len; - char buf[30 + 4]; + uint8_t buf[30 + 4]; if ((len = strlen(string)) >= 30) len = 30; @@ -64,23 +66,24 @@ int canvasWrite(uint8_t col, uint8_t row, char *string) buf[1] = row; buf[2] = col; buf[3] = 0; - memcpy((char *)&buf[4], string, len); + memcpy(&buf[4], string, len); - return canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4); + return canvasOutput(displayPort, MSP_CANVAS, buf, len + 4); } -void canvasResync(displayPort_t *pPort) +static void canvasResync(displayPort_t *displayPort) { - pPort->rows = 13; // XXX Will reflect NTSC/PAL in the future - pPort->rows = 30; + displayPort->rows = 13; // XXX Will reflect NTSC/PAL in the future + displayPort->rows = 30; } -uint32_t canvasTxBytesFree(void) +static uint32_t canvasTxBytesFree(displayPort_t *displayPort) { + UNUSED(displayPort); return mspSerialTxBytesFree(); } -displayPortVTable_t canvasVTable = { +static const displayPortVTable_t canvasVTable = { canvasBegin, canvasEnd, canvasClear, @@ -90,11 +93,10 @@ displayPortVTable_t canvasVTable = { canvasTxBytesFree, }; -void canvasCmsInit(displayPort_t *pPort) +void canvasCmsInit(displayPort_t *displayPort) { - pPort->rows = 13; // XXX Will reflect NTSC/PAL in the future - pPort->cols = 30; - pPort->vTable = &canvasVTable; + displayPort->vTable = &canvasVTable; + canvasResync(displayPort); } void canvasInit() diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 74fce20a91..2d1b8a6021 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -113,48 +113,6 @@ cmsDeviceInitFuncPtr cmsDeviceSelectNext(void) #define CMS_UPDATE_INTERVAL 50 // msec -// XXX Why is this here? Something wrong? -// XXX Something like Drawing Context that holds all state variables would be the way... -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) -{ - instance->vTable->heartbeat(); -} - -void cmsScreenResync(displayPort_t *instance) -{ - instance->vTable->resync(instance); -} - -uint16_t cmsScreenTxBytesFree(displayPort_t *instance) -{ - return instance->vTable->txBytesFree(); -} - void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) { cmsDeviceInitFunc(pDisp); @@ -277,22 +235,22 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr switch (p->type) { case OME_String: if (IS_PRINTVALUE(p) && p->data) { - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data); CLR_PRINTVALUE(p); } break; case OME_Submenu: if (IS_PRINTVALUE(p)) { - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); CLR_PRINTVALUE(p); } break; case OME_Bool: if (IS_PRINTVALUE(p) && p->data) { if (*((uint8_t *)(p->data))) { - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); } else { - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); } CLR_PRINTVALUE(p); } @@ -300,8 +258,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr 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]); + //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, (char *)ptr->names[*ptr->val]); CLR_PRINTVALUE(p); } break; @@ -315,9 +273,9 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr val = (uint16_t *)address; if (VISIBLE(*val)) { - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); } else { - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); } CLR_PRINTVALUE(p); } @@ -328,8 +286,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr 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); + //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, " "); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); CLR_PRINTVALUE(p); } break; @@ -338,7 +296,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr OSD_INT8_t *ptr = p->data; itoa(*ptr->val, buff, 10); cmsPad(buff, 5); - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); CLR_PRINTVALUE(p); } break; @@ -347,7 +305,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); cmsPad(buff, 5); - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); CLR_PRINTVALUE(p); } break; @@ -356,7 +314,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); cmsPad(buff, 5); - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); CLR_PRINTVALUE(p); } break; @@ -365,7 +323,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); cmsPad(buff, 5); - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); // PRINTVALUE not cleared on purpose } break; @@ -374,7 +332,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr OSD_FLOAT_t *ptr = p->data; cmsFormatFloat(*ptr->val * ptr->multipler, buff); cmsPad(buff, 5); - cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ??? + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ??? CLR_PRINTVALUE(p); } break; @@ -399,7 +357,7 @@ void cmsDrawMenu(displayPort_t *pDisplay) static uint8_t pollDenom = 0; bool drawPolled = (++pollDenom % 8 == 0); - uint32_t room = cmsScreenTxBytesFree(pDisplay); + uint32_t room = displayTxBytesFree(pDisplay); if (!currentMenu) return; @@ -425,16 +383,16 @@ void cmsDrawMenu(displayPort_t *pDisplay) while ((currentMenu + currentCursorPos)->type == OME_Label) // skip label currentCursorPos++; - if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) { - room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, lastCursorPos + top, " "); + if (pDisplay->lastCursorPos >= 0 && currentCursorPos != pDisplay->lastCursorPos) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->lastCursorPos + top, " "); } if (room < 30) return; - if (lastCursorPos != currentCursorPos) { - room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >"); - lastCursorPos = currentCursorPos; + if (pDisplay->lastCursorPos != currentCursorPos) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >"); + pDisplay->lastCursorPos = currentCursorPos; } if (room < 30) @@ -443,7 +401,7 @@ void cmsDrawMenu(displayPort_t *pDisplay) // Print text labels for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { if (IS_PRINTLABEL(p)) { - room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text); + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text); CLR_PRINTLABEL(p); if (room < 30) return; @@ -483,7 +441,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) currentMenu = (OSD_Entry *)ptr; currentCursorPos = 0; } - cmsScreenClear(pDisplay); + displayClear(pDisplay); cmsUpdateMaxRows(pDisplay); } @@ -502,7 +460,7 @@ long cmsMenuBack(displayPort_t *pDisplay) cmsx_RateExpoWriteback(); if (menuStackIdx) { - cmsScreenClear(pDisplay); + displayClear(pDisplay); menuStackIdx--; nextPage = NULL; currentMenu = menuStack[menuStackIdx]; @@ -527,7 +485,7 @@ void cmsMenuOpen(void) currentMenu = &menuMain[0]; } else { // Switch display - cmsScreenEnd(¤tDisplay); + displayEnd(¤tDisplay); initfunc = cmsDeviceSelectNext(); } @@ -535,17 +493,17 @@ void cmsMenuOpen(void) return; cmsScreenInit(¤tDisplay, initfunc); - cmsScreenBegin(¤tDisplay); + displayBegin(¤tDisplay); cmsMenuChange(¤tDisplay, currentMenu); } long cmsMenuExit(displayPort_t *pDisplay, void *ptr) { if (ptr) { - cmsScreenClear(pDisplay); + displayClear(pDisplay); - cmsScreenWrite(pDisplay, 5, 3, "REBOOTING..."); - cmsScreenResync(pDisplay); // Was max7456RefreshAll(); why at this timing? + displayWrite(pDisplay, 5, 3, "REBOOTING..."); + displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? stopMotors(); stopPwmAllMotors(); @@ -557,7 +515,7 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr) cmsInMenu = false; - cmsScreenEnd(pDisplay); + displayEnd(pDisplay); currentMenu = NULL; if (ptr) @@ -585,9 +543,8 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (currentCursorPos < currentMenuIdx) { currentCursorPos++; } else { - if (nextPage) // we have more pages - { - cmsScreenClear(pDisplay); + if (nextPage) { // we have more pages + displayClear(pDisplay); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; @@ -607,7 +564,7 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (currentCursorPos == -1 || (currentMenu + currentCursorPos)->type == OME_Label) { if (nextPage) { - cmsScreenClear(pDisplay); + displayClear(pDisplay); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; @@ -803,7 +760,7 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) if (currentTime > lastCmsHeartBeat + 500) { // Heart beat for external CMS display device @ 500msec // (Timeout @ 1000msec) - cmsScreenHeartBeat(¤tDisplay); + displayHeartbeat(¤tDisplay); lastCmsHeartBeat = currentTime; } } diff --git a/src/main/io/cms.h b/src/main/io/cms.h index a7d0d21e08..5e3be53e80 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -1,27 +1,6 @@ #pragma once -struct displayPort_s; - -typedef struct displayPortVTable_s { - int (*begin)(void); - int (*end)(void); - int (*clear)(void); - int (*write)(uint8_t col, uint8_t row, char *text); - int (*heartbeat)(void); - void (*resync)(struct displayPort_s *pPort); - uint32_t (*txBytesFree)(void); -} displayPortVTable_t; - -typedef struct displayPort_s { - displayPortVTable_t *vTable; - uint8_t rows; - uint8_t cols; - uint16_t buftime; - uint16_t bufsize; - - // CMS state - bool cleared; -} displayPort_t; +#include "drivers/display.h" // Device management typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *pPort); @@ -31,11 +10,6 @@ bool cmsDeviceRegister(cmsDeviceInitFuncPtr); void cmsInit(void); void cmsHandler(uint32_t currentTime); -// Required for external CMS tables -void cmsScreenClear(displayPort_t *pPort); -void cmsScreenResync(displayPort_t *pPort); -int cmsScreenWrite(displayPort_t *pPort, uint8_t x, uint8_t y, char *s); - long cmsMenuChange(displayPort_t *pPort, void *ptr); long cmsMenuExit(displayPort_t *pPort, void *ptr); diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c index 3f118f87ee..696b1231c4 100644 --- a/src/main/io/cms_blackbox.c +++ b/src/main/io/cms_blackbox.c @@ -30,17 +30,17 @@ long 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? + displayClear(pDisplay); + displayWrite(pDisplay, 5, 3, "ERASING FLASH..."); + displayResync(pDisplay); // Was max7456RefreshAll(); Why at this timing? flashfsEraseCompletely(); while (!flashfsIsReady()) { delay(100); } - cmsScreenClear(pDisplay); - cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? + displayClear(pDisplay); + displayResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? return 0; } diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index c63b6d90c0..7fc7ab9a30 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -751,68 +751,74 @@ void dashboardDisablePageCycling(void) } #ifdef OLEDCMS -#include "io/cms.h" +#include "drivers/display.h" -int dashboardCmsBegin(void) +static int dashboardBegin(displayPort_t *displayPort) { + UNUSED(displayPort); dashboardInCMS = true; return 0; } -int dashboardCmsEnd(void) +static int dashboardEnd(displayPort_t *displayPort) { + UNUSED(displayPort); dashboardInCMS = false; return 0; } -int dashboardCmsClear(void) +static int dashboardClear(displayPort_t *displayPort) { + UNUSED(displayPort); i2c_OLED_clear_display_quick(); return 0; } -int dashboardCmsWrite(uint8_t x, uint8_t y, char *s) +static int dashboardWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s) { + UNUSED(displayPort); i2c_OLED_set_xy(x, y); i2c_OLED_send_string(s); return 0; } -int dashboardCmsHeartbeat(void) +static int dashboardHeartbeat(displayPort_t *displayPort) { + UNUSED(displayPort); return 0; } -void dashboardCmsResync(displayPort_t *pPort) +static void dashboardResync(displayPort_t *displayPort) { - UNUSED(pPort); + UNUSED(displayPort); } -uint32_t dashboardCmsTxBytesFree(void) +static uint32_t dashboardTxBytesFree(displayPort_t *displayPort) { + UNUSED(displayPort); return UINT32_MAX; } -displayPortVTable_t dashboardCmsVTable = { - dashboardCmsBegin, - dashboardCmsEnd, - dashboardCmsClear, - dashboardCmsWrite, - dashboardCmsHeartbeat, - dashboardCmsResync, - dashboardCmsTxBytesFree, +static const displayPortVTable_t dashboardVTable = { + dashboardBegin, + dashboardEnd, + dashboardClear, + dashboardWrite, + dashboardHeartbeat, + dashboardResync, + dashboardTxBytesFree, }; -void dashboardCmsInit(displayPort_t *pPort) +void dashboardCmsInit(displayPort_t *displayPort) { - pPort->rows = SCREEN_CHARACTER_ROW_COUNT; - pPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; - pPort->vTable = &dashboardCmsVTable; + displayPort->rows = SCREEN_CHARACTER_ROW_COUNT; + displayPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; + displayPort->vTable = &dashboardVTable; } #endif // OLEDCMS -#endif +#endif // USE_DASHBOARD diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c271d4339f..f1b3871e70 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -39,16 +39,15 @@ #include "io/cms.h" #include "io/cms_types.h" - +#include "io/cms_osd.h" #include "io/flashfs.h" #include "io/osd.h" +#include "io/osd_max7456.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" @@ -401,7 +400,7 @@ void osdInit(void) refreshTimeout = 4 * REFRESH_1S; #ifdef CMS - cmsDeviceRegister(osdCmsInit); + cmsDeviceRegister(osdMax7456Init); #endif } @@ -633,60 +632,6 @@ void osdUpdate(uint32_t currentTime) } } -// -// OSD specific CMS functions -// -#include "io/cms_osd.h" - -uint8_t shiftdown; - -int osdMenuBegin(void) -{ - osdResetAlarms(); - osdInMenu = true; - refreshTimeout = 0; - - return 0; -} - -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 + shiftdown, s); - - return 0; -} - -void osdResync(displayPort_t *pPort) -{ - max7456RefreshAll(); - pPort->rows = max7456GetRowsCount() - masterConfig.osdProfile.row_shiftdown; - pPort->cols = 30; -} - -int osdHeartbeat(void) -{ - return 0; -} - -uint32_t osdTxBytesFree(void) -{ - return UINT32_MAX; -} - #ifdef EDIT_ELEMENT_SUPPORT void osdEditElement(void *ptr) { @@ -713,22 +658,6 @@ void osdDrawElementPositioningHelp(void) } #endif -displayPortVTable_t osdVTable = { - osdMenuBegin, - osdMenuEnd, - osdClearScreen, - osdWrite, - osdHeartbeat, - osdResync, - osdTxBytesFree, -}; - -void osdCmsInit(displayPort_t *pPort) -{ - osdResync(pPort); - pPort->vTable = &osdVTable; -} - 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}; diff --git a/src/main/io/osd_max7456.c b/src/main/io/osd_max7456.c new file mode 100644 index 0000000000..7471910c6e --- /dev/null +++ b/src/main/io/osd_max7456.c @@ -0,0 +1,90 @@ +#include +#include + +#include "platform.h" + +#ifdef OSD + +#include "common/utils.h" + +#include "config/config_master.h" + +#include "drivers/display.h" +#include "drivers/max7456.h" + +extern bool osdInMenu; +extern uint16_t refreshTimeout; +void osdResetAlarms(void); + +uint8_t shiftdown; + +static int osdMenuBegin(displayPort_t *displayPort) +{ + UNUSED(displayPort); + osdResetAlarms(); + osdInMenu = true; + refreshTimeout = 0; + + return 0; +} + +static int osdMenuEnd(displayPort_t *displayPort) +{ + UNUSED(displayPort); + osdInMenu = false; + + return 0; +} + +static int osdClearScreen(displayPort_t *displayPort) +{ + UNUSED(displayPort); + max7456ClearScreen(); + + return 0; +} + +static int osdWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s) +{ + UNUSED(displayPort); + max7456Write(x, y + shiftdown, s); + + return 0; +} + +static void osdResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); + max7456RefreshAll(); + displayPort->rows = max7456GetRowsCount() - masterConfig.osdProfile.row_shiftdown; + displayPort->cols = 30; +} + +static int osdHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static uint32_t osdTxBytesFree(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +displayPortVTable_t osdVTable = { + osdMenuBegin, + osdMenuEnd, + osdClearScreen, + osdWrite, + osdHeartbeat, + osdResync, + osdTxBytesFree, +}; + +void osdMax7456Init(displayPort_t *displayPort) +{ + displayPort->vTable = &osdVTable; + osdResync(displayPort); +} +#endif // OSD diff --git a/src/main/io/osd_max7456.h b/src/main/io/osd_max7456.h new file mode 100644 index 0000000000..26d696f304 --- /dev/null +++ b/src/main/io/osd_max7456.h @@ -0,0 +1,3 @@ +#pragma once + +void osdMax7456Init(displayPort_t *displayPort); diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 5a7e1a2382..e30c6530a8 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -15,6 +15,7 @@ TARGET_SRC = \ io/transponder_ir.c \ drivers/max7456.c \ io/osd.c \ + io/osd_max7456.c \ io/canvas.c \ io/cms.c \ io/cms_imu.c \ diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index c7cf0ad933..5cefb51d21 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -7,6 +7,10 @@ TARGET_SRC = \ drivers/compass_hmc5883l.c \ drivers/max7456.c \ io/osd.c \ + io/osd_max7456.c \ io/cms.c \ + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_ledstrip.c \ io/canvas.c From 7100454a1d26582e9830056600c3d4b015c36b4c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 3 Nov 2016 23:40:34 +0100 Subject: [PATCH 091/188] Fix DMA bug on F3 // enable flto again (Thanks to @ronlix for the find) --- Makefile | 4 ---- src/main/drivers/pwm_output_stm32f3xx.c | 1 + 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/Makefile b/Makefile index c6ccb2b20f..fa5bed9106 100644 --- a/Makefile +++ b/Makefile @@ -734,11 +734,7 @@ OPTIMIZE = -O0 LTO_FLAGS = $(OPTIMIZE) else OPTIMIZE = -Os -ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) -else -LTO_FLAGS = -fuse-linker-plugin $(OPTIMIZE) -endif endif DEBUG_FLAGS = -ggdb3 -DDEBUG diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index e72e03234c..0813b76fbb 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -139,6 +139,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / hz) - 1); TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); } From 72d5160262a3977cc5419392695d85f626120ac7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 4 Nov 2016 01:03:19 +0100 Subject: [PATCH 092/188] Add DSHOT support to LUX_RACE target Add TIM88 --- src/main/target/LUX_RACE/target.c | 23 ++++++++++++----------- src/main/target/LUX_RACE/target.h | 4 +++- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 9352353f82..7af0374e42 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -21,18 +21,19 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 }; diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index e2491c73d6..78e3630f07 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -36,6 +36,8 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW +#define USE_DSHOT + #define USE_SPI #define USE_SPI_DEVICE_1 @@ -109,5 +111,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM(8) | TIM_N(15)) From fc81bc668b4a50e236bafd13b7c59364a1e7c1ea Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 3 Nov 2016 17:45:16 +0900 Subject: [PATCH 093/188] Properly exclude include files based on options --- src/main/io/cms.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 2d1b8a6021..e74cb7005e 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -37,11 +37,22 @@ #include "io/cms.h" #include "io/cms_types.h" -#include "io/canvas.h" +#ifdef CANVAS +#include "io/canvas.h" +#endif + +#ifdef USE_FLASHFS #include "io/flashfs.h" +#endif + +#ifdef OSD #include "io/osd.h" +#endif + +#ifdef USE_DASHBOARD #include "io/dashboard.h" +#endif #include "fc/config.h" #include "fc/rc_controls.h" @@ -53,8 +64,6 @@ #include "config/config_master.h" #include "config/feature.h" -#include "io/cms.h" - #include "build/debug.h" // External menu contents From d1aa8a603c0737fb09531a403e3211626718589d Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 4 Nov 2016 09:48:44 +0900 Subject: [PATCH 094/188] begin-end to open-close --- src/main/drivers/display.c | 8 ++++---- src/main/drivers/display.h | 10 ++++------ src/main/io/cms.c | 6 +++--- 3 files changed, 11 insertions(+), 13 deletions(-) diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 2587abb702..688a17cb20 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -35,15 +35,15 @@ void displayClear(displayPort_t *instance) instance->lastCursorPos = -1; } -void displayBegin(displayPort_t *instance) +void displayOpen(displayPort_t *instance) { - instance->vTable->begin(instance); + instance->vTable->open(instance); instance->vTable->clear(instance); } -void displayEnd(displayPort_t *instance) +void displayClose(displayPort_t *instance) { - instance->vTable->end(instance); + instance->vTable->close(instance); } int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s) diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index 2f6f261659..3e5a732090 100644 --- a/src/main/drivers/display.h +++ b/src/main/drivers/display.h @@ -22,8 +22,6 @@ typedef struct displayPort_s { const struct displayPortVTable_s *vTable; uint8_t rows; uint8_t cols; - uint16_t buftime; - uint16_t bufsize; // CMS state bool cleared; @@ -31,8 +29,8 @@ typedef struct displayPort_s { } displayPort_t; typedef struct displayPortVTable_s { - int (*begin)(displayPort_t *displayPort); - int (*end)(displayPort_t *displayPort); + int (*open)(displayPort_t *displayPort); + int (*close)(displayPort_t *displayPort); int (*clear)(displayPort_t *displayPort); int (*write)(displayPort_t *displayPort, uint8_t col, uint8_t row, char *text); int (*heartbeat)(displayPort_t *displayPort); @@ -40,9 +38,9 @@ typedef struct displayPortVTable_s { uint32_t (*txBytesFree)(displayPort_t *displayPort); } displayPortVTable_t; +void displayOpen(displayPort_t *instance); +void displayClose(displayPort_t *instance); void displayClear(displayPort_t *instance); -void displayBegin(displayPort_t *instance); -void displayEnd(displayPort_t *instance); int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s); void displayHeartbeat(displayPort_t *instance); void displayResync(displayPort_t *instance); diff --git a/src/main/io/cms.c b/src/main/io/cms.c index e74cb7005e..90946ab836 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -494,7 +494,7 @@ void cmsMenuOpen(void) currentMenu = &menuMain[0]; } else { // Switch display - displayEnd(¤tDisplay); + displayClose(¤tDisplay); initfunc = cmsDeviceSelectNext(); } @@ -502,7 +502,7 @@ void cmsMenuOpen(void) return; cmsScreenInit(¤tDisplay, initfunc); - displayBegin(¤tDisplay); + displayOpen(¤tDisplay); cmsMenuChange(¤tDisplay, currentMenu); } @@ -524,7 +524,7 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr) cmsInMenu = false; - displayEnd(pDisplay); + displayClose(pDisplay); currentMenu = NULL; if (ptr) From 0a270e878b5ca90a7052c82546c9ff1f5f81fd4e Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 4 Nov 2016 09:49:18 +0900 Subject: [PATCH 095/188] Make travis happy with osd_max7456.c --- src/main/target/SIRINFPV/target.mk | 1 + src/main/target/STM32F3DISCOVERY/target.mk | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index e64c9781b3..038c7da7d6 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -13,6 +13,7 @@ TARGET_SRC = \ drivers/vtx_soft_spi_rtc6705.c \ drivers/max7456.c \ io/osd.c \ + io/osd_max7456.c \ io/canvas.c \ io/cms.c \ io/cms_imu.c \ diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 4142776c47..706b68492b 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -27,6 +27,7 @@ TARGET_SRC = \ drivers/flash_m25p16.c \ drivers/max7456.c \ io/osd.c \ + io/osd_max7456.c \ io/canvas.c \ io/cms.c \ io/cms_imu.c \ From 3d6e0a29f70a84ddd888b911ee49ea236d68710e Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 4 Nov 2016 11:58:45 +1100 Subject: [PATCH 096/188] Added missing TimeBaseStructInit and explicitly set repetitioncounter for F4 and F7 as per F3. --- src/main/drivers/pwm_output_stm32f3xx.c | 3 ++- src/main/drivers/pwm_output_stm32f4xx.c | 4 +++- src/main/drivers/pwm_output_stm32f7xx.c | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 0813b76fbb..5e7b8f7582 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -119,7 +119,8 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t if (configureTimer) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index d2bf798187..e2bf84e12c 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -120,7 +120,8 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t if (configureTimer) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); @@ -140,6 +141,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1; TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); } diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index d2b7842435..fa03d52215 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -145,6 +145,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motor->TimHandle.Instance = timerHardware->tim; motor->TimHandle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;; motor->TimHandle.Init.Period = MOTOR_BITLENGTH; + motor->TimHandle.Init.RepetitionCounter = 0; motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; if(HAL_TIM_PWM_Init(&motor->TimHandle) != HAL_OK) From 0603b1c1255dd2768a74633a57efa389d615a245 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 4 Nov 2016 02:16:30 +0100 Subject: [PATCH 097/188] Add LUXR_RACE Target --- src/main/target/LUX_RACE/LUXR_RACE.mk | 1 + src/main/target/LUX_RACE/target.c | 2 + src/main/target/LUX_RACE/target.h | 78 +++++++++++++++++++++++++-- src/main/target/LUX_RACE/target.mk | 6 +-- 4 files changed, 79 insertions(+), 8 deletions(-) create mode 100755 src/main/target/LUX_RACE/LUXR_RACE.mk diff --git a/src/main/target/LUX_RACE/LUXR_RACE.mk b/src/main/target/LUX_RACE/LUXR_RACE.mk new file mode 100755 index 0000000000..63550affa6 --- /dev/null +++ b/src/main/target/LUX_RACE/LUXR_RACE.mk @@ -0,0 +1 @@ +# LUXR_RACE is new version diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 7af0374e42..48427a90c3 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -29,11 +29,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 +#ifndef LUXR_RACE { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 +#endif }; diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 78e3630f07..d9b65efd89 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -17,16 +17,26 @@ #pragma once +#ifdef LUXR_RACE +#define TARGET_BOARD_IDENTIFIER "LUXR" +#else #define TARGET_BOARD_IDENTIFIER "LUX" +#endif #define BOARD_HAS_VOLTAGE_DIVIDER #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0 PC15 #define LED1 PC14 +#ifndef LUXR_RACE #define LED2 PC13 +#endif +#ifdef LUXR_RACE +#define BEEPER PB9 +#else #define BEEPER PB13 +#endif #define BEEPER_INVERTED // MPU6500 interrupt @@ -40,24 +50,68 @@ #define USE_SPI #define USE_SPI_DEVICE_1 +#ifdef LUXR_RACE +#define USE_SPI_DEVICE_2 +#endif #define SPI1_SCK_PIN PB3 #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 +//#ifndef LUXR_RACE #define SPI1_NSS_PIN PA4 +//#endif +#ifdef LUXR_RACE +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC13 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +#endif + +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_INSTANCE SPI1 #define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_INSTANCE SPI1 #define GYRO +#ifdef LUXR_RACE +#define USE_GYRO_MPU6000 +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG +#else #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG +#endif #define ACC +#ifdef LUXR_RACE +#define USE_ACC_MPU6000 +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG +#else #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG +#endif #define USB_IO @@ -65,7 +119,13 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 +#ifdef LUXR_RACE +#define USE_UART4 +#define USE_UART5 +#define SERIAL_PORT_COUNT 6 +#else #define SERIAL_PORT_COUNT 4 +#endif #define UART1_TX_PIN PC4 #define UART1_RX_PIN PC5 @@ -76,8 +136,7 @@ #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 -#define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#undef USE_I2C #define USE_ADC #define ADC_INSTANCE ADC1 @@ -97,6 +156,11 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#ifdef LUXR_RACE +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define DEFAULT_FEATURES FEATURE_BLACKBOX +#endif + #define SPEKTRUM_BIND // USART1, PC5 #define BIND_PIN PC5 @@ -110,6 +174,10 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#ifdef LUXR_RACE +#define USABLE_TIMER_CHANNEL_COUNT 5 +#else #define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM(8) | TIM_N(15)) +#endif +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/LUX_RACE/target.mk b/src/main/target/LUX_RACE/target.mk index 4833b16e4d..ca59497685 100644 --- a/src/main/target/LUX_RACE/target.mk +++ b/src/main/target/LUX_RACE/target.mk @@ -1,9 +1,9 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP +FEATURES = VCP SDCARD TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_mpu6500.c - + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ No newline at end of file From 383a7b5c40c8a808728d2a24824caf598af6f438 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 3 Nov 2016 16:38:20 +0000 Subject: [PATCH 098/188] Added and used FC_FIRMWARE_NAME #define --- src/main/blackbox/blackbox.c | 2 +- src/main/build/version.h | 1 + src/main/io/serial_cli.c | 3 ++- src/main/vcpf4/usbd_desc.c | 3 ++- 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index cddebdd4ca..7038c7ce20 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1169,7 +1169,7 @@ static bool blackboxWriteSysinfo() switch (xmitState.headerIndex) { BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight"); - BLACKBOX_PRINT_HEADER_LINE("Firmware revision:Betaflight %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName); + BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name); BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); diff --git a/src/main/build/version.h b/src/main/build/version.h index c3c6529ed3..59a418b265 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#define FC_FIRMWARE_NAME "Betaflight" #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f757a87b25..26aa3c3afe 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3608,7 +3608,8 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - cliPrintf("# BetaFlight/%s %s %s / %s (%s)\r\n", + cliPrintf("# %s/%s %s %s / %s (%s)\r\n", + FC_FIRMWARE_NAME, targetName, FC_VERSION_STRING, buildDate, diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index cdf6c4fe7a..c7ac34fd60 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -26,6 +26,7 @@ #include "usbd_conf.h" #include "usb_regs.h" #include "platform.h" +#include "build/version.h" /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY * @{ @@ -57,7 +58,7 @@ * @{ */ #define USBD_LANGID_STRING 0x409 -#define USBD_MANUFACTURER_STRING "BetaFlight" +#define USBD_MANUFACTURER_STRING FC_FIRMWARE_NAME #ifdef USBD_PRODUCT_STRING #define USBD_PRODUCT_HS_STRING USBD_PRODUCT_STRING From 51d4b3454043e3bed3c39101aff8c92f29cf9daa Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 4 Nov 2016 17:22:58 +0900 Subject: [PATCH 099/188] Some names made more intuitive --- src/main/drivers/display.c | 6 +- src/main/drivers/display.h | 2 +- src/main/io/cms.c | 114 ++++++++++++++++--------------- src/main/target/OMNIBUS/target.h | 1 + 4 files changed, 61 insertions(+), 62 deletions(-) diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 688a17cb20..2d7ec79de3 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -24,15 +24,11 @@ #include "display.h" -// XXX Why is this here? Something wrong? -// XXX Something like Drawing Context that holds all state variables would be the way... -int8_t lastCursorPos; - void displayClear(displayPort_t *instance) { instance->vTable->clear(instance); instance->cleared = true; - instance->lastCursorPos = -1; + instance->cursorRow = -1; } void displayOpen(displayPort_t *instance) diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index 3e5a732090..60028b7ebe 100644 --- a/src/main/drivers/display.h +++ b/src/main/drivers/display.h @@ -25,7 +25,7 @@ typedef struct displayPort_s { // CMS state bool cleared; - int8_t lastCursorPos; + int8_t cursorRow; } displayPort_t; typedef struct displayPortVTable_s { diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 90946ab836..807394e389 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -72,7 +72,7 @@ #include "io/cms_vtx.h" #ifdef OSD #include "io/cms_osd.h" -#endif // OSD +#endif #include "io/cms_ledstrip.h" // Forwards @@ -82,7 +82,9 @@ void cmsx_FeatureWriteback(void); // Device management +#ifndef CMS_MAX_DEVICE #define CMS_MAX_DEVICE 4 +#endif cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE]; int cmsDeviceCount; @@ -152,17 +154,21 @@ displayPort_t currentDisplay; bool cmsInMenu = false; -OSD_Entry *menuStack[10]; //tab to save menu stack -uint8_t menuStackHistory[10]; //current position in menu stack +OSD_Entry menuMain[]; + +// XXX Does menu backing support backing into second page??? + +OSD_Entry *menuStack[10]; // Stack to save menu transition +uint8_t menuStackHistory[10]; // cursorRow in a stacked menu uint8_t menuStackIdx = 0; -OSD_Entry menuMain[]; -OSD_Entry *currentMenu = NULL; -OSD_Entry *nextPage = NULL; +OSD_Entry *currentMenu; // Points to top entry of the current page +OSD_Entry *nextPage; // Only 2 pages are allowed (for now) +uint8_t maxRow; // Max row in a page -int8_t currentCursorPos = 0; -uint8_t currentMenuIdx = 0; -uint16_t *currentElement = NULL; +int8_t cursorRow; + +// Stick/key detection #define IS_HI(X) (rcData[X] > 1750) #define IS_LO(X) (rcData[X] < 1250) @@ -179,18 +185,18 @@ uint16_t *currentElement = NULL; #define BUTTON_TIME 250 // msec #define BUTTON_PAUSE 500 // msec -void cmsUpdateMaxRows(displayPort_t *instance) +void cmsUpdateMaxRow(displayPort_t *instance) { OSD_Entry *ptr; - currentMenuIdx = 0; + maxRow = 0; for (ptr = currentMenu; ptr->type != OME_END; ptr++) - currentMenuIdx++; + maxRow++; - if (currentMenuIdx > MAX_MENU_ITEMS(instance)) - currentMenuIdx = MAX_MENU_ITEMS(instance); + if (maxRow > MAX_MENU_ITEMS(instance)) + maxRow = MAX_MENU_ITEMS(instance); - currentMenuIdx--; + maxRow--; } static void cmsFormatFloat(int32_t value, char *floatString) @@ -220,7 +226,7 @@ static void cmsFormatFloat(int32_t value, char *floatString) floatString[0] = ' '; } -void cmsPad(char *buf, int size) +void cmsPadToSize(char *buf, int size) { int i; @@ -294,8 +300,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr if (IS_PRINTVALUE(p) && p->data) { OSD_UINT8_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPad(buff, 5); - //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, " "); + cmsPadToSize(buff, 5); cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); CLR_PRINTVALUE(p); } @@ -304,7 +309,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr if (IS_PRINTVALUE(p) && p->data) { OSD_INT8_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPad(buff, 5); + cmsPadToSize(buff, 5); cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); CLR_PRINTVALUE(p); } @@ -313,7 +318,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr if (IS_PRINTVALUE(p) && p->data) { OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPad(buff, 5); + cmsPadToSize(buff, 5); cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); CLR_PRINTVALUE(p); } @@ -322,7 +327,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr if (IS_PRINTVALUE(p) && p->data) { OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPad(buff, 5); + cmsPadToSize(buff, 5); cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); CLR_PRINTVALUE(p); } @@ -331,7 +336,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr if (p->data && drawPolled) { OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPad(buff, 5); + cmsPadToSize(buff, 5); cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); // PRINTVALUE not cleared on purpose } @@ -340,7 +345,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr if (IS_PRINTVALUE(p) && p->data) { OSD_FLOAT_t *ptr = p->data; cmsFormatFloat(*ptr->val * ptr->multipler, buff); - cmsPad(buff, 5); + cmsPadToSize(buff, 5); cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ??? CLR_PRINTVALUE(p); } @@ -359,7 +364,7 @@ void cmsDrawMenu(displayPort_t *pDisplay) { uint8_t i; OSD_Entry *p; - uint8_t top = (pDisplay->rows - currentMenuIdx) / 2 - 1; + uint8_t top = (pDisplay->rows - maxRow) / 2 - 1; // Polled (dynamic) value display denominator. // XXX Need to denom based on absolute time @@ -389,19 +394,19 @@ void cmsDrawMenu(displayPort_t *pDisplay) // Cursor manipulation - while ((currentMenu + currentCursorPos)->type == OME_Label) // skip label - currentCursorPos++; + while ((currentMenu + cursorRow)->type == OME_Label) // skip label + cursorRow++; - if (pDisplay->lastCursorPos >= 0 && currentCursorPos != pDisplay->lastCursorPos) { - room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->lastCursorPos + top, " "); + if (pDisplay->cursorRow >= 0 && cursorRow != pDisplay->cursorRow) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, " "); } if (room < 30) return; - if (pDisplay->lastCursorPos != currentCursorPos) { - room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >"); - pDisplay->lastCursorPos = currentCursorPos; + if (pDisplay->cursorRow != cursorRow) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, cursorRow + top, " >"); + pDisplay->cursorRow = cursorRow; } if (room < 30) @@ -441,17 +446,19 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) if (ptr == cmsx_menuRateExpo) cmsx_RateExpoRead(); + // Stack the current menu and move to a new menu. + // The (ptr == curretMenu) case occurs when reopening for display sw + 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; + menuStackHistory[menuStackIdx] = cursorRow; menuStackIdx++; currentMenu = (OSD_Entry *)ptr; - currentCursorPos = 0; + cursorRow = 0; } + displayClear(pDisplay); - cmsUpdateMaxRows(pDisplay); + cmsUpdateMaxRow(pDisplay); } return 0; @@ -473,9 +480,8 @@ long cmsMenuBack(displayPort_t *pDisplay) menuStackIdx--; nextPage = NULL; currentMenu = menuStack[menuStackIdx]; - currentCursorPos = menuStackHistory[menuStackIdx]; - - cmsUpdateMaxRows(pDisplay); + cursorRow = menuStackHistory[menuStackIdx]; + cmsUpdateMaxRow(pDisplay); } return 0; @@ -549,46 +555,42 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } if (key == KEY_DOWN) { - if (currentCursorPos < currentMenuIdx) { - currentCursorPos++; + if (cursorRow < maxRow) { + cursorRow++; } else { - if (nextPage) { // we have more pages + if (nextPage) { // we have another page displayClear(pDisplay); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; - currentCursorPos = 0; - cmsUpdateMaxRows(pDisplay); - } else { - currentCursorPos = 0; + cmsUpdateMaxRow(pDisplay); } + cursorRow = 0; // Goto top in any case } } if (key == KEY_UP) { - currentCursorPos--; + cursorRow--; - if ((currentMenu + currentCursorPos)->type == OME_Label && currentCursorPos > 0) - currentCursorPos--; + if ((currentMenu + cursorRow)->type == OME_Label && cursorRow > 0) + cursorRow--; - if (currentCursorPos == -1 || (currentMenu + currentCursorPos)->type == OME_Label) { + if (cursorRow == -1 || (currentMenu + cursorRow)->type == OME_Label) { if (nextPage) { displayClear(pDisplay); p = nextPage; nextPage = currentMenu; currentMenu = (OSD_Entry *)p; - currentCursorPos = 0; - cmsUpdateMaxRows(pDisplay); - } else { - currentCursorPos = currentMenuIdx; + cmsUpdateMaxRow(pDisplay); } + cursorRow = maxRow; // Goto bottom in any case } } if (key == KEY_DOWN || key == KEY_UP) return res; - p = currentMenu + currentCursorPos; + p = currentMenu + cursorRow; switch (p->type) { case OME_Submenu: @@ -759,7 +761,7 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) //lastCalled = currentTime; - if (key && !currentElement) { + if (key) { rcDelay = cmsHandleKey(¤tDisplay, key); return; } diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 13c52bed2f..b47914d1ae 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -96,6 +96,7 @@ // Configuratoin Menu System #define CMS +#define CMS_MAX_DEVICE 4 // Use external OSD to run CMS #define CANVAS From ca1de7a4d92fd41bc0ab591fb75a6f46cda54b7a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 4 Nov 2016 10:25:44 +0000 Subject: [PATCH 100/188] Split OLED displayport out of dashboard --- Makefile | 1 + src/main/drivers/display.c | 1 + src/main/drivers/display.h | 1 + src/main/io/dashboard.c | 91 ++++----------------------------- src/main/io/dashboard.h | 8 --- src/main/io/displayport_oled.c | 92 ++++++++++++++++++++++++++++++++++ src/main/io/displayport_oled.h | 21 ++++++++ src/main/io/osd.c | 4 +- 8 files changed, 129 insertions(+), 90 deletions(-) create mode 100644 src/main/io/displayport_oled.c create mode 100644 src/main/io/displayport_oled.h diff --git a/Makefile b/Makefile index 5696fd6bf6..19a6ec1db9 100644 --- a/Makefile +++ b/Makefile @@ -562,6 +562,7 @@ HIGHEND_SRC = \ flight/gps_conversion.c \ io/gps.c \ io/ledstrip.c \ + io/displayport_oled.c \ io/dashboard.c \ sensors/sonar.c \ sensors/barometer.c \ diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 2d7ec79de3..9b3810c41b 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -29,6 +29,7 @@ void displayClear(displayPort_t *instance) instance->vTable->clear(instance); instance->cleared = true; instance->cursorRow = -1; + instance->inCMS = false; } void displayOpen(displayPort_t *instance) diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index 60028b7ebe..f0075ff264 100644 --- a/src/main/drivers/display.h +++ b/src/main/drivers/display.h @@ -26,6 +26,7 @@ typedef struct displayPort_s { // CMS state bool cleared; int8_t cursorRow; + bool inCMS; } displayPort_t; typedef struct displayPortVTable_s { diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 7fc7ab9a30..6447bd8eaa 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -31,6 +31,7 @@ #include "build/build_config.h" #include "drivers/system.h" +#include "drivers/display.h" #include "drivers/display_ug2864hsweg01.h" #include "common/printf.h" @@ -53,10 +54,9 @@ #include "flight/failsafe.h" #include "io/cms.h" +#include "io/displayport_oled.h" -#ifdef CMS -void dashboardCmsInit(displayPort_t *pPort); // Forward -#endif +displayPort_t *displayPort; #ifdef GPS #include "io/gps.h" @@ -585,17 +585,14 @@ void showDebugPage(void) } #endif -#ifdef OLEDCMS -static bool dashboardInCMS = false; -#endif - void dashboardUpdate(uint32_t currentTime) { static uint8_t previousArmedState = 0; #ifdef OLEDCMS - if (dashboardInCMS) + if (displayPort && displayPort->inCMS) { return; + } #endif const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L; @@ -703,6 +700,12 @@ void dashboardSetPage(pageId_e pageId) pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; } +void dashboardCmsInit(displayPort_t *displayPortToUse) +{ + displayPort = displayPortToUse; + displayPortOledInit(displayPort); +} + void dashboardInit(rxConfig_t *rxConfigToUse) { delay(200); @@ -749,76 +752,4 @@ void dashboardDisablePageCycling(void) { pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED; } - -#ifdef OLEDCMS -#include "drivers/display.h" - -static int dashboardBegin(displayPort_t *displayPort) -{ - UNUSED(displayPort); - dashboardInCMS = true; - - return 0; -} - -static int dashboardEnd(displayPort_t *displayPort) -{ - UNUSED(displayPort); - dashboardInCMS = false; - - return 0; -} - -static int dashboardClear(displayPort_t *displayPort) -{ - UNUSED(displayPort); - i2c_OLED_clear_display_quick(); - - return 0; -} - -static int dashboardWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s) -{ - UNUSED(displayPort); - i2c_OLED_set_xy(x, y); - i2c_OLED_send_string(s); - - return 0; -} - -static int dashboardHeartbeat(displayPort_t *displayPort) -{ - UNUSED(displayPort); - return 0; -} - -static void dashboardResync(displayPort_t *displayPort) -{ - UNUSED(displayPort); -} - -static uint32_t dashboardTxBytesFree(displayPort_t *displayPort) -{ - UNUSED(displayPort); - return UINT32_MAX; -} - -static const displayPortVTable_t dashboardVTable = { - dashboardBegin, - dashboardEnd, - dashboardClear, - dashboardWrite, - dashboardHeartbeat, - dashboardResync, - dashboardTxBytesFree, -}; - -void dashboardCmsInit(displayPort_t *displayPort) -{ - displayPort->rows = SCREEN_CHARACTER_ROW_COUNT; - displayPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; - displayPort->vTable = &dashboardVTable; -} -#endif // OLEDCMS - #endif // USE_DASHBOARD diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h index 5b98b4c282..cca02effdf 100644 --- a/src/main/io/dashboard.h +++ b/src/main/io/dashboard.h @@ -45,11 +45,3 @@ void dashboardEnablePageCycling(void); void dashboardDisablePageCycling(void); void dashboardResetPageCycling(void); void dashboardSetNextPageChangeAt(uint32_t futureMicros); -void displayEnablePageCycling(void); -void displayDisablePageCycling(void); -void displayResetPageCycling(void); -void displaySetNextPageChangeAt(uint32_t futureMicros); - -#ifdef CMS -//void dashboardCmsInit(displayPort_t *pPort); -#endif diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c new file mode 100644 index 0000000000..2e158a3c20 --- /dev/null +++ b/src/main/io/displayport_oled.c @@ -0,0 +1,92 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef OLEDCMS + +#include "common/utils.h" + +#include "drivers/display.h" +#include "drivers/display_ug2864hsweg01.h" + +#include "io/displayport_oled.h" + +static int oledOpen(displayPort_t *displayPort) +{ + displayPort->inCMS = true; + return 0; +} + +static int oledClose(displayPort_t *displayPort) +{ + displayPort->inCMS = false; + return 0; +} + +static int oledClear(displayPort_t *displayPort) +{ + UNUSED(displayPort); + i2c_OLED_clear_display_quick(); + return 0; +} + +static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s) +{ + UNUSED(displayPort); + i2c_OLED_set_xy(x, y); + i2c_OLED_send_string(s); + return 0; +} + +static int oledHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static void oledResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t oledTxBytesFree(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static const displayPortVTable_t oledVTable = { + .open = oledOpen, + .close = oledClose, + .clear = oledClear, + .write = oledWrite, + .heartbeat = oledHeartbeat, + .resync = oledResync, + .txBytesFree = oledTxBytesFree +}; + +void displayPortOledInit(displayPort_t *displayPort) +{ + displayPort->vTable = &oledVTable; + displayPort->rows = SCREEN_CHARACTER_ROW_COUNT; + displayPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; +} +#endif // OLEDCMS diff --git a/src/main/io/displayport_oled.h b/src/main/io/displayport_oled.h new file mode 100644 index 0000000000..4bd02cf652 --- /dev/null +++ b/src/main/io/displayport_oled.h @@ -0,0 +1,21 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +struct displayPort_s; +void displayPortOledInit(struct displayPort_s *displayPort); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f1b3871e70..820f99d72e 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -29,10 +29,10 @@ #include "platform.h" -#include "build/version.h" - #ifdef OSD +#include "build/version.h" + #include "common/utils.h" #include "drivers/system.h" From 7b1ebcc3f6543aa76d3862cc3aa7254061951263 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 4 Nov 2016 11:44:45 +0000 Subject: [PATCH 101/188] Made some cms functions and variables static --- src/main/io/cms.c | 49 +++++++++++++++++++++++------------------------ 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 807394e389..b34c00eaa3 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -86,10 +86,9 @@ void cmsx_FeatureWriteback(void); #define CMS_MAX_DEVICE 4 #endif -cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE]; -int cmsDeviceCount; -int cmsCurrentDevice = -1; -int cmsLastDevice = -1; +static cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE]; +static int cmsDeviceCount; +static int cmsCurrentDevice = -1; bool cmsDeviceRegister(cmsDeviceInitFuncPtr func) { @@ -101,7 +100,7 @@ bool cmsDeviceRegister(cmsDeviceInitFuncPtr func) return true; } -cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void) +static cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void) { if (cmsDeviceCount == 0) return NULL; @@ -112,7 +111,7 @@ cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void) return cmsDeviceInitFunc[cmsCurrentDevice]; } -cmsDeviceInitFuncPtr cmsDeviceSelectNext(void) +static cmsDeviceInitFuncPtr cmsDeviceSelectNext(void) { if (cmsDeviceCount == 0) return NULL; @@ -124,7 +123,7 @@ cmsDeviceInitFuncPtr cmsDeviceSelectNext(void) #define CMS_UPDATE_INTERVAL 50 // msec -void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) +static void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) { cmsDeviceInitFunc(pDisp); } @@ -150,23 +149,23 @@ void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) #define RIGHT_MENU_COLUMN(p) ((p)->cols - 8) #define MAX_MENU_ITEMS(p) ((p)->rows - 2) -displayPort_t currentDisplay; +static displayPort_t currentDisplay; -bool cmsInMenu = false; +static bool cmsInMenu = false; OSD_Entry menuMain[]; // XXX Does menu backing support backing into second page??? -OSD_Entry *menuStack[10]; // Stack to save menu transition -uint8_t menuStackHistory[10]; // cursorRow in a stacked menu -uint8_t menuStackIdx = 0; +static OSD_Entry *menuStack[10]; // Stack to save menu transition +static uint8_t menuStackHistory[10]; // cursorRow in a stacked menu +static uint8_t menuStackIdx = 0; -OSD_Entry *currentMenu; // Points to top entry of the current page -OSD_Entry *nextPage; // Only 2 pages are allowed (for now) -uint8_t maxRow; // Max row in a page +static OSD_Entry *currentMenu; // Points to top entry of the current page +static OSD_Entry *nextPage; // Only 2 pages are allowed (for now) +static uint8_t maxRow; // Max row in a page -int8_t cursorRow; +static int8_t cursorRow; // Stick/key detection @@ -185,7 +184,7 @@ int8_t cursorRow; #define BUTTON_TIME 250 // msec #define BUTTON_PAUSE 500 // msec -void cmsUpdateMaxRow(displayPort_t *instance) +static void cmsUpdateMaxRow(displayPort_t *instance) { OSD_Entry *ptr; @@ -242,7 +241,7 @@ void cmsPadToSize(char *buf, int size) buf[size] = 0; } -int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool drawPolled) +static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool drawPolled) { char buff[10]; int cnt = 0; @@ -360,7 +359,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr return cnt; } -void cmsDrawMenu(displayPort_t *pDisplay) +static void cmsDrawMenu(displayPort_t *pDisplay) { uint8_t i; OSD_Entry *p; @@ -464,7 +463,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) return 0; } -long cmsMenuBack(displayPort_t *pDisplay) +static 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 @@ -487,7 +486,7 @@ long cmsMenuBack(displayPort_t *pDisplay) return 0; } -void cmsMenuOpen(void) +static void cmsMenuOpen(void) { cmsDeviceInitFuncPtr initfunc; @@ -541,7 +540,7 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr) return 0; } -uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) +static uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) { uint16_t res = BUTTON_TIME; OSD_Entry *p; @@ -714,7 +713,7 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) return res; } -void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) +static void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) { static int16_t rcDelay = BUTTON_TIME; static uint32_t lastCalled = 0; @@ -808,7 +807,7 @@ static char infoTargetName[] = __TARGET__; #include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere -OSD_Entry menuInfo[] = { +static OSD_Entry menuInfo[] = { { "--- INFO ---", OME_Label, NULL, NULL, 0 }, { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 }, { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, @@ -830,7 +829,7 @@ void cmsx_InfoInit(void) // Features -OSD_Entry menuFeatures[] = +static OSD_Entry menuFeatures[] = { {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, {"BLACKBOX", OME_Submenu, cmsMenuChange, cmsx_menuBlackbox, 0}, From 54c4be84199d28755fce114daf705fd5be149e23 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 4 Nov 2016 15:16:04 +0100 Subject: [PATCH 102/188] Correct the motor layout for KISSFC --- src/main/target/KISSFC/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 6c27661b0d..ce4a664a08 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -24,10 +24,10 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel3, DMA1_CH3_HANDLER }, + { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel5, DMA1_CH5_HANDLER }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, From 3cff93e868f2567ea8d9707f7c6e447ca82772ab Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 4 Nov 2016 15:31:04 +0100 Subject: [PATCH 103/188] Rename new LUX to LUXV2 --- .../LUX_RACE/{LUXR_RACE.mk => LUXV2_RACE.mk} | 0 src/main/target/LUX_RACE/target.c | 2 +- src/main/target/LUX_RACE/target.h | 22 +++++++++---------- 3 files changed, 12 insertions(+), 12 deletions(-) rename src/main/target/LUX_RACE/{LUXR_RACE.mk => LUXV2_RACE.mk} (100%) diff --git a/src/main/target/LUX_RACE/LUXR_RACE.mk b/src/main/target/LUX_RACE/LUXV2_RACE.mk similarity index 100% rename from src/main/target/LUX_RACE/LUXR_RACE.mk rename to src/main/target/LUX_RACE/LUXV2_RACE.mk diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 48427a90c3..aa4a6abb5c 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -29,7 +29,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 -#ifndef LUXR_RACE +#ifndef LUXV2_RACE { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index d9b65efd89..52a884261d 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -17,7 +17,7 @@ #pragma once -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define TARGET_BOARD_IDENTIFIER "LUXR" #else #define TARGET_BOARD_IDENTIFIER "LUX" @@ -28,11 +28,11 @@ #define LED0 PC15 #define LED1 PC14 -#ifndef LUXR_RACE +#ifndef LUXV2_RACE #define LED2 PC13 #endif -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define BEEPER PB9 #else #define BEEPER PB13 @@ -50,18 +50,18 @@ #define USE_SPI #define USE_SPI_DEVICE_1 -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define USE_SPI_DEVICE_2 #endif #define SPI1_SCK_PIN PB3 #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 -//#ifndef LUXR_RACE +//#ifndef LUXV2_RACE #define SPI1_NSS_PIN PA4 //#endif -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 @@ -92,7 +92,7 @@ #define MPU6500_SPI_INSTANCE SPI1 #define GYRO -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define USE_GYRO_MPU6000 #define USE_GYRO_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW270_DEG @@ -103,7 +103,7 @@ #endif #define ACC -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define USE_ACC_MPU6000 #define USE_ACC_SPI_MPU6000 #define ACC_MPU6000_ALIGN CW270_DEG @@ -119,7 +119,7 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define USE_UART4 #define USE_UART5 #define SERIAL_PORT_COUNT 6 @@ -156,7 +156,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_FEATURES FEATURE_BLACKBOX #endif @@ -174,7 +174,7 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#ifdef LUXR_RACE +#ifdef LUXV2_RACE #define USABLE_TIMER_CHANNEL_COUNT 5 #else #define USABLE_TIMER_CHANNEL_COUNT 11 From 3b506415c9d2f05f59edb60eaf8d1ce3b4e69e4e Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 4 Nov 2016 16:22:17 +0100 Subject: [PATCH 104/188] REVO. Wrong motor order, fixed. --- src/main/target/REVO/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 66da4ce854..77aa1d7d0e 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -30,7 +30,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT From 8304becff77fbb3f2562d2fce6072c48dc90afe1 Mon Sep 17 00:00:00 2001 From: blckmn Date: Thu, 3 Nov 2016 15:35:37 +1100 Subject: [PATCH 105/188] Adding both AirbotF4 and Revolt targets --- src/main/drivers/accgyro_mpu6500.h | 5 ++-- src/main/drivers/accgyro_spi_mpu6500.c | 5 +++- src/main/drivers/dma.h | 1 + src/main/target/REVO/AIRBOTF4.mk | 0 src/main/target/REVO/REVOLT.mk | 0 src/main/target/REVO/target.h | 40 +++++++++++++++++++++----- src/main/target/REVO/target.mk | 2 ++ 7 files changed, 43 insertions(+), 10 deletions(-) create mode 100644 src/main/target/REVO/AIRBOTF4.mk create mode 100644 src/main/target/REVO/REVOLT.mk diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index 32d0cefcc5..f761c835d3 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -15,9 +15,12 @@ * along with Cleanflight. If not, see . */ +#pragma once + #define MPU6500_WHO_AM_I_CONST (0x70) #define MPU9250_WHO_AM_I_CONST (0x71) #define ICM20608G_WHO_AM_I_CONST (0xAF) +#define ICM20602_WHO_AM_I_CONST (0x12) #define MPU6500_BIT_RESET (0x80) #define MPU6500_BIT_INT_ANYRD_2CLEAR (1 << 4) @@ -25,8 +28,6 @@ #define MPU6500_BIT_I2C_IF_DIS (1 << 4) #define MPU6500_BIT_RAW_RDY_EN (0x01) -#pragma once - bool mpu6500AccDetect(acc_t *acc); bool mpu6500GyroDetect(gyro_t *gyro); diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 9c2b7a45df..bfbba6dce5 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -84,7 +84,10 @@ bool mpu6500SpiDetect(void) mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); - if (tmp == MPU6500_WHO_AM_I_CONST || tmp == MPU9250_WHO_AM_I_CONST || tmp == ICM20608G_WHO_AM_I_CONST) { + if (tmp == MPU6500_WHO_AM_I_CONST || + tmp == MPU9250_WHO_AM_I_CONST || + tmp == ICM20608G_WHO_AM_I_CONST || + tmp == ICM20602_WHO_AM_I_CONST) { return true; } diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index ea2c3484f5..cd6ecc10be 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#pragma once struct dmaChannelDescriptor_s; typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channelDescriptor); diff --git a/src/main/target/REVO/AIRBOTF4.mk b/src/main/target/REVO/AIRBOTF4.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/REVO/REVOLT.mk b/src/main/target/REVO/REVOLT.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 4d73c99355..f3a065f73b 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -17,20 +17,33 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "REVO" - #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) +#if defined(AIRBOTF4) +#define TARGET_BOARD_IDENTIFIER "AIR4" +#define USBD_PRODUCT_STRING "AirbotF4" + +#elif defined(REVOLT) +#define TARGET_BOARD_IDENTIFIER "RVLT" +#define USBD_PRODUCT_STRING "Revolt" + +#else +#define TARGET_BOARD_IDENTIFIER "REVO" #define USBD_PRODUCT_STRING "Revolution" + #ifdef OPBL #define USBD_SERIALNUMBER_STRING "0x8020000" #endif +#endif + #define USE_DSHOT #define LED0 PB5 // Disable LED1, conflicts with AirbotF4/Flip32F4 beeper -//#define LED1 PB4 +#if !defined(AIRBOTF4) +#define LED1 PB4 +#endif #define BEEPER PB4 #define BEEPER_INVERTED @@ -41,19 +54,31 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 + #define ACC #define USE_ACC_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW270_DEG +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG + #define GYRO #define USE_GYRO_SPI_MPU6000 #define ACC_MPU6000_ALIGN CW270_DEG +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU9250_ALIGN CW270_DEG + // MPU6000 interrupts #define USE_EXTI #define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL +#if !defined(AIRBOTF4) && !defined(REVOLT) #define MAG #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW90_DEG @@ -67,6 +92,7 @@ //#define PITOT //#define USE_PITOT_MS4525 //#define MS4525_BUS I2C_DEVICE_EXT +#endif #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 @@ -78,8 +104,8 @@ #define VBUS_SENSING_PIN PC5 #define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 @@ -106,7 +132,7 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define I2C_DEVICE (I2CDEV_1) #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 @@ -126,7 +152,7 @@ #define WS2811_DMA_FLAG DMA_FLAG_TCIF4 #define WS2811_DMA_IT DMA_IT_TCIF4 -#define SENSORS_SET (SENSOR_ACC) +#define SENSORS_SET (SENSOR_ACC) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_BLACKBOX) diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index 2711b19dac..3d8b57a8a6 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -3,5 +3,7 @@ FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c From c9e190fd7a4170de6c2c7349d0119d9c81729e0a Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 5 Nov 2016 06:31:31 +1100 Subject: [PATCH 106/188] Leaving beeper defined so it can be assigned to a motor pin on revo --- src/main/target/REVO/target.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index f3a065f73b..f6517b3632 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -41,12 +41,14 @@ #define LED0 PB5 // Disable LED1, conflicts with AirbotF4/Flip32F4 beeper -#if !defined(AIRBOTF4) -#define LED1 PB4 -#endif - +#if defined(AIRBOTF4) #define BEEPER PB4 #define BEEPER_INVERTED +#else +#define LED1 PB4 +#define BEEPER NONE +#endif + #define INVERTER PC0 // PC0 used as inverter select GPIO #define INVERTER_USART USART1 From ff5c44b4dcd3b72a593a58cf2c90d930f2c43c80 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 5 Nov 2016 06:51:43 +1100 Subject: [PATCH 107/188] Allow any allocation for motors and servos --- src/main/drivers/pwm_output.c | 4 ++-- src/main/drivers/timer.c | 2 +- src/main/drivers/timer.h | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index ea28c3e48e..b98367c53a 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -208,7 +208,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot break; } - const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_MOTOR); + const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY); if (timerHardware == NULL) { /* flag failure and disable ability to arm */ @@ -271,7 +271,7 @@ void servoInit(const servoConfig_t *servoConfig) IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_OUTPUT, RESOURCE_INDEX(servoIndex)); IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP); - const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_SERVO); + const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY); if (timer == NULL) { /* flag failure and disable ability to arm */ diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 77096a1cb7..4a6dad8f48 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -759,7 +759,7 @@ const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { - if (timerHardware[i].usageFlags & flag) { + if (timerHardware[i].usageFlags & flag || flag == 0) { return &timerHardware[i]; } } diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 9a86a92101..f84b15e293 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -55,6 +55,7 @@ typedef uint32_t timCNT_t; #endif typedef enum { + TIM_USE_ANY = 0x0, TIM_USE_PPM = 0x1, TIM_USE_PWM = 0x2, TIM_USE_MOTOR = 0x4, From c4f0bb18b8cf77022fadb128749662d98d21e435 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 5 Nov 2016 10:17:04 +1100 Subject: [PATCH 108/188] Revolt adjustments. LED will be enabled once remapping is turned on for LEDs --- src/main/target/REVO/target.c | 6 +++++- src/main/target/REVO/target.h | 4 ++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 77aa1d7d0e..ef6023948b 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -34,6 +34,10 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT +#ifdef REVOLT + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_LED, 0, GPIO_AF_TIM4, DMA1_Stream0, DMA_Channel_2, DMA1_ST0_HANDLER }, // LED for REVOLT +#else + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT / LED for REVO { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT +#endif }; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index f6517b3632..f5540f713b 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -40,8 +40,8 @@ #define USE_DSHOT #define LED0 PB5 -// Disable LED1, conflicts with AirbotF4/Flip32F4 beeper -#if defined(AIRBOTF4) +// Disable LED1, conflicts with AirbotF4/Flip32F4/Revolt beeper +#if defined(AIRBOTF4) || defined(REVOLT) #define BEEPER PB4 #define BEEPER_INVERTED #else From 4c0273c119fa8ac44daf91bcc17011aea6c232bc Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 5 Nov 2016 11:13:58 +1100 Subject: [PATCH 109/188] Revolt has only 11 usable timers defined. --- src/main/target/REVO/target.h | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index f5540f713b..dacaff8980 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -46,11 +46,12 @@ #define BEEPER_INVERTED #else #define LED1 PB4 +// Leave beeper here but with none as io - so disabled unless mapped. #define BEEPER NONE #endif - -#define INVERTER PC0 // PC0 used as inverter select GPIO +// PC0 used as inverter select GPIO +#define INVERTER PC0 #define INVERTER_USART USART1 #define MPU6000_CS_PIN PA4 @@ -168,7 +169,12 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#ifdef REVOLT +#define USABLE_TIMER_CHANNEL_COUNT 11 +#else #define USABLE_TIMER_CHANNEL_COUNT 12 +#endif + #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) From 1961e50f0be725bfa933f7154e9f0937daa91dba Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 5 Nov 2016 10:11:34 +0900 Subject: [PATCH 110/188] Introduce CMS_Menu --- src/main/io/cms.c | 230 ++++++++++++++++++++++++++----------- src/main/io/cms_blackbox.c | 43 ++++--- src/main/io/cms_blackbox.h | 5 +- src/main/io/cms_imu.c | 81 ++++++++++--- src/main/io/cms_imu.h | 15 +-- src/main/io/cms_ledstrip.c | 60 +++++++--- src/main/io/cms_ledstrip.h | 7 +- src/main/io/cms_osd.h | 4 +- src/main/io/cms_types.h | 21 +++- src/main/io/cms_vtx.c | 76 ++++++------ src/main/io/cms_vtx.h | 8 +- src/main/io/osd.c | 35 +++++- 12 files changed, 400 insertions(+), 185 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 807394e389..85a2b5f9ae 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -19,6 +19,7 @@ Created by Marcin Baliniak OSD-CMS separation by jflyper */ +#define CMS_MENU_DEBUG // For external menu content creators #include #include @@ -76,9 +77,9 @@ #include "io/cms_ledstrip.h" // Forwards -void cmsx_InfoInit(void); -void cmsx_FeatureRead(void); -void cmsx_FeatureWriteback(void); +long cmsx_InfoInit(void); +long cmsx_FeatureRead(void); +long cmsx_FeatureWriteback(void); // Device management @@ -154,19 +155,41 @@ displayPort_t currentDisplay; bool cmsInMenu = false; -OSD_Entry menuMain[]; +CMS_Menu menuMain; +CMS_Menu *currentMenu; // Points to top entry of the current page // XXX Does menu backing support backing into second page??? -OSD_Entry *menuStack[10]; // Stack to save menu transition +CMS_Menu *menuStack[10]; // Stack to save menu transition uint8_t menuStackHistory[10]; // cursorRow in a stacked menu uint8_t menuStackIdx = 0; -OSD_Entry *currentMenu; // Points to top entry of the current page -OSD_Entry *nextPage; // Only 2 pages are allowed (for now) -uint8_t maxRow; // Max row in a page +OSD_Entry *pageTop; // Points to top entry of the current page +OSD_Entry *pageTopAlt; // Only 2 pages are allowed (for now) +uint8_t maxRow; // Max row in the current page -int8_t cursorRow; +int8_t entryPos; // Absolute position of the cursor +int8_t cursorRow; // Position of the cursor relative to pageTop + +// Broken menu substitution + +char menuErrLabel[21 + 1]; + +OSD_Entry menuErrEntries[] = { + { "BROKEN MENU", OME_Label, NULL, NULL, 0 }, + { menuErrLabel, OME_String, NULL, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu menuErr = { + "MENU CONTENT BROKEN", + OME_MENU, + NULL, + NULL, + NULL, + menuErrEntries, +}; // Stick/key detection @@ -190,7 +213,7 @@ void cmsUpdateMaxRow(displayPort_t *instance) OSD_Entry *ptr; maxRow = 0; - for (ptr = currentMenu; ptr->type != OME_END; ptr++) + for (ptr = pageTop; ptr->type != OME_END; ptr++) maxRow++; if (maxRow > MAX_MENU_ITEMS(instance)) @@ -355,6 +378,12 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr case OME_END: case OME_Back: break; + case OME_MENU: +#ifdef CMS_MENU_DEBUG + // Shouldn't happen. Notify creator of this menu content. + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT"); +#endif + break; } return cnt; @@ -373,20 +402,20 @@ void cmsDrawMenu(displayPort_t *pDisplay) uint32_t room = displayTxBytesFree(pDisplay); - if (!currentMenu) + if (!pageTop) return; if (pDisplay->cleared) { - for (p = currentMenu, i= 0; p->type != OME_END; p++, i++) { + for (p = pageTop, i= 0; p->type != OME_END; p++, i++) { SET_PRINTLABEL(p); SET_PRINTVALUE(p); } if (i > MAX_MENU_ITEMS(pDisplay)) // max per page { - nextPage = currentMenu + MAX_MENU_ITEMS(pDisplay); - if (nextPage->type == OME_END) - nextPage = NULL; + pageTopAlt = pageTop + MAX_MENU_ITEMS(pDisplay); + if (pageTopAlt->type == OME_END) + pageTopAlt = NULL; } pDisplay->cleared = false; @@ -394,7 +423,7 @@ void cmsDrawMenu(displayPort_t *pDisplay) // Cursor manipulation - while ((currentMenu + cursorRow)->type == OME_Label) // skip label + while ((pageTop + cursorRow)->type == OME_Label) // skip label cursorRow++; if (pDisplay->cursorRow >= 0 && cursorRow != pDisplay->cursorRow) { @@ -413,7 +442,7 @@ void cmsDrawMenu(displayPort_t *pDisplay) return; // Print text labels - for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { if (IS_PRINTLABEL(p)) { room -= displayWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text); CLR_PRINTLABEL(p); @@ -427,7 +456,7 @@ void cmsDrawMenu(displayPort_t *pDisplay) // XXX Polled values at latter positions in the list may not be // XXX printed if not enough room in the middle of the list. - for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { if (IS_PRINTVALUE(p)) { room -= cmsDrawMenuEntry(pDisplay, p, top + i, drawPolled); if (room < 30) @@ -438,25 +467,39 @@ void cmsDrawMenu(displayPort_t *pDisplay) long cmsMenuChange(displayPort_t *pDisplay, void *ptr) { - if (ptr) { - // XXX (jflyper): This can be avoided by adding pre- and post- - // XXX (or onEnter and onExit) functions? - if (ptr == cmsx_menuPid) - cmsx_PidRead(); - if (ptr == cmsx_menuRateExpo) - cmsx_RateExpoRead(); + CMS_Menu *pMenu = (CMS_Menu *)ptr; + + if (pMenu) { +#ifdef CMS_MENU_DEBUG + if (pMenu->GUARD_type != OME_MENU) { + // ptr isn't pointing to a CMS_Menu. + if (pMenu->GUARD_type < OME_MENU) { + strncpy(menuErrLabel, pMenu->GUARD_text, 21); + } else { + strncpy(menuErrLabel, "LABEL UNKNOWN", 21); + } + pMenu = &menuErr; + } +#endif // Stack the current menu and move to a new menu. - // The (ptr == curretMenu) case occurs when reopening for display sw + // The (pMenu == curretMenu) case occurs when reopening for display sw - if ((OSD_Entry *)ptr != currentMenu) { + if (pMenu != currentMenu) { menuStack[menuStackIdx] = currentMenu; + cursorRow += pageTop - currentMenu->entries; // Convert cursorRow to absolute value menuStackHistory[menuStackIdx] = cursorRow; menuStackIdx++; - currentMenu = (OSD_Entry *)ptr; + + currentMenu = (CMS_Menu *)ptr; cursorRow = 0; + + if (pMenu->onEnter) + pMenu->onEnter(); } + pageTop = currentMenu->entries; + displayClear(pDisplay); cmsUpdateMaxRow(pDisplay); } @@ -466,22 +509,22 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) 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 - if (currentMenu == cmsx_menuPid) - cmsx_PidWriteback(); - - // hack - save rate config for current profile - if (currentMenu == cmsx_menuRateExpo) - cmsx_RateExpoWriteback(); + if (currentMenu->onExit) + currentMenu->onExit(); if (menuStackIdx) { displayClear(pDisplay); menuStackIdx--; - nextPage = NULL; currentMenu = menuStack[menuStackIdx]; cursorRow = menuStackHistory[menuStackIdx]; + pageTop = currentMenu->entries; // Temporary for cmsUpdateMaxRow() cmsUpdateMaxRow(pDisplay); + if (cursorRow > maxRow) { + pageTopAlt = currentMenu->entries; + pageTop = pageTopAlt + maxRow + 1; + cursorRow -= (maxRow + 1); + cmsUpdateMaxRow(pDisplay); + } } return 0; @@ -496,8 +539,7 @@ void cmsMenuOpen(void) cmsInMenu = true; DISABLE_ARMING_FLAG(OK_TO_ARM); initfunc = cmsDeviceSelectCurrent(); - cmsx_FeatureRead(); - currentMenu = &menuMain[0]; + currentMenu = &menuMain; } else { // Switch display displayClose(¤tDisplay); @@ -512,6 +554,18 @@ void cmsMenuOpen(void) cmsMenuChange(¤tDisplay, currentMenu); } +void cmsTraverseGlobalExit(CMS_Menu *pMenu) +{ + OSD_Entry *p; + + for (p = pMenu->entries; p->type != OME_END ; p++) + if (p->type == OME_Submenu) + cmsTraverseGlobalExit(p->data); + + if (pMenu->onGlobalExit) + pMenu->onGlobalExit(); +} + long cmsMenuExit(displayPort_t *pDisplay, void *ptr) { if (ptr) { @@ -524,8 +578,10 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr) stopPwmAllMotors(); delay(200); - // save local variables to configuration - cmsx_FeatureWriteback(); + cmsTraverseGlobalExit(&menuMain); + + if (currentMenu->onExit) + currentMenu->onExit(); } cmsInMenu = false; @@ -558,11 +614,11 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (cursorRow < maxRow) { cursorRow++; } else { - if (nextPage) { // we have another page + if (pageTopAlt) { // we have another page displayClear(pDisplay); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; + p = pageTopAlt; + pageTopAlt = pageTop; + pageTop = (OSD_Entry *)p; cmsUpdateMaxRow(pDisplay); } cursorRow = 0; // Goto top in any case @@ -572,15 +628,15 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (key == KEY_UP) { cursorRow--; - if ((currentMenu + cursorRow)->type == OME_Label && cursorRow > 0) + if ((pageTop + cursorRow)->type == OME_Label && cursorRow > 0) cursorRow--; - if (cursorRow == -1 || (currentMenu + cursorRow)->type == OME_Label) { - if (nextPage) { + if (cursorRow == -1 || (pageTop + cursorRow)->type == OME_Label) { + if (pageTopAlt) { displayClear(pDisplay); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; + p = pageTopAlt; + pageTopAlt = pageTop; + pageTop = (OSD_Entry *)p; cmsUpdateMaxRow(pDisplay); } cursorRow = maxRow; // Goto bottom in any case @@ -590,7 +646,7 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (key == KEY_DOWN || key == KEY_UP) return res; - p = currentMenu + cursorRow; + p = pageTop + cursorRow; switch (p->type) { case OME_Submenu: @@ -710,6 +766,9 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_Label: case OME_END: break; + case OME_MENU: + // Shouldn't happen + break; } return res; } @@ -753,7 +812,7 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) key = KEY_RIGHT; rcDelay = BUTTON_TIME; } - else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != cmsx_menuRc) // this menu is used to check transmitter signals so can't exit using YAW + else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != &cmsx_menuRc) // this menu is used to check transmitter signals so can't exit using YAW { key = KEY_ESC; rcDelay = BUTTON_TIME; @@ -794,7 +853,7 @@ void cmsHandler(uint32_t currentTime) void cmsInit(void) { - cmsx_InfoInit(); + //cmsx_InfoInit(); } // @@ -808,7 +867,7 @@ static char infoTargetName[] = __TARGET__; #include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere -OSD_Entry menuInfo[] = { +OSD_Entry menuInfoEntries[] = { { "--- INFO ---", OME_Label, NULL, NULL, 0 }, { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 }, { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, @@ -818,7 +877,16 @@ OSD_Entry menuInfo[] = { { NULL, OME_END, NULL, NULL, 0 } }; -void cmsx_InfoInit(void) +CMS_Menu menuInfo = { + "MENUINFO", + OME_MENU, + cmsx_InfoInit, + NULL, + NULL, + menuInfoEntries, +}; + +long cmsx_InfoInit(void) { for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') @@ -826,16 +894,18 @@ void cmsx_InfoInit(void) else infoGitRev[i] = shortGitRevision[i]; } + + return 0; } // Features -OSD_Entry menuFeatures[] = +OSD_Entry menuFeaturesEntries[] = { {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, - {"BLACKBOX", OME_Submenu, cmsMenuChange, cmsx_menuBlackbox, 0}, + {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0}, #if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, cmsMenuChange, cmsx_menuVtx, 0}, + {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, #endif // VTX || USE_RTC6705 #ifdef LED_STRIP {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, @@ -844,24 +914,45 @@ OSD_Entry menuFeatures[] = {NULL, OME_END, NULL, NULL, 0} }; +CMS_Menu menuFeatures = { + "MENUFEATURES", + OME_MENU, + NULL, + NULL, + NULL, + menuFeaturesEntries, +}; + // Main -OSD_Entry menuMain[] = +OSD_Entry menuMainEntries[] = { {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, - {"CFG&IMU", OME_Submenu, cmsMenuChange, cmsx_menuImu, 0}, - {"FEATURES", OME_Submenu, cmsMenuChange, menuFeatures, 0}, + {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0}, + {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, #ifdef OSD - {"SCR LAYOUT", OME_Submenu, cmsMenuChange, cmsx_menuOsdLayout, 0}, - {"ALARMS", OME_Submenu, cmsMenuChange, cmsx_menuAlarms, 0}, + {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, #endif - {"FC&FW INFO", OME_Submenu, cmsMenuChange, menuInfo, 0}, + {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, + {"FC&FW INFO1", OME_Submenu, cmsMenuChange, &menuInfo, 0}, + {"FC&FW INFO2", OME_Submenu, cmsMenuChange, &menuInfo, 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) +CMS_Menu menuMain = { + "MENUMAIN", + OME_MENU, + NULL, + NULL, + NULL, + menuMainEntries, +}; + +#if 0 +long cmsx_FeatureRead(void) { cmsx_Blackbox_FeatureRead(); @@ -874,9 +965,11 @@ void cmsx_FeatureRead(void) cmsx_Vtx_FeatureRead(); cmsx_Vtx_ConfigRead(); #endif // VTX || USE_RTC6705 + + return 0; } -void cmsx_FeatureWriteback(void) +long cmsx_FeatureWriteback(void) { cmsx_Blackbox_FeatureWriteback(); @@ -890,6 +983,9 @@ void cmsx_FeatureWriteback(void) #endif // VTX || USE_RTC6705 saveConfigAndNotify(); + + return 0; } +#endif #endif // CMS diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c index 696b1231c4..e3ed288abf 100644 --- a/src/main/io/cms_blackbox.c +++ b/src/main/io/cms_blackbox.c @@ -1,7 +1,7 @@ // // CMS things for blackbox and flashfs. -// Should be part of blackbox.c (or new blackbox/blackbox_cms.c) and io/flashfs.c -// +// + #include #include #include @@ -48,9 +48,26 @@ long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) uint8_t cmsx_FeatureBlackbox; +long cmsx_Blackbox_FeatureRead(void) +{ + cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; + + return 0; +} + +long cmsx_Blackbox_FeatureWriteback(void) +{ + if (cmsx_FeatureBlackbox) + featureSet(FEATURE_BLACKBOX); + else + featureClear(FEATURE_BLACKBOX); + + return 0; +} + OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; -OSD_Entry cmsx_menuBlackbox[] = +OSD_Entry cmsx_menuBlackboxEntries[] = { {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, {"ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0}, @@ -62,16 +79,12 @@ OSD_Entry cmsx_menuBlackbox[] = {NULL, OME_END, NULL, NULL, 0} }; -void cmsx_Blackbox_FeatureRead(void) -{ - cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; -} - -void cmsx_Blackbox_FeatureWriteback(void) -{ - if (cmsx_FeatureBlackbox) - featureSet(FEATURE_BLACKBOX); - else - featureClear(FEATURE_BLACKBOX); -} +CMS_Menu cmsx_menuBlackbox = { + "MENUBB", + OME_MENU, + cmsx_Blackbox_FeatureRead, + NULL, + cmsx_Blackbox_FeatureWriteback, + cmsx_menuBlackboxEntries, +}; #endif diff --git a/src/main/io/cms_blackbox.h b/src/main/io/cms_blackbox.h index 6e7d02eff2..11f8b1b450 100644 --- a/src/main/io/cms_blackbox.h +++ b/src/main/io/cms_blackbox.h @@ -1,4 +1 @@ -extern OSD_Entry cmsx_menuBlackbox[]; - -void cmsx_Blackbox_FeatureRead(void); -void cmsx_Blackbox_FeatureWriteback(void); +extern CMS_Menu cmsx_menuBlackbox; diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c index 9428d993d6..5b6d742f4e 100644 --- a/src/main/io/cms_imu.c +++ b/src/main/io/cms_imu.c @@ -47,7 +47,7 @@ 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) +long cmsx_PidRead(void) { uint8_t i; @@ -59,9 +59,11 @@ void cmsx_PidRead(void) 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]; + + return 0; } -void cmsx_PidWriteback(void) +long cmsx_PidWriteback(void) { uint8_t i; @@ -74,9 +76,11 @@ void cmsx_PidWriteback(void) 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]; + + return 0; } -OSD_Entry cmsx_menuPid[] = +OSD_Entry cmsx_menuPidEntries[] = { {"--- PID ---", OME_Label, NULL, NULL, 0}, {"ROLL P", OME_UINT8, NULL, &entryRollP, 0}, @@ -95,6 +99,15 @@ OSD_Entry cmsx_menuPid[] = {NULL, OME_END, NULL, NULL, 0} }; +CMS_Menu cmsx_menuPid = { + "MENUPID", + OME_MENU, + cmsx_PidRead, + cmsx_PidWriteback, + NULL, + cmsx_menuPidEntries, +}; + // // Rate & Expo // @@ -112,17 +125,21 @@ 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() +long cmsx_RateExpoRead(void) { memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); + + return 0; } -void cmsx_RateExpoWriteback() +long cmsx_RateExpoWriteback(void) { memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); + + return 0; } -OSD_Entry cmsx_menuRateExpo[] = +OSD_Entry cmsx_menuRateExpoEntries[] = { {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0}, {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0}, @@ -140,6 +157,16 @@ OSD_Entry cmsx_menuRateExpo[] = {NULL, OME_END, NULL, NULL, 0} }; +CMS_Menu cmsx_menuRateExpo = { + "MENURTEX", + OME_MENU, + cmsx_RateExpoRead, + cmsx_RateExpoWriteback, + NULL, + cmsx_menuRateExpoEntries, +}; + + // // RC preview // @@ -152,7 +179,7 @@ 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 cmsx_menuRc[] = +OSD_Entry cmsx_menuRcEntries[] = { {"--- RC PREV ---", OME_Label, NULL, NULL, 0}, {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0}, @@ -167,6 +194,16 @@ OSD_Entry cmsx_menuRc[] = {NULL, OME_END, NULL, NULL, 0} }; +CMS_Menu cmsx_menuRc = { + "MENURC", + OME_MENU, + NULL, + NULL, + NULL, + cmsx_menuRcEntries, +}; + + // // Misc // @@ -178,7 +215,7 @@ OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; -OSD_Entry menuImuMisc[]= +OSD_Entry menuImuMiscEntries[]= { {"--- MISC ---", OME_Label, NULL, NULL, 0}, {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, @@ -192,15 +229,33 @@ OSD_Entry menuImuMisc[]= {NULL, OME_END, NULL, NULL, 0} }; -OSD_Entry cmsx_menuImu[] = +CMS_Menu menuImuMisc = { + "MENUIMU", + OME_MENU, + NULL, + NULL, + NULL, + menuImuMiscEntries, +}; + +OSD_Entry cmsx_menuImuEntries[] = { {"--- CFG.IMU ---", OME_Label, NULL, NULL, 0}, {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, - {"PID", OME_Submenu, cmsMenuChange, cmsx_menuPid, 0}, - {"RATE&RXPO", OME_Submenu, cmsMenuChange, cmsx_menuRateExpo, 0}, - {"RC PREV", OME_Submenu, cmsMenuChange, cmsx_menuRc, 0}, - {"MISC", OME_Submenu, cmsMenuChange, menuImuMisc, 0}, + {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, + {"RATE&RXPO", OME_Submenu, cmsMenuChange, &cmsx_menuRateExpo, 0}, + {"RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRc, 0}, + {"MISC", OME_Submenu, cmsMenuChange, &menuImuMisc, 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; + +CMS_Menu cmsx_menuImu = { + "MENUIMU", + OME_MENU, + NULL, + NULL, + NULL, + cmsx_menuImuEntries, +}; #endif // CMS diff --git a/src/main/io/cms_imu.h b/src/main/io/cms_imu.h index b78c6ddb4b..0a05ed8daa 100644 --- a/src/main/io/cms_imu.h +++ b/src/main/io/cms_imu.h @@ -1,13 +1,4 @@ -extern OSD_Entry cmsx_menuImu[]; - -// All of below should be gone. - -extern OSD_Entry cmsx_menuPid[]; -extern OSD_Entry cmsx_menuRc[]; -extern OSD_Entry cmsx_menuRateExpo[]; - -void cmsx_PidRead(void); -void cmsx_PidWriteback(void); -void cmsx_RateExpoRead(void); -void cmsx_RateExpoWriteback(void); +extern CMS_Menu cmsx_menuImu; +// This should be gone +extern CMS_Menu cmsx_menuRc; diff --git a/src/main/io/cms_ledstrip.c b/src/main/io/cms_ledstrip.c index 63e15d74e6..62d473055c 100644 --- a/src/main/io/cms_ledstrip.c +++ b/src/main/io/cms_ledstrip.c @@ -76,7 +76,39 @@ uint8_t cmsx_FeatureLedstrip; OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; -OSD_Entry cmsx_menuLedstrip[] = +long cmsx_Ledstrip_FeatureRead(void) +{ + cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; + + return 0; +} + +long cmsx_Ledstrip_FeatureWriteback(void) +{ + if (cmsx_FeatureLedstrip) + featureSet(FEATURE_LED_STRIP); + else + featureClear(FEATURE_LED_STRIP); + + return 0; +} + +long cmsx_Ledstrip_ConfigRead(void) +{ + cmsx_GetLedColor(); + + return 0; +} + +long cmsx_Ledstrip_onEnter(void) +{ + cmsx_Ledstrip_FeatureRead(); + cmsx_Ledstrip_ConfigRead(); + + return 0; +} + +OSD_Entry cmsx_menuLedstripEntries[] = { {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, {"ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0}, @@ -85,23 +117,13 @@ OSD_Entry cmsx_menuLedstrip[] = {NULL, OME_END, NULL, NULL, 0} }; -void cmsx_Ledstrip_FeatureRead(void) -{ - cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; -} - -void cmsx_Ledstrip_FeatureWriteback(void) -{ - if (cmsx_FeatureLedstrip) - featureSet(FEATURE_LED_STRIP); - else - featureClear(FEATURE_LED_STRIP); -} - -void cmsx_Ledstrip_ConfigRead(void) -{ - cmsx_GetLedColor(); -} - +CMS_Menu cmsx_menuLedstrip = { + "MENULED", + OME_MENU, + cmsx_Ledstrip_onEnter, + NULL, + cmsx_Ledstrip_FeatureWriteback, + cmsx_menuLedstripEntries, +}; #endif // LED_STRIP #endif // CMS diff --git a/src/main/io/cms_ledstrip.h b/src/main/io/cms_ledstrip.h index 1ce1d5009f..52081b0177 100644 --- a/src/main/io/cms_ledstrip.h +++ b/src/main/io/cms_ledstrip.h @@ -1,6 +1 @@ -extern OSD_Entry cmsx_menuLedstrip[]; - -void cmsx_Ledstrip_FeatureRead(void); -void cmsx_Ledstrip_FeatureWriteback(void); - -void cmsx_Ledstrip_ConfigRead(void); +extern CMS_Menu cmsx_menuLedstrip; diff --git a/src/main/io/cms_osd.h b/src/main/io/cms_osd.h index 5c93a65215..9e780ecc4e 100644 --- a/src/main/io/cms_osd.h +++ b/src/main/io/cms_osd.h @@ -1,2 +1,2 @@ -extern OSD_Entry cmsx_menuAlarms[]; -extern OSD_Entry cmsx_menuOsdLayout[]; +extern CMS_Menu cmsx_menuAlarms; +extern CMS_Menu cmsx_menuOsdLayout; diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index 973b6610b7..6a051de382 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -5,7 +5,7 @@ #pragma once -typedef long (*OSDMenuFuncPtr)(displayPort_t *, void *); +typedef long (*CMSEntryFuncPtr)(displayPort_t *, void *); //type of elements @@ -29,13 +29,16 @@ typedef enum #endif OME_TAB, OME_END, + + // Debug aid + OME_MENU } OSD_MenuElement; typedef struct { char *text; OSD_MenuElement type; - OSDMenuFuncPtr func; + CMSEntryFuncPtr func; void *data; uint8_t flags; } OSD_Entry; @@ -52,6 +55,20 @@ typedef struct #define SET_PRINTLABEL(p) { (p)->flags |= PRINT_LABEL; } #define CLR_PRINTLABEL(p) { (p)->flags &= ~PRINT_LABEL; } +typedef long (*CMSMenuFuncPtr)(void); + +typedef struct +{ + // These two are debug aids for menu content creators. + char *GUARD_text; + OSD_MenuElement GUARD_type; + + CMSMenuFuncPtr onEnter; + CMSMenuFuncPtr onExit; + CMSMenuFuncPtr onGlobalExit; + OSD_Entry *entries; +} CMS_Menu; + typedef struct { uint8_t *val; diff --git a/src/main/io/cms_vtx.c b/src/main/io/cms_vtx.c index f133f4e7f1..1bd2b49e33 100644 --- a/src/main/io/cms_vtx.c +++ b/src/main/io/cms_vtx.c @@ -20,39 +20,6 @@ uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; -static const char * const vtxBandNames[] = { - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; - -OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; -OSD_UINT8_t entryVtxChannel = {&cmsx_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 cmsx_menuVtx[] = -{ - {"--- VTX ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0}, -#ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, -#endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, -#ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0}, -#endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - void cmsx_Vtx_FeatureRead(void) { cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; @@ -66,6 +33,17 @@ void cmsx_Vtx_FeatureWriteback(void) featureClear(FEATURE_VTX); } +static const char * const vtxBandNames[] = { + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; +OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; + void cmsx_Vtx_ConfigRead(void) { #ifdef VTX @@ -91,6 +69,36 @@ void cmsx_Vtx_ConfigWriteback(void) #endif // USE_RTC6705 } -#endif // VTX || USE_RTC6705 +#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 cmsx_menuVtxEntries[] = +{ + {"--- VTX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0}, +#ifdef VTX + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, +#endif // VTX + {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, +#ifdef USE_RTC6705 + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0}, +#endif // USE_RTC6705 + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuVtx = { + "MENUVTX", + OME_MENU, + NULL, + NULL, + NULL, + cmsx_menuVtxEntries, +}; + +#endif // VTX || USE_RTC6705 #endif // CMS diff --git a/src/main/io/cms_vtx.h b/src/main/io/cms_vtx.h index 412a9e4f73..f0541c0370 100644 --- a/src/main/io/cms_vtx.h +++ b/src/main/io/cms_vtx.h @@ -1,7 +1 @@ -extern OSD_Entry cmsx_menuVtx[]; - -void cmsx_Vtx_FeatureRead(void); -void cmsx_Vtx_FeatureWriteback(void); - -void cmsx_Vtx_ConfigRead(void); -void cmsx_Vtx_ConfigWriteback(void); +extern CMS_Menu cmsx_menuVtx; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f1b3871e70..59041a3e69 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -663,7 +663,7 @@ OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000 OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1}; OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1}; -OSD_Entry cmsx_menuAlarms[] = +OSD_Entry cmsx_menuAlarmsEntries[] = { {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0}, @@ -674,7 +674,16 @@ OSD_Entry cmsx_menuAlarms[] = {NULL, OME_END, NULL, NULL, 0} }; -OSD_Entry menuOsdActiveElems[] = +CMS_Menu cmsx_menuAlarms = { + "MENUALARMS", + OME_MENU, + NULL, + NULL, + NULL, + cmsx_menuAlarmsEntries, +}; + +OSD_Entry menuOsdActiveElemsEntries[] = { {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, @@ -700,12 +709,30 @@ OSD_Entry menuOsdActiveElems[] = {NULL, OME_END, NULL, NULL, 0} }; -OSD_Entry cmsx_menuOsdLayout[] = +CMS_Menu menuOsdActiveElems = { + "MENUOSDACT", + OME_MENU, + NULL, + NULL, + NULL, + menuOsdActiveElemsEntries, +}; + +OSD_Entry cmsx_menuOsdLayoutEntries[] = { {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, - {"ACTIVE ELEM.", OME_Submenu, cmsMenuChange, &menuOsdActiveElems[0], 0}, + {"ACTIVE ELEM", OME_Submenu, cmsMenuChange, &menuOsdActiveElems, 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; +CMS_Menu cmsx_menuOsdLayout = { + "MENULAYOUT", + OME_MENU, + NULL, + NULL, + NULL, + cmsx_menuOsdLayoutEntries, +}; + #endif // OSD From e56f915018c1693e0ce7504d0c4aae4898078aa6 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 5 Nov 2016 13:35:20 +1100 Subject: [PATCH 111/188] Moving LED strip to be configurable via resource command --- src/main/config/config_master.h | 7 +- src/main/drivers/light_ws2811strip.c | 5 +- src/main/drivers/light_ws2811strip.h | 6 +- src/main/drivers/light_ws2811strip_hal.c | 65 +++++------ .../drivers/light_ws2811strip_stm32f10x.c | 53 +++++---- .../drivers/light_ws2811strip_stm32f30x.c | 73 +++++++------ .../drivers/light_ws2811strip_stm32f4xx.c | 101 ++++++++++-------- src/main/drivers/pwm_output_stm32f3xx.c | 1 - src/main/drivers/pwm_output_stm32f4xx.c | 1 + src/main/drivers/timer.c | 4 +- src/main/drivers/timer.h | 5 +- src/main/drivers/timer_hal.c | 15 +++ src/main/fc/config.c | 34 ++++-- src/main/fc/fc_msp.c | 14 +-- src/main/io/ledstrip.c | 60 ++++++----- src/main/io/ledstrip.h | 12 ++- src/main/io/serial_cli.c | 37 ++++--- src/main/main.c | 2 +- 18 files changed, 281 insertions(+), 214 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4caf57f1e0..dc5912e80f 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -161,12 +161,7 @@ typedef struct master_s { #endif #ifdef LED_STRIP - ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; - hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; - modeColorIndexes_t modeColors[LED_MODE_COUNT]; - specialColorIndexes_t specialColors; - uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on - rc_alias_e ledstrip_aux_channel; + ledStripConfig_t ledStripConfig; #endif #ifdef TRANSPONDER diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index c7110799c9..149c9b6129 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -37,6 +37,7 @@ #include "common/color.h" #include "common/colorconversion.h" #include "dma.h" +#include "io.h" #include "light_ws2811strip.h" #if defined(STM32F4) || defined(STM32F7) @@ -84,10 +85,10 @@ void setStripColors(const hsvColor_t *colors) } } -void ws2811LedStripInit(void) +void ws2811LedStripInit(ioTag_t ioTag) { memset(&ledStripDMABuffer, 0, WS2811_DMA_BUFFER_SIZE); - ws2811LedStripHardwareInit(); + ws2811LedStripHardwareInit(ioTag); ws2811UpdateStrip(); } diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index c980353abb..c81be02795 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -17,6 +17,8 @@ #pragma once +#include "io.h" + #define WS2811_LED_STRIP_LENGTH 32 #define WS2811_BITS_PER_LED 24 #define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay @@ -36,9 +38,9 @@ #define BIT_COMPARE_0 9 // timer compare value for logical 0 #endif -void ws2811LedStripInit(void); +void ws2811LedStripInit(ioTag_t ioTag); -void ws2811LedStripHardwareInit(void); +void ws2811LedStripHardwareInit(ioTag_t ioTag); void ws2811LedStripDMAEnable(void); void ws2811UpdateStrip(void); diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index 28bafabbf7..5d8e4cd0c2 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -31,26 +31,15 @@ #include "rcc.h" #include "timer.h" -#if !defined(WS2811_PIN) -#define WS2811_PIN PA0 -#define WS2811_TIMER TIM5 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_IT DMA_IT_TCIF2 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_TIMER_GPIO_AF GPIO_AF2_TIM5 -#endif - static IO_t ws2811IO = IO_NONE; -static uint16_t timDMASource = 0; bool ws2811Initialised = false; static TIM_HandleTypeDef TimHandle; +static uint16_t timerChannel = 0; void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) { - if(htim->Instance==WS2811_TIMER) + if(htim->Instance == TimHandle.Instance) { //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL); ws2811LedDataTransferInProgress = 0; @@ -62,9 +51,20 @@ void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) HAL_DMA_IRQHandler(TimHandle.hdma[descriptor->userParam]); } -void ws2811LedStripHardwareInit(void) +void ws2811LedStripHardwareInit(ioTag_t ioTag) { - TimHandle.Instance = WS2811_TIMER; + if (!ioTag) { + return; + } + + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + TIM_TypeDef *timer = timerHardware->tim; + timerChannel = timerHardware->channel; + + if (timerHardware->dmaStream == NULL) { + return; + } + TimHandle.Instance = timer; TimHandle.Init.Prescaler = 1; TimHandle.Init.Period = 135; // 800kHz @@ -78,16 +78,14 @@ void ws2811LedStripHardwareInit(void) static DMA_HandleTypeDef hdma_tim; - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); - /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + ws2811IO = IOGetByTag(ioTag); IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); - IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), WS2811_TIMER_GPIO_AF); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction); __DMA1_CLK_ENABLE(); - /* Set the parameters to be configured */ - hdma_tim.Init.Channel = WS2811_DMA_CHANNEL; + hdma_tim.Init.Channel = timerHardware->dmaChannel; hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; hdma_tim.Init.MemInc = DMA_MINC_ENABLE; @@ -101,30 +99,17 @@ void ws2811LedStripHardwareInit(void) hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; /* Set hdma_tim instance */ - hdma_tim.Instance = WS2811_DMA_STREAM; + hdma_tim.Instance = timerHardware->dmaStream; - switch (WS2811_TIMER_CHANNEL) { - case TIM_CHANNEL_1: - timDMASource = TIM_DMA_ID_CC1; - break; - case TIM_CHANNEL_2: - timDMASource = TIM_DMA_ID_CC2; - break; - case TIM_CHANNEL_3: - timDMASource = TIM_DMA_ID_CC3; - break; - case TIM_CHANNEL_4: - timDMASource = TIM_DMA_ID_CC4; - break; - } + uint16_t dmaSource = timerDmaSource(timerChannel); /* Link hdma_tim to hdma[x] (channelx) */ - __HAL_LINKDMA(&TimHandle, hdma[timDMASource], hdma_tim); + __HAL_LINKDMA(&TimHandle, hdma[dmaSource], hdma_tim); - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, timDMASource); + dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaSource); /* Initialize TIMx DMA handle */ - if(HAL_DMA_Init(TimHandle.hdma[timDMASource]) != HAL_OK) + if(HAL_DMA_Init(TimHandle.hdma[dmaSource]) != HAL_OK) { /* Initialization Error */ return; @@ -140,7 +125,7 @@ void ws2811LedStripHardwareInit(void) TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; - if(HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, WS2811_TIMER_CHANNEL) != HAL_OK) + if(HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, timerChannel) != HAL_OK) { /* Configuration Error */ return; @@ -160,7 +145,7 @@ void ws2811LedStripDMAEnable(void) return; } - if( HAL_TIM_PWM_Start_DMA(&TimHandle, WS2811_TIMER_CHANNEL, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) + if( HAL_TIM_PWM_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { /* Starting PWM generation Error */ ws2811LedDataTransferInProgress = 0; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 911b7cdc66..3ccfafe2fa 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -30,8 +30,13 @@ #include "rcc.h" #include "timer.h" +#define WS2811_TIMER_HZ 24000000 +#define WS2811_TIMER_PERIOD 29 + static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; +static DMA_Channel_TypeDef *dmaChannel = NULL; +static TIM_TypeDef *timer = NULL; static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { @@ -41,32 +46,38 @@ static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { } } -void ws2811LedStripHardwareInit(void) +void ws2811LedStripHardwareInit(ioTag_t ioTag) { + if (!ioTag) { + return; + } + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; - uint16_t prescalerValue; + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + timer = timerHardware->tim; - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + if (timerHardware->dmaChannel == NULL) { + return; + } ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); -/* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); IOConfigGPIO(ws2811IO, IO_CONFIG(GPIO_Speed_50MHz, GPIO_Mode_AF_PP)); RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); /* Compute the prescaler value */ - prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; + uint16_t prescalerValue = (uint16_t) (SystemCoreClock / WS2811_TIMER_HZ) - 1; /* Time base configuration */ TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz + TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ TIM_OCStructInit(&TIM_OCInitStructure); @@ -74,20 +85,17 @@ void ws2811LedStripHardwareInit(void) TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - TIM_OC1Init(TIM3, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); + TIM_OC1Init(timer, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable); - TIM_CtrlPWMOutputs(TIM3, ENABLE); + TIM_CtrlPWMOutputs(timer, ENABLE); /* configure DMA */ - /* DMA clock enable */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); - /* DMA1 Channel6 Config */ - DMA_DeInit(DMA1_Channel6); + DMA_DeInit(dmaChannel); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR1; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; @@ -99,12 +107,13 @@ void ws2811LedStripHardwareInit(void) DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel6, &DMA_InitStructure); + DMA_Init(dmaChannel, &DMA_InitStructure); /* TIM3 CC1 DMA Request enable */ - TIM_DMACmd(TIM3, TIM_DMA_CC1, ENABLE); + TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE); + DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); + dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; @@ -117,10 +126,10 @@ void ws2811LedStripDMAEnable(void) if (!ws2811Initialised) return; - DMA_SetCurrDataCounter(DMA1_Channel6, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(TIM3, 0); - TIM_Cmd(TIM3, ENABLE); - DMA_Cmd(DMA1_Channel6, ENABLE); + DMA_SetCurrDataCounter(dmaChannel, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(timer, 0); + TIM_Cmd(timer, ENABLE); + DMA_Cmd(dmaChannel, ENABLE); } #endif diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8a0b073c78..1e8c7a8062 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -31,16 +31,13 @@ #include "rcc.h" #include "timer.h" -#ifndef WS2811_PIN -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 -#endif +#define WS2811_TIMER_HZ 24000000 +#define WS2811_TIMER_PERIOD 29 static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; +static DMA_Channel_TypeDef *dmaChannel = NULL; +static TIM_TypeDef *timer = NULL; static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { @@ -50,51 +47,66 @@ static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { } } -void ws2811LedStripHardwareInit(void) +void ws2811LedStripHardwareInit(ioTag_t ioTag) { + if (!ioTag) { + return; + } + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; - uint16_t prescalerValue; + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + timer = timerHardware->tim; - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + if (timerHardware->dmaChannel == NULL) { + return; + } ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); - /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); - IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), WS2811_TIMER_GPIO_AF); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); - RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + RCC_ClockCmd(timerRCC(timer), ENABLE); /* Compute the prescaler value */ - prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; + uint16_t prescalerValue = (uint16_t) (SystemCoreClock / WS2811_TIMER_HZ) - 1; + /* Time base configuration */ TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz + TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); /* PWM1 Mode configuration */ TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + } TIM_OCInitStructure.TIM_Pulse = 0; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + TIM_OC1Init(timer, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable); + TIM_CtrlPWMOutputs(timer, ENABLE); - TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE); + dmaChannel = timerHardware->dmaChannel; /* configure DMA */ /* DMA1 Channel Config */ - DMA_DeInit(WS2811_DMA_CHANNEL); + DMA_DeInit(dmaChannel); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&WS2811_TIMER->CCR1; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; @@ -106,11 +118,12 @@ void ws2811LedStripHardwareInit(void) DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(WS2811_DMA_CHANNEL, &DMA_InitStructure); + DMA_Init(dmaChannel, &DMA_InitStructure); - TIM_DMACmd(WS2811_TIMER, TIM_DMA_CC1, ENABLE); + TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE); + DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); + dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; @@ -123,10 +136,10 @@ void ws2811LedStripDMAEnable(void) if (!ws2811Initialised) return; - DMA_SetCurrDataCounter(WS2811_DMA_CHANNEL, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(WS2811_TIMER, 0); - TIM_Cmd(WS2811_TIMER, ENABLE); - DMA_Cmd(WS2811_DMA_CHANNEL, ENABLE); + DMA_SetCurrDataCounter(dmaChannel, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(timer, 0); + TIM_Cmd(timer, ENABLE); + DMA_Cmd(dmaChannel, ENABLE); } #endif diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 46bcdd0e46..4fd38325ee 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -31,85 +31,93 @@ #include "rcc.h" #include "timer.h" #include "timer_stm32f4xx.h" +#include "io.h" -#if !defined(WS2811_PIN) -#define WS2811_PIN PA0 -#define WS2811_TIMER TIM5 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5 -#endif +#define WS2811_TIMER_HZ 84000000 +#define WS2811_TIMER_PERIOD 104 static IO_t ws2811IO = IO_NONE; -static uint16_t timDMASource = 0; bool ws2811Initialised = false; +static DMA_Stream_TypeDef *stream = NULL; +static TIM_TypeDef *timer = NULL; static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { ws2811LedDataTransferInProgress = 0; DMA_Cmd(descriptor->stream, DISABLE); - TIM_DMACmd(WS2811_TIMER, timDMASource, DISABLE); DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); } } -void ws2811LedStripHardwareInit(void) +void ws2811LedStripHardwareInit(ioTag_t ioTag) { + if (!ioTag) { + return; + } + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; - uint16_t prescalerValue; + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + timer = timerHardware->tim; - RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + if (timerHardware->dmaStream == NULL) { + return; + } - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + RCC_ClockCmd(timerRCC(timer), ENABLE); + + ws2811IO = IOGetByTag(ioTag); /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); - IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), WS2811_TIMER_GPIO_AF); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); // Stop timer - TIM_Cmd(WS2811_TIMER, DISABLE); + TIM_Cmd(timer, DISABLE); /* Compute the prescaler value */ - prescalerValue = (uint16_t)(SystemCoreClock / 2 / 84000000) - 1; + uint16_t prescalerValue = (uint16_t)(SystemCoreClock / timerClockDivisor(timer) / WS2811_TIMER_HZ) - 1; /* Time base configuration */ - TIM_TimeBaseStructure.TIM_Period = 104; // 800kHz + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ + TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + } TIM_OCInitStructure.TIM_Pulse = 0; - timerOCInit(WS2811_TIMER, WS2811_TIMER_CHANNEL, &TIM_OCInitStructure); - timerOCPreloadConfig(WS2811_TIMER, WS2811_TIMER_CHANNEL, TIM_OCPreload_Enable); - timDMASource = timerDmaSource(WS2811_TIMER_CHANNEL); + timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); + timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); - TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE); - TIM_ARRPreloadConfig(WS2811_TIMER, ENABLE); + TIM_CtrlPWMOutputs(timer, ENABLE); + TIM_ARRPreloadConfig(timer, ENABLE); - TIM_CCxCmd(WS2811_TIMER, WS2811_TIMER_CHANNEL, TIM_CCx_Enable); - TIM_Cmd(WS2811_TIMER, ENABLE); + TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); + TIM_Cmd(timer, ENABLE); + stream = timerHardware->dmaStream; /* configure DMA */ - DMA_Cmd(WS2811_DMA_STREAM, DISABLE); - DMA_DeInit(WS2811_DMA_STREAM); + DMA_Cmd(stream, DISABLE); + DMA_DeInit(stream); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_Channel = WS2811_DMA_CHANNEL; - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(WS2811_TIMER, WS2811_TIMER_CHANNEL); + DMA_InitStructure.DMA_Channel = timerHardware->dmaChannel; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; @@ -124,14 +132,15 @@ void ws2811LedStripHardwareInit(void) DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(WS2811_DMA_STREAM, &DMA_InitStructure); + DMA_Init(stream, &DMA_InitStructure); + TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - DMA_ITConfig(WS2811_DMA_STREAM, DMA_IT_TC, ENABLE); - DMA_ClearITPendingBit(WS2811_DMA_STREAM, dmaFlag_IT_TCIF(WS2811_DMA_STREAM)); + DMA_ITConfig(stream, DMA_IT_TC, ENABLE); + DMA_ClearITPendingBit(stream, dmaFlag_IT_TCIF(stream)); - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - const hsvColor_t hsv_white = { 0, 255, 255}; + const hsvColor_t hsv_white = { 0, 255, 255 }; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); @@ -142,10 +151,10 @@ void ws2811LedStripDMAEnable(void) if (!ws2811Initialised) return; - DMA_SetCurrDataCounter(WS2811_DMA_STREAM, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(WS2811_TIMER, 0); - DMA_Cmd(WS2811_DMA_STREAM, ENABLE); - TIM_DMACmd(WS2811_TIMER, timDMASource, ENABLE); + DMA_SetCurrDataCounter(stream, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(timer, 0); + TIM_Cmd(timer, ENABLE); + DMA_Cmd(stream, ENABLE); } #endif diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 5e7b8f7582..8a14161062 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -156,7 +156,6 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } - TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index e2bf84e12c..5e8287cfa8 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -146,6 +146,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); } + TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 77096a1cb7..c62739f20a 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -810,6 +810,7 @@ volatile timCCR_t* timerCCR(TIM_TypeDef *tim, uint8_t channel) return (volatile timCCR_t*)((volatile char*)&tim->CCR1 + channel); } +#ifndef USE_HAL_DRIVER uint16_t timerDmaSource(uint8_t channel) { switch (channel) { @@ -823,4 +824,5 @@ uint16_t timerDmaSource(uint8_t channel) return TIM_DMA_CC4; } return 0; -} \ No newline at end of file +} +#endif diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 9a86a92101..689602e809 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -55,6 +55,7 @@ typedef uint32_t timCNT_t; #endif typedef enum { + TIM_USE_ANY = 0x0, TIM_USE_PPM = 0x1, TIM_USE_PWM = 0x2, TIM_USE_MOTOR = 0x4, @@ -92,11 +93,11 @@ typedef struct timerHardware_s { #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) uint8_t alternateFunction; #endif -#if defined(USE_DSHOT) +#if defined(USE_DSHOT) || defined(LED_STRIP) #if defined(STM32F4) || defined(STM32F7) DMA_Stream_TypeDef *dmaStream; uint32_t dmaChannel; -#elif defined(STM32F3) +#elif defined(STM32F3) || defined(STM32F1) DMA_Channel_TypeDef *dmaChannel; #endif uint8_t dmaIrqHandler; diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 8268bd3497..cb9ef8bf52 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -867,3 +867,18 @@ const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) } return NULL; } + +uint16_t timerDmaSource(uint8_t channel) +{ + switch (channel) { + case TIM_CHANNEL_1: + return TIM_DMA_ID_CC1; + case TIM_CHANNEL_2: + return TIM_DMA_ID_CC2; + case TIM_CHANNEL_3: + return TIM_DMA_ID_CC3; + case TIM_CHANNEL_4: + return TIM_DMA_ID_CC4; + } + return 0; +} diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c4668bf077..296df6570a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -42,6 +42,7 @@ #include "drivers/pwm_output.h" #include "drivers/max7456.h" #include "drivers/sound_beeper.h" +#include "drivers/light_ws2811strip.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -238,6 +239,26 @@ void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) sensorAlignmentConfig->mag_align = ALIGN_DEFAULT; } +#ifdef USE_LEDSTRIP +void resetLedStripConfig(ledStripConfig_t *ledStripConfig) +{ + applyDefaultColors(ledStripConfig->colors); + applyDefaultLedStripConfig(ledStripConfig->ledConfigs); + applyDefaultModeColors(ledStripConfig->modeColors); + applyDefaultSpecialColors(&(ledStripConfig->specialColors)); + ledStripConfig->ledstrip_visual_beeper = 0; + ledStripConfig->ledstrip_aux_channel = THROTTLE; + + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if (timerHardware[i].usageFlags & TIM_USE_LED) { + ledStripConfig->ioTag = timerHardware[i].tag; + return; + } + } + ledStripConfig->ioTag = IO_TAG_NONE; +} +#endif + #ifdef USE_SERVOS void resetServoConfig(servoConfig_t *servoConfig) { @@ -597,6 +618,10 @@ void createDefaultConfig(master_t *config) #endif resetFlight3DConfig(&config->flight3DConfig); +#ifdef USE_LEDSTRIP + resetLedStripConfig(&config->ledStripConfig); +#endif + #ifdef GPS // gps/nav stuff config->gpsConfig.provider = GPS_NMEA; @@ -666,15 +691,6 @@ void createDefaultConfig(master_t *config) config->customMotorMixer[i].throttle = 0.0f; } -#ifdef LED_STRIP - applyDefaultColors(config->colors); - applyDefaultLedStripConfig(config->ledConfigs); - applyDefaultModeColors(config->modeColors); - applyDefaultSpecialColors(&(config->specialColors)); - config->ledstrip_visual_beeper = 0; - config->ledstrip_aux_channel = THROTTLE; -#endif - #ifdef VTX config->vtx_band = 4; //Fatshark/Airwaves config->vtx_channel = 1; //CH1 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1662bc1f7b..016c9ccaac 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -932,7 +932,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #ifdef LED_STRIP case MSP_LED_COLORS: for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; + hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; sbufWriteU16(dst, color->h); sbufWriteU8(dst, color->s); sbufWriteU8(dst, color->v); @@ -941,7 +941,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_LED_STRIP_CONFIG: for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; sbufWriteU32(dst, *ledConfig); } break; @@ -951,19 +951,19 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn for (int j = 0; j < LED_DIRECTION_COUNT; j++) { sbufWriteU8(dst, i); sbufWriteU8(dst, j); - sbufWriteU8(dst, masterConfig.modeColors[i].color[j]); + sbufWriteU8(dst, masterConfig.ledStripConfig.modeColors[i].color[j]); } } for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { sbufWriteU8(dst, LED_MODE_COUNT); sbufWriteU8(dst, j); - sbufWriteU8(dst, masterConfig.specialColors.color[j]); + sbufWriteU8(dst, masterConfig.ledStripConfig.specialColors.color[j]); } sbufWriteU8(dst, LED_AUX_CHANNEL); sbufWriteU8(dst, 0); - sbufWriteU8(dst, masterConfig.ledstrip_aux_channel); + sbufWriteU8(dst, masterConfig.ledStripConfig.ledstrip_aux_channel); break; #endif @@ -1656,7 +1656,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef LED_STRIP case MSP_SET_LED_COLORS: for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; + hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; color->h = sbufReadU16(src); color->s = sbufReadU8(src); color->v = sbufReadU8(src); @@ -1670,7 +1670,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; } - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; *ledConfig = sbufReadU32(src); reevaluateLedConfig(); } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 1892afa876..812d57a67f 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -88,6 +88,7 @@ PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR static bool ledStripInitialised = false; static bool ledStripEnabled = true; +static ledStripConfig_t * currentLedStripConfig; static void ledStripDisable(void); @@ -180,7 +181,7 @@ STATIC_UNIT_TESTED void determineLedStripDimensions(void) int maxY = 0; for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; maxX = MAX(ledGetX(ledConfig), maxX); maxY = MAX(ledGetY(ledConfig), maxY); @@ -202,7 +203,7 @@ STATIC_UNIT_TESTED void updateLedCount(void) int count = 0, countRing = 0, countScanner= 0; for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if (!(*ledConfig)) break; @@ -232,7 +233,7 @@ void reevaluateLedConfig(void) // get specialColor by index static hsvColor_t* getSC(ledSpecialColorIds_e index) { - return &masterConfig.colors[masterConfig.specialColors.color[index]]; + return ¤tLedStripConfig->colors[currentLedStripConfig->specialColors.color[index]]; } static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; @@ -256,7 +257,7 @@ bool parseLedStripConfig(int ledIndex, const char *config) }; static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', ':', '\0'}; - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; memset(ledConfig, 0, sizeof(ledConfig_t)); int x = 0, y = 0, color = 0; // initialize to prevent warnings @@ -375,7 +376,7 @@ typedef enum { static quadrant_e getLedQuadrant(const int ledIndex) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; int x = ledGetX(ledConfig); int y = ledGetY(ledConfig); @@ -417,7 +418,7 @@ static const struct { static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; quadrant_e quad = getLedQuadrant(ledIndex); for (unsigned i = 0; i < ARRAYLEN(directionQuadrantMap); i++) { @@ -425,7 +426,7 @@ static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIn quadrant_e quadMask = directionQuadrantMap[i].quadrantMask; if (ledGetDirectionBit(ledConfig, dir) && (quad & quadMask)) - return &masterConfig.colors[modeColors->color[dir]]; + return ¤tLedStripConfig->colors[modeColors->color[dir]]; } return NULL; } @@ -451,7 +452,7 @@ static const struct { static void applyLedFixedLayers() { for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); int fn = ledGetFunction(ledConfig); @@ -459,13 +460,13 @@ static void applyLedFixedLayers() switch (fn) { case LED_FUNCTION_COLOR: - color = masterConfig.colors[ledGetColor(ledConfig)]; + color = currentLedStripConfig->colors[ledGetColor(ledConfig)]; break; case LED_FUNCTION_FLIGHT_MODE: for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { - hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, ¤tLedStripConfig->modeColors[flightModeToLed[i].ledMode]); if (directionalColor) { color = *directionalColor; } @@ -506,7 +507,7 @@ static void applyLedFixedLayers() static void applyLedHsv(uint32_t mask, const hsvColor_t *color) { for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if ((*ledConfig & mask) == mask) setLedHsv(ledIndex, color); } @@ -702,7 +703,7 @@ static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) } for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) { if (getLedQuadrant(ledIndex) & quadrants) setLedHsv(ledIndex, flashColor); @@ -743,7 +744,7 @@ static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) } for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) { bool applyColor; @@ -754,7 +755,7 @@ static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) } if (applyColor) { - const hsvColor_t *ringColor = &masterConfig.colors[ledGetColor(ledConfig)]; + const hsvColor_t *ringColor = ¤tLedStripConfig->colors[ledGetColor(ledConfig)]; setLedHsv(ledIndex, ringColor); } @@ -870,7 +871,7 @@ static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if (ledGetY(ledConfig) == previousRow) { setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); @@ -932,7 +933,7 @@ void ledStripUpdate(uint32_t currentTime) return; } - if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(masterConfig.ledstrip_visual_beeper && isBeeperOn())) { + if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(currentLedStripConfig->ledstrip_visual_beeper && isBeeperOn())) { if (ledStripEnabled) { ledStripDisable(); ledStripEnabled = false; @@ -963,7 +964,7 @@ void ledStripUpdate(uint32_t currentTime) // apply all layers; triggered timed functions has to update timers scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; - scaledAux = scaleRange(rcData[masterConfig.ledstrip_aux_channel], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1); + scaledAux = scaleRange(rcData[currentLedStripConfig->ledstrip_aux_channel], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1); applyLedFixedLayers(); @@ -979,7 +980,7 @@ bool parseColor(int index, const char *colorConfig) { const char *remainingCharacters = colorConfig; - hsvColor_t *color = &masterConfig.colors[index]; + hsvColor_t *color = ¤tLedStripConfig->colors[index]; bool result = true; static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = { @@ -1032,15 +1033,15 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) return false; - masterConfig.modeColors[modeIndex].color[modeColorIndex] = colorIndex; + currentLedStripConfig->modeColors[modeIndex].color[modeColorIndex] = colorIndex; } else if (modeIndex == LED_SPECIAL) { if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT) return false; - masterConfig.specialColors.color[modeColorIndex] = colorIndex; + currentLedStripConfig->specialColors.color[modeColorIndex] = colorIndex; } else if (modeIndex == LED_AUX_CHANNEL) { if (modeColorIndex < 0 || modeColorIndex >= 1) return false; - masterConfig.ledstrip_aux_channel = colorIndex; + currentLedStripConfig->ledstrip_aux_channel = colorIndex; } else { return false; } @@ -1098,21 +1099,26 @@ void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); } -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse) +void ledStripInit(ledStripConfig_t *ledStripConfig) { - ledConfigs = ledConfigsToUse; - colors = colorsToUse; - modeColors = modeColorsToUse; - specialColors = *specialColorsToUse; + currentLedStripConfig = ledStripConfig; + + ledConfigs = currentLedStripConfig->ledConfigs; + colors = currentLedStripConfig->colors; + modeColors = currentLedStripConfig->modeColors; + specialColors = currentLedStripConfig->specialColors; ledStripInitialised = false; } void ledStripEnable(void) { + if (currentLedStripConfig == NULL) { + return; + } reevaluateLedConfig(); ledStripInitialised = true; - ws2811LedStripInit(); + ws2811LedStripInit(currentLedStripConfig->ioTag); } static void ledStripDisable(void) diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index de5439902a..9b0d1d1c94 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -18,6 +18,7 @@ #pragma once #include "common/color.h" +#include "drivers/io.h" #define LED_MAX_STRIP_LENGTH 32 #define LED_CONFIGURABLE_COLOR_COUNT 16 @@ -135,6 +136,15 @@ typedef struct ledCounts_s { uint8_t ringSeqLen; } ledCounts_t; +typedef struct ledStripConfig_s { + ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; + hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; + modeColorIndexes_t modeColors[LED_MODE_COUNT]; + specialColorIndexes_t specialColors; + uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on + rc_alias_e ledstrip_aux_channel; + ioTag_t ioTag; +} ledStripConfig_t; ledConfig_t *ledConfigs; hsvColor_t *colors; @@ -166,7 +176,7 @@ bool parseLedStripConfig(int ledIndex, const char *config); void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize); void reevaluateLedConfig(void); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); +void ledStripInit(ledStripConfig_t *ledStripConfig); void ledStripEnable(void); void ledStripUpdate(uint32_t currentTime); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5c66734986..72c9aeccb1 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -935,7 +935,7 @@ const clivalue_t valueTable[] = { { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } }, #endif #ifdef LED_STRIP - { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, + { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledStripConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, #endif #ifdef USE_RTC6705 { "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, @@ -1705,8 +1705,8 @@ static void printLed(uint8_t dumpMask, master_t *defaultConfig) char ledConfigBuffer[20]; char ledConfigDefaultBuffer[20]; for (uint32_t i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig = masterConfig.ledConfigs[i]; - ledConfigDefault = defaultConfig->ledConfigs[i]; + ledConfig = masterConfig.ledStripConfig.ledConfigs[i]; + ledConfigDefault = defaultConfig->ledStripConfig.ledConfigs[i]; equalsDefault = ledConfig == ledConfigDefault; generateLedConfig(&ledConfig, ledConfigBuffer, sizeof(ledConfigBuffer)); generateLedConfig(&ledConfigDefault, ledConfigDefaultBuffer, sizeof(ledConfigDefaultBuffer)); @@ -1743,8 +1743,8 @@ static void printColor(uint8_t dumpMask, master_t *defaultConfig) hsvColor_t *colorDefault; bool equalsDefault; for (uint32_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - color = &masterConfig.colors[i]; - colorDefault = &defaultConfig->colors[i]; + color = &masterConfig.ledStripConfig.colors[i]; + colorDefault = &defaultConfig->ledStripConfig.colors[i]; equalsDefault = color->h == colorDefault->h && color->s == colorDefault->s && color->v == colorDefault->v; @@ -1789,8 +1789,8 @@ static void printModeColor(uint8_t dumpMask, master_t *defaultConfig) { for (uint32_t i = 0; i < LED_MODE_COUNT; i++) { for (uint32_t j = 0; j < LED_DIRECTION_COUNT; j++) { - int colorIndex = masterConfig.modeColors[i].color[j]; - int colorIndexDefault = defaultConfig->modeColors[i].color[j]; + int colorIndex = masterConfig.ledStripConfig.modeColors[i].color[j]; + int colorIndexDefault = defaultConfig->ledStripConfig.modeColors[i].color[j]; const char *format = "mode_color %u %u %u\r\n"; cliDefaultPrintf(dumpMask, colorIndex == colorIndexDefault, format, i, j, colorIndexDefault); cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, format, i, j, colorIndex); @@ -1799,14 +1799,14 @@ static void printModeColor(uint8_t dumpMask, master_t *defaultConfig) const char *format = "mode_color %u %u %u\r\n"; for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - int colorIndex = masterConfig.specialColors.color[j]; - int colorIndexDefault = defaultConfig->specialColors.color[j]; + int colorIndex = masterConfig.ledStripConfig.specialColors.color[j]; + int colorIndexDefault = defaultConfig->ledStripConfig.specialColors.color[j]; cliDefaultPrintf(dumpMask, colorIndex == colorIndexDefault, format, LED_SPECIAL, j, colorIndexDefault); cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, format, LED_SPECIAL, j, colorIndex); } - int ledStripAuxChannel = masterConfig.ledstrip_aux_channel; - int ledStripAuxChannelDefault = defaultConfig->ledstrip_aux_channel; + int ledStripAuxChannel = masterConfig.ledStripConfig.ledstrip_aux_channel; + int ledStripAuxChannelDefault = defaultConfig->ledStripConfig.ledstrip_aux_channel; cliDefaultPrintf(dumpMask, ledStripAuxChannel == ledStripAuxChannelDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannelDefault); cliDumpPrintf(dumpMask, ledStripAuxChannel == ledStripAuxChannelDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannel); } @@ -3743,19 +3743,22 @@ typedef struct { const cliResourceValue_t resourceTable[] = { #ifdef BEEPER - { OWNER_BEEPER, &masterConfig.beeperConfig.ioTag, 0 }, + { OWNER_BEEPER, &masterConfig.beeperConfig.ioTag, 0 }, #endif - { OWNER_MOTOR, &masterConfig.motorConfig.ioTags[0], MAX_SUPPORTED_MOTORS }, + { OWNER_MOTOR, &masterConfig.motorConfig.ioTags[0], MAX_SUPPORTED_MOTORS }, #ifdef USE_SERVOS - { OWNER_SERVO, &masterConfig.servoConfig.ioTags[0], MAX_SUPPORTED_SERVOS }, + { OWNER_SERVO, &masterConfig.servoConfig.ioTags[0], MAX_SUPPORTED_SERVOS }, #endif #ifndef SKIP_RX_PWM_PPM - { OWNER_PPMINPUT, &masterConfig.ppmConfig.ioTag, 0 }, - { OWNER_PWMINPUT, &masterConfig.pwmConfig.ioTags[0], PWM_INPUT_PORT_COUNT }, + { OWNER_PPMINPUT, &masterConfig.ppmConfig.ioTag, 0 }, + { OWNER_PWMINPUT, &masterConfig.pwmConfig.ioTags[0], PWM_INPUT_PORT_COUNT }, #endif #ifdef SONAR { OWNER_SONAR_TRIGGER, &masterConfig.sonarConfig.triggerTag, 0 }, - { OWNER_SONAR_ECHO, &masterConfig.sonarConfig.echoTag, 0 }, + { OWNER_SONAR_ECHO, &masterConfig.sonarConfig.echoTag, 0 }, +#endif +#ifdef LED_STRIP + { OWNER_LED_STRIP, &masterConfig.ledStripConfig.ioTag, 0 }, #endif }; diff --git a/src/main/main.c b/src/main/main.c index a6fccd0ebe..6a0cdf2514 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -482,7 +482,7 @@ void init(void) #endif #ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); + ledStripInit(&masterConfig.ledStripConfig); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); From 90081168238d2938ec5d19aba741a1eb4e378304 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 5 Nov 2016 13:14:54 +0900 Subject: [PATCH 112/188] onExit is now called with pointer to OSD_Entry at the time of exit This will remove the final remaining reference to RC preview menu (which required not to exit with KEY_ESC). --- src/main/io/cms.c | 14 +++++--------- src/main/io/cms_imu.c | 18 +++++++++++++++--- src/main/io/cms_imu.h | 3 --- src/main/io/cms_types.h | 11 ++++++++++- 4 files changed, 30 insertions(+), 16 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 610e2a3d9a..1bc51778eb 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -78,10 +78,6 @@ // Forwards static long cmsx_InfoInit(void); -#if 0 -long cmsx_FeatureRead(void); -long cmsx_FeatureWriteback(void); -#endif // Device management @@ -509,8 +505,8 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) static long cmsMenuBack(displayPort_t *pDisplay) { - if (currentMenu->onExit) - currentMenu->onExit(); + if (currentMenu->onExit && currentMenu->onExit(pageTop + cursorRow) < 0) + return -1; if (menuStackIdx) { displayClear(pDisplay); @@ -581,7 +577,7 @@ static long cmsMenuExit(displayPort_t *pDisplay, void *ptr) cmsTraverseGlobalExit(&menuMain); if (currentMenu->onExit) - currentMenu->onExit(); + currentMenu->onExit((OSD_Entry *)NULL); // Forced exit } cmsInMenu = false; @@ -812,7 +808,7 @@ static void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) key = KEY_RIGHT; rcDelay = BUTTON_TIME; } - else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != &cmsx_menuRc) // this menu is used to check transmitter signals so can't exit using YAW + else if (IS_HI(YAW) || IS_LO(YAW)) { key = KEY_ESC; rcDelay = BUTTON_TIME; @@ -856,7 +852,7 @@ void cmsInit(void) } // -// Menu contents +// Built-in menu contents and support functions // // Info diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c index 5b6d742f4e..385c462398 100644 --- a/src/main/io/cms_imu.c +++ b/src/main/io/cms_imu.c @@ -63,8 +63,10 @@ long cmsx_PidRead(void) return 0; } -long cmsx_PidWriteback(void) +long cmsx_PidWriteback(OSD_Entry *self) { + UNUSED(self); + uint8_t i; for (i = 0; i < 3; i++) { @@ -132,13 +134,23 @@ long cmsx_RateExpoRead(void) return 0; } -long cmsx_RateExpoWriteback(void) +long cmsx_RateExpoWriteback(OSD_Entry *self) { + UNUSED(self); + memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); return 0; } +long cmsx_menuRcConfirmBack(OSD_Entry *self) +{ + if (self && self->type == OME_Back) + return 0; + else + return -1; +} + OSD_Entry cmsx_menuRateExpoEntries[] = { {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0}, @@ -198,7 +210,7 @@ CMS_Menu cmsx_menuRc = { "MENURC", OME_MENU, NULL, - NULL, + cmsx_menuRcConfirmBack, NULL, cmsx_menuRcEntries, }; diff --git a/src/main/io/cms_imu.h b/src/main/io/cms_imu.h index 0a05ed8daa..9fb44f05b9 100644 --- a/src/main/io/cms_imu.h +++ b/src/main/io/cms_imu.h @@ -1,4 +1 @@ extern CMS_Menu cmsx_menuImu; - -// This should be gone -extern CMS_Menu cmsx_menuRc; diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index 6a051de382..3243e05bd5 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -57,6 +57,15 @@ typedef struct typedef long (*CMSMenuFuncPtr)(void); +/* +onExit function is called with self: +(1) Pointer to an OSD_Entry when cmsMenuBack() was called. + Point to an OSD_Entry with type == OME_Back if BACK was selected. +(2) 0 if called from menu exit (forced exit). +*/ + +typedef long (*CMSMenuOnExitPtr)(OSD_Entry *self); + typedef struct { // These two are debug aids for menu content creators. @@ -64,7 +73,7 @@ typedef struct OSD_MenuElement GUARD_type; CMSMenuFuncPtr onEnter; - CMSMenuFuncPtr onExit; + CMSMenuOnExitPtr onExit; CMSMenuFuncPtr onGlobalExit; OSD_Entry *entries; } CMS_Menu; From e6dd0e6c5ce1b47efa02386f71848c87d8efb246 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 5 Nov 2016 14:24:18 +0900 Subject: [PATCH 113/188] DYNAMIC bit is added to entry flags This will stash the special type OME_Polled_INT16 for RC preview menu, and add capability to all value types to be polled dynamically. --- src/main/io/cms.c | 43 ++++++++++++++++++++++++++++++----------- src/main/io/cms_imu.c | 16 +++++++-------- src/main/io/cms_types.h | 7 +++++-- 3 files changed, 45 insertions(+), 21 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 1bc51778eb..b2dc9a4a99 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -120,7 +120,8 @@ static cmsDeviceInitFuncPtr cmsDeviceSelectNext(void) return cmsDeviceInitFunc[cmsCurrentDevice]; } -#define CMS_UPDATE_INTERVAL 50 // msec +#define CMS_UPDATE_INTERVAL 50 // Interval of key scans (msec) +#define CMS_POLL_INTERVAL 100 // Interval of polling dynamic values (msec) static void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) { @@ -261,7 +262,7 @@ static void cmsPadToSize(char *buf, int size) buf[size] = 0; } -static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool drawPolled) +static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) { char buff[10]; int cnt = 0; @@ -351,6 +352,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, CLR_PRINTVALUE(p); } break; +#if 0 case OME_Poll_INT16: if (p->data && drawPolled) { OSD_UINT16_t *ptr = p->data; @@ -360,6 +362,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, // PRINTVALUE not cleared on purpose } break; +#endif case OME_FLOAT: if (IS_PRINTVALUE(p) && p->data) { OSD_FLOAT_t *ptr = p->data; @@ -387,20 +390,26 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, static void cmsDrawMenu(displayPort_t *pDisplay) { + if (!pageTop) + return; + uint8_t i; OSD_Entry *p; uint8_t top = (pDisplay->rows - maxRow) / 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); + + bool drawPolled = false; + static uint32_t lastPolled = 0; + uint32_t now = millis(); // Argh... + + if (now > lastPolled + CMS_POLL_INTERVAL) { + drawPolled = true; + lastPolled = now; + } uint32_t room = displayTxBytesFree(pDisplay); - if (!pageTop) - return; - if (pDisplay->cleared) { for (p = pageTop, i= 0; p->type != OME_END; p++, i++) { SET_PRINTLABEL(p); @@ -415,6 +424,11 @@ static void cmsDrawMenu(displayPort_t *pDisplay) } pDisplay->cleared = false; + } else if (drawPolled) { + for (p = pageTop ; p <= pageTop + maxRow ; p++) { + if (IS_DYNAMIC(p)) + SET_PRINTVALUE(p); + } } // Cursor manipulation @@ -454,7 +468,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay) for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { if (IS_PRINTVALUE(p)) { - room -= cmsDrawMenuEntry(pDisplay, p, top + i, drawPolled); + room -= cmsDrawMenuEntry(pDisplay, p, top + i); if (room < 30) return; } @@ -505,6 +519,8 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) static long cmsMenuBack(displayPort_t *pDisplay) { + // Let onExit function decide whether to allow exit or not. + if (currentMenu->onExit && currentMenu->onExit(pageTop + cursorRow) < 0) return -1; @@ -513,13 +529,19 @@ static long cmsMenuBack(displayPort_t *pDisplay) menuStackIdx--; currentMenu = menuStack[menuStackIdx]; cursorRow = menuStackHistory[menuStackIdx]; + + // cursorRow is absolute offset of a focused entry when stacked. + // Convert it back to page and relative offset. + pageTop = currentMenu->entries; // Temporary for cmsUpdateMaxRow() cmsUpdateMaxRow(pDisplay); + if (cursorRow > maxRow) { + // Cursor was in the second page. pageTopAlt = currentMenu->entries; pageTop = pageTopAlt + maxRow + 1; cursorRow -= (maxRow + 1); - cmsUpdateMaxRow(pDisplay); + cmsUpdateMaxRow(pDisplay); // Update maxRow for the second page } } @@ -758,7 +780,6 @@ static uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) break; case OME_String: break; - case OME_Poll_INT16: case OME_Label: case OME_END: break; diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c index 385c462398..c4a71aa3f5 100644 --- a/src/main/io/cms_imu.c +++ b/src/main/io/cms_imu.c @@ -194,14 +194,14 @@ static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0}; OSD_Entry cmsx_menuRcEntries[] = { {"--- 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}, + {"ROLL", OME_INT16, NULL, &entryRcRoll, DYNAMIC}, + {"PITCH", OME_INT16, NULL, &entryRcPitch, DYNAMIC}, + {"THR", OME_INT16, NULL, &entryRcThr, DYNAMIC}, + {"YAW", OME_INT16, NULL, &entryRcYaw, DYNAMIC}, + {"AUX1", OME_INT16, NULL, &entryRcAux1, DYNAMIC}, + {"AUX2", OME_INT16, NULL, &entryRcAux2, DYNAMIC}, + {"AUX3", OME_INT16, NULL, &entryRcAux3, DYNAMIC}, + {"AUX4", OME_INT16, NULL, &entryRcAux4, DYNAMIC}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index 3243e05bd5..a09f62895d 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -20,7 +20,6 @@ typedef enum OME_UINT8, OME_UINT16, OME_INT16, - OME_Poll_INT16, OME_String, OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's //wlasciwosci elementow @@ -46,6 +45,7 @@ typedef struct // Bits in flags #define PRINT_VALUE 0x01 // Value has been changed, need to redraw #define PRINT_LABEL 0x02 // Text label should be printed +#define DYNAMIC 0x04 // Value should be updated dynamically #define IS_PRINTVALUE(p) ((p)->flags & PRINT_VALUE) #define SET_PRINTVALUE(p) { (p)->flags |= PRINT_VALUE; } @@ -55,13 +55,16 @@ typedef struct #define SET_PRINTLABEL(p) { (p)->flags |= PRINT_LABEL; } #define CLR_PRINTLABEL(p) { (p)->flags &= ~PRINT_LABEL; } +#define IS_DYNAMIC(p) ((p)->flags & DYNAMIC) + + typedef long (*CMSMenuFuncPtr)(void); /* onExit function is called with self: (1) Pointer to an OSD_Entry when cmsMenuBack() was called. Point to an OSD_Entry with type == OME_Back if BACK was selected. -(2) 0 if called from menu exit (forced exit). +(2) NULL if called from menu exit (forced exit at top level). */ typedef long (*CMSMenuOnExitPtr)(OSD_Entry *self); From a01a71cddb0e9cedf1058d61f0f813408cdc1e0c Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 5 Nov 2016 17:53:43 +1100 Subject: [PATCH 114/188] As per @martinbudden suggestion including io_types.h only, and fixed osd.c master config changes --- src/main/drivers/light_ws2811strip.h | 2 +- src/main/io/ledstrip.h | 2 +- src/main/io/osd.c | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index c81be02795..894ef4b1d6 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" #define WS2811_LED_STRIP_LENGTH 32 #define WS2811_BITS_PER_LED 24 diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 9b0d1d1c94..7bc5ab36ab 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -18,7 +18,7 @@ #pragma once #include "common/color.h" -#include "drivers/io.h" +#include "drivers/io_types.h" #define LED_MAX_STRIP_LENGTH 32 #define LED_CONFIGURABLE_COLOR_COUNT 16 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c9ad4c1d58..56e027e361 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -305,7 +305,7 @@ static const char * const LED_COLOR_NAMES[] = { void getLedColor(void) { for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[ledIndex]; int fn = ledGetFunction(ledConfig); @@ -321,7 +321,7 @@ void applyLedColor(void * ptr) { UNUSED(ptr); for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[ledIndex]; if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); } From abcbd4cb849974ba2f19321859bf27bf94673513 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 5 Nov 2016 19:09:08 +1100 Subject: [PATCH 115/188] Formatting --- src/main/drivers/light_ws2811strip_stm32f10x.c | 3 ++- src/main/drivers/light_ws2811strip_stm32f30x.c | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 3ccfafe2fa..250dc556dc 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -38,7 +38,8 @@ bool ws2811Initialised = false; static DMA_Channel_TypeDef *dmaChannel = NULL; static TIM_TypeDef *timer = NULL; -static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { +static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) +{ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { ws2811LedDataTransferInProgress = 0; DMA_Cmd(descriptor->channel, DISABLE); diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 1e8c7a8062..6cc0ed9768 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -39,7 +39,8 @@ bool ws2811Initialised = false; static DMA_Channel_TypeDef *dmaChannel = NULL; static TIM_TypeDef *timer = NULL; -static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { +static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) +{ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { ws2811LedDataTransferInProgress = 0; DMA_Cmd(descriptor->channel, DISABLE); From 131147322d49da8e1d9ecbb0d117b8dd1a1d75f0 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 5 Nov 2016 17:26:45 +0900 Subject: [PATCH 116/188] Add menuErr to notify obvious error in menu contents --- src/main/io/cms.c | 31 +++++++++++-------------------- src/main/io/cms_types.h | 4 +++- 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index b2dc9a4a99..21e27ae392 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -170,17 +170,17 @@ static int8_t cursorRow; // Broken menu substitution -static char menuErrLabel[21 + 1]; +static char menuErrLabel[21 + 1] = "RAMDOM DATA"; static OSD_Entry menuErrEntries[] = { { "BROKEN MENU", OME_Label, NULL, NULL, 0 }, - { menuErrLabel, OME_String, NULL, NULL, 0 }, + { menuErrLabel, OME_Label, NULL, NULL, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; static CMS_Menu menuErr = { - "MENU CONTENT BROKEN", + "MENUERR", OME_MENU, NULL, NULL, @@ -352,17 +352,6 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; -#if 0 - case OME_Poll_INT16: - if (p->data && drawPolled) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); - // PRINTVALUE not cleared on purpose - } - break; -#endif case OME_FLOAT: if (IS_PRINTVALUE(p) && p->data) { OSD_FLOAT_t *ptr = p->data; @@ -378,6 +367,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) case OME_Back: break; case OME_MENU: + // Fall through + default: #ifdef CMS_MENU_DEBUG // Shouldn't happen. Notify creator of this menu content. cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT"); @@ -483,10 +474,10 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) #ifdef CMS_MENU_DEBUG if (pMenu->GUARD_type != OME_MENU) { // ptr isn't pointing to a CMS_Menu. - if (pMenu->GUARD_type < OME_MENU) { - strncpy(menuErrLabel, pMenu->GUARD_text, 21); + if (pMenu->GUARD_type <= OME_MAX) { + strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1); } else { - strncpy(menuErrLabel, "LABEL UNKNOWN", 21); + strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1); } pMenu = &menuErr; } @@ -501,7 +492,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) menuStackHistory[menuStackIdx] = cursorRow; menuStackIdx++; - currentMenu = (CMS_Menu *)ptr; + currentMenu = pMenu; cursorRow = 0; if (pMenu->onEnter) @@ -950,8 +941,8 @@ static OSD_Entry menuMainEntries[] = {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0}, {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, #endif - {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, - {"FC&FW INFO1", OME_Submenu, cmsMenuChange, &menuInfo, 0}, + {"ERR DEMO", OME_Submenu, cmsMenuChange, &menuErr, 0}, + {"REAL ERR", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0}, {"FC&FW INFO2", OME_Submenu, cmsMenuChange, &menuInfo, 0}, {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index a09f62895d..5dbafd6e02 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -30,7 +30,9 @@ typedef enum OME_END, // Debug aid - OME_MENU + OME_MENU, + + OME_MAX = OME_MENU } OSD_MenuElement; typedef struct From 1296169091749f5c8ee0b5b5ca1de94bb07aa3de Mon Sep 17 00:00:00 2001 From: hori64 Date: Tue, 24 May 2016 17:54:29 +0200 Subject: [PATCH 117/188] Smart Port telemetry GPS ground speed unit is not applicable. (#2166) Second try --- src/main/telemetry/smartport.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index c9be67089c..0076b35425 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -628,8 +628,10 @@ void handleSmartPortTelemetry(void) #ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; - smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H + //convert to knots: 1cm/s = 0.0194384449 knots + //Speed should be sent in knots/1000 (GPS speed is in cm/s) + uint32_t tmpui = GPS_speed * 1944 / 100; + smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; } break; From d2981cd7042fe5707146386672d2fbaec5133ef1 Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sat, 5 Nov 2016 11:21:36 +0200 Subject: [PATCH 118/188] escprog inverted output support multiport test, usage "escprog ki 255" --- src/main/drivers/serial_escserial.c | 181 ++++++++++++++++++++-------- src/main/io/serial_cli.c | 16 ++- 2 files changed, 140 insertions(+), 57 deletions(-) diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 577d305d52..1ae53a3659 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -17,6 +17,7 @@ #include #include +#include #include "platform.h" @@ -25,6 +26,13 @@ typedef enum { BAUDRATE_KISS = 38400 } escBaudRate_e; +typedef enum { + PROTOCOL_SIMONK = 0, + PROTOCOL_BLHELI = 1, + PROTOCOL_KISS = 2, + PROTOCOL_KISSALL = 3 +} escProtocol_e; + #if defined(USE_ESCSERIAL) #include "build/build_config.h" @@ -80,11 +88,19 @@ typedef struct escSerial_s { uint8_t escSerialPortIndex; uint8_t mode; + uint8_t outputCount; timerCCHandlerRec_t timerCb; timerCCHandlerRec_t edgeCb; } escSerial_t; +typedef struct { + IO_t io; + uint8_t inverted; +} escOutputs_t; + +escOutputs_t escOutputs[MAX_SUPPORTED_MOTORS]; + extern timerHardware_t* serialTimerHardware; extern escSerial_t escSerialPorts[]; @@ -97,14 +113,36 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); -static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); +static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) { - if (state) { - IOHi(escSerial->txIO); - } else { - IOLo(escSerial->txIO); + if((escSerial->mode = PROTOCOL_KISSALL)) + { + for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) { + uint8_t state_temp = state; + if(escOutputs[i].inverted) { + state_temp ^= ENABLE; + } + + if (state_temp) { + IOHi(escOutputs[i].io); + } else { + IOLo(escOutputs[i].io); + } + } + } + else + { + if(escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) { + state ^= ENABLE; + } + + if (state) { + IOHi(escSerial->txIO); + } else { + IOLo(escSerial->txIO); + } } } @@ -118,7 +156,7 @@ static void escSerialGPIOConfig(ioTag_t tag, ioConfig_t cfg) IOConfigGPIO(IOGetByTag(tag), cfg); } -void serialInputPortConfigEsc(const timerHardware_t *timerHardwarePtr) +void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr) { #ifdef STM32F10X escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU); @@ -164,12 +202,12 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 uint8_t mhz = SystemCoreClock / 2000000; TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, 0xFFFF, mhz); - serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); } -static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) +static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { uint32_t timerPeriod=34; TIM_DeInit(timerHardwarePtr->tim); @@ -178,7 +216,7 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); } -static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) +static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { TIM_ICInitTypeDef TIM_ICInitStructure; @@ -192,17 +230,17 @@ static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) TIM_ICInit(tim, &TIM_ICInitStructure); } -static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) +static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { // start bit is usually a FALLING signal TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, 0xFFFF, 1); - serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); + escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); } -static void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr) +static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr) { escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_OUT_PP); timerChITConfig(timerHardwarePtr,DISABLE); @@ -225,7 +263,11 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac { escSerial_t *escSerial = &(escSerialPorts[portIndex]); - escSerial->rxTimerHardware = &(timerHardware[output]); + if(mode != PROTOCOL_KISSALL){ + escSerial->rxTimerHardware = &(timerHardware[output]); + } + + escSerial->mode = mode; escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]); escSerial->port.vTable = escSerialVTable; @@ -247,30 +289,56 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac escSerial->escSerialPortIndex = portIndex; - escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag); - serialInputPortConfigEsc(escSerial->rxTimerHardware); - - setTxSignalEsc(escSerial, ENABLE); + if(mode != PROTOCOL_KISSALL) + { + escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag); + escSerialInputPortConfig(escSerial->rxTimerHardware); + setTxSignalEsc(escSerial, ENABLE); + } delay(50); - if(mode==0){ - serialTimerTxConfig(escSerial->txTimerHardware, portIndex); - serialTimerRxConfig(escSerial->rxTimerHardware, portIndex); + if(mode==PROTOCOL_SIMONK){ + escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex); + escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex); } - else if(mode==1){ + else if(mode==PROTOCOL_BLHELI){ serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options); } - else if(mode==2) { - serialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used + else if(mode==PROTOCOL_KISS) { + escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used + serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); + } + else if(mode==PROTOCOL_KISSALL) { + escSerial->outputCount = 0; + memset(&escOutputs, 0, sizeof(escOutputs)); + pwmOutputPort_t *pwmMotors = pwmGetMotors(); + for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + if (pwmMotors[i].enabled) { + if (pwmMotors[i].io != IO_NONE) { + for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) { + if(pwmMotors[i].io == IOGetByTag(timerHardware[j].tag)) + { + escSerialOutputPortConfig(&timerHardware[j]); + if(timerHardware[j].output & TIMER_OUTPUT_INVERTED) { + escOutputs[escSerial->outputCount].inverted = 1; + } + break; + } + } + escOutputs[escSerial->outputCount].io = pwmMotors[i].io; + escSerial->outputCount++; + } + } + } + setTxSignalEsc(escSerial, ENABLE); serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); } - escSerial->mode = mode; return &escSerial->port; } -void serialInputPortDeConfig(const timerHardware_t *timerHardwarePtr) +void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr) { timerChClearCCFlag(timerHardwarePtr); timerChITConfig(timerHardwarePtr,DISABLE); @@ -284,7 +352,7 @@ void closeEscSerial(escSerialPortIndex_e portIndex, uint16_t output) escSerial->rxTimerHardware = &(timerHardware[output]); escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]); - serialInputPortDeConfig(escSerial->rxTimerHardware); + escSerialInputPortDeConfig(escSerial->rxTimerHardware); timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL); timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL); TIM_DeInit(escSerial->txTimerHardware->tim); @@ -339,7 +407,7 @@ reload: escSerial->isTransmittingData = true; //set output - serialOutputPortConfig(escSerial->rxTimerHardware); + escSerialOutputPortConfig(escSerial->rxTimerHardware); return; } @@ -383,7 +451,7 @@ reload: if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { escSerial->isTransmittingData = false; - serialInputPortConfigEsc(escSerial->rxTimerHardware); + escSerialInputPortConfig(escSerial->rxTimerHardware); } } @@ -417,7 +485,9 @@ void processTxStateBL(escSerial_t *escSerial) //set output - serialOutputPortConfig(escSerial->rxTimerHardware); + if(escSerial->mode==PROTOCOL_BLHELI) { + escSerialOutputPortConfig(escSerial->rxTimerHardware); + } return; } @@ -432,9 +502,9 @@ void processTxStateBL(escSerial_t *escSerial) escSerial->isTransmittingData = false; if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { - if(escSerial->mode==1) + if(escSerial->mode==PROTOCOL_BLHELI) { - serialInputPortConfigEsc(escSerial->rxTimerHardware); + escSerialInputPortConfig(escSerial->rxTimerHardware); } } } @@ -463,7 +533,7 @@ void prepareForNextRxByteBL(escSerial_t *escSerial) escSerial->isSearchingForStartBit = true; if (escSerial->rxEdge == LEADING) { escSerial->rxEdge = TRAILING; - serialICConfig( + escSerialICConfig( escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, (escSerial->port.options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling @@ -551,7 +621,7 @@ void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) escSerial->transmissionErrors++; } - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); escSerial->rxEdge = LEADING; escSerial->rxBitIndex = 0; @@ -569,10 +639,10 @@ void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) if (escSerial->rxEdge == TRAILING) { escSerial->rxEdge = LEADING; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); } else { escSerial->rxEdge = TRAILING; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); } } /*-------------------------BL*/ @@ -605,7 +675,7 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture) { escSerial->isReceivingData=0; escSerial->receiveTimeout=0; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling); } } @@ -655,7 +725,7 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture bits=1; escSerial->internalRxBuffer = 0x80; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising); } } escSerial->receiveTimeout = 0; @@ -763,7 +833,7 @@ void escSerialInitialize() for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { // set outputs to pullup - if(timerHardware[i].output==1) + if(timerHardware[i].output & TIMER_OUTPUT_ENABLED) { escSerialGPIOConfig(timerHardware[i].tag, IOCFG_IPU); //GPIO_Mode_IPU } @@ -844,27 +914,34 @@ static bool ProcessExitCommand(uint8_t c) void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode) { bool exitEsc = false; + uint8_t motor_output = 0; LED0_OFF; LED1_OFF; //StopPwmAllMotors(); pwmDisableMotors(); passPort = escPassthroughPort; - uint8_t first_output = 0; - for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if(timerHardware[i].output==1) - { - first_output=i; - break; - } + uint32_t escBaudrate = (mode == PROTOCOL_KISS) ? BAUDRATE_KISS : BAUDRATE_NORMAL; + + if((mode == PROTOCOL_KISS) && (output == 255)){ + motor_output = 255; + mode = PROTOCOL_KISSALL; } + else { + uint8_t first_output = 0; + for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if(timerHardware[i].output & TIMER_OUTPUT_ENABLED) + { + first_output=i; + break; + } + } - //doesn't work with messy timertable - uint8_t motor_output=first_output+output-1; - if(motor_output >=USABLE_TIMER_CHANNEL_COUNT) - return; - - uint32_t escBaudrate = (mode == 2) ? BAUDRATE_KISS : BAUDRATE_NORMAL; + //doesn't work with messy timertable + motor_output=first_output+output-1; + if(motor_output >=USABLE_TIMER_CHANNEL_COUNT) + return; + } escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode); uint8_t ch; @@ -898,7 +975,7 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin closeEscSerial(ESCSERIAL1, output); return; } - if(mode==1){ + if(mode==PROTOCOL_BLHELI){ serialWrite(escPassthroughPort, ch); // blheli loopback } serialWrite(escPort, ch); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 72c9aeccb1..bcb7f1542f 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3000,12 +3000,18 @@ static void cliEscPassthrough(char *cmdline) break; case 1: index = atoi(pch); - if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) { - printf("passthru at pwm output %d enabled\r\n", index); + if(mode == 2 && index == 255) + { + printf("passthru on all pwm outputs enabled\r\n"); } - else { - printf("invalid pwm output, valid range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); - return; + else{ + if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) { + printf("passthru at pwm output %d enabled\r\n", index); + } + else { + printf("invalid pwm output, valid range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); + return; + } } break; } From 3395d9ca2c3e0ca80c2b68d3aa50cb494441c72d Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 5 Nov 2016 21:08:11 +1100 Subject: [PATCH 119/188] Fixes reset LED issue (bad define) --- src/main/drivers/io_types.h | 2 +- src/main/fc/config.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h index 78a3158e34..ccd7e0a960 100644 --- a/src/main/drivers/io_types.h +++ b/src/main/drivers/io_types.h @@ -8,7 +8,7 @@ typedef uint8_t ioTag_t; // packet tag to specify IO pin typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change // NONE initializer for ioTag_t variables -#define IO_TAG_NONE ((ioTag_t)0) +#define IO_TAG_NONE IO_TAG(NONE) // NONE initializer for IO_t variable #define IO_NONE ((IO_t)0) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 296df6570a..5077d876f8 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -239,7 +239,7 @@ void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) sensorAlignmentConfig->mag_align = ALIGN_DEFAULT; } -#ifdef USE_LEDSTRIP +#ifdef LED_STRIP void resetLedStripConfig(ledStripConfig_t *ledStripConfig) { applyDefaultColors(ledStripConfig->colors); @@ -618,7 +618,7 @@ void createDefaultConfig(master_t *config) #endif resetFlight3DConfig(&config->flight3DConfig); -#ifdef USE_LEDSTRIP +#ifdef LED_STRIP resetLedStripConfig(&config->ledStripConfig); #endif From 4ae6f8c1c4568cf648e0eaa880dd9e37099d0f4b Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 6 Nov 2016 02:37:45 +0900 Subject: [PATCH 120/188] displayPort device registration changed --- src/main/drivers/display.c | 3 +- src/main/io/canvas.c | 15 +- src/main/io/cms.c | 209 +++++---------------- src/main/io/cms.h | 5 +- src/main/io/cms_builtin.c | 125 ++++++++++++ src/main/io/cms_builtin.h | 1 + src/main/io/dashboard.c | 14 +- src/main/io/displayport_oled.c | 21 ++- src/main/io/displayport_oled.h | 5 +- src/main/io/osd.c | 12 +- src/main/io/osd_max7456.c | 21 ++- src/main/io/osd_max7456.h | 4 +- src/main/target/OMNIBUS/target.mk | 1 + src/main/target/OMNIBUSF4/target.mk | 1 + src/main/target/SIRINFPV/target.mk | 1 + src/main/target/SPRACINGF3/target.mk | 1 + src/main/target/STM32F3DISCOVERY/target.mk | 1 + 17 files changed, 224 insertions(+), 216 deletions(-) create mode 100644 src/main/io/cms_builtin.c create mode 100644 src/main/io/cms_builtin.h diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 9b3810c41b..753afacf76 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -29,18 +29,19 @@ void displayClear(displayPort_t *instance) instance->vTable->clear(instance); instance->cleared = true; instance->cursorRow = -1; - instance->inCMS = false; } void displayOpen(displayPort_t *instance) { instance->vTable->open(instance); instance->vTable->clear(instance); + instance->inCMS = true; } void displayClose(displayPort_t *instance) { instance->vTable->close(instance); + instance->inCMS = false; } int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s) diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index bc76e09e4c..f805c862cf 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -20,6 +20,8 @@ #include "msp/msp_protocol.h" #include "msp/msp_serial.h" +static displayPort_t canvasDisplayPort; + static int canvasOutput(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len) { UNUSED(displayPort); @@ -74,7 +76,7 @@ static int canvasWrite(displayPort_t *displayPort, uint8_t col, uint8_t row, cha static void canvasResync(displayPort_t *displayPort) { displayPort->rows = 13; // XXX Will reflect NTSC/PAL in the future - displayPort->rows = 30; + displayPort->cols = 30; } static uint32_t canvasTxBytesFree(displayPort_t *displayPort) @@ -93,14 +95,11 @@ static const displayPortVTable_t canvasVTable = { canvasTxBytesFree, }; -void canvasCmsInit(displayPort_t *displayPort) -{ - displayPort->vTable = &canvasVTable; - canvasResync(displayPort); -} - void canvasInit() { - cmsDeviceRegister(canvasCmsInit); + canvasDisplayPort.vTable = &canvasVTable; + canvasDisplayPort.inCMS = false; + canvasResync(&canvasDisplayPort); + cmsDisplayPortRegister(&canvasDisplayPort); } #endif diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 21e27ae392..4fe5e59252 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -16,10 +16,12 @@ */ /* - Created by Marcin Baliniak + Original OSD code created by Marcin Baliniak OSD-CMS separation by jflyper + CMS-displayPort separation by jflyper and martinbudden */ -#define CMS_MENU_DEBUG // For external menu content creators + +//#define CMS_MENU_DEBUG // For external menu content creators #include #include @@ -32,74 +34,54 @@ #ifdef CMS +#include "build/debug.h" + #include "drivers/system.h" #include "common/typeconversion.h" -#include "io/cms.h" -#include "io/cms_types.h" - -#ifdef CANVAS -#include "io/canvas.h" -#endif - -#ifdef USE_FLASHFS -#include "io/flashfs.h" -#endif - -#ifdef OSD -#include "io/osd.h" -#endif - -#ifdef USE_DASHBOARD -#include "io/dashboard.h" -#endif - +// For 'ARM' related #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/pid.h" - +// For rcData, stopAllMotors, stopPwmAllMotors #include "config/config_profile.h" #include "config/config_master.h" #include "config/feature.h" -#include "build/debug.h" +// For VISIBLE* (Actually, included by config_master.h) +#include "io/osd.h" -// External menu contents -#include "io/cms_imu.h" -#include "io/cms_blackbox.h" -#include "io/cms_vtx.h" -#ifdef OSD -#include "io/cms_osd.h" -#endif -#include "io/cms_ledstrip.h" +#include "io/cms.h" +#include "io/cms_types.h" -// Forwards -static long cmsx_InfoInit(void); +// Menu contents +#include "io/cms_builtin.h" -// Device management +// DisplayPort management #ifndef CMS_MAX_DEVICE #define CMS_MAX_DEVICE 4 #endif -static cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE]; +static displayPort_t *pCurrentDisplay; + +static displayPort_t *cmsDisplayPorts[CMS_MAX_DEVICE]; static int cmsDeviceCount; static int cmsCurrentDevice = -1; -bool cmsDeviceRegister(cmsDeviceInitFuncPtr func) +bool cmsDisplayPortRegister(displayPort_t *pDisplay) { if (cmsDeviceCount == CMS_MAX_DEVICE) return false; - cmsDeviceInitFunc[cmsDeviceCount++] = func; + cmsDisplayPorts[cmsDeviceCount++] = pDisplay; return true; } -static cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void) +static displayPort_t *cmsDisplayPortSelectCurrent(void) { if (cmsDeviceCount == 0) return NULL; @@ -107,28 +89,22 @@ static cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void) if (cmsCurrentDevice < 0) cmsCurrentDevice = 0; - return cmsDeviceInitFunc[cmsCurrentDevice]; + return cmsDisplayPorts[cmsCurrentDevice]; } -static cmsDeviceInitFuncPtr cmsDeviceSelectNext(void) +static displayPort_t *cmsDisplayPortSelectNext(void) { if (cmsDeviceCount == 0) return NULL; cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay - return cmsDeviceInitFunc[cmsCurrentDevice]; + return cmsDisplayPorts[cmsCurrentDevice]; } #define CMS_UPDATE_INTERVAL 50 // Interval of key scans (msec) #define CMS_POLL_INTERVAL 100 // Interval of polling dynamic values (msec) -static void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) -{ - cmsDeviceInitFunc(pDisp); -} - - // 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. @@ -149,11 +125,8 @@ static void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceIn #define RIGHT_MENU_COLUMN(p) ((p)->cols - 8) #define MAX_MENU_ITEMS(p) ((p)->rows - 2) -static displayPort_t currentDisplay; - static bool cmsInMenu = false; -static CMS_Menu menuMain; static CMS_Menu *currentMenu; // Points to top entry of the current page // XXX Does menu backing support backing into second page??? @@ -168,7 +141,7 @@ static uint8_t maxRow; // Max row in the current page static int8_t cursorRow; -// Broken menu substitution +#ifdef CMS_MENU_DEBUG // For external menu content creators static char menuErrLabel[21 + 1] = "RAMDOM DATA"; @@ -187,6 +160,7 @@ static CMS_Menu menuErr = { NULL, menuErrEntries, }; +#endif // Stick/key detection @@ -500,6 +474,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) } pageTop = currentMenu->entries; + pageTopAlt = NULL; displayClear(pDisplay); cmsUpdateMaxRow(pDisplay); @@ -541,26 +516,26 @@ static long cmsMenuBack(displayPort_t *pDisplay) static void cmsMenuOpen(void) { - cmsDeviceInitFuncPtr initfunc; - if (!cmsInMenu) { // New open + pCurrentDisplay = cmsDisplayPortSelectCurrent(); + if (!pCurrentDisplay) + return; cmsInMenu = true; - DISABLE_ARMING_FLAG(OK_TO_ARM); - initfunc = cmsDeviceSelectCurrent(); currentMenu = &menuMain; + DISABLE_ARMING_FLAG(OK_TO_ARM); } else { // Switch display - displayClose(¤tDisplay); - initfunc = cmsDeviceSelectNext(); + displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); + if (pNextDisplay != pCurrentDisplay) { + displayClose(pCurrentDisplay); + pCurrentDisplay = pNextDisplay; + } else { + return; + } } - - if (!initfunc) - return; - - cmsScreenInit(¤tDisplay, initfunc); - displayOpen(¤tDisplay); - cmsMenuChange(¤tDisplay, currentMenu); + displayOpen(pCurrentDisplay); + cmsMenuChange(pCurrentDisplay, currentMenu); } static void cmsTraverseGlobalExit(CMS_Menu *pMenu) @@ -575,7 +550,7 @@ static void cmsTraverseGlobalExit(CMS_Menu *pMenu) pMenu->onGlobalExit(); } -static long cmsMenuExit(displayPort_t *pDisplay, void *ptr) +long cmsMenuExit(displayPort_t *pDisplay, void *ptr) { if (ptr) { displayClear(pDisplay); @@ -829,7 +804,7 @@ static void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) //lastCalled = currentTime; if (key) { - rcDelay = cmsHandleKey(¤tDisplay, key); + rcDelay = cmsHandleKey(pCurrentDisplay, key); return; } @@ -838,7 +813,7 @@ static void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) if (currentTime > lastCmsHeartBeat + 500) { // Heart beat for external CMS display device @ 500msec // (Timeout @ 1000msec) - displayHeartbeat(¤tDisplay); + displayHeartbeat(pCurrentDisplay); lastCmsHeartBeat = currentTime; } } @@ -854,107 +829,15 @@ void cmsHandler(uint32_t currentTime) const uint32_t now = currentTime / 1000; if (now - lastCalled >= CMS_UPDATE_INTERVAL) { - cmsUpdate(¤tDisplay, now); + cmsUpdate(pCurrentDisplay, now); lastCalled = now; } } +// Will initializing with menuMain be better? +// Can it be done with the current main()? void cmsInit(void) { } -// -// Built-in menu contents and support functions -// - -// Info - -static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; -static char infoTargetName[] = __TARGET__; - -#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere - -static OSD_Entry menuInfoEntries[] = { - { "--- INFO ---", OME_Label, NULL, NULL, 0 }, - { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 }, - { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, - { "GITREV", OME_String, NULL, infoGitRev, 0 }, - { "TARGET", OME_String, NULL, infoTargetName, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } -}; - -static CMS_Menu menuInfo = { - "MENUINFO", - OME_MENU, - cmsx_InfoInit, - NULL, - NULL, - menuInfoEntries, -}; - -static long cmsx_InfoInit(void) -{ - for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { - if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') - infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; - else - infoGitRev[i] = shortGitRevision[i]; - } - - return 0; -} - -// Features - -static OSD_Entry menuFeaturesEntries[] = -{ - {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, - {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0}, -#if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, -#endif // VTX || USE_RTC6705 -#ifdef LED_STRIP - {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, -#endif // LED_STRIP - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -static CMS_Menu menuFeatures = { - "MENUFEATURES", - OME_MENU, - NULL, - NULL, - NULL, - menuFeaturesEntries, -}; - -// Main - -static OSD_Entry menuMainEntries[] = -{ - {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, - {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0}, - {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, -#ifdef OSD - {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0}, - {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, -#endif - {"ERR DEMO", OME_Submenu, cmsMenuChange, &menuErr, 0}, - {"REAL ERR", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0}, - {"FC&FW INFO2", OME_Submenu, cmsMenuChange, &menuInfo, 0}, - {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, - {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, - {NULL,OME_END, NULL, NULL, 0} -}; - -static CMS_Menu menuMain = { - "MENUMAIN", - OME_MENU, - NULL, - NULL, - NULL, - menuMainEntries, -}; #endif // CMS diff --git a/src/main/io/cms.h b/src/main/io/cms.h index e1a1c3be6b..ff3fa46bf2 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -3,15 +3,14 @@ #include "drivers/display.h" // Device management -typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *pPort); -bool cmsDeviceRegister(cmsDeviceInitFuncPtr); +bool cmsDisplayPortRegister(displayPort_t *pDisplay); // For main.c and scheduler void cmsInit(void); void cmsHandler(uint32_t currentTime); long cmsMenuChange(displayPort_t *pPort, void *ptr); -//long cmsMenuExit(displayPort_t *pPort, void *ptr); +long cmsMenuExit(displayPort_t *pPort, void *ptr); #define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" #define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" diff --git a/src/main/io/cms_builtin.c b/src/main/io/cms_builtin.c new file mode 100644 index 0000000000..518c283fbe --- /dev/null +++ b/src/main/io/cms_builtin.c @@ -0,0 +1,125 @@ +// +// Built-in menu contents and support functions +// + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_imu.h" + +// Sub menus + +#include "io/cms_imu.h" +#include "io/cms_blackbox.h" +#include "io/cms_vtx.h" +#ifdef OSD +#include "io/cms_osd.h" +#endif +#include "io/cms_ledstrip.h" + + +// Info + +static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; +static char infoTargetName[] = __TARGET__; + +#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere + +static long cmsx_InfoInit(void) +{ + for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { + if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') + infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; + else + infoGitRev[i] = shortGitRevision[i]; + } + + return 0; +} + +static OSD_Entry menuInfoEntries[] = { + { "--- INFO ---", OME_Label, NULL, NULL, 0 }, + { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 }, + { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, + { "GITREV", OME_String, NULL, infoGitRev, 0 }, + { "TARGET", OME_String, NULL, infoTargetName, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu menuInfo = { + "MENUINFO", + OME_MENU, + cmsx_InfoInit, + NULL, + NULL, + menuInfoEntries, +}; + +// Features + +static OSD_Entry menuFeaturesEntries[] = +{ + {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, + {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0}, +#if defined(VTX) || defined(USE_RTC6705) + {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, +#endif // VTX || USE_RTC6705 +#ifdef LED_STRIP + {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, +#endif // LED_STRIP + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +static CMS_Menu menuFeatures = { + "MENUFEATURES", + OME_MENU, + NULL, + NULL, + NULL, + menuFeaturesEntries, +}; + +// Main + +static OSD_Entry menuMainEntries[] = +{ + {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, + {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0}, + {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, +#ifdef OSD + {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, +#endif + {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, +#ifdef CMS_MENU_DEBUG + {"ERR SAMPLE", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0}, +#endif + {NULL,OME_END, NULL, NULL, 0} +}; + +CMS_Menu menuMain = { + "MENUMAIN", + OME_MENU, + NULL, + NULL, + NULL, + menuMainEntries, +}; + +#endif diff --git a/src/main/io/cms_builtin.h b/src/main/io/cms_builtin.h new file mode 100644 index 0000000000..f5ef1e0fc7 --- /dev/null +++ b/src/main/io/cms_builtin.h @@ -0,0 +1 @@ +extern CMS_Menu menuMain; diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index b12b451264..5800f216f1 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -56,9 +56,6 @@ #include "io/cms.h" #include "io/displayport_oled.h" -displayPort_t *displayPort; -bool dashboardInCMS = false; // temporary - #ifdef GPS #include "io/gps.h" #include "flight/navigation.h" @@ -591,8 +588,7 @@ void dashboardUpdate(uint32_t currentTime) static uint8_t previousArmedState = 0; #ifdef OLEDCMS - if (dashboardInCMS) return; - if (displayPort && displayPort->inCMS) { + if (oledDisplayPort.inCMS) { return; } #endif @@ -702,12 +698,6 @@ void dashboardSetPage(pageId_e pageId) pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; } -void dashboardCmsInit(displayPort_t *displayPortToUse) -{ - displayPort = displayPortToUse; - displayPortOledInit(displayPort); -} - void dashboardInit(rxConfig_t *rxConfigToUse) { delay(200); @@ -715,7 +705,7 @@ void dashboardInit(rxConfig_t *rxConfigToUse) delay(200); #if defined(CMS) && defined(OLEDCMS) - cmsDeviceRegister(dashboardCmsInit); + displayPortOledInit(); #endif rxConfig = rxConfigToUse; diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c index a8689ecaa9..e77abb5cea 100644 --- a/src/main/io/displayport_oled.c +++ b/src/main/io/displayport_oled.c @@ -27,21 +27,21 @@ #include "drivers/display.h" #include "drivers/display_ug2864hsweg01.h" +#include "io/cms.h" #include "io/displayport_oled.h" -extern bool dashboardInCMS; // temporary +// Exported +displayPort_t oledDisplayPort; static int oledOpen(displayPort_t *displayPort) { -dashboardInCMS = true; - displayPort->inCMS = true; + UNUSED(displayPort); return 0; } static int oledClose(displayPort_t *displayPort) { -dashboardInCMS = false; - displayPort->inCMS = false; + UNUSED(displayPort); return 0; } @@ -87,10 +87,13 @@ static const displayPortVTable_t oledVTable = { .txBytesFree = oledTxBytesFree }; -void displayPortOledInit(displayPort_t *displayPort) +void displayPortOledInit() { - displayPort->vTable = &oledVTable; - displayPort->rows = SCREEN_CHARACTER_ROW_COUNT; - displayPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; + oledDisplayPort.vTable = &oledVTable; + oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT; + oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT; + oledDisplayPort.inCMS = false; + + cmsDisplayPortRegister(&oledDisplayPort); } #endif // OLEDCMS diff --git a/src/main/io/displayport_oled.h b/src/main/io/displayport_oled.h index 4bd02cf652..2d0bc20509 100644 --- a/src/main/io/displayport_oled.h +++ b/src/main/io/displayport_oled.h @@ -17,5 +17,6 @@ #pragma once -struct displayPort_s; -void displayPortOledInit(struct displayPort_s *displayPort); +void displayPortOledInit(void); + +extern displayPort_t oledDisplayPort; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ed220fa2d2..c6aa8dde40 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -92,8 +92,6 @@ void osdEditElement(void *ptr); void osdDrawElements(void); void osdDrawSingleElement(uint8_t item); -bool 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 @@ -110,7 +108,7 @@ void osdDrawElements(void) if (false) ; #endif - else if (sensors(SENSOR_ACC) || osdInMenu) + else if (sensors(SENSOR_ACC) || osd7456DisplayPort.inCMS) { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); osdDrawSingleElement(OSD_CROSSHAIRS); @@ -129,7 +127,7 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_ALTITUDE); #ifdef GPS - if (sensors(SENSOR_GPS) || osdInMenu) { + if (sensors(SENSOR_GPS) || osd7456DisplayPort.inCMS) { osdDrawSingleElement(OSD_GPS_SATS); osdDrawSingleElement(OSD_GPS_SPEED); } @@ -400,7 +398,7 @@ void osdInit(void) refreshTimeout = 4 * REFRESH_1S; #ifdef CMS - cmsDeviceRegister(osdMax7456Init); + osd7456DisplayPortInit(); #endif } @@ -576,7 +574,7 @@ void updateOsd(uint32_t currentTime) max7456DrawScreen(); // do not allow ARM if we are in menu - if (osdInMenu) + if (osd7456DisplayPort.inCMS) DISABLE_ARMING_FLAG(OK_TO_ARM); } @@ -622,7 +620,7 @@ void osdUpdate(uint32_t currentTime) blinkState = (millis() / 200) % 2; - if (!osdInMenu) { + if (!osd7456DisplayPort.inCMS) { osdUpdateAlarms(); osdDrawElements(); #ifdef OSD_CALLS_CMS diff --git a/src/main/io/osd_max7456.c b/src/main/io/osd_max7456.c index 7471910c6e..66d89c1e4e 100644 --- a/src/main/io/osd_max7456.c +++ b/src/main/io/osd_max7456.c @@ -12,17 +12,16 @@ #include "drivers/display.h" #include "drivers/max7456.h" -extern bool osdInMenu; +displayPort_t osd7456DisplayPort; + extern uint16_t refreshTimeout; void osdResetAlarms(void); -uint8_t shiftdown; - static int osdMenuBegin(displayPort_t *displayPort) { UNUSED(displayPort); osdResetAlarms(); - osdInMenu = true; + displayPort->inCMS = true; refreshTimeout = 0; return 0; @@ -31,7 +30,7 @@ static int osdMenuBegin(displayPort_t *displayPort) static int osdMenuEnd(displayPort_t *displayPort) { UNUSED(displayPort); - osdInMenu = false; + displayPort->inCMS = false; return 0; } @@ -47,7 +46,7 @@ static int osdClearScreen(displayPort_t *displayPort) static int osdWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s) { UNUSED(displayPort); - max7456Write(x, y + shiftdown, s); + max7456Write(x, y, s); return 0; } @@ -56,7 +55,7 @@ static void osdResync(displayPort_t *displayPort) { UNUSED(displayPort); max7456RefreshAll(); - displayPort->rows = max7456GetRowsCount() - masterConfig.osdProfile.row_shiftdown; + displayPort->rows = max7456GetRowsCount(); displayPort->cols = 30; } @@ -82,9 +81,11 @@ displayPortVTable_t osdVTable = { osdTxBytesFree, }; -void osdMax7456Init(displayPort_t *displayPort) +void osd7456DisplayPortInit(void) { - displayPort->vTable = &osdVTable; - osdResync(displayPort); + osd7456DisplayPort.vTable = &osdVTable; + osd7456DisplayPort.inCMS = false; + osdResync(&osd7456DisplayPort); + cmsDisplayPortRegister(&osd7456DisplayPort); } #endif // OSD diff --git a/src/main/io/osd_max7456.h b/src/main/io/osd_max7456.h index 26d696f304..22c64faa79 100644 --- a/src/main/io/osd_max7456.h +++ b/src/main/io/osd_max7456.h @@ -1,3 +1,5 @@ #pragma once -void osdMax7456Init(displayPort_t *displayPort); +extern displayPort_t osd7456DisplayPort; + +void osd7456DisplayPortInit(void); diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index e30c6530a8..756617182c 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -18,6 +18,7 @@ TARGET_SRC = \ io/osd_max7456.c \ io/canvas.c \ io/cms.c \ + io/cms_builtin.c \ io/cms_imu.c \ io/cms_blackbox.c \ io/cms_vtx.c \ diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index 5cefb51d21..d805edc7f7 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -9,6 +9,7 @@ TARGET_SRC = \ io/osd.c \ io/osd_max7456.c \ io/cms.c \ + io/cms_builtin.c \ io/cms_imu.c \ io/cms_blackbox.c \ io/cms_ledstrip.c \ diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 038c7da7d6..f123795910 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -16,6 +16,7 @@ TARGET_SRC = \ io/osd_max7456.c \ io/canvas.c \ io/cms.c \ + io/cms_builtin.c \ io/cms_imu.c \ io/cms_blackbox.c \ io/cms_vtx.c \ diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index f2a3df7442..b37d1b2b0e 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -11,6 +11,7 @@ TARGET_SRC = \ drivers/compass_hmc5883l.c \ io/canvas.c \ io/cms.c \ + io/cms_builtin.c \ io/cms_imu.c \ io/cms_blackbox.c \ io/cms_vtx.c \ diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 706b68492b..8189f15f4a 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -30,6 +30,7 @@ TARGET_SRC = \ io/osd_max7456.c \ io/canvas.c \ io/cms.c \ + io/cms_builtin.c \ io/cms_imu.c \ io/cms_blackbox.c \ io/cms_vtx.c \ From a6b03a9473af5d0857451f9e25ac4814941a1f6f Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 6 Nov 2016 04:34:19 +0900 Subject: [PATCH 121/188] Touch-ups after rebase MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Led strip’s color manipulation was dropped due to the drastically change in the config structure; led strip menu now only has feature on/off. --- src/main/io/cms.c | 18 ++++++--- src/main/io/cms_blackbox.c | 18 +++++---- src/main/io/cms_builtin.c | 1 - src/main/io/cms_imu.c | 42 +++++++++---------- src/main/io/cms_ledstrip.c | 83 +++++--------------------------------- src/main/io/cms_vtx.c | 26 +++++++----- src/main/io/osd.h | 1 + src/main/io/osd_max7456.c | 5 +-- 8 files changed, 72 insertions(+), 122 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 4fe5e59252..d39aafa032 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -542,12 +542,18 @@ static void cmsTraverseGlobalExit(CMS_Menu *pMenu) { OSD_Entry *p; - for (p = pMenu->entries; p->type != OME_END ; p++) - if (p->type == OME_Submenu) - cmsTraverseGlobalExit(p->data); + debug[0]++; - if (pMenu->onGlobalExit) + for (p = pMenu->entries; p->type != OME_END ; p++) { + if (p->type == OME_Submenu) { + cmsTraverseGlobalExit(p->data); + } + } + + if (pMenu->onGlobalExit) { + debug[1]++; pMenu->onGlobalExit(); + } } long cmsMenuExit(displayPort_t *pDisplay, void *ptr) @@ -566,6 +572,8 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr) if (currentMenu->onExit) currentMenu->onExit((OSD_Entry *)NULL); // Forced exit + + saveConfigAndNotify(); } cmsInMenu = false; @@ -834,7 +842,7 @@ void cmsHandler(uint32_t currentTime) } } -// Will initializing with menuMain be better? +// Is initializing with menuMain better? // Can it be done with the current main()? void cmsInit(void) { diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c index e3ed288abf..bdcdc085b6 100644 --- a/src/main/io/cms_blackbox.c +++ b/src/main/io/cms_blackbox.c @@ -26,7 +26,7 @@ #include "io/flashfs.h" #ifdef USE_FLASHFS -long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) +static long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) { UNUSED(ptr); @@ -46,16 +46,20 @@ long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) } #endif // USE_FLASHFS -uint8_t cmsx_FeatureBlackbox; +static bool featureRead = false; +static uint8_t cmsx_FeatureBlackbox; -long cmsx_Blackbox_FeatureRead(void) +static long cmsx_Blackbox_FeatureRead(void) { - cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; + if (!featureRead) { + cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; + featureRead = true; + } return 0; } -long cmsx_Blackbox_FeatureWriteback(void) +static long cmsx_Blackbox_FeatureWriteback(void) { if (cmsx_FeatureBlackbox) featureSet(FEATURE_BLACKBOX); @@ -65,9 +69,9 @@ long cmsx_Blackbox_FeatureWriteback(void) return 0; } -OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; +static OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; -OSD_Entry cmsx_menuBlackboxEntries[] = +static OSD_Entry cmsx_menuBlackboxEntries[] = { {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, {"ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0}, diff --git a/src/main/io/cms_builtin.c b/src/main/io/cms_builtin.c index 518c283fbe..ba42784d4c 100644 --- a/src/main/io/cms_builtin.c +++ b/src/main/io/cms_builtin.c @@ -121,5 +121,4 @@ CMS_Menu menuMain = { NULL, menuMainEntries, }; - #endif diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c index c4a71aa3f5..0f6c28f1ee 100644 --- a/src/main/io/cms_imu.c +++ b/src/main/io/cms_imu.c @@ -31,9 +31,9 @@ #include "config/config_master.h" #include "config/feature.h" -OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; +static OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; -uint8_t tempPid[4][3]; +static 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}; @@ -47,7 +47,7 @@ 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}; -long cmsx_PidRead(void) +static long cmsx_PidRead(void) { uint8_t i; @@ -63,7 +63,7 @@ long cmsx_PidRead(void) return 0; } -long cmsx_PidWriteback(OSD_Entry *self) +static long cmsx_PidWriteback(OSD_Entry *self) { UNUSED(self); @@ -82,7 +82,7 @@ long cmsx_PidWriteback(OSD_Entry *self) return 0; } -OSD_Entry cmsx_menuPidEntries[] = +static OSD_Entry cmsx_menuPidEntries[] = { {"--- PID ---", OME_Label, NULL, NULL, 0}, {"ROLL P", OME_UINT8, NULL, &entryRollP, 0}, @@ -101,7 +101,7 @@ OSD_Entry cmsx_menuPidEntries[] = {NULL, OME_END, NULL, NULL, 0} }; -CMS_Menu cmsx_menuPid = { +static CMS_Menu cmsx_menuPid = { "MENUPID", OME_MENU, cmsx_PidRead, @@ -113,7 +113,7 @@ CMS_Menu cmsx_menuPid = { // // Rate & Expo // -controlRateConfig_t rateProfile; +static 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}; @@ -127,14 +127,14 @@ 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}; -long cmsx_RateExpoRead(void) +static long cmsx_RateExpoRead(void) { memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); return 0; } -long cmsx_RateExpoWriteback(OSD_Entry *self) +static long cmsx_RateExpoWriteback(OSD_Entry *self) { UNUSED(self); @@ -143,7 +143,7 @@ long cmsx_RateExpoWriteback(OSD_Entry *self) return 0; } -long cmsx_menuRcConfirmBack(OSD_Entry *self) +static long cmsx_menuRcConfirmBack(OSD_Entry *self) { if (self && self->type == OME_Back) return 0; @@ -151,7 +151,7 @@ long cmsx_menuRcConfirmBack(OSD_Entry *self) return -1; } -OSD_Entry cmsx_menuRateExpoEntries[] = +static OSD_Entry cmsx_menuRateExpoEntries[] = { {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0}, {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0}, @@ -191,7 +191,7 @@ 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 cmsx_menuRcEntries[] = +static OSD_Entry cmsx_menuRcEntries[] = { {"--- RC PREV ---", OME_Label, NULL, NULL, 0}, {"ROLL", OME_INT16, NULL, &entryRcRoll, DYNAMIC}, @@ -219,15 +219,15 @@ CMS_Menu cmsx_menuRc = { // // 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}; +static OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; +static OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; +static OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5}; +static OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5}; +static OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5}; +static OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; +static OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; -OSD_Entry menuImuMiscEntries[]= +static OSD_Entry menuImuMiscEntries[]= { {"--- MISC ---", OME_Label, NULL, NULL, 0}, {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, @@ -250,7 +250,7 @@ CMS_Menu menuImuMisc = { menuImuMiscEntries, }; -OSD_Entry cmsx_menuImuEntries[] = +static OSD_Entry cmsx_menuImuEntries[] = { {"--- CFG.IMU ---", OME_Label, NULL, NULL, 0}, {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, diff --git a/src/main/io/cms_ledstrip.c b/src/main/io/cms_ledstrip.c index 62d473055c..0daddaf6a9 100644 --- a/src/main/io/cms_ledstrip.c +++ b/src/main/io/cms_ledstrip.c @@ -21,69 +21,20 @@ #ifdef LED_STRIP -//local variable to keep color value -uint8_t ledColor; +static bool featureRead = false; +static uint8_t cmsx_FeatureLedstrip; -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 -void cmsx_GetLedColor(void) +static long cmsx_Ledstrip_FeatureRead(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 long 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); + if (!featureRead) { + cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; + featureRead = true; } return 0; } -uint8_t cmsx_FeatureLedstrip; - -OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; - -long cmsx_Ledstrip_FeatureRead(void) -{ - cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; - - return 0; -} - -long cmsx_Ledstrip_FeatureWriteback(void) +static long cmsx_Ledstrip_FeatureWriteback(void) { if (cmsx_FeatureLedstrip) featureSet(FEATURE_LED_STRIP); @@ -93,26 +44,10 @@ long cmsx_Ledstrip_FeatureWriteback(void) return 0; } -long cmsx_Ledstrip_ConfigRead(void) -{ - cmsx_GetLedColor(); - - return 0; -} - -long cmsx_Ledstrip_onEnter(void) -{ - cmsx_Ledstrip_FeatureRead(); - cmsx_Ledstrip_ConfigRead(); - - return 0; -} - -OSD_Entry cmsx_menuLedstripEntries[] = +static OSD_Entry cmsx_menuLedstripEntries[] = { {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, {"ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; @@ -120,7 +55,7 @@ OSD_Entry cmsx_menuLedstripEntries[] = CMS_Menu cmsx_menuLedstrip = { "MENULED", OME_MENU, - cmsx_Ledstrip_onEnter, + cmsx_Ledstrip_FeatureRead, NULL, cmsx_Ledstrip_FeatureWriteback, cmsx_menuLedstripEntries, diff --git a/src/main/io/cms_vtx.c b/src/main/io/cms_vtx.c index 1bd2b49e33..903d5acdf6 100644 --- a/src/main/io/cms_vtx.c +++ b/src/main/io/cms_vtx.c @@ -18,14 +18,18 @@ #if defined(VTX) || defined(USE_RTC6705) -uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; +static bool featureRead = false; +static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; -void cmsx_Vtx_FeatureRead(void) +static void cmsx_Vtx_FeatureRead(void) { - cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; + if (!featureRead) { + cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; + featureRead = true; + } } -void cmsx_Vtx_FeatureWriteback(void) +static void cmsx_Vtx_FeatureWriteback(void) { if (cmsx_featureVtx) featureSet(FEATURE_VTX); @@ -41,10 +45,10 @@ static const char * const vtxBandNames[] = { "RACEBAND", }; -OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; -OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; +static OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; +static OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; -void cmsx_Vtx_ConfigRead(void) +static void cmsx_Vtx_ConfigRead(void) { #ifdef VTX cmsx_vtxBand = masterConfig.vtxBand; @@ -57,7 +61,7 @@ void cmsx_Vtx_ConfigRead(void) #endif // USE_RTC6705 } -void cmsx_Vtx_ConfigWriteback(void) +static void cmsx_Vtx_ConfigWriteback(void) { #ifdef VTX masterConfig.vtxBand = cmsx_vtxBand; @@ -70,11 +74,11 @@ void cmsx_Vtx_ConfigWriteback(void) } #ifdef VTX -OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; -OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; +static OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; +static OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; #endif // VTX -OSD_Entry cmsx_menuVtxEntries[] = +static OSD_Entry cmsx_menuVtxEntries[] = { {"--- VTX ---", OME_Label, NULL, NULL, 0}, {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0}, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 24cf3ad99b..3f4400d74d 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -68,6 +68,7 @@ typedef struct { void updateOsd(uint32_t currentTime); void osdInit(void); void resetOsdConfig(osd_profile_t *osdProfile); +void osdResetAlarms(void); #ifdef CMS void osdCmsInit(displayPort_t *); diff --git a/src/main/io/osd_max7456.c b/src/main/io/osd_max7456.c index 66d89c1e4e..52ac8d6bc2 100644 --- a/src/main/io/osd_max7456.c +++ b/src/main/io/osd_max7456.c @@ -12,10 +12,9 @@ #include "drivers/display.h" #include "drivers/max7456.h" -displayPort_t osd7456DisplayPort; +displayPort_t osd7456DisplayPort; // Referenced from osd.c extern uint16_t refreshTimeout; -void osdResetAlarms(void); static int osdMenuBegin(displayPort_t *displayPort) { @@ -71,7 +70,7 @@ static uint32_t osdTxBytesFree(displayPort_t *displayPort) return UINT32_MAX; } -displayPortVTable_t osdVTable = { +static displayPortVTable_t osdVTable = { osdMenuBegin, osdMenuEnd, osdClearScreen, From 0c57456ae7cb1b63458b5c5b0a0129e8c772cda1 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 6 Nov 2016 10:21:57 +0900 Subject: [PATCH 122/188] Fix reference to cms in non-CMS case (osd.c) --- src/main/io/osd.c | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c6aa8dde40..903cbb5b95 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -108,7 +108,11 @@ void osdDrawElements(void) if (false) ; #endif +#ifdef CMS else if (sensors(SENSOR_ACC) || osd7456DisplayPort.inCMS) +#else + else if (sensors(SENSOR_ACC)) +#endif { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); osdDrawSingleElement(OSD_CROSSHAIRS); @@ -127,7 +131,12 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_ALTITUDE); #ifdef GPS - if (sensors(SENSOR_GPS) || osd7456DisplayPort.inCMS) { +#ifdef CMS + if (sensors(SENSOR_GPS) || osd7456DisplayPort.inCMS) +#else + if (sensors(SENSOR_GPS)) +#endif + { osdDrawSingleElement(OSD_GPS_SATS); osdDrawSingleElement(OSD_GPS_SPEED); } @@ -573,9 +582,11 @@ void updateOsd(uint32_t currentTime) else // rest of time redraw screen 10 chars per idle to don't lock the main idle max7456DrawScreen(); +#ifdef CMS // do not allow ARM if we are in menu if (osd7456DisplayPort.inCMS) DISABLE_ARMING_FLAG(OK_TO_ARM); +#endif } void osdUpdate(uint32_t currentTime) @@ -620,6 +631,7 @@ void osdUpdate(uint32_t currentTime) blinkState = (millis() / 200) % 2; +#ifdef CMS if (!osd7456DisplayPort.inCMS) { osdUpdateAlarms(); osdDrawElements(); @@ -628,6 +640,7 @@ void osdUpdate(uint32_t currentTime) cmsUpdate(currentTime); #endif } +#endif } #ifdef EDIT_ELEMENT_SUPPORT From 31b6e7e02482cf1c832bcf62490b7283c4e49aea Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 6 Nov 2016 16:54:53 +0900 Subject: [PATCH 123/188] Menu tidy --- src/main/io/cms_blackbox.c | 16 +-- src/main/io/cms_imu.c | 221 +++++++++++++++++-------------------- src/main/io/cms_ledstrip.c | 9 +- 3 files changed, 117 insertions(+), 129 deletions(-) diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c index bdcdc085b6..0de1ac8eb1 100644 --- a/src/main/io/cms_blackbox.c +++ b/src/main/io/cms_blackbox.c @@ -69,18 +69,18 @@ static long cmsx_Blackbox_FeatureWriteback(void) return 0; } -static OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; - static OSD_Entry cmsx_menuBlackboxEntries[] = { - {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0}, + { "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, + { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, + { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &masterConfig.blackbox_rate_denom,1,32,1 }, 0 }, + #ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0}, + { "ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0 }, #endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; CMS_Menu cmsx_menuBlackbox = { diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c index 0f6c28f1ee..92329f9d57 100644 --- a/src/main/io/cms_imu.c +++ b/src/main/io/cms_imu.c @@ -31,34 +31,39 @@ #include "config/config_master.h" #include "config/feature.h" -static OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; - +// +// PID +// +static uint8_t pidProfileIndex; static 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 long cmsx_menuImu_onEnter(void) +{ + pidProfileIndex = masterConfig.current_profile_index + 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}; + return 0; +} -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}; +static long cmsx_menuImu_onExit(OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.current_profile_index = pidProfileIndex - 1; + + return 0; +} static long 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]; + for (uint8_t i = 0; i < 3; i++) { + tempPid[i][0] = masterConfig.profile[pidProfileIndex].pidProfile.P8[i]; + tempPid[i][1] = masterConfig.profile[pidProfileIndex].pidProfile.I8[i]; + tempPid[i][2] = masterConfig.profile[pidProfileIndex].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]; + tempPid[3][0] = masterConfig.profile[pidProfileIndex].pidProfile.P8[PIDLEVEL]; + tempPid[3][1] = masterConfig.profile[pidProfileIndex].pidProfile.I8[PIDLEVEL]; + tempPid[3][2] = masterConfig.profile[pidProfileIndex].pidProfile.D8[PIDLEVEL]; return 0; } @@ -67,42 +72,41 @@ static long cmsx_PidWriteback(OSD_Entry *self) { UNUSED(self); - 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]; + for (uint8_t i = 0; i < 3; i++) { + masterConfig.profile[pidProfileIndex].pidProfile.P8[i] = tempPid[i][0]; + masterConfig.profile[pidProfileIndex].pidProfile.I8[i] = tempPid[i][1]; + masterConfig.profile[pidProfileIndex].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]; + masterConfig.profile[pidProfileIndex].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + masterConfig.profile[pidProfileIndex].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + masterConfig.profile[pidProfileIndex].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; return 0; } static OSD_Entry cmsx_menuPidEntries[] = { - {"--- 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}, + { "-- PID --", OME_Label, NULL, NULL, 0}, - {"PITCH P", OME_UINT8, NULL, &entryPitchP, 0}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI, 0}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD, 0}, + { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][0], 10, 150, 1 }, 0 }, + { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][1], 1, 150, 1 }, 0 }, + { "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][2], 0, 150, 1 }, 0 }, - {"YAW P", OME_UINT8, NULL, &entryYawP, 0}, - {"YAW I", OME_UINT8, NULL, &entryYawI, 0}, - {"YAW D", OME_UINT8, NULL, &entryYawD, 0}, + { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][0], 10, 150, 1 }, 0 }, + { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][1], 1, 150, 1 }, 0 }, + { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][2], 0, 150, 1 }, 0 }, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} + { "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][0], 10, 150, 1 }, 0 }, + { "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][1], 1, 150, 1 }, 0 }, + { "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][2], 0, 150, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; static CMS_Menu cmsx_menuPid = { - "MENUPID", + "XPID", OME_MENU, cmsx_PidRead, cmsx_PidWriteback, @@ -115,18 +119,6 @@ static CMS_Menu cmsx_menuPid = { // static 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}; - static long cmsx_RateExpoRead(void) { memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); @@ -153,24 +145,29 @@ static long cmsx_menuRcConfirmBack(OSD_Entry *self) static OSD_Entry cmsx_menuRateExpoEntries[] = { - {"--- 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} + { "-- RATE&EXPO --", OME_Label, NULL, NULL, 0 }, + + { "RC RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRate8, 0, 200, 1, 10 }, 0 }, + { "RC YAW RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawRate8, 0, 200, 1, 10 }, 0 }, + + { "ROLL SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[0], 0, 250, 1, 10 }, 0 }, + { "PITCH SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[1], 0, 250, 1, 10 }, 0 }, + { "YAW SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[2], 0, 250, 1, 10 }, 0 }, + + { "RC EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo8, 0, 100, 1, 10 }, 0 }, + { "RC YAW EXP", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawExpo8, 0, 100, 1, 10 }, 0 }, + + { "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID, 0, 70, 1, 10}, 0 }, + { "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1100, 1800, 10}, 0 }, + { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, + { "SETPT RLX", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; CMS_Menu cmsx_menuRateExpo = { - "MENURTEX", + "MENURATE", OME_MENU, cmsx_RateExpoRead, cmsx_RateExpoWriteback, @@ -182,32 +179,26 @@ CMS_Menu cmsx_menuRateExpo = { // // 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}; - static OSD_Entry cmsx_menuRcEntries[] = { - {"--- RC PREV ---", OME_Label, NULL, NULL, 0}, - {"ROLL", OME_INT16, NULL, &entryRcRoll, DYNAMIC}, - {"PITCH", OME_INT16, NULL, &entryRcPitch, DYNAMIC}, - {"THR", OME_INT16, NULL, &entryRcThr, DYNAMIC}, - {"YAW", OME_INT16, NULL, &entryRcYaw, DYNAMIC}, - {"AUX1", OME_INT16, NULL, &entryRcAux1, DYNAMIC}, - {"AUX2", OME_INT16, NULL, &entryRcAux2, DYNAMIC}, - {"AUX3", OME_INT16, NULL, &entryRcAux3, DYNAMIC}, - {"AUX4", OME_INT16, NULL, &entryRcAux4, DYNAMIC}, - {"BACK", OME_Back, NULL, NULL, 0}, + { "-- RC PREV --", OME_Label, NULL, NULL, 0}, + + { "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[ROLL], 1, 2500, 0 }, DYNAMIC }, + { "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[PITCH], 1, 2500, 0 }, DYNAMIC }, + { "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[THROTTLE], 1, 2500, 0 }, DYNAMIC }, + { "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[YAW], 1, 2500, 0 }, DYNAMIC }, + + { "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX1], 1, 2500, 0 }, DYNAMIC }, + { "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX2], 1, 2500, 0 }, DYNAMIC }, + { "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX3], 1, 2500, 0 }, DYNAMIC }, + { "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX4], 1, 2500, 0 }, DYNAMIC }, + + { "BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; -CMS_Menu cmsx_menuRc = { - "MENURC", +CMS_Menu cmsx_menuRcPreview = { + "XRCPREV", OME_MENU, NULL, cmsx_menuRcConfirmBack, @@ -219,30 +210,24 @@ CMS_Menu cmsx_menuRc = { // // Misc // -static OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; -static OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; -static OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5}; -static OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5}; -static OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5}; -static OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; -static OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; - static OSD_Entry menuImuMiscEntries[]= { - {"--- 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} + { "-- MISC --", OME_Label, NULL, NULL, 0 }, + + { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, + { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5 }, 0 }, + { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5 }, 0 }, + { "YAW P LIM", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5 }, 0 }, + { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1020, 1300, 10 }, 0 }, + { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 }, + { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0}, + { NULL, OME_END, NULL, NULL, 0} }; CMS_Menu menuImuMisc = { - "MENUIMU", + "XIMUMISC", OME_MENU, NULL, NULL, @@ -252,21 +237,23 @@ CMS_Menu menuImuMisc = { static OSD_Entry cmsx_menuImuEntries[] = { - {"--- CFG.IMU ---", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, - {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, - {"RATE&RXPO", OME_Submenu, cmsMenuChange, &cmsx_menuRateExpo, 0}, - {"RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRc, 0}, - {"MISC", OME_Submenu, cmsMenuChange, &menuImuMisc, 0}, + { "-- IMU --", OME_Label, NULL, NULL, 0}, + + {"PID PROF", OME_UINT8, NULL, &(OSD_UINT8_t){ &pidProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, + {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, + {"RATE&RXPO", OME_Submenu, cmsMenuChange, &cmsx_menuRateExpo, 0}, + {"RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, + {"MISC", OME_Submenu, cmsMenuChange, &menuImuMisc, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; CMS_Menu cmsx_menuImu = { - "MENUIMU", + "XIMU", OME_MENU, - NULL, - NULL, + cmsx_menuImu_onEnter, + cmsx_menuImu_onExit, NULL, cmsx_menuImuEntries, }; diff --git a/src/main/io/cms_ledstrip.c b/src/main/io/cms_ledstrip.c index 0daddaf6a9..284dd50b58 100644 --- a/src/main/io/cms_ledstrip.c +++ b/src/main/io/cms_ledstrip.c @@ -46,10 +46,11 @@ static long cmsx_Ledstrip_FeatureWriteback(void) static OSD_Entry cmsx_menuLedstripEntries[] = { - {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} + { "-- LED STRIP --", OME_Label, NULL, NULL, 0 }, + { "ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; CMS_Menu cmsx_menuLedstrip = { From a7a428a5bb5b7070cf138dd7b2ce833c7405cb32 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 6 Nov 2016 08:01:34 +0000 Subject: [PATCH 124/188] Code tidy and addition of GPL headers --- Makefile | 13 +++- src/main/drivers/display.c | 17 ++++-- src/main/drivers/display.h | 11 ++-- src/main/drivers/max7456.c | 4 +- src/main/drivers/max7456.h | 4 +- src/main/fc/fc_msp.c | 2 +- src/main/fc/fc_tasks.c | 4 +- src/main/fc/rc_controls.c | 2 - src/main/io/canvas.c | 69 ++++++++++++++-------- src/main/io/canvas.h | 22 ++++++- src/main/io/cms_blackbox.c | 17 ++++++ src/main/io/cms_blackbox.h | 19 ++++++ src/main/io/cms_builtin.c | 17 ++++++ src/main/io/cms_builtin.h | 19 ++++++ src/main/io/cms_imu.c | 16 +++++ src/main/io/cms_imu.h | 19 ++++++ src/main/io/cms_ledstrip.c | 17 ++++++ src/main/io/cms_ledstrip.h | 19 ++++++ src/main/io/cms_osd.h | 19 ++++++ src/main/io/cms_types.h | 17 ++++++ src/main/io/cms_vtx.c | 17 ++++++ src/main/io/cms_vtx.h | 19 ++++++ src/main/io/dashboard.c | 6 +- src/main/io/displayport_oled.c | 20 ++----- src/main/io/displayport_oled.h | 4 +- src/main/io/gps.c | 2 - src/main/io/osd.c | 28 ++++++--- src/main/io/osd.h | 24 ++------ src/main/io/osd_max7456.c | 49 ++++++++++----- src/main/io/osd_max7456.h | 21 ++++++- src/main/io/serial_cli.c | 2 +- src/main/main.c | 2 +- src/main/msp/msp.h | 1 - src/main/msp/msp_serial.c | 24 ++++---- src/main/msp/msp_serial.h | 2 +- src/main/target/OMNIBUS/target.h | 1 - src/main/target/OMNIBUS/target.mk | 13 +--- src/main/target/OMNIBUSF4/target.mk | 10 +--- src/main/target/SIRINFPV/target.mk | 11 +--- src/main/target/SPRACINGF3/target.mk | 9 +-- src/main/target/STM32F3DISCOVERY/target.mk | 12 +--- 41 files changed, 424 insertions(+), 180 deletions(-) diff --git a/Makefile b/Makefile index 99bf159fac..684dc1bfca 100644 --- a/Makefile +++ b/Makefile @@ -560,10 +560,19 @@ HIGHEND_SRC = \ flight/gtune.c \ flight/navigation.c \ flight/gps_conversion.c \ - io/gps.c \ - io/ledstrip.c \ + io/canvas.c \ + io/cms.c \ + io/cms_builtin.c \ + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c \ io/displayport_oled.c \ io/dashboard.c \ + io/gps.c \ + io/ledstrip.c \ + io/osd.c \ + io/osd_max7456.c \ sensors/sonar.c \ sensors/barometer.c \ telemetry/telemetry.c \ diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 753afacf76..2b247022d9 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -35,16 +35,25 @@ void displayOpen(displayPort_t *instance) { instance->vTable->open(instance); instance->vTable->clear(instance); - instance->inCMS = true; + instance->isOpen = true; } void displayClose(displayPort_t *instance) { instance->vTable->close(instance); - instance->inCMS = false; + instance->isOpen = false; } -int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s) +bool displayIsOpen(const displayPort_t *instance) +{ + if (instance && instance->isOpen) { // can be called before initialised + return true; + } else { + return false; + } +} + +int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s) { return instance->vTable->write(instance, x, y, s); } @@ -59,7 +68,7 @@ void displayResync(displayPort_t *instance) instance->vTable->resync(instance); } -uint16_t displayTxBytesFree(displayPort_t *instance) +uint16_t displayTxBytesFree(const displayPort_t *instance) { return instance->vTable->txBytesFree(instance); } diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index f0075ff264..aabf8517bc 100644 --- a/src/main/drivers/display.h +++ b/src/main/drivers/display.h @@ -26,23 +26,24 @@ typedef struct displayPort_s { // CMS state bool cleared; int8_t cursorRow; - bool inCMS; + bool isOpen; } displayPort_t; typedef struct displayPortVTable_s { int (*open)(displayPort_t *displayPort); int (*close)(displayPort_t *displayPort); int (*clear)(displayPort_t *displayPort); - int (*write)(displayPort_t *displayPort, uint8_t col, uint8_t row, char *text); + int (*write)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text); int (*heartbeat)(displayPort_t *displayPort); void (*resync)(displayPort_t *displayPort); - uint32_t (*txBytesFree)(displayPort_t *displayPort); + uint32_t (*txBytesFree)(const displayPort_t *displayPort); } displayPortVTable_t; void displayOpen(displayPort_t *instance); void displayClose(displayPort_t *instance); +bool displayIsOpen(const displayPort_t *instance); void displayClear(displayPort_t *instance); -int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s); +int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s); void displayHeartbeat(displayPort_t *instance); void displayResync(displayPort_t *instance); -uint16_t displayTxBytesFree(displayPort_t *instance); +uint16_t displayTxBytesFree(const displayPort_t *instance); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index a552673556..10cc071411 100755 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -294,7 +294,7 @@ void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c) screenBuffer[y*30+x] = c; } -void max7456Write(uint8_t x, uint8_t y, char *buff) +void max7456Write(uint8_t x, uint8_t y, const char *buff) { uint8_t i = 0; for (i = 0; *(buff+i); i++) @@ -387,7 +387,7 @@ void max7456RefreshAll(void) } } -void max7456WriteNvm(uint8_t char_address, uint8_t *font_data) +void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) { uint8_t x; diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 6663b12934..a4ebe0ab4e 100755 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -146,9 +146,9 @@ extern uint16_t maxScreenSize; void max7456Init(uint8_t system); void max7456DrawScreen(void); -void max7456WriteNvm(uint8_t char_address, uint8_t *font_data); +void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data); uint8_t max7456GetRowsCount(void); -void max7456Write(uint8_t x, uint8_t y, char *buff); +void max7456Write(uint8_t x, uint8_t y, const char *buff); void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c); void max7456ClearScreen(void); void max7456RefreshAll(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 57a011a1a5..d5aedd7a15 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1010,7 +1010,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, masterConfig.osdProfile.time_alarm); sbufWriteU16(dst, masterConfig.osdProfile.alt_alarm); - for (i = 0; i < OSD_MAX_ITEMS; i++) { + for (i = 0; i < OSD_ITEM_COUNT; i++) { sbufWriteU16(dst, masterConfig.osdProfile.item_pos[i]); } #else diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index d9ca1c027e..9e31554249 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -40,13 +40,11 @@ #include "flight/pid.h" #include "flight/altitudehold.h" -#include "io/cms.h" - #include "io/beeper.h" +#include "io/cms.h" #include "io/dashboard.h" #include "io/gps.h" #include "io/ledstrip.h" -#include "io/cms.h" #include "io/osd.h" #include "io/serial.h" #include "io/serial_cli.h" diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 016c4287ec..91123768e3 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -40,8 +40,6 @@ #include "fc/rc_curves.h" #include "fc/runtime_config.h" -#include "io/cms.h" - #include "io/gps.h" #include "io/beeper.h" #include "io/motors.h" diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index f805c862cf..85f0232ba7 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + #include #include #include @@ -22,47 +39,47 @@ static displayPort_t canvasDisplayPort; -static int canvasOutput(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len) +static int canvasOutput(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, int len) { UNUSED(displayPort); - mspSerialPush(cmd, buf, len); - - return 6 + len; + return mspSerialPush(cmd, buf, len); } -static int canvasBegin(displayPort_t *displayPort) +static int canvasOpen(displayPort_t *displayPort) { - uint8_t subcmd[] = { 0 }; + const uint8_t subcmd[] = { 0 }; return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); } static int canvasHeartBeat(displayPort_t *displayPort) { - return canvasBegin(displayPort); + return canvasOpen(displayPort); } -static int canvasEnd(displayPort_t *displayPort) +static int canvasClose(displayPort_t *displayPort) { - uint8_t subcmd[] = { 1 }; + const uint8_t subcmd[] = { 1 }; return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); } static int canvasClear(displayPort_t *displayPort) { - uint8_t subcmd[] = { 2 }; + const uint8_t subcmd[] = { 2 }; return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); } -static int canvasWrite(displayPort_t *displayPort, uint8_t col, uint8_t row, char *string) +static int canvasWrite(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string) { - int len; - uint8_t buf[30 + 4]; +#define MSP_CANVAS_MAX_STRING_LENGTH 30 + uint8_t buf[MSP_CANVAS_MAX_STRING_LENGTH + 4]; - if ((len = strlen(string)) >= 30) - len = 30; + int len = strlen(string); + if (len >= MSP_CANVAS_MAX_STRING_LENGTH) { + len = MSP_CANVAS_MAX_STRING_LENGTH; + } buf[0] = 3; buf[1] = row; @@ -79,27 +96,27 @@ static void canvasResync(displayPort_t *displayPort) displayPort->cols = 30; } -static uint32_t canvasTxBytesFree(displayPort_t *displayPort) +static uint32_t canvasTxBytesFree(const displayPort_t *displayPort) { UNUSED(displayPort); return mspSerialTxBytesFree(); } static const displayPortVTable_t canvasVTable = { - canvasBegin, - canvasEnd, - canvasClear, - canvasWrite, - canvasHeartBeat, - canvasResync, - canvasTxBytesFree, + .open = canvasOpen, + .close = canvasClose, + .clear = canvasClear, + .write = canvasWrite, + .heartbeat = canvasHeartBeat, + .resync = canvasResync, + .txBytesFree = canvasTxBytesFree }; -void canvasInit() +displayPort_t *canvasInit(void) { canvasDisplayPort.vTable = &canvasVTable; - canvasDisplayPort.inCMS = false; + canvasDisplayPort.isOpen = false; canvasResync(&canvasDisplayPort); - cmsDisplayPortRegister(&canvasDisplayPort); + return &canvasDisplayPort; } #endif diff --git a/src/main/io/canvas.h b/src/main/io/canvas.h index d288656f6a..7a5520e23f 100644 --- a/src/main/io/canvas.h +++ b/src/main/io/canvas.h @@ -1,3 +1,21 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + #pragma once -void canvasInit(void); -void canvasCmsInit(displayPort_t *dPort); + +struct displayPort_s; +struct displayPort_s *canvasInit(void); diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c index 0de1ac8eb1..a3ab982f13 100644 --- a/src/main/io/cms_blackbox.c +++ b/src/main/io/cms_blackbox.c @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + // // CMS things for blackbox and flashfs. // diff --git a/src/main/io/cms_blackbox.h b/src/main/io/cms_blackbox.h index 11f8b1b450..a35ac4211b 100644 --- a/src/main/io/cms_blackbox.h +++ b/src/main/io/cms_blackbox.h @@ -1 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + extern CMS_Menu cmsx_menuBlackbox; diff --git a/src/main/io/cms_builtin.c b/src/main/io/cms_builtin.c index ba42784d4c..3275f8ac9a 100644 --- a/src/main/io/cms_builtin.c +++ b/src/main/io/cms_builtin.c @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + // // Built-in menu contents and support functions // diff --git a/src/main/io/cms_builtin.h b/src/main/io/cms_builtin.h index f5ef1e0fc7..7717c5998d 100644 --- a/src/main/io/cms_builtin.h +++ b/src/main/io/cms_builtin.h @@ -1 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + extern CMS_Menu menuMain; diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c index 92329f9d57..d2772a62ef 100644 --- a/src/main/io/cms_imu.c +++ b/src/main/io/cms_imu.c @@ -1,3 +1,19 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ // Menu contents for PID, RATES, RC preview, misc // Should be part of the relevant .c file. diff --git a/src/main/io/cms_imu.h b/src/main/io/cms_imu.h index 9fb44f05b9..a5e05e3103 100644 --- a/src/main/io/cms_imu.h +++ b/src/main/io/cms_imu.h @@ -1 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + extern CMS_Menu cmsx_menuImu; diff --git a/src/main/io/cms_ledstrip.c b/src/main/io/cms_ledstrip.c index 284dd50b58..08186a1af6 100644 --- a/src/main/io/cms_ledstrip.c +++ b/src/main/io/cms_ledstrip.c @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + #include #include #include diff --git a/src/main/io/cms_ledstrip.h b/src/main/io/cms_ledstrip.h index 52081b0177..f740b8911c 100644 --- a/src/main/io/cms_ledstrip.h +++ b/src/main/io/cms_ledstrip.h @@ -1 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + extern CMS_Menu cmsx_menuLedstrip; diff --git a/src/main/io/cms_osd.h b/src/main/io/cms_osd.h index 9e780ecc4e..9b0b001d28 100644 --- a/src/main/io/cms_osd.h +++ b/src/main/io/cms_osd.h @@ -1,2 +1,21 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + extern CMS_Menu cmsx_menuAlarms; extern CMS_Menu cmsx_menuOsdLayout; diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index 5dbafd6e02..a2336e2c77 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + // // Menu element types // XXX Upon separation, all OME would be renamed to CME_ or similar. diff --git a/src/main/io/cms_vtx.c b/src/main/io/cms_vtx.c index 903d5acdf6..cf0333830c 100644 --- a/src/main/io/cms_vtx.c +++ b/src/main/io/cms_vtx.c @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + #include #include #include diff --git a/src/main/io/cms_vtx.h b/src/main/io/cms_vtx.h index f0541c0370..2e15372cad 100644 --- a/src/main/io/cms_vtx.h +++ b/src/main/io/cms_vtx.h @@ -1 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + extern CMS_Menu cmsx_menuVtx; diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 5800f216f1..017ea4cfff 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -83,6 +83,7 @@ static uint32_t nextDisplayUpdateAt = 0; static bool dashboardPresent = false; static rxConfig_t *rxConfig; +static displayPort_t *displayPort; #define PAGE_TITLE_LINE_COUNT 1 @@ -588,7 +589,7 @@ void dashboardUpdate(uint32_t currentTime) static uint8_t previousArmedState = 0; #ifdef OLEDCMS - if (oledDisplayPort.inCMS) { + if (displayIsOpen(displayPort)) { return; } #endif @@ -704,8 +705,9 @@ void dashboardInit(rxConfig_t *rxConfigToUse) resetDisplay(); delay(200); + displayPort = displayPortOledInit(); #if defined(CMS) && defined(OLEDCMS) - displayPortOledInit(); + cmsDisplayPortRegister(displayPort); #endif rxConfig = rxConfigToUse; diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c index e77abb5cea..68a6552689 100644 --- a/src/main/io/displayport_oled.c +++ b/src/main/io/displayport_oled.c @@ -20,18 +20,12 @@ #include "platform.h" -#ifdef OLEDCMS - #include "common/utils.h" #include "drivers/display.h" #include "drivers/display_ug2864hsweg01.h" -#include "io/cms.h" -#include "io/displayport_oled.h" - -// Exported -displayPort_t oledDisplayPort; +static displayPort_t oledDisplayPort; static int oledOpen(displayPort_t *displayPort) { @@ -52,7 +46,7 @@ static int oledClear(displayPort_t *displayPort) return 0; } -static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s) +static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) { UNUSED(displayPort); i2c_OLED_set_xy(x, y); @@ -71,7 +65,7 @@ static void oledResync(displayPort_t *displayPort) UNUSED(displayPort); } -static uint32_t oledTxBytesFree(displayPort_t *displayPort) +static uint32_t oledTxBytesFree(const displayPort_t *displayPort) { UNUSED(displayPort); return UINT32_MAX; @@ -87,13 +81,11 @@ static const displayPortVTable_t oledVTable = { .txBytesFree = oledTxBytesFree }; -void displayPortOledInit() +displayPort_t *displayPortOledInit(void) { oledDisplayPort.vTable = &oledVTable; oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT; oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT; - oledDisplayPort.inCMS = false; - - cmsDisplayPortRegister(&oledDisplayPort); + oledDisplayPort.isOpen = false; + return &oledDisplayPort; } -#endif // OLEDCMS diff --git a/src/main/io/displayport_oled.h b/src/main/io/displayport_oled.h index 2d0bc20509..4daa6de1cd 100644 --- a/src/main/io/displayport_oled.h +++ b/src/main/io/displayport_oled.h @@ -17,6 +17,4 @@ #pragma once -void displayPortOledInit(void); - -extern displayPort_t oledDisplayPort; +displayPort_t *displayPortOledInit(void); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index d4bdc01261..2cd9873baf 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -38,8 +38,6 @@ #include "sensors/sensors.h" -#include "io/cms.h" - #include "io/serial.h" #include "io/dashboard.h" #include "io/gps.h" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 903cbb5b95..cf70094d15 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -74,14 +74,24 @@ bool blinkState = true; //extern uint8_t RSSI; // TODO: not used? static uint16_t flyTime = 0; -uint8_t statRssi; +static uint8_t statRssi; -statistic_t stats; +typedef struct statistic_s { + int16_t max_speed; + int16_t min_voltage; // /10 + int16_t max_current; // /10 + int16_t min_rssi; + int16_t max_altitude; +} statistic_t; + +static statistic_t stats; uint16_t refreshTimeout = 0; #define REFRESH_1S 12 -uint8_t armState; +static uint8_t armState; + +static displayPort_t *osd7456DisplayPort; // OSD forwards @@ -109,7 +119,7 @@ void osdDrawElements(void) ; #endif #ifdef CMS - else if (sensors(SENSOR_ACC) || osd7456DisplayPort.inCMS) + else if (sensors(SENSOR_ACC) || displayIsOpen(osd7456DisplayPort)) #else else if (sensors(SENSOR_ACC)) #endif @@ -132,7 +142,7 @@ void osdDrawElements(void) #ifdef GPS #ifdef CMS - if (sensors(SENSOR_GPS) || osd7456DisplayPort.inCMS) + if (sensors(SENSOR_GPS) || displayIsOpen(osd7456DisplayPort)) #else if (sensors(SENSOR_GPS)) #endif @@ -406,8 +416,9 @@ void osdInit(void) refreshTimeout = 4 * REFRESH_1S; + osd7456DisplayPort = osd7456DisplayPortInit(); #ifdef CMS - osd7456DisplayPortInit(); + cmsDisplayPortRegister(osd7456DisplayPort); #endif } @@ -584,8 +595,9 @@ void updateOsd(uint32_t currentTime) #ifdef CMS // do not allow ARM if we are in menu - if (osd7456DisplayPort.inCMS) + if (displayIsOpen(osd7456DisplayPort)) { DISABLE_ARMING_FLAG(OK_TO_ARM); + } #endif } @@ -632,7 +644,7 @@ void osdUpdate(uint32_t currentTime) blinkState = (millis() / 200) % 2; #ifdef CMS - if (!osd7456DisplayPort.inCMS) { + if (!displayIsOpen(osd7456DisplayPort)) { osdUpdateAlarms(); osdDrawElements(); #ifdef OSD_CALLS_CMS diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 3f4400d74d..b6600953cc 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -34,16 +34,16 @@ typedef enum { OSD_GPS_SPEED, OSD_GPS_SATS, OSD_ALTITUDE, - OSD_MAX_ITEMS, // MUST BE LAST -} osd_items_t; + OSD_ITEM_COUNT // MUST BE LAST +} osd_items_e; typedef enum { OSD_UNIT_IMPERIAL, OSD_UNIT_METRIC -} osd_unit_t; +} osd_unit_e; -typedef struct { - uint16_t item_pos[OSD_MAX_ITEMS]; +typedef struct osd_profile_s { + uint16_t item_pos[OSD_ITEM_COUNT]; // Alarms uint8_t rssi_alarm; @@ -54,26 +54,14 @@ typedef struct { uint8_t video_system; uint8_t row_shiftdown; - osd_unit_t units; + osd_unit_e units; } osd_profile_t; -typedef struct { - int16_t max_speed; - int16_t min_voltage; // /10 - int16_t max_current; // /10 - int16_t min_rssi; - int16_t max_altitude; -} statistic_t; - void updateOsd(uint32_t currentTime); void osdInit(void); void resetOsdConfig(osd_profile_t *osdProfile); void osdResetAlarms(void); -#ifdef CMS -void osdCmsInit(displayPort_t *); -#endif - // Character coordinate and attributes #define OSD_POS(x,y) (x | (y << 5)) diff --git a/src/main/io/osd_max7456.c b/src/main/io/osd_max7456.c index 52ac8d6bc2..246eb37215 100644 --- a/src/main/io/osd_max7456.c +++ b/src/main/io/osd_max7456.c @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + #include #include @@ -16,20 +33,20 @@ displayPort_t osd7456DisplayPort; // Referenced from osd.c extern uint16_t refreshTimeout; -static int osdMenuBegin(displayPort_t *displayPort) +static int osdMenuOpen(displayPort_t *displayPort) { UNUSED(displayPort); osdResetAlarms(); - displayPort->inCMS = true; + displayPort->isOpen = true; refreshTimeout = 0; return 0; } -static int osdMenuEnd(displayPort_t *displayPort) +static int osdMenuClose(displayPort_t *displayPort) { UNUSED(displayPort); - displayPort->inCMS = false; + displayPort->isOpen = false; return 0; } @@ -42,7 +59,7 @@ static int osdClearScreen(displayPort_t *displayPort) return 0; } -static int osdWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s) +static int osdWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) { UNUSED(displayPort); max7456Write(x, y, s); @@ -64,27 +81,27 @@ static int osdHeartbeat(displayPort_t *displayPort) return 0; } -static uint32_t osdTxBytesFree(displayPort_t *displayPort) +static uint32_t osdTxBytesFree(const displayPort_t *displayPort) { UNUSED(displayPort); return UINT32_MAX; } static displayPortVTable_t osdVTable = { - osdMenuBegin, - osdMenuEnd, - osdClearScreen, - osdWrite, - osdHeartbeat, - osdResync, - osdTxBytesFree, + .open = osdMenuOpen, + .close = osdMenuClose, + .clear = osdClearScreen, + .write = osdWrite, + .heartbeat = osdHeartbeat, + .resync = osdResync, + .txBytesFree = osdTxBytesFree, }; -void osd7456DisplayPortInit(void) +displayPort_t *osd7456DisplayPortInit(void) { osd7456DisplayPort.vTable = &osdVTable; - osd7456DisplayPort.inCMS = false; + osd7456DisplayPort.isOpen = false; osdResync(&osd7456DisplayPort); - cmsDisplayPortRegister(&osd7456DisplayPort); + return &osd7456DisplayPort; } #endif // OSD diff --git a/src/main/io/osd_max7456.h b/src/main/io/osd_max7456.h index 22c64faa79..a116a6e4ff 100644 --- a/src/main/io/osd_max7456.h +++ b/src/main/io/osd_max7456.h @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + #pragma once -extern displayPort_t osd7456DisplayPort; - -void osd7456DisplayPortInit(void); +displayPort_t *osd7456DisplayPortInit(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 6dd51d2250..2d9c9f5dab 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -945,7 +945,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_row_shiftdown", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.row_shiftdown, .config.minmax = { 0, 1 } }, { "osd_units", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.osdProfile.units, .config.lookup = { TABLE_UNIT } }, { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.rssi_alarm, .config.minmax = { 0, 100 } }, diff --git a/src/main/main.c b/src/main/main.c index 0fefddfaaf..5e5b27ec72 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -463,7 +463,7 @@ void init(void) #ifdef CANVAS if (feature(FEATURE_CANVAS)) { - canvasInit(); + cmsDisplayPortRegister(canvasInit()); } #endif diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index eaca6e2868..b322ee7df6 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -35,4 +35,3 @@ typedef struct mspPacket_s { struct serialPort_s; typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc. typedef mspResult_e (*mspProcessCommandFnPtr)(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); -typedef void (*mspPushCommandFnPtr)(mspPacket_t *push, uint8_t *, int); diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 409e60afc8..9b38932bfd 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -120,7 +120,7 @@ static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int l #define JUMBO_FRAME_SIZE_LIMIT 255 -static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) +static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) { serialBeginWrite(msp->port); const int len = sbufBytesRemaining(&packet->buf); @@ -140,6 +140,7 @@ static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) } serialWrite(msp->port, checksum); serialEndWrite(msp->port); + return sizeof(hdr) + len + 1; // header, data, and checksum } static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn) @@ -211,9 +212,10 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } -void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) +int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) { static uint8_t pushBuf[30]; + int ret = 0; mspPacket_t push = { .buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), }, @@ -221,7 +223,7 @@ void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) .result = 0, }; - for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { mspPort_t * const mspPort = &mspPorts[portIndex]; if (!mspPort->port) { continue; @@ -236,15 +238,16 @@ void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) sbufSwitchToReader(&push.buf, pushBuf); - mspSerialEncode(mspPort, &push); + ret = mspSerialEncode(mspPort, &push); } + return ret; // return the number of bytes written } uint32_t mspSerialTxBytesFree() { - uint32_t minroom = UINT16_MAX; + uint32_t ret = UINT32_MAX; - for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { mspPort_t * const mspPort = &mspPorts[portIndex]; if (!mspPort->port) { continue; @@ -255,12 +258,11 @@ uint32_t mspSerialTxBytesFree() continue; } - uint32_t room = serialTxBytesFree(mspPort->port); - - if (room < minroom) { - minroom = room; + const uint32_t bytesFree = serialTxBytesFree(mspPort->port); + if (bytesFree < ret) { + ret = bytesFree; } } - return minroom; + return ret; } diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 0f1c8a2fff..f6e2954a24 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -66,5 +66,5 @@ void mspSerialInit(void); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); -void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen); +int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen); uint32_t mspSerialTxBytesFree(void); diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index b47914d1ae..6f7e839b2b 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -1,4 +1,3 @@ -#define USE_DPRINTF /* * This file is part of Cleanflight. * diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 756617182c..5878246a44 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -9,17 +9,8 @@ TARGET_SRC = \ drivers/compass_ak8963.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ + drivers/max7456.c \ drivers/serial_usb_vcp.c \ drivers/transponder_ir.c \ drivers/transponder_ir_stm32f30x.c \ - io/transponder_ir.c \ - drivers/max7456.c \ - io/osd.c \ - io/osd_max7456.c \ - io/canvas.c \ - io/cms.c \ - io/cms_builtin.c \ - io/cms_imu.c \ - io/cms_blackbox.c \ - io/cms_vtx.c \ - io/cms_ledstrip.c + io/transponder_ir.c diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index d805edc7f7..333e4cdcb9 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -5,13 +5,5 @@ TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ - drivers/max7456.c \ - io/osd.c \ - io/osd_max7456.c \ - io/cms.c \ - io/cms_builtin.c \ - io/cms_imu.c \ - io/cms_blackbox.c \ - io/cms_ledstrip.c \ - io/canvas.c + drivers/max7456.c diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index f123795910..23a8b46be3 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -11,13 +11,4 @@ TARGET_SRC = \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ drivers/vtx_soft_spi_rtc6705.c \ - drivers/max7456.c \ - io/osd.c \ - io/osd_max7456.c \ - io/canvas.c \ - io/cms.c \ - io/cms_builtin.c \ - io/cms_imu.c \ - io/cms_blackbox.c \ - io/cms_vtx.c \ - io/cms_ledstrip.c + drivers/max7456.c diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index b37d1b2b0e..5b3a330295 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -8,12 +8,5 @@ TARGET_SRC = \ drivers/barometer_bmp085.c \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - io/canvas.c \ - io/cms.c \ - io/cms_builtin.c \ - io/cms_imu.c \ - io/cms_blackbox.c \ - io/cms_vtx.c \ - io/cms_ledstrip.c + drivers/compass_hmc5883l.c diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 8189f15f4a..cef064b7e3 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -25,14 +25,4 @@ TARGET_SRC = \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ - drivers/max7456.c \ - io/osd.c \ - io/osd_max7456.c \ - io/canvas.c \ - io/cms.c \ - io/cms_builtin.c \ - io/cms_imu.c \ - io/cms_blackbox.c \ - io/cms_vtx.c \ - io/cms_ledstrip.c - + drivers/max7456.c From f3690fc769407ecce2a733ba14b63fe5204d7b68 Mon Sep 17 00:00:00 2001 From: francoisduf Date: Sun, 6 Nov 2016 11:31:58 +0100 Subject: [PATCH 125/188] Gyro change --- src/main/target/YUPIF4/target.h | 14 ++++++-------- src/main/target/YUPIF4/target.mk | 3 +-- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 55c60a4c3c..6ec3b44a56 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -38,18 +38,16 @@ #define MPU_INT_EXTI PC4 //MPU6500 -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define ICM20689_CS_PIN PA4 +#define ICM20689_SPI_INSTANCE SPI1 #define ACC -#define USE_ACC_MPU6500 -#define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG +#define USE_ACC_SPI_ICM20689 +#define ACC_ICM20689_ALIGN CW90_DEG #define GYRO -#define USE_GYRO_MPU6500 -#define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define USE_GYRO_SPI_ICM20689 +#define GYRO_ICM20689_ALIGN CW90_DEG #define USE_VCP //#define VBUS_SENSING_PIN PA8 diff --git a/src/main/target/YUPIF4/target.mk b/src/main/target/YUPIF4/target.mk index a5e7683b42..61d26037c4 100644 --- a/src/main/target/YUPIF4/target.mk +++ b/src/main/target/YUPIF4/target.mk @@ -2,6 +2,5 @@ F405_TARGETS += $(TARGET) FEATURES += SDCARD VCP TARGET_SRC = \ - drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_mpu6500.c + drivers/accgyro_spi_icm20689.c From 74477c8edd6c7d748d4669640c1efba727351016 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 6 Nov 2016 17:22:46 +0000 Subject: [PATCH 126/188] Fixed COLIBRI_RACE build --- src/main/target/COLIBRI_RACE/i2c_bst.c | 24 ++++++++++----------- src/main/target/COLIBRI_RACE/target.c | 30 +++++++++++++------------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 30dd0f082f..1981cfd144 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -28,8 +28,10 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" -#include "rx/rx.h" -#include "rx/msp.h" +#include "fc/config.h" +#include "fc/mw.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" #include "io/motors.h" #include "io/servos.h" @@ -40,7 +42,8 @@ #include "io/flashfs.h" #include "io/beeper.h" -#include "telemetry/telemetry.h" +#include "rx/rx.h" +#include "rx/msp.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" @@ -51,6 +54,8 @@ #include "sensors/compass.h" #include "sensors/gyro.h" +#include "telemetry/telemetry.h" + #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" @@ -58,11 +63,6 @@ #include "flight/navigation.h" #include "flight/altitudehold.h" -#include "fc/config.h" -#include "fc/mw.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - #include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" @@ -935,7 +935,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) #ifdef LED_STRIP case BST_LED_COLORS: for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; + hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; bstWrite16(color->h); bstWrite8(color->s); bstWrite8(color->v); @@ -944,7 +944,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) case BST_LED_STRIP_CONFIG: for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; bstWrite32(*ledConfig); } break; @@ -1367,7 +1367,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) //for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { { i = bstRead8(); - hsvColor_t *color = &masterConfig.colors[i]; + hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; color->h = bstRead16(); color->s = bstRead8(); color->v = bstRead8(); @@ -1380,7 +1380,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ret = BST_FAILED; break; } - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; *ledConfig = bstRead32(); reevaluateLedConfig(); } diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 08fd1255e1..fa60ae69cc 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -18,21 +18,21 @@ #include #include + #include "drivers/io.h" - +#include "drivers/dma.h" #include "drivers/timer.h" - -const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM11 - PB15 -}; +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 +}; From 18decaf436571dcfd0bf0ab5ab34f1f9f6d752f4 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 7 Nov 2016 05:49:40 +1100 Subject: [PATCH 127/188] Remaining LED strip driver issues resolved. --- .../drivers/light_ws2811strip_stm32f10x.c | 4 +-- .../drivers/light_ws2811strip_stm32f30x.c | 2 +- src/main/fc/config.c | 22 ------------ src/main/io/ledstrip.h | 2 +- src/main/target/AIORACERF3/target.c | 25 ++++++------- src/main/target/AIORACERF3/target.h | 8 ----- src/main/target/AIR32/target.c | 20 ++++++----- src/main/target/AIR32/target.h | 30 +--------------- src/main/target/AIRHEROF3/target.c | 29 +++++++-------- src/main/target/AIRHEROF3/target.h | 6 ---- src/main/target/ALIENFLIGHTF1/target.c | 30 ++++++++-------- src/main/target/ALIENFLIGHTF1/target.h | 5 --- src/main/target/ALIENFLIGHTF4/target.h | 6 ---- src/main/target/ANYFCF7/target.c | 33 ++++++++--------- src/main/target/ANYFCF7/target.h | 12 ------- src/main/target/BETAFLIGHTF3/target.c | 3 +- src/main/target/BETAFLIGHTF3/target.h | 9 ----- src/main/target/BLUEJAYF4/target.h | 10 ------ src/main/target/CC3D/target.h | 6 ---- src/main/target/COLIBRI/target.c | 34 +++++++++--------- src/main/target/COLIBRI/target.h | 14 ++------ src/main/target/COLIBRI_RACE/target.h | 12 ++----- src/main/target/DOGE/target.c | 21 +++++------ src/main/target/DOGE/target.h | 12 +------ src/main/target/FURYF3/target.c | 3 +- src/main/target/FURYF3/target.h | 9 ----- src/main/target/FURYF7/target.h | 21 ----------- src/main/target/IMPULSERCF3/target.c | 18 +++++----- src/main/target/IMPULSERCF3/target.h | 9 ----- src/main/target/ISHAPEDF3/target.c | 35 +++++++++--------- src/main/target/LUX_RACE/target.c | 1 + src/main/target/LUX_RACE/target.h | 11 ++---- src/main/target/MICROSCISKY/target.c | 30 ++++++++-------- src/main/target/MICROSCISKY/target.h | 5 --- src/main/target/MOTOLAB/target.c | 20 ++++++----- src/main/target/MOTOLAB/target.h | 31 +--------------- src/main/target/NAZE/target.c | 30 ++++++++-------- src/main/target/NAZE/target.h | 4 --- src/main/target/OMNIBUS/target.c | 2 +- src/main/target/OMNIBUS/target.h | 9 +---- src/main/target/OMNIBUSF4/target.c | 4 +-- src/main/target/OMNIBUSF4/target.h | 10 ------ src/main/target/PIKOBLX/target.c | 20 ++++++----- src/main/target/PIKOBLX/target.h | 10 +----- src/main/target/RACEBASE/target.c | 15 ++++---- src/main/target/RACEBASE/target.h | 13 ++----- src/main/target/RCEXPLORERF3/target.c | 14 ++++---- src/main/target/RCEXPLORERF3/target.h | 14 +------- src/main/target/REVO/target.h | 11 ------ src/main/target/RMDO/target.c | 36 +++++++++---------- src/main/target/RMDO/target.h | 9 ----- src/main/target/SINGULARITY/target.c | 21 +++++------ src/main/target/SINGULARITY/target.h | 9 ----- src/main/target/SOULF4/target.c | 25 ++++++------- src/main/target/SOULF4/target.h | 7 ---- src/main/target/SPARKY/target.c | 23 ++++++------ src/main/target/SPARKY/target.h | 8 ----- src/main/target/SPARKY2/SPARKY_OPBL.mk | 0 src/main/target/SPARKY2/target.h | 3 -- src/main/target/SPRACINGF3/target.c | 3 +- src/main/target/SPRACINGF3/target.h | 9 ----- src/main/target/SPRACINGF3EVO/target.c | 26 +++++++------- src/main/target/SPRACINGF3EVO/target.h | 8 ----- src/main/target/SPRACINGF3MINI/target.c | 27 +++++++------- src/main/target/STM32F3DISCOVERY/target.c | 2 +- src/main/target/STM32F3DISCOVERY/target.h | 8 ----- src/main/target/X_RACERSPI/target.c | 32 ++++++++--------- src/main/target/X_RACERSPI/target.h | 9 ----- src/main/target/YUPIF4/target.c | 16 +++++---- src/main/target/YUPIF4/target.h | 10 ------ 70 files changed, 326 insertions(+), 669 deletions(-) delete mode 100644 src/main/target/SPARKY2/SPARKY_OPBL.mk diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 250dc556dc..1933e0a1c8 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -64,11 +64,11 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) return; } - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + ws2811IO = IOGetByTag(ioTag); IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); IOConfigGPIO(ws2811IO, IO_CONFIG(GPIO_Speed_50MHz, GPIO_Mode_AF_PP)); - RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + RCC_ClockCmd(timerRCC(timer), ENABLE); /* Compute the prescaler value */ uint16_t prescalerValue = (uint16_t) (SystemCoreClock / WS2811_TIMER_HZ) - 1; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 6cc0ed9768..ffb6269d85 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -65,7 +65,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) return; } - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + ws2811IO = IOGetByTag(ioTag); IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5077d876f8..c5a24dde41 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -892,21 +892,6 @@ void validateAndFixConfig(void) } #endif -#if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) - if (featureConfigured(FEATURE_SOFTSERIAL) && ( - 0 -#ifdef USE_SOFTSERIAL1 - || (WS2811_TIMER == SOFTSERIAL_1_TIMER) -#endif -#ifdef USE_SOFTSERIAL2 - || (WS2811_TIMER == SOFTSERIAL_2_TIMER) -#endif - )) { - // led strip needs the same timer as softserial - featureClear(FEATURE_LED_STRIP); - } -#endif - #if defined(NAZE) && defined(SONAR) if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); @@ -925,13 +910,6 @@ void validateAndFixConfig(void) } #endif -/*#if defined(LED_STRIP) && defined(TRANSPONDER) // TODO - Add transponder feature - if ((WS2811_DMA_TC_FLAG == TRANSPONDER_DMA_TC_FLAG) && featureConfigured(FEATURE_TRANSPONDER) && featureConfigured(FEATURE_LED_STRIP)) { - featureClear(FEATURE_LED_STRIP); - } -#endif -*/ - #if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO) // shared pin if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) { diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 7bc5ab36ab..a417509075 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -142,7 +142,7 @@ typedef struct ledStripConfig_s { modeColorIndexes_t modeColors[LED_MODE_COUNT]; specialColorIndexes_t specialColors; uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on - rc_alias_e ledstrip_aux_channel; + uint8_t ledstrip_aux_channel; ioTag_t ioTag; } ledStripConfig_t; diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index 591219e747..3d034dbac6 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -21,19 +21,20 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM3 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, //LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, //LED_STRIP }; diff --git a/src/main/target/AIORACERF3/target.h b/src/main/target/AIORACERF3/target.h index 2698fa6ec5..bf112222f2 100644 --- a/src/main/target/AIORACERF3/target.h +++ b/src/main/target/AIORACERF3/target.h @@ -125,14 +125,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 1709455c7d..2c7e3b0031 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -21,15 +21,17 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 0cff2b6aab..eb848b4532 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -100,34 +100,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#if 1 - -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 -#endif - -#if 0 -// Alternate LED strip pin -// FIXME DMA IRQ Transfer Complete is never called because the TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1 -#define LED_STRIP_TIMER TIM17 -#define USE_LED_STRIP_ON_DMA1_CHANNEL7 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_7 // TIM17_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource7 -#define WS2811_TIMER TIM17 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 -#define WS2811_DMA_CHANNEL DMA1_Channel7 -#define WS2811_IRQ DMA1_Channel7_IRQn -#endif - #define SPEKTRUM_BIND // USART2, PB4 @@ -143,6 +115,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c index e7ca5a7261..e72d48ef69 100755 --- a/src/main/target/AIRHEROF3/target.c +++ b/src/main/target/AIRHEROF3/target.c @@ -21,20 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_11}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM | TIM_USE_LED, 0, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0}, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_11, NULL, 0}, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0} // PWM14 - OUT6 }; diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index e77e042cad..7e88ffe13d 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -85,12 +85,6 @@ #define VBAT_ADC_PIN PA4 #define LED_STRIP -#define WS2811_TIMER TIM3 -#define WS2811_PIN PA6 -#define WS2811_DMA_CHANNEL DMA1_Channel6 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_2 #define GPS diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index baa453c020..ddfdba9460 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -21,21 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; - diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index d4e7d32467..875d5e5122 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -59,11 +59,6 @@ #define LED_STRIP -#define WS2811_TIMER TIM3 -#define WS2811_PIN PA6 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_2 #undef GPS diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index bc899a217d..f9c45d08df 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -147,12 +147,6 @@ // LED strip configuration using RC5 pin. //#define LED_STRIP -//#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -//#define WS2811_PIN PB15 // TIM8_CH3 -//#define WS2811_TIMER TIM8 -//#define WS2811_DMA_CHANNEL DMA1_Channel3 -//#define WS2811_IRQ DMA1_Channel3_IRQn - #define SPEKTRUM_BIND // USART2, PA3 #define BIND_PIN PA3 diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index e2f55aa290..deaec905c6 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -47,26 +47,27 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #else // STANDARD LAYOUT const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF9_TIM12}, // S1_IN - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF9_TIM12}, // S2_IN - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S3_IN - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S4_IN - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S5_IN - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S6_IN + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4}, // S10_OUT 1 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S6_OUT 2 - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4}, // S5_OUT 3 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S1_OUT 4 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5}, // S2_OUT - { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9}, // S3_OUT - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3}, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5}, // S7_OUT - { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S8_OUT - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3}, // S9_OUT + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S6_OUT 2 + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, 0 }, // S2_OUT + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, NULL, 0, 0 }, // S7_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT }; #endif + // ALTERNATE LAYOUT //const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S1_IN diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index c36c5b529a..f1c5546a53 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -142,18 +142,6 @@ #define LED_STRIP -// LED Strip can run off Pin 6 (PA0) of the ESC outputs. -#define WS2811_PIN PA1 -#define WS2811_TIMER TIM5 -#define WS2811_TIMER_CHANNEL TIM_CHANNEL_2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream4 -#define WS2811_DMA_FLAG DMA_FLAG_TCIF4 -#define WS2811_DMA_IT DMA_IT_TCIF4 -#define WS2811_DMA_CHANNEL DMA_CHANNEL_6 -#define WS2811_DMA_IRQ DMA1_Stream4_IRQn -#define WS2811_TIMER_GPIO_AF GPIO_AF2_TIM5 - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_FEATURES (FEATURE_BLACKBOX) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index e88dce7400..26f59c33d6 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -35,6 +35,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 4806240672..7c911b3669 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -120,15 +120,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 50b0d87ddd..5341a536a3 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -145,16 +145,6 @@ #define USE_DSHOT #define LED_STRIP -// LED Strip can run off Pin 6 (PB1) of the ESC outputs. -#define WS2811_PIN PB1 -#define WS2811_TIMER TIM3 -#define WS2811_TIMER_CHANNEL TIM_Channel_4 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM3 -#define WS2811_DMA_CHANNEL DMA_Channel_5 -#define WS2811_DMA_IRQ DMA1_Stream2_IRQn - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 7dc877cd1d..e57b5cbfa8 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -96,12 +96,6 @@ #define VBAT_ADC_PIN PA0 #define RSSI_ADC_PIN PB0 -//#define LED_STRIP -//#define WS2811_PIN PB4 -//#define WS2811_TIMER TIM3 -//#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER - #define SPEKTRUM_BIND // USART3, PB11 (Flexport) #define BIND_PIN PB11 diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index fd5c5d4272..1bc779114c 100644 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -22,22 +22,24 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // S1_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S2_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S5_IN - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S6_IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 }, // S7_IN - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 }, // S8_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S3_OUT - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12 }, // S4_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S5_OUT - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12 }, // S6_OUT - { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM10 }, // S7_OUT - { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM11 }, // S8_OUT + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1, NULL, 0, 0 }, // S1_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S4_IN + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 , NULL, 0, 0 }, // S5_IN + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 , NULL, 0, 0 }, // S6_IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 , NULL, 0, 0 }, // S7_IN + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 , NULL, 0, 0 }, // S8_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S1_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S3_OUT + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12, NULL, 0, 0 }, // S4_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S5_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12, NULL, 0, 0 }, // S6_OUT + { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM10, NULL, 0, 0 }, // S7_OUT + { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM11, NULL, 0, 0 }, // S8_OUT + { TIM4, IO_TAG(PB7), TIM_Channel_2, 0, TIM_USE_LED , 0, GPIO_AF_TIM11, DMA1_Stream3, DMA_Channel_2, DMA1_ST3_HANDLER }, // S8_OUT }; diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index d30ab8e774..702f04c740 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -122,16 +122,6 @@ #define SENSORS_SET (SENSOR_ACC) #define LED_STRIP -#define WS2811_PIN PB7 // Shared UART1 -#define WS2811_TIMER TIM4 -#define WS2811_TIMER_CHANNEL TIM_Channel_2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST3_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream3 -#define WS2811_DMA_FLAG DMA_FLAG_TCIF3 -#define WS2811_DMA_IT DMA_IT_TCIF3 -#define WS2811_DMA_CHANNEL DMA_Channel_2 -#define WS2811_DMA_IRQ DMA1_Stream3_IRQn -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM4 // alternative defaults for Colibri/Gemini target #define TARGET_CONFIG @@ -148,5 +138,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define USABLE_TIMER_CHANNEL_COUNT 16 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11)) +#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11)) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 2c4fd4c334..2f1d7d1b3e 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -116,14 +116,6 @@ #define LED_STRIP -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 - #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -138,6 +130,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) +#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16)) diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 58f98c9a5c..d6345cd47e 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -21,16 +21,17 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB9 - { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PMW4 - PA10 - { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM5 - PA9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM8 - PB1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM9 - PB0 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 , NULL, 0 }, // PWM1 - PA8 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM2 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM3 - PB9 + { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 }, // PMW4 - PA10 + { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 }, // PWM5 - PA9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0 }, // PWM7 - PA1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM8 - PB1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM9 - PB0 + { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM9 - PB0 }; - diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index dc31a1640a..ff03fbe1bd 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -125,16 +125,6 @@ #define LED_STRIP -// tqfp48 pin 16 -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND @@ -152,6 +142,6 @@ // timer definitions in drivers/timer.c // channel mapping in drivers/pwm_mapping.c // only 6 outputs available on hardware -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 4f50ab5d01..9c445c2de8 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -32,6 +32,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, (1 | TIMER_OUTPUT_N_CHANNEL), GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO TIMER - LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO TIMER - LED_STRIP }; - diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index c7292349b6..abbf99371b 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -148,15 +148,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define SONAR #define SONAR_ECHO_PIN PB1 #define SONAR_TRIGGER_PIN PB0 diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 8da6562712..ae25088955 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -131,27 +131,6 @@ #define RSSI_ADC_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -//#define LED_STRIP -//#define WS2811_PIN PA0 -//#define WS2811_TIMER TIM5 -//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -//#define WS2811_DMA_STREAM DMA1_Stream2 -//#define WS2811_DMA_IT DMA_IT_TCIF2 -//#define WS2811_DMA_CHANNEL DMA_Channel_6 -//#define WS2811_TIMER_CHANNEL TIM_Channel_1 - -// LED Strip can run off Pin 6 (PA0) of the ESC outputs. - -//#define WS2811_PIN PA1 -//#define WS2811_TIMER TIM5 -//#define WS2811_TIMER_CHANNEL TIM_CHANNEL_2 -//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER -//#define WS2811_DMA_STREAM DMA1_Stream4 -//#define WS2811_DMA_FLAG DMA_FLAG_TCIF4 -//#define WS2811_DMA_IT DMA_IT_TCIF4 -//#define WS2811_DMA_CHANNEL DMA_CHANNEL_6 -//#define WS2811_DMA_IRQ DMA1_Stream4_IRQn - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define SENSORS_SET (SENSOR_ACC) diff --git a/src/main/target/IMPULSERCF3/target.c b/src/main/target/IMPULSERCF3/target.c index 6e8a09cba2..6e04ea9c8c 100644 --- a/src/main/target/IMPULSERCF3/target.c +++ b/src/main/target/IMPULSERCF3/target.c @@ -20,15 +20,15 @@ #include #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1}, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM6 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6}, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM2 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0}, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0}, // PWM6 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER}, // LED_STRIP }; - diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index 41b18c9bf6..c8e6d50425 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -81,15 +81,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) diff --git a/src/main/target/ISHAPEDF3/target.c b/src/main/target/ISHAPEDF3/target.c index 5a4f973de0..c15a51d1e3 100644 --- a/src/main/target/ISHAPEDF3/target.c +++ b/src/main/target/ISHAPEDF3/target.c @@ -21,26 +21,27 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM |TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM |TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 , DMA1_Channel2, DMA1_CH2_HANDLER}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index aa4a6abb5c..8478f789cc 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -37,5 +37,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 #endif + { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, }; diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 52a884261d..4a3f30cbc7 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -146,13 +146,6 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM @@ -175,9 +168,9 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #ifdef LUXV2_RACE -#define USABLE_TIMER_CHANNEL_COUNT 5 +#define USABLE_TIMER_CHANNEL_COUNT 6 #else -#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USABLE_TIMER_CHANNEL_COUNT 12 #endif #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index 4319f6fb45..c27cedf087 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -21,21 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM | TIM_USE_LED, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; - diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index f61806f3b7..e29948be63 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -71,11 +71,6 @@ #define I2C_DEVICE (I2CDEV_2) #define LED_STRIP -#define WS2811_TIMER TIM3 -#define WS2811_PIN PA6 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_2 #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index 10cb9ad654..4e53bcdea4 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -21,16 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 93e92d61cb..a5429e9683 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -102,35 +102,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#if 1 - -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 -#endif - -#if 0 -// Alternate LED strip pin -// FIXME DMA IRQ Transfer Complete is never called because the TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1 -#define LED_STRIP_TIMER TIM17 - -#define USE_LED_STRIP_ON_DMA1_CHANNEL7 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_7 // TIM17_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource7 -#define WS2811_TIMER TIM17 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 -#define WS2811_DMA_CHANNEL DMA1_Channel7 -#define WS2811_IRQ DMA1_Channel7_IRQn -#endif - #define SPEKTRUM_BIND // USART2, PB4 @@ -146,6 +117,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index c9bc95fe04..9c716a5435 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -21,21 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1 } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM | TIM_USE_LED, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; - diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index d7586fe858..d1d09f6043 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -150,10 +150,6 @@ #define EXTERNAL1_ADC_PIN PA5 #define LED_STRIP -#define WS2811_TIMER TIM3 -#define WS2811_PIN PA6 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER #undef GPS diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 0e15861a61..d9c15988dc 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -38,5 +38,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 - //{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index f2af32cd3d..2eaa230431 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -148,13 +148,6 @@ //#define ADC_INSTANCE ADC3 #define LED_STRIP -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA @@ -198,5 +191,5 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 7 // PPM + 6 Outputs (2 shared with UART3) +#define USABLE_TIMER_CHANNEL_COUNT 8 // PPM + 6 Outputs (2 shared with UART3) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index e088371565..9f4d170851 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -35,6 +35,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT }; diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index d7137502d8..e4893c33c0 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -122,19 +122,9 @@ #define USE_DSHOT #define LED_STRIP -// LED Strip can run off Pin 5 (PA1) of the MOTOR outputs. -#define WS2811_PIN PA1 -#define WS2811_TIMER TIM5 -#define WS2811_TIMER_CHANNEL TIM_Channel_2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream4 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_DMA_IRQ DMA1_Stream4_IRQn -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5 #define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index 10cb9ad654..4e53bcdea4 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -21,16 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 2b08b62b2e..9e8984bae3 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -81,14 +81,6 @@ #define VBAT_ADC_PIN PA5 #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA @@ -117,6 +109,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/RACEBASE/target.c b/src/main/target/RACEBASE/target.c index dc726133e3..2a261533a6 100755 --- a/src/main/target/RACEBASE/target.c +++ b/src/main/target/RACEBASE/target.c @@ -20,14 +20,13 @@ #include #include "drivers/io.h" #include "drivers/timer.h" - +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 1, GPIO_AF_1}, - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM2 - PC6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM3 - PC7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PMW4 - PC8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 1, GPIO_AF_1, NULL, 0 }, + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PC6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PC7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PMW4 - PC8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PC9 + { TIM1, IO_TAG(PA8), TIM_Channel_1, 0, TIM_USE_LED, 1, GPIO_AF_2, DMA1_Channel2, DMA1_CH2_HANDLER }, // PWM5 - PC9 }; - - diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index fd44bd0415..470347b9a8 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -98,14 +98,6 @@ #define RSSI_ADC_PIN PA6 #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define OSD @@ -124,9 +116,8 @@ #define TARGET_IO_PORTC (BIT(5)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 5 +#define USABLE_TIMER_CHANNEL_COUNT 6 #define USED_TIMERS (TIM_N(2) | TIM_N(4)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM4) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index 730d71b682..6e15446a0e 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -22,12 +22,14 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM1 - PA4 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM2 - PA7 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6}, // PWM3 - PA8 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM4 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM5 - PB1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1}, // PWM6 - PPM + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PA7 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM3 - PA8 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PWM6 - PPM + { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER}, // PWM6 - PPM }; diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 2f19300b0d..f0133e68f1 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -27,9 +27,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 6 - - #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL #define MPU_INT_EXTI PA15 @@ -107,15 +104,6 @@ #define LED_STRIP // LED strip configuration using PWM motor output pin 5. -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 - #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -144,5 +132,5 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 6 +#define USABLE_TIMER_CHANNEL_COUNT 7 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(17)) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index dacaff8980..80ff2728cf 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -143,17 +143,6 @@ //#define RSSI_ADC_PIN PA0 #define LED_STRIP -// LED Strip can run off Pin 5 (PA1) of the MOTOR outputs. -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5 -#define WS2811_PIN PA1 -#define WS2811_TIMER TIM5 -#define WS2811_TIMER_CHANNEL TIM_Channel_2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream4 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_DMA_IRQ DMA1_Stream4_IRQn -#define WS2811_DMA_FLAG DMA_FLAG_TCIF4 -#define WS2811_DMA_IT DMA_IT_TCIF4 #define SENSORS_SET (SENSOR_ACC) diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index 72523ef2b9..171cbfb19f 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -21,24 +21,24 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0}, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0}, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index e7c5744bd2..69d3575f58 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -96,15 +96,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #undef GPS #define SPEKTRUM_BIND diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 7176e7bc7d..aa992d656f 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -21,17 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // SOFTSERIAL1 RX (NC) - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // SOFTSERIAL1 TX - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // SOFTSERIAL1 RX (NC) + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // SOFTSERIAL1 TX + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // LED_STRIP }; diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 75895fc12a..315e3e4a5b 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -88,15 +88,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) diff --git a/src/main/target/SOULF4/target.c b/src/main/target/SOULF4/target.c index 0b8819b4a3..b3e2ef104b 100644 --- a/src/main/target/SOULF4/target.c +++ b/src/main/target/SOULF4/target.c @@ -20,18 +20,19 @@ #include #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 , NULL, 0, 0 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 , NULL, 0, 0 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 , DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT }; diff --git a/src/main/target/SOULF4/target.h b/src/main/target/SOULF4/target.h index aa8f6fa676..84752a82ce 100644 --- a/src/main/target/SOULF4/target.h +++ b/src/main/target/SOULF4/target.h @@ -104,13 +104,6 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define LED_STRIP -#define WS2811_PIN PA0 -#define WS2811_TIMER TIM5 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5 #define SPEKTRUM_BIND // USART3, diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index f7a44f00d7..fd009e5af9 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -21,21 +21,22 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // 6 3-pin headers - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, TIM_USE_MOTOR | TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 // PWM7 - PMW10 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PWM, 1, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PWM, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index e98ef0aea8..d24276c1b4 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -86,14 +86,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPARKY2/SPARKY_OPBL.mk b/src/main/target/SPARKY2/SPARKY_OPBL.mk deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 28f480e4bd..48a01c39a4 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -118,9 +118,6 @@ #define VBAT_ADC_PIN PC3 #define CURRENT_METER_ADC_PIN PC2 -#define LED_STRIP -#define LED_STRIP_TIMER TIM5 - #define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index fe9518d8a9..419775eb67 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -43,6 +43,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 207a7a2dd2..c685e61c31 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -111,15 +111,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 3aeac1bd84..dc9484b242 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -21,20 +21,20 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM3 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 6e3991b631..280af8c8a5 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -122,14 +122,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index 6d40631b0a..a0ad2fdb2b 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -21,31 +21,32 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM - PB5 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - PA6 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PA7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM5 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM6 - PA3 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA1 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, 1, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, 1, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) // LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 54a5dbded7..bd9c576fbe 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -24,7 +24,7 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_PPM | TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, 0, GPIO_AF_1, NULL, 0 }, { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 766447a09e..97a409253e 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -170,14 +170,6 @@ #define USE_DSHOT #define LED_STRIP -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define LED_STRIP_TIMER TIM16 -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 #define SPEKTRUM_BIND #define BIND_PIN PA3 // USART2, PA3 diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index d83ee38e58..38acf1457f 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -6,22 +6,22 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6 , DMA1_Channel2, DMA1_CH2_HANDLER}, // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 7e3bee0a54..34e51b5885 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -103,15 +103,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 20dbd87572..8d51c13bc7 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -20,13 +20,15 @@ #include #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8 }, // PPM IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // MS5 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // MS6 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // MS5 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // MS6 }; + diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 6ec3b44a56..07d6236f2d 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -125,16 +125,6 @@ #define LED_STRIP -// LED Strip can run off Pin 6 (PB1) -#define WS2811_PIN PB1 -#define WS2811_TIMER TIM3 -#define WS2811_TIMER_CHANNEL TIM_Channel_4 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_CHANNEL DMA_Channel_5 -#define WS2811_DMA_IRQ DMA1_Stream2_IRQn -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM3 - // Default configuration #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT From e3ab93a416035b094082639cc231584845c60f62 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 7 Nov 2016 06:02:32 +1100 Subject: [PATCH 128/188] Adding LED strip to CR --- src/main/target/COLIBRI_RACE/target.c | 1 + src/main/target/COLIBRI_RACE/target.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index fa60ae69cc..ee262ef27e 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -35,4 +35,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 + { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM11 - PB15 }; diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 2f1d7d1b3e..1959a3e952 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -131,5 +131,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 12 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) From d8a83518db26f57c97423f6175d4532ba7a898fb Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 6 Nov 2016 20:24:33 +0000 Subject: [PATCH 129/188] CMS file renaming. displayPort renaming --- Makefile | 18 +-- src/main/{io => cms}/cms.c | 21 ++- src/main/{io => cms}/cms.h | 0 .../cms_menu_blackbox.c} | 8 +- .../cms_menu_blackbox.h} | 0 .../cms_builtin.c => cms/cms_menu_builtin.c} | 22 ++-- .../cms_builtin.h => cms/cms_menu_builtin.h} | 2 + src/main/{io/cms_imu.c => cms/cms_menu_imu.c} | 10 +- src/main/{io/cms_imu.h => cms/cms_menu_imu.h} | 0 .../cms_menu_ledstrip.c} | 6 +- .../cms_menu_ledstrip.h} | 0 src/main/{io/cms_osd.h => cms/cms_menu_osd.h} | 0 src/main/{io/cms_vtx.c => cms/cms_menu_vtx.c} | 6 +- src/main/{io/cms_vtx.h => cms/cms_menu_vtx.h} | 0 src/main/{io => cms}/cms_types.h | 0 src/main/config/config_master.h | 3 +- src/main/drivers/display.c | 16 +-- src/main/drivers/display.h | 12 +- src/main/fc/config.c | 3 +- src/main/fc/config.h | 2 +- src/main/fc/fc_tasks.c | 3 +- src/main/io/canvas.c | 122 ------------------ src/main/io/dashboard.c | 5 +- .../{osd_max7456.c => displayport_max7456.c} | 46 +++---- .../{osd_max7456.h => displayport_max7456.h} | 2 +- src/main/io/displayport_msp.c | 119 +++++++++++++++++ src/main/io/{canvas.h => displayport_msp.h} | 2 +- src/main/io/displayport_oled.c | 10 +- src/main/io/osd.c | 20 +-- src/main/io/serial_cli.c | 5 +- src/main/main.c | 13 +- src/main/msp/msp_protocol.h | 4 +- src/main/target/OMNIBUS/target.h | 2 +- src/main/target/OMNIBUSF4/target.h | 2 +- src/main/target/REVO/target.mk | 4 +- src/main/target/SIRINFPV/target.h | 2 +- src/main/target/SPRACINGF3/target.h | 2 +- 37 files changed, 245 insertions(+), 247 deletions(-) rename src/main/{io => cms}/cms.c (98%) rename src/main/{io => cms}/cms.h (100%) rename src/main/{io/cms_blackbox.c => cms/cms_menu_blackbox.c} (97%) rename src/main/{io/cms_blackbox.h => cms/cms_menu_blackbox.h} (100%) rename src/main/{io/cms_builtin.c => cms/cms_menu_builtin.c} (93%) rename src/main/{io/cms_builtin.h => cms/cms_menu_builtin.h} (96%) rename src/main/{io/cms_imu.c => cms/cms_menu_imu.c} (99%) rename src/main/{io/cms_imu.h => cms/cms_menu_imu.h} (100%) rename src/main/{io/cms_ledstrip.c => cms/cms_menu_ledstrip.c} (95%) rename src/main/{io/cms_ledstrip.h => cms/cms_menu_ledstrip.h} (100%) rename src/main/{io/cms_osd.h => cms/cms_menu_osd.h} (100%) rename src/main/{io/cms_vtx.c => cms/cms_menu_vtx.c} (97%) rename src/main/{io/cms_vtx.h => cms/cms_menu_vtx.h} (100%) rename src/main/{io => cms}/cms_types.h (100%) delete mode 100644 src/main/io/canvas.c rename src/main/io/{osd_max7456.c => displayport_max7456.c} (60%) rename src/main/io/{osd_max7456.h => displayport_max7456.h} (94%) create mode 100644 src/main/io/displayport_msp.c rename src/main/io/{canvas.h => displayport_msp.h} (93%) diff --git a/Makefile b/Makefile index 684dc1bfca..7479252aa4 100644 --- a/Makefile +++ b/Makefile @@ -551,6 +551,12 @@ COMMON_SRC = \ HIGHEND_SRC = \ blackbox/blackbox.c \ blackbox/blackbox_io.c \ + cms/cms.c \ + cms/cms_menu_blackbox.c \ + cms/cms_menu_builtin.c \ + cms/cms_menu_imu.c \ + cms/cms_menu_ledstrip.c \ + cms/cms_menu_vtx.c \ common/colorconversion.c \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ @@ -560,19 +566,13 @@ HIGHEND_SRC = \ flight/gtune.c \ flight/navigation.c \ flight/gps_conversion.c \ - io/canvas.c \ - io/cms.c \ - io/cms_builtin.c \ - io/cms_imu.c \ - io/cms_blackbox.c \ - io/cms_vtx.c \ - io/cms_ledstrip.c \ - io/displayport_oled.c \ io/dashboard.c \ + io/displayport_max7456.c \ + io/displayport_msp.c \ + io/displayport_oled.c \ io/gps.c \ io/ledstrip.c \ io/osd.c \ - io/osd_max7456.c \ sensors/sonar.c \ sensors/barometer.c \ telemetry/telemetry.c \ diff --git a/src/main/io/cms.c b/src/main/cms/cms.c similarity index 98% rename from src/main/io/cms.c rename to src/main/cms/cms.c index d39aafa032..e5017ae5c7 100644 --- a/src/main/io/cms.c +++ b/src/main/cms/cms.c @@ -30,16 +30,19 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" #include "build/debug.h" -#include "drivers/system.h" +#include "cms/cms.h" +#include "cms/cms_menu_builtin.h" +#include "cms/cms_types.h" #include "common/typeconversion.h" +#include "drivers/system.h" + // For 'ARM' related #include "fc/config.h" #include "fc/rc_controls.h" @@ -53,12 +56,6 @@ // For VISIBLE* (Actually, included by config_master.h) #include "io/osd.h" -#include "io/cms.h" -#include "io/cms_types.h" - -// Menu contents -#include "io/cms_builtin.h" - // DisplayPort management #ifndef CMS_MAX_DEVICE @@ -528,13 +525,13 @@ static void cmsMenuOpen(void) // Switch display displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); if (pNextDisplay != pCurrentDisplay) { - displayClose(pCurrentDisplay); + displayRelease(pCurrentDisplay); pCurrentDisplay = pNextDisplay; } else { return; } } - displayOpen(pCurrentDisplay); + displayGrab(pCurrentDisplay); // grab the display for use by the CMS cmsMenuChange(pCurrentDisplay, currentMenu); } @@ -578,7 +575,7 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr) cmsInMenu = false; - displayClose(pDisplay); + displayRelease(pDisplay); currentMenu = NULL; if (ptr) diff --git a/src/main/io/cms.h b/src/main/cms/cms.h similarity index 100% rename from src/main/io/cms.h rename to src/main/cms/cms.h diff --git a/src/main/io/cms_blackbox.c b/src/main/cms/cms_menu_blackbox.c similarity index 97% rename from src/main/io/cms_blackbox.c rename to src/main/cms/cms_menu_blackbox.c index a3ab982f13..67b180802d 100644 --- a/src/main/io/cms_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -32,14 +32,14 @@ #include "drivers/system.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_blackbox.h" + #include "config/config_profile.h" #include "config/config_master.h" #include "config/feature.h" -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_blackbox.h" - #include "io/flashfs.h" #ifdef USE_FLASHFS diff --git a/src/main/io/cms_blackbox.h b/src/main/cms/cms_menu_blackbox.h similarity index 100% rename from src/main/io/cms_blackbox.h rename to src/main/cms/cms_menu_blackbox.h diff --git a/src/main/io/cms_builtin.c b/src/main/cms/cms_menu_builtin.c similarity index 93% rename from src/main/io/cms_builtin.c rename to src/main/cms/cms_menu_builtin.c index 3275f8ac9a..7b094b0b3f 100644 --- a/src/main/io/cms_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -26,25 +26,23 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" + #include "drivers/system.h" -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_imu.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_builtin.h" // Sub menus -#include "io/cms_imu.h" -#include "io/cms_blackbox.h" -#include "io/cms_vtx.h" -#ifdef OSD -#include "io/cms_osd.h" -#endif -#include "io/cms_ledstrip.h" +#include "cms/cms_menu_imu.h" +#include "cms/cms_menu_blackbox.h" +#include "cms/cms_menu_vtx.h" +#include "cms/cms_menu_osd.h" +#include "cms/cms_menu_ledstrip.h" // Info diff --git a/src/main/io/cms_builtin.h b/src/main/cms/cms_menu_builtin.h similarity index 96% rename from src/main/io/cms_builtin.h rename to src/main/cms/cms_menu_builtin.h index 7717c5998d..8bb33757aa 100644 --- a/src/main/io/cms_builtin.h +++ b/src/main/cms/cms_menu_builtin.h @@ -17,4 +17,6 @@ #pragma once +#include "cms/cms_types.h" + extern CMS_Menu menuMain; diff --git a/src/main/io/cms_imu.c b/src/main/cms/cms_menu_imu.c similarity index 99% rename from src/main/io/cms_imu.c rename to src/main/cms/cms_menu_imu.c index d2772a62ef..9f8d920202 100644 --- a/src/main/io/cms_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -25,17 +25,17 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" + #include "drivers/system.h" //#include "common/typeconversion.h" -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_imu.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_imu.h" #include "fc/config.h" #include "fc/rc_controls.h" diff --git a/src/main/io/cms_imu.h b/src/main/cms/cms_menu_imu.h similarity index 100% rename from src/main/io/cms_imu.h rename to src/main/cms/cms_menu_imu.h diff --git a/src/main/io/cms_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c similarity index 95% rename from src/main/io/cms_ledstrip.c rename to src/main/cms/cms_menu_ledstrip.c index 08186a1af6..00ff866bf2 100644 --- a/src/main/io/cms_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -32,9 +32,9 @@ #include "config/config_master.h" #include "config/feature.h" -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_blackbox.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_ledstrip.h" #ifdef LED_STRIP diff --git a/src/main/io/cms_ledstrip.h b/src/main/cms/cms_menu_ledstrip.h similarity index 100% rename from src/main/io/cms_ledstrip.h rename to src/main/cms/cms_menu_ledstrip.h diff --git a/src/main/io/cms_osd.h b/src/main/cms/cms_menu_osd.h similarity index 100% rename from src/main/io/cms_osd.h rename to src/main/cms/cms_menu_osd.h diff --git a/src/main/io/cms_vtx.c b/src/main/cms/cms_menu_vtx.c similarity index 97% rename from src/main/io/cms_vtx.c rename to src/main/cms/cms_menu_vtx.c index cf0333830c..f96eb8f2da 100644 --- a/src/main/io/cms_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -23,9 +23,9 @@ #include "build/version.h" -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_vtx.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_vtx.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/io/cms_vtx.h b/src/main/cms/cms_menu_vtx.h similarity index 100% rename from src/main/io/cms_vtx.h rename to src/main/cms/cms_menu_vtx.h diff --git a/src/main/io/cms_types.h b/src/main/cms/cms_types.h similarity index 100% rename from src/main/io/cms_types.h rename to src/main/cms/cms_types.h diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 893ecf4b51..60a2f35548 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -21,6 +21,8 @@ #include "config/config_profile.h" +#include "cms/cms.h" + #include "drivers/pwm_rx.h" #include "drivers/sound_beeper.h" #include "drivers/sonar_hcsr04.h" @@ -38,7 +40,6 @@ #include "io/motors.h" #include "io/servos.h" #include "io/gps.h" -#include "io/cms.h" #include "io/osd.h" #include "io/ledstrip.h" #include "io/vtx.h" diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 2b247022d9..834ce92bb7 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -31,22 +31,22 @@ void displayClear(displayPort_t *instance) instance->cursorRow = -1; } -void displayOpen(displayPort_t *instance) +void displayGrab(displayPort_t *instance) { - instance->vTable->open(instance); + instance->vTable->grab(instance); instance->vTable->clear(instance); - instance->isOpen = true; + instance->isGrabbed = true; } -void displayClose(displayPort_t *instance) +void displayRelease(displayPort_t *instance) { - instance->vTable->close(instance); - instance->isOpen = false; + instance->vTable->release(instance); + instance->isGrabbed = false; } -bool displayIsOpen(const displayPort_t *instance) +bool displayIsGrabbed(const displayPort_t *instance) { - if (instance && instance->isOpen) { // can be called before initialised + if (instance && instance->isGrabbed) { // can be called before initialised return true; } else { return false; diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index aabf8517bc..1605237736 100644 --- a/src/main/drivers/display.h +++ b/src/main/drivers/display.h @@ -26,12 +26,12 @@ typedef struct displayPort_s { // CMS state bool cleared; int8_t cursorRow; - bool isOpen; + bool isGrabbed; } displayPort_t; typedef struct displayPortVTable_s { - int (*open)(displayPort_t *displayPort); - int (*close)(displayPort_t *displayPort); + int (*grab)(displayPort_t *displayPort); + int (*release)(displayPort_t *displayPort); int (*clear)(displayPort_t *displayPort); int (*write)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text); int (*heartbeat)(displayPort_t *displayPort); @@ -39,9 +39,9 @@ typedef struct displayPortVTable_s { uint32_t (*txBytesFree)(const displayPort_t *displayPort); } displayPortVTable_t; -void displayOpen(displayPort_t *instance); -void displayClose(displayPort_t *instance); -bool displayIsOpen(const displayPort_t *instance); +void displayGrab(displayPort_t *instance); +void displayRelease(displayPort_t *instance); +bool displayIsGrabbed(const displayPort_t *instance); void displayClear(displayPort_t *instance); int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s); void displayHeartbeat(displayPort_t *instance); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index a8d1b03efc..a55c774164 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -26,6 +26,8 @@ #include "blackbox/blackbox_io.h" +#include "cms/cms.h" + #include "common/color.h" #include "common/axis.h" #include "common/maths.h" @@ -64,7 +66,6 @@ #include "io/servos.h" #include "io/ledstrip.h" #include "io/gps.h" -#include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 5da0c5b53b..0934833a98 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -55,7 +55,7 @@ typedef enum { FEATURE_VTX = 1 << 24, FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, - FEATURE_CANVAS = 1 << 27, + FEATURE_MSP_OSD = 1 << 27, } features_e; void beeperOffSet(uint32_t mask); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 9e31554249..c88ef2eebe 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -21,6 +21,8 @@ #include +#include "cms/cms.h" + #include "common/axis.h" #include "common/color.h" #include "common/utils.h" @@ -41,7 +43,6 @@ #include "flight/altitudehold.h" #include "io/beeper.h" -#include "io/cms.h" #include "io/dashboard.h" #include "io/gps.h" #include "io/ledstrip.h" diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c deleted file mode 100644 index 85f0232ba7..0000000000 --- a/src/main/io/canvas.c +++ /dev/null @@ -1,122 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include -#include - -#include "platform.h" - -#ifdef CANVAS - -#include "build/version.h" - -#include "common/utils.h" - -#include "drivers/system.h" - -#include "io/cms.h" - -#include "fc/fc_msp.h" - -#include "msp/msp_protocol.h" -#include "msp/msp_serial.h" - -static displayPort_t canvasDisplayPort; - -static int canvasOutput(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, int len) -{ - UNUSED(displayPort); - return mspSerialPush(cmd, buf, len); -} - -static int canvasOpen(displayPort_t *displayPort) -{ - const uint8_t subcmd[] = { 0 }; - - return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); -} - -static int canvasHeartBeat(displayPort_t *displayPort) -{ - return canvasOpen(displayPort); -} - -static int canvasClose(displayPort_t *displayPort) -{ - const uint8_t subcmd[] = { 1 }; - - return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); -} - -static int canvasClear(displayPort_t *displayPort) -{ - const uint8_t subcmd[] = { 2 }; - - return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd)); -} - -static int canvasWrite(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string) -{ -#define MSP_CANVAS_MAX_STRING_LENGTH 30 - uint8_t buf[MSP_CANVAS_MAX_STRING_LENGTH + 4]; - - int len = strlen(string); - if (len >= MSP_CANVAS_MAX_STRING_LENGTH) { - len = MSP_CANVAS_MAX_STRING_LENGTH; - } - - buf[0] = 3; - buf[1] = row; - buf[2] = col; - buf[3] = 0; - memcpy(&buf[4], string, len); - - return canvasOutput(displayPort, MSP_CANVAS, buf, len + 4); -} - -static void canvasResync(displayPort_t *displayPort) -{ - displayPort->rows = 13; // XXX Will reflect NTSC/PAL in the future - displayPort->cols = 30; -} - -static uint32_t canvasTxBytesFree(const displayPort_t *displayPort) -{ - UNUSED(displayPort); - return mspSerialTxBytesFree(); -} - -static const displayPortVTable_t canvasVTable = { - .open = canvasOpen, - .close = canvasClose, - .clear = canvasClear, - .write = canvasWrite, - .heartbeat = canvasHeartBeat, - .resync = canvasResync, - .txBytesFree = canvasTxBytesFree -}; - -displayPort_t *canvasInit(void) -{ - canvasDisplayPort.vTable = &canvasVTable; - canvasDisplayPort.isOpen = false; - canvasResync(&canvasDisplayPort); - return &canvasDisplayPort; -} -#endif diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 017ea4cfff..8675ec5ef8 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -34,6 +34,8 @@ #include "drivers/display.h" #include "drivers/display_ug2864hsweg01.h" +#include "cms/cms.h" + #include "common/printf.h" #include "common/maths.h" #include "common/axis.h" @@ -53,7 +55,6 @@ #include "flight/imu.h" #include "flight/failsafe.h" -#include "io/cms.h" #include "io/displayport_oled.h" #ifdef GPS @@ -589,7 +590,7 @@ void dashboardUpdate(uint32_t currentTime) static uint8_t previousArmedState = 0; #ifdef OLEDCMS - if (displayIsOpen(displayPort)) { + if (displayIsGrabbed(displayPort)) { return; } #endif diff --git a/src/main/io/osd_max7456.c b/src/main/io/displayport_max7456.c similarity index 60% rename from src/main/io/osd_max7456.c rename to src/main/io/displayport_max7456.c index 246eb37215..0bfbb4231c 100644 --- a/src/main/io/osd_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -29,29 +29,29 @@ #include "drivers/display.h" #include "drivers/max7456.h" -displayPort_t osd7456DisplayPort; // Referenced from osd.c +displayPort_t max7456DisplayPort; // Referenced from osd.c extern uint16_t refreshTimeout; -static int osdMenuOpen(displayPort_t *displayPort) +static int grab(displayPort_t *displayPort) { UNUSED(displayPort); osdResetAlarms(); - displayPort->isOpen = true; + displayPort->isGrabbed = true; refreshTimeout = 0; return 0; } -static int osdMenuClose(displayPort_t *displayPort) +static int release(displayPort_t *displayPort) { UNUSED(displayPort); - displayPort->isOpen = false; + displayPort->isGrabbed = false; return 0; } -static int osdClearScreen(displayPort_t *displayPort) +static int clearScreen(displayPort_t *displayPort) { UNUSED(displayPort); max7456ClearScreen(); @@ -59,7 +59,7 @@ static int osdClearScreen(displayPort_t *displayPort) return 0; } -static int osdWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) +static int write(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) { UNUSED(displayPort); max7456Write(x, y, s); @@ -67,7 +67,7 @@ static int osdWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char return 0; } -static void osdResync(displayPort_t *displayPort) +static void resync(displayPort_t *displayPort) { UNUSED(displayPort); max7456RefreshAll(); @@ -75,33 +75,33 @@ static void osdResync(displayPort_t *displayPort) displayPort->cols = 30; } -static int osdHeartbeat(displayPort_t *displayPort) +static int heartbeat(displayPort_t *displayPort) { UNUSED(displayPort); return 0; } -static uint32_t osdTxBytesFree(const displayPort_t *displayPort) +static uint32_t txBytesFree(const displayPort_t *displayPort) { UNUSED(displayPort); return UINT32_MAX; } -static displayPortVTable_t osdVTable = { - .open = osdMenuOpen, - .close = osdMenuClose, - .clear = osdClearScreen, - .write = osdWrite, - .heartbeat = osdHeartbeat, - .resync = osdResync, - .txBytesFree = osdTxBytesFree, +static displayPortVTable_t max7456VTable = { + .grab = grab, + .release = release, + .clear = clearScreen, + .write = write, + .heartbeat = heartbeat, + .resync = resync, + .txBytesFree = txBytesFree, }; -displayPort_t *osd7456DisplayPortInit(void) +displayPort_t *max7456DisplayPortInit(void) { - osd7456DisplayPort.vTable = &osdVTable; - osd7456DisplayPort.isOpen = false; - osdResync(&osd7456DisplayPort); - return &osd7456DisplayPort; + max7456DisplayPort.vTable = &max7456VTable; + max7456DisplayPort.isGrabbed = false; + resync(&max7456DisplayPort); + return &max7456DisplayPort; } #endif // OSD diff --git a/src/main/io/osd_max7456.h b/src/main/io/displayport_max7456.h similarity index 94% rename from src/main/io/osd_max7456.h rename to src/main/io/displayport_max7456.h index a116a6e4ff..bbb4aced35 100644 --- a/src/main/io/osd_max7456.h +++ b/src/main/io/displayport_max7456.h @@ -17,4 +17,4 @@ #pragma once -displayPort_t *osd7456DisplayPortInit(void); +displayPort_t *max7456DisplayPortInit(void); diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c new file mode 100644 index 0000000000..2178888905 --- /dev/null +++ b/src/main/io/displayport_msp.c @@ -0,0 +1,119 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_MSP_OSD + +#include "common/utils.h" + +#include "drivers/display.h" +#include "drivers/system.h" + +#include "fc/fc_msp.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +static displayPort_t mspDisplayPort; + +static int output(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, int len) +{ + UNUSED(displayPort); + return mspSerialPush(cmd, buf, len); +} + +static int grab(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 0 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int heartbeat(displayPort_t *displayPort) +{ + return grab(displayPort); // ensure display is not released by MW OSD software +} + +static int release(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 1 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int clear(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 2 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int write(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string) +{ +#define MSP_OSD_MAX_STRING_LENGTH 30 + uint8_t buf[MSP_OSD_MAX_STRING_LENGTH + 4]; + + int len = strlen(string); + if (len >= MSP_OSD_MAX_STRING_LENGTH) { + len = MSP_OSD_MAX_STRING_LENGTH; + } + + buf[0] = 3; + buf[1] = row; + buf[2] = col; + buf[3] = 0; + memcpy(&buf[4], string, len); + + return output(displayPort, MSP_DISPLAYPORT, buf, len + 4); +} + +static void resync(displayPort_t *displayPort) +{ + displayPort->rows = 13; // XXX Will reflect NTSC/PAL in the future + displayPort->cols = 30; +} + +static uint32_t txBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return mspSerialTxBytesFree(); +} + +static const displayPortVTable_t mspDisplayPortVTable = { + .grab = grab, + .release = release, + .clear = clear, + .write = write, + .heartbeat = heartbeat, + .resync = resync, + .txBytesFree = txBytesFree +}; + +displayPort_t *displayPortMspInit(void) +{ + mspDisplayPort.vTable = &mspDisplayPortVTable; + mspDisplayPort.isGrabbed = false; + resync(&mspDisplayPort); + return &mspDisplayPort; +} +#endif // USE_MSP_OSD diff --git a/src/main/io/canvas.h b/src/main/io/displayport_msp.h similarity index 93% rename from src/main/io/canvas.h rename to src/main/io/displayport_msp.h index 7a5520e23f..955d0b852c 100644 --- a/src/main/io/canvas.h +++ b/src/main/io/displayport_msp.h @@ -18,4 +18,4 @@ #pragma once struct displayPort_s; -struct displayPort_s *canvasInit(void); +struct displayPort_s *displayPortMspInit(void); diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c index 68a6552689..8bb20329fc 100644 --- a/src/main/io/displayport_oled.c +++ b/src/main/io/displayport_oled.c @@ -27,13 +27,13 @@ static displayPort_t oledDisplayPort; -static int oledOpen(displayPort_t *displayPort) +static int oledGrab(displayPort_t *displayPort) { UNUSED(displayPort); return 0; } -static int oledClose(displayPort_t *displayPort) +static int oledRelease(displayPort_t *displayPort) { UNUSED(displayPort); return 0; @@ -72,8 +72,8 @@ static uint32_t oledTxBytesFree(const displayPort_t *displayPort) } static const displayPortVTable_t oledVTable = { - .open = oledOpen, - .close = oledClose, + .grab = oledGrab, + .release = oledRelease, .clear = oledClear, .write = oledWrite, .heartbeat = oledHeartbeat, @@ -86,6 +86,6 @@ displayPort_t *displayPortOledInit(void) oledDisplayPort.vTable = &oledVTable; oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT; oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT; - oledDisplayPort.isOpen = false; + oledDisplayPort.isGrabbed = false; return &oledDisplayPort; } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index cf70094d15..ae96d31782 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -35,14 +35,16 @@ #include "common/utils.h" +#include "drivers/display.h" #include "drivers/system.h" -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_osd.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_osd.h" + +#include "io/displayport_max7456.h" #include "io/flashfs.h" #include "io/osd.h" -#include "io/osd_max7456.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -119,7 +121,7 @@ void osdDrawElements(void) ; #endif #ifdef CMS - else if (sensors(SENSOR_ACC) || displayIsOpen(osd7456DisplayPort)) + else if (sensors(SENSOR_ACC) || displayIsGrabbed(osd7456DisplayPort)) #else else if (sensors(SENSOR_ACC)) #endif @@ -142,7 +144,7 @@ void osdDrawElements(void) #ifdef GPS #ifdef CMS - if (sensors(SENSOR_GPS) || displayIsOpen(osd7456DisplayPort)) + if (sensors(SENSOR_GPS) || displayIsGrabbed(osd7456DisplayPort)) #else if (sensors(SENSOR_GPS)) #endif @@ -416,7 +418,7 @@ void osdInit(void) refreshTimeout = 4 * REFRESH_1S; - osd7456DisplayPort = osd7456DisplayPortInit(); + osd7456DisplayPort = max7456DisplayPortInit(); #ifdef CMS cmsDisplayPortRegister(osd7456DisplayPort); #endif @@ -595,7 +597,7 @@ void updateOsd(uint32_t currentTime) #ifdef CMS // do not allow ARM if we are in menu - if (displayIsOpen(osd7456DisplayPort)) { + if (displayIsGrabbed(osd7456DisplayPort)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } #endif @@ -644,7 +646,7 @@ void osdUpdate(uint32_t currentTime) blinkState = (millis() / 200) % 2; #ifdef CMS - if (!displayIsOpen(osd7456DisplayPort)) { + if (!displayIsGrabbed(osd7456DisplayPort)) { osdUpdateAlarms(); osdDrawElements(); #ifdef OSD_CALLS_CMS diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2d9c9f5dab..21bddf0704 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -35,6 +35,8 @@ uint8_t cliMode = 0; #include "build/debug.h" #include "build/version.h" +#include "cms/cms.h" + #include "common/axis.h" #include "common/color.h" #include "common/maths.h" @@ -70,7 +72,6 @@ uint8_t cliMode = 0; #include "io/flashfs.h" #include "io/beeper.h" #include "io/asyncfatfs/asyncfatfs.h" -#include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" @@ -228,7 +229,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", "CANVAS", NULL + " ", "VTX", "RX_SPI", "SOFTSPI", "MSP_OSD", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/main.c b/src/main/main.c index 5e5b27ec72..6ba2d07a22 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -29,8 +29,9 @@ #include "common/maths.h" #include "common/printf.h" -#include "drivers/nvic.h" +#include "cms/cms.h" +#include "drivers/nvic.h" #include "drivers/sensor.h" #include "drivers/system.h" #include "drivers/dma.h" @@ -74,8 +75,6 @@ #include "rx/rx.h" #include "rx/spektrum.h" -#include "io/cms.h" - #include "io/beeper.h" #include "io/serial.h" #include "io/flashfs.h" @@ -89,7 +88,7 @@ #include "io/serial_cli.h" #include "io/transponder_ir.h" #include "io/osd.h" -#include "io/canvas.h" +#include "io/displayport_msp.h" #include "io/vtx.h" #include "scheduler/scheduler.h" @@ -461,9 +460,9 @@ void init(void) mspFcInit(); mspSerialInit(); -#ifdef CANVAS - if (feature(FEATURE_CANVAS)) { - cmsDisplayPortRegister(canvasInit()); +#ifdef USE_MSP_OSD + if (feature(FEATURE_MSP_OSD)) { + cmsDisplayPortRegister(displayPortMspInit()); } #endif diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 809f4684f0..84f0a60a1b 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -216,8 +216,8 @@ #define MSP_OSD_VIDEO_CONFIG 180 #define MSP_SET_OSD_VIDEO_CONFIG 181 -// External OSD canvas mode messages -#define MSP_CANVAS 182 +// External OSD displayport mode messages +#define MSP_DISPLAYPORT 182 // // Multwii original MSP commands diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 6f7e839b2b..e17e84b8f3 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -98,7 +98,7 @@ #define CMS_MAX_DEVICE 4 // Use external OSD to run CMS -#define CANVAS +#define USE_MSP_OSD // USE I2C OLED display to run CMS #define OLEDCMS diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index a1dd90dfaf..c6dcb25d31 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -71,7 +71,7 @@ //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER #define CMS -#define CANVAS +#define USE_MSP_OSD //#define PITOT //#define USE_PITOT_MS4525 diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index aed71f993d..3d8b57a8a6 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -6,6 +6,4 @@ TARGET_SRC = \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \ drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c \ - io/cms.c \ - io/canvas.c + drivers/compass_hmc5883l.c diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index d1fb69fc83..86c4a82569 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -145,7 +145,7 @@ #define CMS // Use external OSD to run CMS -#define CANVAS +#define USE_MSP_OSD // USE I2C OLED display to run CMS #define OLEDCMS diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index da2e8eaca6..1c09f18bf3 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -146,7 +146,7 @@ #define CMS // Use external OSD to run CMS -#define CANVAS +#define USE_MSP_OSD // USE I2C OLED display to run CMS #define OLEDCMS From 44d633e5e71e4caac543924d377134b4b8198afd Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 6 Nov 2016 20:40:42 +0000 Subject: [PATCH 130/188] Changed USE_MSP_OSD to USE_MSP_DISPLAYPORT --- src/main/io/displayport_msp.c | 4 ++-- src/main/main.c | 2 +- src/main/target/OMNIBUS/target.h | 4 ++-- src/main/target/SIRINFPV/target.h | 4 ++-- src/main/target/SPRACINGF3/target.h | 4 ++-- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index 2178888905..ff16c04c8f 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -22,7 +22,7 @@ #include "platform.h" -#ifdef USE_MSP_OSD +#ifdef USE_MSP_DISPLAYPORT #include "common/utils.h" @@ -116,4 +116,4 @@ displayPort_t *displayPortMspInit(void) resync(&mspDisplayPort); return &mspDisplayPort; } -#endif // USE_MSP_OSD +#endif // USE_MSP_DISPLAYPORT diff --git a/src/main/main.c b/src/main/main.c index 6ba2d07a22..7aef157dcf 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -460,7 +460,7 @@ void init(void) mspFcInit(); mspSerialInit(); -#ifdef USE_MSP_OSD +#ifdef USE_MSP_DISPLAYPORT if (feature(FEATURE_MSP_OSD)) { cmsDisplayPortRegister(displayPortMspInit()); } diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index e17e84b8f3..e8b78ee868 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -97,8 +97,8 @@ #define CMS #define CMS_MAX_DEVICE 4 -// Use external OSD to run CMS -#define USE_MSP_OSD +// Use external display to run CMS +#define USE_MSP_DISPLAYPORT // USE I2C OLED display to run CMS #define OLEDCMS diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 86c4a82569..f32419f0f7 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -144,8 +144,8 @@ // Configuratoin Menu System #define CMS -// Use external OSD to run CMS -#define USE_MSP_OSD +// Use external display to run CMS +#define USE_MSP_DISPLAYPORT // USE I2C OLED display to run CMS #define OLEDCMS diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 1c09f18bf3..5cf175633e 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -145,8 +145,8 @@ // Configuratoin Menu System #define CMS -// Use external OSD to run CMS -#define USE_MSP_OSD +// Use external display to run CMS +#define USE_MSP_DISPLAYPORT // USE I2C OLED display to run CMS #define OLEDCMS From 7ff8547c700374bb59e7ece5b06dbfacffdd0b61 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 6 Nov 2016 21:11:08 +0000 Subject: [PATCH 131/188] Fixed overlooked USE_MSP_OSD --- src/main/target/OMNIBUSF4/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index c6dcb25d31..3533a7a9ec 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -71,7 +71,7 @@ //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER #define CMS -#define USE_MSP_OSD +#define USE_MSP_DISPLAYPORT //#define PITOT //#define USE_PITOT_MS4525 From eb53cbb40d33d34db295926471253834d1a97a39 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 7 Nov 2016 08:57:54 +1100 Subject: [PATCH 132/188] Renaming resourceOwner_t to resourceOwner_e --- src/main/drivers/io.c | 6 +++--- src/main/drivers/io.h | 6 +++--- src/main/drivers/io_impl.h | 4 ++-- src/main/drivers/resource.h | 4 ++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 5104e5cf72..39de281fc7 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -231,7 +231,7 @@ void IOToggle(IO_t io) } // claim IO pin, set owner and resources -void IOInit(IO_t io, resourceOwner_t owner, resourceType_t resource, uint8_t index) +void IOInit(IO_t io, resourceOwner_e owner, resourceType_e resource, uint8_t index) { ioRec_t *ioRec = IO_Rec(io); ioRec->owner = owner; @@ -245,13 +245,13 @@ void IORelease(IO_t io) ioRec->owner = OWNER_FREE; } -resourceOwner_t IOGetOwner(IO_t io) +resourceOwner_e IOGetOwner(IO_t io) { ioRec_t *ioRec = IO_Rec(io); return ioRec->owner; } -resourceType_t IOGetResource(IO_t io) +resourceType_e IOGetResource(IO_t io) { ioRec_t *ioRec = IO_Rec(io); return ioRec->resource; diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index d3f85eaf77..23e43b92a1 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -84,10 +84,10 @@ void IOHi(IO_t io); void IOLo(IO_t io); void IOToggle(IO_t io); -void IOInit(IO_t io, resourceOwner_t owner, resourceType_t resource, uint8_t index); +void IOInit(IO_t io, resourceOwner_e owner, resourceType_e resource, uint8_t index); void IORelease(IO_t io); // unimplemented -resourceOwner_t IOGetOwner(IO_t io); -resourceType_t IOGetResources(IO_t io); +resourceOwner_e IOGetOwner(IO_t io); +resourceType_e IOGetResources(IO_t io); IO_t IOGetByTag(ioTag_t tag); void IOConfigGPIO(IO_t io, ioConfig_t cfg); diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h index 0c880fd78d..0db154b852 100644 --- a/src/main/drivers/io_impl.h +++ b/src/main/drivers/io_impl.h @@ -11,8 +11,8 @@ typedef struct ioDef_s { typedef struct ioRec_s { GPIO_TypeDef *gpio; uint16_t pin; - resourceOwner_t owner; - resourceType_t resource; + resourceOwner_e owner; + resourceType_e resource; uint8_t index; } ioRec_t; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 7d1649c5f6..44ec615660 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -34,7 +34,7 @@ typedef enum { OWNER_SOFTSPI, OWNER_RX_SPI, OWNER_TOTAL_COUNT -} resourceOwner_t; +} resourceOwner_e; extern const char * const ownerNames[OWNER_TOTAL_COUNT]; @@ -51,6 +51,6 @@ typedef enum { RESOURCE_ADC_BATTERY, RESOURCE_ADC_RSSI, RESOURCE_ADC_EXTERNAL1, RESOURCE_ADC_CURRENT, RESOURCE_RX_CE, RESOURCE_TOTAL_COUNT -} resourceType_t; +} resourceType_e; extern const char * const resourceNames[RESOURCE_TOTAL_COUNT]; From 159236093ec46e8b7e9478fe431bafdb7b5adef5 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 7 Nov 2016 09:54:55 +1100 Subject: [PATCH 133/188] Remove need for IRQ to be provided in target.c --- src/main/drivers/timer.c | 20 +++++++++++++++---- src/main/drivers/timer.h | 6 +++++- src/main/drivers/timer_hal.c | 22 +++++++++++++++----- src/main/drivers/timer_stm32f30x.c | 20 +++++++++---------- src/main/drivers/timer_stm32f4xx.c | 32 +++++++++++++++++------------- src/main/drivers/timer_stm32f7xx.c | 28 +++++++++++++------------- 6 files changed, 80 insertions(+), 48 deletions(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 510055aaa0..2ffa962ebc 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -207,6 +207,16 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) return 0; } +uint8_t timerInputIrq(TIM_TypeDef *tim) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].inputIrq; + } + } + return 0; +} + void timerNVICConfigure(uint8_t irq) { NVIC_InitTypeDef NVIC_InitStructure; @@ -239,9 +249,11 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui { configTimeBase(timerHardwarePtr->tim, period, mhz); TIM_Cmd(timerHardwarePtr->tim, ENABLE); - timerNVICConfigure(timerHardwarePtr->irq); + + uint8_t irq = timerInputIrq(timerHardwarePtr->tim); + timerNVICConfigure(irq); // HACK - enable second IRQ on timers that need it - switch(timerHardwarePtr->irq) { + switch(irq) { #if defined(STM32F10X) case TIM1_CC_IRQn: timerNVICConfigure(TIM1_UP_IRQn); @@ -271,7 +283,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui } // allocate and configure timer channel. Timer priority is set to highest priority of its channels -void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority) +void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq) { unsigned channel = timHw - timerHardware; if(channel >= USABLE_TIMER_CHANNEL_COUNT) @@ -288,7 +300,7 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = timHw->irq; + NVIC_InitStructure.NVIC_IRQChannel = irq; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority); NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 689602e809..491a6ad696 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -81,6 +81,7 @@ typedef struct timerOvrHandlerRec_s { typedef struct timerDef_s { TIM_TypeDef *TIMx; rccPeriphTag_t rcc; + uint8_t inputIrq; } timerDef_t; typedef struct timerHardware_s { @@ -121,6 +122,8 @@ typedef enum { #endif #elif defined(STM32F3) #define HARDWARE_TIMER_DEFINITION_COUNT 10 +#elif defined(STM32F411xE) +#define HARDWARE_TIMER_DEFINITION_COUNT 10 #elif defined(STM32F4) #define HARDWARE_TIMER_DEFINITION_COUNT 14 #elif defined(STM32F7) @@ -167,7 +170,7 @@ void timerChITConfigDualLo(const timerHardware_t* timHw, FunctionalState newStat void timerChITConfig(const timerHardware_t* timHw, FunctionalState newState); void timerChClearCCFlag(const timerHardware_t* timHw); -void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority); +void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq); void timerInit(void); void timerStart(void); @@ -178,6 +181,7 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim); void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration rccPeriphTag_t timerRCC(TIM_TypeDef *tim); +uint8_t timerInputIrq(TIM_TypeDef *tim); const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag); diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index cb9ef8bf52..d643ed1285 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -216,6 +216,16 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) return 0; } +uint8_t timerInputIrq(TIM_TypeDef *tim) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].inputIrq; + } + } + return 0; +} + void timerNVICConfigure(uint8_t irq) { HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER)); @@ -285,9 +295,11 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui configTimeBase(timerHardwarePtr->tim, period, mhz); HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle); - timerNVICConfigure(timerHardwarePtr->irq); + + uint8_t irq = timerInputIrq(timerHardwarePtr->tim); + timerNVICConfigure(irq); // HACK - enable second IRQ on timers that need it - switch(timerHardwarePtr->irq) { + switch(irq) { case TIM1_CC_IRQn: timerNVICConfigure(TIM1_UP_TIM10_IRQn); @@ -300,7 +312,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui } // allocate and configure timer channel. Timer priority is set to highest priority of its channels -void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority) +void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq) { uint8_t timerIndex = lookupTimerIndex(timHw->tim); if (timerIndex >= USED_TIMER_COUNT) { @@ -320,8 +332,8 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle); - HAL_NVIC_SetPriority(timHw->irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); - HAL_NVIC_EnableIRQ(timHw->irq); + HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); + HAL_NVIC_EnableIRQ(irq); timerInfo[timer].priority = irqPriority; } diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 6265d7fd77..ec3e2ed5ba 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -17,16 +17,16 @@ #include "timer.h" const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1) }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2) }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3) }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4) }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) }, - { .TIMx = TIM15, .rcc = RCC_APB2(TIM15) }, - { .TIMx = TIM16, .rcc = RCC_APB2(TIM16) }, - { .TIMx = TIM17, .rcc = RCC_APB2(TIM17) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), .inputIrq = TIM3_IRQn }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), .inputIrq = TIM4_IRQn }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn }, + { .TIMx = TIM15, .rcc = RCC_APB2(TIM15), .inputIrq = TIM1_BRK_TIM15_IRQn }, + { .TIMx = TIM16, .rcc = RCC_APB2(TIM16), .inputIrq = TIM1_UP_TIM16_IRQn }, + { .TIMx = TIM17, .rcc = RCC_APB2(TIM17), .inputIrq = TIM1_TRG_COM_TIM17_IRQn }, }; uint8_t timerClockDivisor(TIM_TypeDef *tim) diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index da13ae77f3..6e0de79c44 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -42,20 +42,24 @@ #define CCMR_Offset ((uint16_t)0x0018) const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1) }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2) }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3) }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4) }, - { .TIMx = TIM5, .rcc = RCC_APB1(TIM5) }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) }, - { .TIMx = TIM9, .rcc = RCC_APB2(TIM9) }, - { .TIMx = TIM10, .rcc = RCC_APB2(TIM10) }, - { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, - { .TIMx = TIM12, .rcc = RCC_APB1(TIM12) }, - { .TIMx = TIM13, .rcc = RCC_APB1(TIM13) }, - { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn}, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn}, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), .inputIrq = TIM3_IRQn}, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), .inputIrq = TIM4_IRQn}, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), .inputIrq = TIM5_IRQn}, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = 0}, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = 0}, +#ifndef STM32F411xE + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn}, +#endif + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), .inputIrq = TIM1_BRK_TIM9_IRQn}, + { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), .inputIrq = TIM1_UP_TIM10_IRQn}, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), .inputIrq = TIM1_TRG_COM_TIM11_IRQn}, +#ifndef STM32F411xE + { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), .inputIrq = TIM8_BRK_TIM12_IRQn}, + { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), .inputIrq = TIM8_UP_TIM13_IRQn}, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), .inputIrq = TIM8_TRG_COM_TIM14_IRQn}, +#endif }; /* diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c index e9992d9b19..7f0077dca0 100644 --- a/src/main/drivers/timer_stm32f7xx.c +++ b/src/main/drivers/timer_stm32f7xx.c @@ -42,20 +42,20 @@ #define CCMR_Offset ((uint16_t)0x0018) const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1) }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2) }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3) }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4) }, - { .TIMx = TIM5, .rcc = RCC_APB1(TIM5) }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) }, - { .TIMx = TIM9, .rcc = RCC_APB2(TIM9) }, - { .TIMx = TIM10, .rcc = RCC_APB2(TIM10) }, - { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, - { .TIMx = TIM12, .rcc = RCC_APB1(TIM12) }, - { .TIMx = TIM13, .rcc = RCC_APB1(TIM13) }, - { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn}, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn}, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), .inputIrq = TIM3_IRQn}, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), .inputIrq = TIM4_IRQn}, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), .inputIrq = TIM5_IRQn}, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = 0}, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = 0}, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn}, + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), .inputIrq = TIM1_BRK_TIM9_IRQn}, + { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), .inputIrq = TIM1_UP_TIM10_IRQn}, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), .inputIrq = TIM1_TRG_COM_TIM11_IRQn}, + { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), .inputIrq = TIM8_BRK_TIM12_IRQn}, + { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), .inputIrq = TIM8_UP_TIM13_IRQn}, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), .inputIrq = TIM8_TRG_COM_TIM14_IRQn}, }; /* From d7fe69a421da7f664fde655350472d139bc0a5b0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 7 Nov 2016 15:04:13 +1100 Subject: [PATCH 134/188] Removing irq completely from target.c --- src/main/drivers/timer.h | 1 - src/main/target/AIORACERF3/target.c | 24 ++++----- src/main/target/AIR32/target.c | 20 +++---- src/main/target/AIRHEROF3/target.c | 28 +++++----- src/main/target/ALIENFLIGHTF1/target.c | 28 +++++----- src/main/target/ALIENFLIGHTF3/target.c | 22 ++++---- src/main/target/ALIENFLIGHTF4/target.c | 26 ++++----- src/main/target/ANYFCF7/target.c | 64 +++++++++++------------ src/main/target/BETAFLIGHTF3/target.c | 20 +++---- src/main/target/BLUEJAYF4/target.c | 14 ++--- src/main/target/CC3D/target.c | 24 ++++----- src/main/target/CHEBUZZF3/target.c | 36 ++++++------- src/main/target/CJMCU/target.c | 28 +++++----- src/main/target/COLIBRI/target.c | 34 ++++++------ src/main/target/COLIBRI_RACE/target.c | 24 ++++----- src/main/target/DOGE/target.c | 20 +++---- src/main/target/F4BY/target.c | 34 ++++++------ src/main/target/FURYF3/target.c | 16 +++--- src/main/target/FURYF4/target.c | 10 ++-- src/main/target/FURYF7/target.c | 10 ++-- src/main/target/IMPULSERCF3/target.c | 16 +++--- src/main/target/IRCFUSIONF3/target.c | 34 ++++++------ src/main/target/ISHAPEDF3/target.c | 34 ++++++------ src/main/target/KISSFC/target.c | 20 +++---- src/main/target/LUX_RACE/target.c | 24 ++++----- src/main/target/MICROSCISKY/target.c | 28 +++++----- src/main/target/MOTOLAB/target.c | 20 +++---- src/main/target/NAZE/target.c | 28 +++++----- src/main/target/OMNIBUS/target.c | 20 +++---- src/main/target/OMNIBUSF4/target.c | 24 ++++----- src/main/target/PIKOBLX/target.c | 20 +++---- src/main/target/RACEBASE/target.c | 12 ++--- src/main/target/RCEXPLORERF3/target.c | 14 ++--- src/main/target/REVO/target.c | 26 ++++----- src/main/target/REVONANO/target.c | 24 ++++----- src/main/target/RMDO/target.c | 34 ++++++------ src/main/target/SINGULARITY/target.c | 20 +++---- src/main/target/SIRINFPV/target.c | 14 ++--- src/main/target/SOULF4/target.c | 24 ++++----- src/main/target/SPARKY/target.c | 22 ++++---- src/main/target/SPARKY2/target.c | 22 ++++---- src/main/target/SPRACINGF3/target.c | 32 ++++++------ src/main/target/SPRACINGF3EVO/target.c | 24 ++++----- src/main/target/SPRACINGF3MINI/target.c | 26 ++++----- src/main/target/STM32F3DISCOVERY/target.c | 28 +++++----- src/main/target/VRRACE/target.c | 24 ++++----- src/main/target/X_RACERSPI/target.c | 30 +++++------ src/main/target/YUPIF4/target.c | 14 ++--- src/main/target/ZCOREF3/target.c | 34 ++++++------ 49 files changed, 587 insertions(+), 588 deletions(-) diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 491a6ad696..a6fa48f8a9 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -88,7 +88,6 @@ typedef struct timerHardware_s { TIM_TypeDef *tim; ioTag_t tag; uint8_t channel; - uint8_t irq; timerUsageFlag_e usageFlags; uint8_t output; #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index 3d034dbac6..1a9a98d65a 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -25,16 +25,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, //LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, //LED_STRIP }; diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 2c7e3b0031..a6b455f81b 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -24,14 +24,14 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c index e72d48ef69..0ba72a6bca 100755 --- a/src/main/target/AIRHEROF3/target.c +++ b/src/main/target/AIRHEROF3/target.c @@ -24,18 +24,18 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM | TIM_USE_LED, 0, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_11, NULL, 0}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM | TIM_USE_LED, 0, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0}, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_11, NULL, 0}, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0} // PWM14 - OUT6 }; diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index ddfdba9460..b7a0fa3288 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -24,18 +24,18 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index 4673d05a46..71849ec4dc 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -24,16 +24,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // up to 10 Motor Outputs - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index 1fddc56b96..36d786ff20 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -23,18 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 - { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 - { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 - { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 + { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 + { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 + { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 + { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 }; diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index deaec905c6..f05f3ceeae 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -26,44 +26,44 @@ #if defined(USE_DSHOT) // DSHOT TEST const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_2, DMA1_ST7_HANDLER }, // S10_OUT 1 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream5, DMA_CHANNEL_5, DMA1_ST5_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream2, DMA_CHANNEL_6, DMA1_ST2_HANDLER }, // S7_OUT - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 - { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT - { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_2, DMA1_ST7_HANDLER }, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream5, DMA_CHANNEL_5, DMA1_ST5_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream2, DMA_CHANNEL_6, DMA1_ST2_HANDLER }, // S7_OUT + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT }; #else // STANDARD LAYOUT const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S10_OUT 1 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S6_OUT 2 - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S1_OUT 4 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, 0 }, // S2_OUT - { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, NULL, 0, 0 }, // S7_OUT - { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S6_OUT 2 + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, 0 }, // S2_OUT + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, NULL, 0, 0 }, // S7_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT }; #endif diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 26f59c33d6..af3f4daa69 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -24,16 +24,16 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // RC PPM - PB7 - TIM17_CH1N AF1, TIM4_CH2 AF2, TIM8_BKIN AF5, TIM3_CH4 AF10 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // RC PPM - PB7 - TIM17_CH1N AF1, TIM4_CH2 AF2, TIM8_BKIN AF5, TIM3_CH4 AF10 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index 3e7a18ac30..d0d2b450de 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -23,12 +23,12 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT }; diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index 71a6462bb4..9d34d96dc1 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -23,17 +23,17 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, }, // S1_IN - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, }, // S4_IN - Current - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, }, // S5_IN - Vbattery - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, }, // S6_IN - PPM IN - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S1_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S2_OUT - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S3_OUT - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, }, // S4_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, } // S6_OUT + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_PWM, 0, }, // S1_IN + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, }, // S4_IN - Current + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM, 0, }, // S5_IN - Vbattery + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM | TIM_USE_PPM, 0, }, // S6_IN - PPM IN + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, }, // S1_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, }, // S2_OUT + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, }, // S3_OUT + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, }, // S4_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, } // S6_OUT }; diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 1986a350ce..6484ee4cc6 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -24,23 +24,23 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // INPUTS CH1-8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM1 - PA8 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM2 - PB8 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM3 - PB9 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM4 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM5 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM6 - PC8 - { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM7 - PF9 - { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM8 - PF10 - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PD12 - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM10 - PD13 - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM11 - PD14 - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM12 - PD15 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM13 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM14 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM15 - PA3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM16 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM17 - PB1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 } // PWM18 - PA4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM1 - PA8 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM2 - PB8 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM3 - PB9 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM4 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM5 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM6 - PC8 + { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM7 - PF9 + { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM8 - PF10 + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PD12 + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM10 - PD13 + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM11 - PD14 + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM12 - PD15 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM13 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM14 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM15 - PA3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM16 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM17 - PB1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 } // PWM18 - PA4 }; diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index 30f0e1b00e..2478c668d4 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index 1bc779114c..0c273a64e6 100644 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -25,21 +25,21 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1, NULL, 0, 0 }, // S1_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S4_IN - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 , NULL, 0, 0 }, // S5_IN - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 , NULL, 0, 0 }, // S6_IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 , NULL, 0, 0 }, // S7_IN - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 , NULL, 0, 0 }, // S8_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S1_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S3_OUT - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12, NULL, 0, 0 }, // S4_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S5_OUT - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12, NULL, 0, 0 }, // S6_OUT - { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM10, NULL, 0, 0 }, // S7_OUT - { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM11, NULL, 0, 0 }, // S8_OUT - { TIM4, IO_TAG(PB7), TIM_Channel_2, 0, TIM_USE_LED , 0, GPIO_AF_TIM11, DMA1_Stream3, DMA_Channel_2, DMA1_ST3_HANDLER }, // S8_OUT + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1, NULL, 0, 0 }, // S1_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S4_IN + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM2 , NULL, 0, 0 }, // S5_IN + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM2 , NULL, 0, 0 }, // S6_IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM5 , NULL, 0, 0 }, // S7_IN + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM5 , NULL, 0, 0 }, // S8_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S1_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S3_OUT + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM12, NULL, 0, 0 }, // S4_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S5_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM12, NULL, 0, 0 }, // S6_OUT + { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM10, NULL, 0, 0 }, // S7_OUT + { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM11, NULL, 0, 0 }, // S8_OUT + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_LED , 0, GPIO_AF_TIM11, DMA1_Stream3, DMA_Channel_2, DMA1_ST3_HANDLER }, // S8_OUT }; diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index ee262ef27e..8cfeb6edbb 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -24,16 +24,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 - { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM11 - PB15 }; diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index d6345cd47e..243de4dc18 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -24,14 +24,14 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 , NULL, 0 }, // PWM1 - PA8 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM2 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM3 - PB9 - { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 }, // PMW4 - PA10 - { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 }, // PWM5 - PA9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0 }, // PWM7 - PA1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM8 - PB1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM9 - PB0 - { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM9 - PB0 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6 , NULL, 0 }, // PWM1 - PA8 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM2 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM3 - PB9 + { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 }, // PMW4 - PA10 + { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 }, // PWM5 - PA9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0 }, // PWM7 - PA1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM8 - PB1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM9 - PB0 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM9 - PB0 }; diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index cf7aab2df1..249e4dede6 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -6,22 +6,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S8_IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S8_OUT - { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_TIM9 }, // sonar echo if needed + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S8_IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S8_OUT + { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM_USE_MOTOR, 0, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 9c445c2de8..33bcf44f4a 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -25,12 +25,12 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_6, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM4 - S1 - { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 - { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, (1 | TIMER_OUTPUT_N_CHANNEL), GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO TIMER - LED_STRIP + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_6, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM4 - S1 + { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 + { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM_USE_MOTOR, (1 | TIMER_OUTPUT_N_CHANNEL), GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO TIMER - LED_STRIP }; diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index 322753dd75..a57e0e3351 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -24,11 +24,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM_IN - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM_IN + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT // { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5 }, // LED Strip }; diff --git a/src/main/target/FURYF7/target.c b/src/main/target/FURYF7/target.c index 9d934b12c6..eb48b71181 100644 --- a/src/main/target/FURYF7/target.c +++ b/src/main/target/FURYF7/target.c @@ -25,12 +25,12 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF3_TIM8, NULL, 0, 0 }, // PPM_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_USE_PPM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // PPM_IN - { TIM3, IO_TAG(PB0), TIM_CHANNEL_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, DMA1_Stream7, DMA_CHANNEL_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, DMA1_Stream2, DMA_CHANNEL_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM3, IO_TAG(PB0), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream7, DMA_CHANNEL_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream2, DMA_CHANNEL_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S4_OUT // { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5 }, // LED Strip }; \ No newline at end of file diff --git a/src/main/target/IMPULSERCF3/target.c b/src/main/target/IMPULSERCF3/target.c index 6e04ea9c8c..a5674d80de 100644 --- a/src/main/target/IMPULSERCF3/target.c +++ b/src/main/target/IMPULSERCF3/target.c @@ -23,12 +23,12 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0}, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0}, // PWM6 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER}, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM2 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0}, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0}, // PWM6 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER}, // LED_STRIP }; diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index 5add051e06..b0eb9d70b2 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -23,22 +23,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/ISHAPEDF3/target.c b/src/main/target/ISHAPEDF3/target.c index c15a51d1e3..03015e22ce 100644 --- a/src/main/target/ISHAPEDF3/target.c +++ b/src/main/target/ISHAPEDF3/target.c @@ -24,24 +24,24 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM |TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM |TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 , DMA1_Channel2, DMA1_CH2_HANDLER}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6 , DMA1_Channel2, DMA1_CH2_HANDLER}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index ce4a664a08..f276f5845e 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -24,17 +24,17 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel3, DMA1_CH3_HANDLER }, - { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel5, DMA1_CH5_HANDLER }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, + { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel3, DMA1_CH3_HANDLER }, + { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, + { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel5, DMA1_CH5_HANDLER }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, //{ TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_10, NULL, 0}, //{ TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_5, NULL, 0}, }; diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 8478f789cc..d94c5a6869 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -24,19 +24,19 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 #ifndef LUXV2_RACE - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 #endif - { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, }; diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index c27cedf087..fd9678c799 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -24,18 +24,18 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM | TIM_USE_LED, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM | TIM_USE_LED, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index 4e53bcdea4..4a9603edec 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -24,15 +24,15 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 9c716a5435..12ce8d40ed 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -24,18 +24,18 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, NULL, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM | TIM_USE_LED, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PPM | TIM_USE_PWM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM | TIM_USE_LED, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index d9c15988dc..58be31b640 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -25,18 +25,18 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 // UART3 RX/TX - //{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) - //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + //{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) + //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 9f4d170851..eaaba9beec 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -24,17 +24,17 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT }; diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index 4e53bcdea4..4a9603edec 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -24,15 +24,15 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/RACEBASE/target.c b/src/main/target/RACEBASE/target.c index 2a261533a6..ff5ec100c7 100755 --- a/src/main/target/RACEBASE/target.c +++ b/src/main/target/RACEBASE/target.c @@ -23,10 +23,10 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 1, GPIO_AF_1, NULL, 0 }, - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PC6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PC7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PMW4 - PC8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PC9 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 0, TIM_USE_LED, 1, GPIO_AF_2, DMA1_Channel2, DMA1_CH2_HANDLER }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PPM, 1, GPIO_AF_1, NULL, 0 }, + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PC6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PC7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PMW4 - PC8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PC9 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_2, DMA1_Channel2, DMA1_CH2_HANDLER }, // PWM5 - PC9 }; diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index 6e15446a0e..63ac464d5d 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -25,11 +25,11 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PA7 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM3 - PA8 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PWM6 - PPM - { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER}, // PWM6 - PPM + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PA7 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM3 - PA8 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PWM6 - PPM + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER}, // PWM6 - PPM }; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index ef6023948b..b9164d7b81 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -24,20 +24,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT #ifdef REVOLT - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_LED, 0, GPIO_AF_TIM4, DMA1_Stream0, DMA_Channel_2, DMA1_ST0_HANDLER }, // LED for REVOLT + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_TIM4, DMA1_Stream0, DMA_Channel_2, DMA1_ST0_HANDLER }, // LED for REVOLT #else - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT / LED for REVO - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT / LED for REVO + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT #endif }; diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 1ddf9641cc..44a60162f3 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -23,18 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S5_IN - { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S6_IN - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S1_OUT - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S3_OUT - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 }, // S5_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM2 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S5_IN + { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S6_IN + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S1_OUT + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S3_OUT + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 }, // S5_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index 171cbfb19f..ebaeeaf24a 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -24,21 +24,21 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0}, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0}, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0}, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index aa992d656f..88b9213abc 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -24,15 +24,15 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // SOFTSERIAL1 RX (NC) - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // SOFTSERIAL1 TX - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // SOFTSERIAL1 RX (NC) + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // SOFTSERIAL1 TX + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // LED_STRIP }; diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index 2927c0b960..920ca000c5 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -24,13 +24,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PB6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PB6 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB9 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PB6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PB6 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB9 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y }; diff --git a/src/main/target/SOULF4/target.c b/src/main/target/SOULF4/target.c index b3e2ef104b..f1aeb50ff6 100644 --- a/src/main/target/SOULF4/target.c +++ b/src/main/target/SOULF4/target.c @@ -23,16 +23,16 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 , NULL, 0, 0 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 , NULL, 0, 0 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 , DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 , NULL, 0, 0 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 , NULL, 0, 0 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 , DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT }; diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index fd009e5af9..cfd1441285 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -25,18 +25,18 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // 6 3-pin headers - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, TIM_USE_MOTOR | TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 0, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 // PWM7 - PMW10 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PWM, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PWM, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c index 359c09910c..22a00fa936 100644 --- a/src/main/target/SPARKY2/target.c +++ b/src/main/target/SPARKY2/target.c @@ -23,16 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM8 }, // PPM IN - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S2_IN - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S5_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM8 }, // PPM IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S2_IN + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S5_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index 419775eb67..150389f054 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -24,24 +24,24 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N // Production boards swapped RC_CH3/4 swapped to make it easierTIM_USE_MOTOR, to using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index dc9484b242..247082280f 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -25,16 +25,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index a0ad2fdb2b..7147c4a1fd 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -26,27 +26,27 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB5 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - PA6 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PA7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - PA6 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PA7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM5 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM6 - PA3 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA1 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, 0, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, 0, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) // LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index bd9c576fbe..bc29d23c14 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -24,19 +24,19 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_PPM | TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, 0, GPIO_AF_1, NULL, 0 }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, 0, GPIO_AF_1, NULL, 0 }, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, 0, GPIO_AF_1, NULL, 0 } + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_PPM | TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, + { TIM17, IO_TAG(PB9), TIM_Channel_1, 0, 0, GPIO_AF_1, NULL, 0 }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, + { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM3, IO_TAG(PA4), TIM_Channel_2, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD12), TIM_Channel_1, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD13), TIM_Channel_2, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD14), TIM_Channel_3, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD15), TIM_Channel_4, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, 0, GPIO_AF_1, NULL, 0 }, + { TIM2, IO_TAG(PA2), TIM_Channel_3, 0, 0, GPIO_AF_1, NULL, 0 } }; diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index c5fc07b418..aff64adfb4 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -23,16 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PPM - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S2_IN - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S3_IN - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S4_IN - { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM1_BRK_TIM9_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S5_IN - { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S6_IN - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S3_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S4_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S5_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S6_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PPM + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S2_IN + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S3_IN + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S4_IN + { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S5_IN + { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S6_IN + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S4_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S5_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S6_OUT }; diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index 38acf1457f..84de8e437c 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -9,19 +9,19 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6 , DMA1_Channel2, DMA1_CH2_HANDLER}, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6 , DMA1_Channel2, DMA1_CH2_HANDLER}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 8d51c13bc7..74a21fdb61 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -23,12 +23,12 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // MS5 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // MS6 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // MS5 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // MS6 }; diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index 9f99cf2be4..72d860c3f0 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -8,22 +8,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; From ee6641a917be97d533cd80cf40120aee9b4b306f Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 7 Nov 2016 14:33:12 +0900 Subject: [PATCH 135/188] Fix profile number offset problem --- src/main/cms/cms_menu_imu.c | 34 ++++++++++++++++++---------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 9f8d920202..5dc0dab09c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -50,12 +50,14 @@ // // PID // -static uint8_t pidProfileIndex; +static uint8_t tmpProfileIndex; +static uint8_t profileIndex; static uint8_t tempPid[4][3]; static long cmsx_menuImu_onEnter(void) { - pidProfileIndex = masterConfig.current_profile_index + 1; + profileIndex = masterConfig.current_profile_index; + tmpProfileIndex = profileIndex + 1; return 0; } @@ -64,7 +66,7 @@ static long cmsx_menuImu_onExit(OSD_Entry *self) { UNUSED(self); - masterConfig.current_profile_index = pidProfileIndex - 1; + masterConfig.current_profile_index = tmpProfileIndex - 1; return 0; } @@ -73,13 +75,13 @@ static long cmsx_PidRead(void) { for (uint8_t i = 0; i < 3; i++) { - tempPid[i][0] = masterConfig.profile[pidProfileIndex].pidProfile.P8[i]; - tempPid[i][1] = masterConfig.profile[pidProfileIndex].pidProfile.I8[i]; - tempPid[i][2] = masterConfig.profile[pidProfileIndex].pidProfile.D8[i]; + tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i]; + tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i]; + tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i]; } - tempPid[3][0] = masterConfig.profile[pidProfileIndex].pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = masterConfig.profile[pidProfileIndex].pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = masterConfig.profile[pidProfileIndex].pidProfile.D8[PIDLEVEL]; + tempPid[3][0] = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL]; + tempPid[3][1] = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL]; + tempPid[3][2] = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL]; return 0; } @@ -89,14 +91,14 @@ static long cmsx_PidWriteback(OSD_Entry *self) UNUSED(self); for (uint8_t i = 0; i < 3; i++) { - masterConfig.profile[pidProfileIndex].pidProfile.P8[i] = tempPid[i][0]; - masterConfig.profile[pidProfileIndex].pidProfile.I8[i] = tempPid[i][1]; - masterConfig.profile[pidProfileIndex].pidProfile.D8[i] = tempPid[i][2]; + masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0]; + masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1]; + masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2]; } - masterConfig.profile[pidProfileIndex].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - masterConfig.profile[pidProfileIndex].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - masterConfig.profile[pidProfileIndex].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; + masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; return 0; } @@ -255,7 +257,7 @@ static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, NULL, &(OSD_UINT8_t){ &pidProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, + {"PID PROF", OME_UINT8, NULL, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, {"RATE&RXPO", OME_Submenu, cmsMenuChange, &cmsx_menuRateExpo, 0}, {"RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, From 941c3abb1269a55b4d2b2d38ec17bcf99569434a Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 7 Nov 2016 16:51:29 +0900 Subject: [PATCH 136/188] IMU menu: fixed ranges to match that of cli. --- src/main/cms/cms_menu_imu.c | 40 ++++++++++++++++++------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 5dc0dab09c..279a265490 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -107,17 +107,17 @@ static OSD_Entry cmsx_menuPidEntries[] = { { "-- PID --", OME_Label, NULL, NULL, 0}, - { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][0], 10, 150, 1 }, 0 }, - { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][1], 1, 150, 1 }, 0 }, - { "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][2], 0, 150, 1 }, 0 }, + { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][0], 0, 200, 1 }, 0 }, + { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][1], 0, 200, 1 }, 0 }, + { "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][2], 0, 200, 1 }, 0 }, - { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][0], 10, 150, 1 }, 0 }, - { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][1], 1, 150, 1 }, 0 }, - { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][2], 0, 150, 1 }, 0 }, + { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][0], 0, 200, 1 }, 0 }, + { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][1], 0, 200, 1 }, 0 }, + { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][2], 0, 200, 1 }, 0 }, - { "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][0], 10, 150, 1 }, 0 }, - { "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][1], 1, 150, 1 }, 0 }, - { "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][2], 0, 150, 1 }, 0 }, + { "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][0], 0, 200, 1 }, 0 }, + { "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][1], 0, 200, 1 }, 0 }, + { "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][2], 0, 200, 1 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } @@ -165,18 +165,18 @@ static OSD_Entry cmsx_menuRateExpoEntries[] = { { "-- RATE&EXPO --", OME_Label, NULL, NULL, 0 }, - { "RC RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRate8, 0, 200, 1, 10 }, 0 }, - { "RC YAW RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawRate8, 0, 200, 1, 10 }, 0 }, + { "RC RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRate8, 0, 255, 1, 10 }, 0 }, + { "RC YAW RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawRate8, 0, 255, 1, 10 }, 0 }, - { "ROLL SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[0], 0, 250, 1, 10 }, 0 }, - { "PITCH SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[1], 0, 250, 1, 10 }, 0 }, - { "YAW SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[2], 0, 250, 1, 10 }, 0 }, + { "ROLL SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[0], 0, 100, 1, 10 }, 0 }, + { "PITCH SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[1], 0, 100, 1, 10 }, 0 }, + { "YAW SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[2], 0, 100, 1, 10 }, 0 }, { "RC EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo8, 0, 100, 1, 10 }, 0 }, { "RC YAW EXP", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawExpo8, 0, 100, 1, 10 }, 0 }, - { "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID, 0, 70, 1, 10}, 0 }, - { "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1100, 1800, 10}, 0 }, + { "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID, 0, 100, 1, 10}, 0 }, + { "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10}, 0 }, { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, { "SETPT RLX", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, @@ -233,10 +233,10 @@ static OSD_Entry menuImuMiscEntries[]= { "-- MISC --", OME_Label, NULL, NULL, 0 }, { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, - { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5 }, 0 }, - { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5 }, 0 }, - { "YAW P LIM", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5 }, 0 }, - { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1020, 1300, 10 }, 0 }, + { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 1 }, 0 }, + { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 1 }, 0 }, + { "YAW P LIM", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 1 }, 0 }, + { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1000, 2000, 1 }, 0 }, { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 }, { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 }, From 3bce0fb5f100526bc4fb072dd26b269df6f48afb Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 7 Nov 2016 16:54:22 +0900 Subject: [PATCH 137/188] Overlooked menu separation from osd.c This was overlooked (cms_menu_osd.h was already there). Now the element position editing capability is absolutely gone. --- Makefile | 1 + src/main/cms/cms_menu_osd.c | 113 ++++++++++++++++++++++++++++++++++++ src/main/io/osd.c | 103 -------------------------------- 3 files changed, 114 insertions(+), 103 deletions(-) create mode 100644 src/main/cms/cms_menu_osd.c diff --git a/Makefile b/Makefile index 7479252aa4..62a455084c 100644 --- a/Makefile +++ b/Makefile @@ -556,6 +556,7 @@ HIGHEND_SRC = \ cms/cms_menu_builtin.c \ cms/cms_menu_imu.c \ cms/cms_menu_ledstrip.c \ + cms/cms_menu_osd.c \ cms/cms_menu_vtx.c \ common/colorconversion.c \ drivers/display_ug2864hsweg01.c \ diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c new file mode 100644 index 0000000000..e85a9f173e --- /dev/null +++ b/src/main/cms/cms_menu_osd.c @@ -0,0 +1,113 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_osd.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#ifdef CMS + +OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; +OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; +OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1}; +OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1}; + +OSD_Entry cmsx_menuAlarmsEntries[] = +{ + {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0}, + {"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0}, + {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0}, + {"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuAlarms = { + "MENUALARMS", + OME_MENU, + NULL, + NULL, + NULL, + cmsx_menuAlarmsEntries, +}; + +OSD_Entry menuOsdActiveElemsEntries[] = +{ + {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0}, + {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0}, + {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0}, + {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0}, + {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0}, + {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0}, + {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0}, +#ifdef VTX + {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, +#endif // VTX + {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0}, + {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0}, +#ifdef GPS + {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0}, + {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0}, +#endif // GPS + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu menuOsdActiveElems = { + "MENUOSDACT", + OME_MENU, + NULL, + NULL, + NULL, + menuOsdActiveElemsEntries, +}; + +OSD_Entry cmsx_menuOsdLayoutEntries[] = +{ + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, + {"ACTIVE ELEM", OME_Submenu, cmsMenuChange, &menuOsdActiveElems, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuOsdLayout = { + "MENULAYOUT", + OME_MENU, + NULL, + NULL, + NULL, + cmsx_menuOsdLayoutEntries, +}; + +#endif // CMS diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ae96d31782..ce01260dc9 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -657,107 +657,4 @@ void osdUpdate(uint32_t currentTime) #endif } -#ifdef EDIT_ELEMENT_SUPPORT -void osdEditElement(void *ptr) -{ - uint32_t address = (uint32_t)ptr; - - // zsave position on menu stack - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; - - currentElement = (uint16_t *)address; - - *currentElement |= BLINK_FLAG; - max7456ClearScreen(); -} - -void osdDrawElementPositioningHelp(void) -{ - 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 - -OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; -OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; -OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1}; -OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1}; - -OSD_Entry cmsx_menuAlarmsEntries[] = -{ - {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, - {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0}, - {"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0}, - {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0}, - {"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -CMS_Menu cmsx_menuAlarms = { - "MENUALARMS", - OME_MENU, - NULL, - NULL, - NULL, - cmsx_menuAlarmsEntries, -}; - -OSD_Entry menuOsdActiveElemsEntries[] = -{ - {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, - {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, - {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0}, - {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0}, - {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0}, - {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0}, - {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0}, - {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0}, - {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0}, - {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0}, -#ifdef VTX - {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, -#endif // VTX - {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0}, - {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0}, -#ifdef GPS - {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0}, - {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0}, -#endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -CMS_Menu menuOsdActiveElems = { - "MENUOSDACT", - OME_MENU, - NULL, - NULL, - NULL, - menuOsdActiveElemsEntries, -}; - -OSD_Entry cmsx_menuOsdLayoutEntries[] = -{ - {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, - {"ACTIVE ELEM", OME_Submenu, cmsMenuChange, &menuOsdActiveElems, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -CMS_Menu cmsx_menuOsdLayout = { - "MENULAYOUT", - OME_MENU, - NULL, - NULL, - NULL, - cmsx_menuOsdLayoutEntries, -}; - #endif // OSD From 3c2849ffdcd7791a78e7579bb3ada9bb8250f384 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 7 Nov 2016 17:12:57 +0900 Subject: [PATCH 138/188] VTX menu: onEntry and onExit should be working now MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Don’t know why this was overlooked. Needs testing. --- src/main/cms/cms_menu_builtin.c | 2 +- src/main/cms/cms_menu_vtx.c | 31 ++++++++++++++++++++++++++----- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 7b094b0b3f..72365d4ae2 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -112,7 +112,7 @@ static CMS_Menu menuFeatures = { static OSD_Entry menuMainEntries[] = { - {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, + {"-- MAIN MENU --", OME_Label, NULL, NULL, 0}, {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0}, {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, #ifdef OSD diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index f96eb8f2da..32d921d62f 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -38,20 +38,24 @@ static bool featureRead = false; static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; -static void cmsx_Vtx_FeatureRead(void) +static long cmsx_Vtx_FeatureRead(void) { if (!featureRead) { cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; featureRead = true; } + + return 0; } -static void cmsx_Vtx_FeatureWriteback(void) +static long cmsx_Vtx_FeatureWriteback(void) { if (cmsx_featureVtx) featureSet(FEATURE_VTX); else featureClear(FEATURE_VTX); + + return 0; } static const char * const vtxBandNames[] = { @@ -90,6 +94,23 @@ static void cmsx_Vtx_ConfigWriteback(void) #endif // USE_RTC6705 } +static long cmsx_Vtx_onEnter(void) +{ + cmsx_Vtx_FeatureRead(); + cmsx_Vtx_ConfigRead(); + + return 0; +} + +static long cmsx_Vtx_onExit(OSD_Entry *self) +{ + UNUSED(self); + + cmsx_Vtx_ConfigWriteback(); + + return 0; +} + #ifdef VTX static OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; static OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; @@ -115,9 +136,9 @@ static OSD_Entry cmsx_menuVtxEntries[] = CMS_Menu cmsx_menuVtx = { "MENUVTX", OME_MENU, - NULL, - NULL, - NULL, + cmsx_Vtx_onEnter, + cmsx_Vtx_onExit, + cmsx_Vtx_FeatureWriteback, cmsx_menuVtxEntries, }; From 1da183728818777c02d8b51d6250fbb668d55bf9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 7 Nov 2016 18:53:23 +0900 Subject: [PATCH 139/188] Change feature name to MSP_DISPLAYPORT And some build option fix. --- src/main/cms/cms_menu_osd.c | 2 +- src/main/fc/config.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/main.c | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index e85a9f173e..f64c3a777f 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -31,7 +31,7 @@ #include "config/config_master.h" #include "config/feature.h" -#ifdef CMS +#if defined(OSD) && defined(CMS) OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 0934833a98..ce9cf8d5af 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -55,7 +55,7 @@ typedef enum { FEATURE_VTX = 1 << 24, FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, - FEATURE_MSP_OSD = 1 << 27, + FEATURE_MSP_DISPLAYPORT = 1 << 27, } features_e; void beeperOffSet(uint32_t mask); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 21bddf0704..7f2c858df1 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -229,7 +229,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", "MSP_OSD", NULL + " ", "VTX", "RX_SPI", "SOFTSPI", "MSP_DISPLAYPORT", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/main.c b/src/main/main.c index 7aef157dcf..515573887b 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -461,7 +461,7 @@ void init(void) mspSerialInit(); #ifdef USE_MSP_DISPLAYPORT - if (feature(FEATURE_MSP_OSD)) { + if (feature(FEATURE_MSP_DISPLAYPORT)) { cmsDisplayPortRegister(displayPortMspInit()); } #endif From aed2e48266f02ad527ef7c9b271198f3c512758c Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 7 Nov 2016 20:17:52 +0900 Subject: [PATCH 140/188] Tidy --- src/main/drivers/display.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 834ce92bb7..6589a7e930 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -46,11 +46,8 @@ void displayRelease(displayPort_t *instance) bool displayIsGrabbed(const displayPort_t *instance) { - if (instance && instance->isGrabbed) { // can be called before initialised - return true; - } else { - return false; - } + // can be called before initialised + return (instance && instance->isGrabbed); } int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s) From d572043b8afae3c22cab5239c7027af5af250727 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 7 Nov 2016 12:33:12 +0100 Subject: [PATCH 141/188] Remap TIM17 DMA on SPF3 --- src/main/target/SPRACINGF3/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index c685e61c31..718f15c982 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -103,6 +103,7 @@ #define RSSI_ADC_PIN PB2 #define USE_DSHOT +#define REMAP_TIM17_DMA // UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4 #if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT) From 5a6828b87fed44f5efa5f1fa157258bcfc99dc78 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 7 Nov 2016 20:35:01 +0900 Subject: [PATCH 142/188] RAMDOM!!! --- src/main/cms/cms.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index e5017ae5c7..ebfd9524eb 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -140,7 +140,7 @@ static int8_t cursorRow; #ifdef CMS_MENU_DEBUG // For external menu content creators -static char menuErrLabel[21 + 1] = "RAMDOM DATA"; +static char menuErrLabel[21 + 1] = "RANDOM DATA"; static OSD_Entry menuErrEntries[] = { { "BROKEN MENU", OME_Label, NULL, NULL, 0 }, From c12942ed41e0cb35c26b82712a02f78345dba7cc Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 7 Nov 2016 22:29:35 +0900 Subject: [PATCH 143/188] Substitute FEATURE_MSP_DISPLAYPORT with !defined(OSD) Save feature bits. --- src/main/fc/config.h | 1 - src/main/io/serial_cli.c | 2 +- src/main/main.c | 6 ++---- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ce9cf8d5af..e6cc491229 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -55,7 +55,6 @@ typedef enum { FEATURE_VTX = 1 << 24, FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, - FEATURE_MSP_DISPLAYPORT = 1 << 27, } features_e; void beeperOffSet(uint32_t mask); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7f2c858df1..4ee0324e62 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -229,7 +229,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", "MSP_DISPLAYPORT", NULL + " ", "VTX", "RX_SPI", "SOFTSPI", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/main.c b/src/main/main.c index 515573887b..6ad8fbc9c7 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -460,10 +460,8 @@ void init(void) mspFcInit(); mspSerialInit(); -#ifdef USE_MSP_DISPLAYPORT - if (feature(FEATURE_MSP_DISPLAYPORT)) { - cmsDisplayPortRegister(displayPortMspInit()); - } +#if defined(USE_MSP_DISPLAYPORT) && defined(CMS) + cmsDisplayPortRegister(displayPortMspInit()); #endif #ifdef USE_CLI From 39784a74fed73e1ea1202a53a4408aa76d284758 Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Tue, 8 Nov 2016 01:55:19 +1100 Subject: [PATCH 144/188] First test // Props off --- src/main/target/X_RACERSPI/target.c | 10 ++++++---- src/main/target/X_RACERSPI/target.h | 7 +++++++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index 84de8e437c..045633276a 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -15,10 +15,12 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM4 - PA12 + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 34e51b5885..1b812cc8ba 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -101,6 +101,13 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 +#define USE_DSHOT +#define REMAP_TIM17_DMA +// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4 +#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT) +#undef USE_UART1_TX_DMA +#endif + #define LED_STRIP #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT From 576ad19914b5ac67c3852ab80a26747289bfe250 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 8 Nov 2016 05:27:09 +0900 Subject: [PATCH 145/188] Add OME_Funcall to call a function. With the introduction of the CME_Menu, OME_Submenu became unsuitable for general function calls. Actually, it works and call the designated function, but attempt to externally traverse the menu structure fail because it expects CME_Menu * as data for OME_Submenu. --- src/main/cms/cms.c | 2 ++ src/main/cms/cms_menu_blackbox.c | 2 +- src/main/cms/cms_types.h | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index ebfd9524eb..cbf6ae56bd 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -246,6 +246,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) } break; case OME_Submenu: + case OME_Funcall: if (IS_PRINTVALUE(p)) { cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); CLR_PRINTVALUE(p); @@ -639,6 +640,7 @@ static uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) switch (p->type) { case OME_Submenu: + case OME_Funcall: case OME_OSD_Exit: if (p->func && key == KEY_RIGHT) { p->func(pDisplay, p->data); diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 67b180802d..661ba9c4cc 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -93,7 +93,7 @@ static OSD_Entry cmsx_menuBlackboxEntries[] = { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &masterConfig.blackbox_rate_denom,1,32,1 }, 0 }, #ifdef USE_FLASHFS - { "ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0 }, + { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, #endif // USE_FLASHFS { "BACK", OME_Back, NULL, NULL, 0 }, diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index a2336e2c77..de46c34ea8 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -32,6 +32,7 @@ typedef enum OME_Back, OME_OSD_Exit, OME_Submenu, + OME_Funcall, OME_Bool, OME_INT8, OME_UINT8, From 2c9d71aeae382d468f3658819b754d486bdb4e3c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 7 Nov 2016 20:50:15 +0000 Subject: [PATCH 146/188] Added CMS test code --- src/main/cms/cms.c | 13 ++- src/main/cms/cms_types.h | 4 +- src/test/Makefile | 41 +++++++ src/test/unit/cms_unittest.cc | 207 ++++++++++++++++++++++++++++++++++ src/test/unit/target.h | 2 + 5 files changed, 260 insertions(+), 7 deletions(-) create mode 100644 src/test/unit/cms_unittest.cc diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index cbf6ae56bd..82b92276de 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -32,8 +32,9 @@ #ifdef CMS -#include "build/version.h" +#include "build/build_config.h" #include "build/debug.h" +#include "build/version.h" #include "cms/cms.h" #include "cms/cms_menu_builtin.h" @@ -124,7 +125,7 @@ static displayPort_t *cmsDisplayPortSelectNext(void) static bool cmsInMenu = false; -static CMS_Menu *currentMenu; // Points to top entry of the current page +STATIC_UNIT_TESTED CMS_Menu *currentMenu; // Points to top entry of the current page // XXX Does menu backing support backing into second page??? @@ -481,7 +482,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr) return 0; } -static long cmsMenuBack(displayPort_t *pDisplay) +STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay) { // Let onExit function decide whether to allow exit or not. @@ -512,7 +513,7 @@ static long cmsMenuBack(displayPort_t *pDisplay) return 0; } -static void cmsMenuOpen(void) +STATIC_UNIT_TESTED void cmsMenuOpen(void) { if (!cmsInMenu) { // New open @@ -587,7 +588,7 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr) return 0; } -static uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) +STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) { uint16_t res = BUTTON_TIME; OSD_Entry *p; @@ -845,6 +846,8 @@ void cmsHandler(uint32_t currentTime) // Can it be done with the current main()? void cmsInit(void) { + cmsDeviceCount = 0; + cmsCurrentDevice = -1; } #endif // CMS diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index de46c34ea8..fd60cb5480 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -55,7 +55,7 @@ typedef enum typedef struct { - char *text; + const char *text; OSD_MenuElement type; CMSEntryFuncPtr func; void *data; @@ -92,7 +92,7 @@ typedef long (*CMSMenuOnExitPtr)(OSD_Entry *self); typedef struct { // These two are debug aids for menu content creators. - char *GUARD_text; + const char *GUARD_text; OSD_MenuElement GUARD_type; CMSMenuFuncPtr onEnter; diff --git a/src/test/Makefile b/src/test/Makefile index 6581e7ae2b..d8eb183f34 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -110,6 +110,14 @@ $(OBJECT_DIR)/common/maths.o : \ @mkdir -p $(dir $@) $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@ +$(OBJECT_DIR)/common/typeconversion.o : \ + $(USER_DIR)/common/typeconversion.c \ + $(USER_DIR)/common/typeconversion.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/typeconversion.c -o $@ + $(OBJECT_DIR)/common/filter.o : \ $(USER_DIR)/common/filter.c \ $(USER_DIR)/common/filter.h \ @@ -574,6 +582,39 @@ $(OBJECT_DIR)/alignsensor_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/drivers/display.o : \ + $(USER_DIR)/drivers/display.c \ + $(USER_DIR)/drivers/display.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/display.c -o $@ + +$(OBJECT_DIR)/cms/cms.o : \ + $(USER_DIR)/cms/cms.c \ + $(USER_DIR)/cms/cms.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/cms/cms.c -o $@ + +$(OBJECT_DIR)/cms_unittest.o : \ + $(TEST_DIR)/cms_unittest.cc \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/cms_unittest.cc -o $@ + +$(OBJECT_DIR)/cms_unittest : \ + $(OBJECT_DIR)/cms_unittest.o \ + $(OBJECT_DIR)/common/typeconversion.o \ + $(OBJECT_DIR)/drivers/display.o \ + $(OBJECT_DIR)/cms/cms.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + + ## test : Build and run the Unit Tests test: $(TESTS:%=test-%) diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc new file mode 100644 index 0000000000..a417e8b7ee --- /dev/null +++ b/src/test/unit/cms_unittest.cc @@ -0,0 +1,207 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include + +#define BARO + +extern "C" { + #include "target.h" + #include "drivers/display.h" + #include "cms/cms.h" + #include "cms/cms_types.h" + void cmsMenuOpen(void); + long cmsMenuBack(displayPort_t *pDisplay); + uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key); + extern CMS_Menu *currentMenu; // Points to top entry of the current page +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +static displayPort_t testDisplayPort; +static int displayPortTestGrab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestRelease(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestClear(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) +{ + UNUSED(displayPort); + UNUSED(x); + UNUSED(y); + UNUSED(s); + return 0; +} + +static int displayPortTestHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static void displayPortTestResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static const displayPortVTable_t testDisplayPortVTable = { + .grab = displayPortTestGrab, + .release = displayPortTestRelease, + .clear = displayPortTestClear, + .write = displayPortTestWrite, + .heartbeat = displayPortTestHeartbeat, + .resync = displayPortTestResync, + .txBytesFree = displayPortTestTxBytesFree +}; + +displayPort_t *displayPortTestInit(void) +{ + testDisplayPort.vTable = &testDisplayPortVTable; + testDisplayPort.rows = 10; + testDisplayPort.cols = 40; + testDisplayPort.isGrabbed = false; + return &testDisplayPort; +} + +TEST(CMSUnittest, TestCmsDisplayPortRegister) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + for (int ii = 0; ii < CMS_MAX_DEVICE; ++ii) { + const bool registered = cmsDisplayPortRegister(displayPort); + EXPECT_EQ(true, registered); + } + const bool registered = cmsDisplayPortRegister(displayPort); + EXPECT_EQ(false, registered); +} + +TEST(CMSUnittest, TestCmsMenuOpen) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + displayGrab(displayPort); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); +} + +TEST(CMSUnittest, TestCmsMenuExit0) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuExit(displayPort, (void*)0); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuExit1) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuExit(displayPort, (void*)0); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuBack) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuBack(displayPort); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuKey) +{ +#define KEY_ENTER 0 +#define KEY_UP 1 +#define KEY_DOWN 2 +#define KEY_LEFT 3 +#define KEY_RIGHT 4 +#define KEY_ESC 5 +#define BUTTON_TIME 250 // msec +#define BUTTON_PAUSE 500 // msec + cmsInit(); + displayPort_t *displayPort = &testDisplayPort; + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + uint16_t result = cmsHandleKey(displayPort, KEY_ESC); + EXPECT_EQ(BUTTON_PAUSE, result); +} +// STUBS + +extern "C" { +static OSD_Entry menuMainEntries[] = +{ + {"-- MAIN MENU --", OME_Label, NULL, NULL, 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; +CMS_Menu menuMain = { + "MENUMAIN", + OME_MENU, + NULL, + NULL, + NULL, + menuMainEntries, +}; +uint8_t armingFlags; +int16_t debug[4]; +int16_t rcData[18]; +void delay(uint32_t) {} +uint32_t micros(void) { return 0; } +uint32_t millis(void) { return 0; } +void saveConfigAndNotify(void) {} +void stopMotors(void) {} +void stopPwmAllMotors(void) {} +void systemReset(void) {} +} \ No newline at end of file diff --git a/src/test/unit/target.h b/src/test/unit/target.h index b7899e43a2..c2ebf479ed 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -17,6 +17,8 @@ #pragma once +#define CMS +#define CMS_MAX_DEVICE 4 #define MAG #define BARO #define GPS From 8dc3bee80d1b16a315651dcd79313a68b4952c87 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 8 Nov 2016 13:47:37 +1100 Subject: [PATCH 147/188] Adding DMA ownership for ADC, LED STRIP, USART and MOTOR --- src/main/drivers/adc_impl.h | 1 - src/main/drivers/adc_stm32f10x.c | 6 +- src/main/drivers/adc_stm32f30x.c | 8 +- src/main/drivers/adc_stm32f4xx.c | 8 +- src/main/drivers/adc_stm32f7xx.c | 8 +- src/main/drivers/dma.c | 29 ++++++- src/main/drivers/dma.h | 82 +++++++++++-------- src/main/drivers/dma_stm32f4xx.c | 31 ++++++- src/main/drivers/dma_stm32f7xx.c | 49 +++++++---- src/main/drivers/io.c | 2 +- src/main/drivers/light_ws2811strip_hal.c | 1 + .../drivers/light_ws2811strip_stm32f10x.c | 2 + .../drivers/light_ws2811strip_stm32f30x.c | 2 + .../drivers/light_ws2811strip_stm32f4xx.c | 1 + src/main/drivers/pwm_output_stm32f3xx.c | 1 + src/main/drivers/pwm_output_stm32f4xx.c | 6 +- src/main/drivers/pwm_output_stm32f7xx.c | 1 + src/main/drivers/resource.h | 1 + src/main/drivers/serial_uart_stm32f10x.c | 5 +- src/main/drivers/serial_uart_stm32f30x.c | 6 ++ src/main/drivers/serial_uart_stm32f4xx.c | 8 +- src/main/io/serial_cli.c | 40 ++++++--- src/main/main.c | 4 - 23 files changed, 212 insertions(+), 90 deletions(-) diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 66e2f887c9..f908440a1e 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -51,7 +51,6 @@ typedef struct adcTagMap_s { typedef struct adcDevice_s { ADC_TypeDef* ADCx; rccPeriphTag_t rccADC; - rccPeriphTag_t rccDMA; #if defined(STM32F4) || defined(STM32F7) DMA_Stream_TypeDef* DMAy_Streamx; uint32_t channel; diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 19d7edf6f9..00fd32bcb0 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -32,13 +32,14 @@ #include "adc_impl.h" #include "io.h" #include "rcc.h" +#include "dma.h" #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Channelx = DMA1_Channel1 } }; ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) @@ -131,7 +132,8 @@ void adcInit(drv_adc_config_t *init) RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) RCC_ClockCmd(adc.rccADC, ENABLE); - RCC_ClockCmd(adc.rccDMA, ENABLE); + + dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, RESOURCE_INDEX(device)); DMA_DeInit(adc.DMAy_Channelx); DMA_InitTypeDef DMA_InitStructure; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index f58a8086d6..599ab4296e 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -30,6 +30,7 @@ #include "adc_impl.h" #include "io.h" #include "rcc.h" +#include "dma.h" #include "common/utils.h" @@ -38,8 +39,8 @@ #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }, - { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } + { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA1_Channel1 }, + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 } }; const adcTagMap_t adcTagMap[] = { @@ -149,7 +150,8 @@ void adcInit(drv_adc_config_t *init) RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz RCC_ClockCmd(adc.rccADC, ENABLE); - RCC_ClockCmd(adc.rccDMA, ENABLE); + + dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, RESOURCE_INDEX(device)); DMA_DeInit(adc.DMAy_Channelx); diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 968f05a67e..9655381977 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -25,6 +25,7 @@ #include "io.h" #include "io_impl.h" #include "rcc.h" +#include "dma.h" #include "sensor.h" #include "accgyro.h" @@ -37,8 +38,8 @@ #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, - //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } }; /* note these could be packed up for saving space */ @@ -140,9 +141,10 @@ void adcInit(drv_adc_config_t *init) adcConfig[i].enabled = true; } - RCC_ClockCmd(adc.rccDMA, ENABLE); RCC_ClockCmd(adc.rccADC, ENABLE); + dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, RESOURCE_INDEX(device)); + DMA_DeInit(adc.DMAy_Streamx); DMA_StructInit(&DMA_InitStructure); diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index 7bdba67f52..b6c84fbaad 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -25,6 +25,7 @@ #include "io.h" #include "io_impl.h" #include "rcc.h" +#include "dma.h" #include "sensor.h" #include "accgyro.h" @@ -37,8 +38,8 @@ #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_CHANNEL_0 }, - //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_CHANNEL_0 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } }; /* note these could be packed up for saving space */ @@ -138,8 +139,9 @@ void adcInit(drv_adc_config_t *init) adcConfig[i].enabled = true; } - RCC_ClockCmd(adc.rccDMA, ENABLE); + RCC_ClockCmd(adc.rccADC, ENABLE); + dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, RESOURCE_INDEX(device)); ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; ADCHandle.Init.ContinuousConvMode = ENABLE; diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index 7e4942fbe1..ac46ff4f2b 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -63,16 +63,18 @@ DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_CH4_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_CH5_HANDLER) #endif - -void dmaInit(void) +void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) { - // TODO: Do we need this? + RCC_AHBPeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); + dmaDescriptors[identifier].owner = owner; + dmaDescriptors[identifier].resourceIndex = resourceIndex; } -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) { NVIC_InitTypeDef NVIC_InitStructure; + /* TODO: remove this - enforce the init */ RCC_AHBPeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); dmaDescriptors[identifier].irqHandlerCallback = callback; dmaDescriptors[identifier].userParam = userParam; @@ -84,3 +86,22 @@ void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr NVIC_Init(&NVIC_InitStructure); } +resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].owner; +} + +uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].resourceIndex; +} + +dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel) +{ + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + if (dmaDescriptors[i].channel == channel) { + return i; + } + } + return 0; +} \ No newline at end of file diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index cd6ecc10be..e34233e570 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -17,9 +17,27 @@ #pragma once +#include "resource.h" + struct dmaChannelDescriptor_s; typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channelDescriptor); +typedef struct dmaChannelDescriptor_s { + DMA_TypeDef* dma; +#if defined(STM32F4) || defined(STM32F7) + DMA_Stream_TypeDef* stream; +#else + DMA_Channel_TypeDef* channel; +#endif + dmaCallbackHandlerFuncPtr irqHandlerCallback; + uint8_t flagsShift; + IRQn_Type irqN; + uint32_t rcc; + uint32_t userParam; + resourceOwner_e owner; + uint8_t resourceIndex; +} dmaChannelDescriptor_t; + #if defined(STM32F4) || defined(STM32F7) uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream); @@ -41,19 +59,15 @@ typedef enum { DMA2_ST5_HANDLER, DMA2_ST6_HANDLER, DMA2_ST7_HANDLER, -} dmaHandlerIdentifier_e; + DMA_MAX_DESCRIPTORS +} dmaIdentifier_e; -typedef struct dmaChannelDescriptor_s { - DMA_TypeDef* dma; - DMA_Stream_TypeDef* stream; - dmaCallbackHandlerFuncPtr irqHandlerCallback; - uint8_t flagsShift; - IRQn_Type irqN; - uint32_t rcc; - uint32_t userParam; -} dmaChannelDescriptor_t; +#define DMA_MOD_VALUE 8 +#define DMA_MOD_OFFSET 0 +#define DMA_OUTPUT_INDEX 0 +#define DMA_OUTPUT_STRING "DMA%d Stream %d:" -#define DEFINE_DMA_CHANNEL(d, s, f, i, r) {.dma = d, .stream = s, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0} +#define DEFINE_DMA_CHANNEL(d, s, f, i, r) {.dma = d, .stream = s, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0, .owner = 0, .resourceIndex = 0 } #define DEFINE_DMA_IRQ_HANDLER(d, s, i) void DMA ## d ## _Stream ## s ## _IRQHandler(void) {\ if (dmaDescriptors[i].irqHandlerCallback)\ dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ @@ -63,11 +77,13 @@ typedef struct dmaChannelDescriptor_s { #define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) -#define DMA_IT_TCIF ((uint32_t)0x00000020) -#define DMA_IT_HTIF ((uint32_t)0x00000010) -#define DMA_IT_TEIF ((uint32_t)0x00000008) -#define DMA_IT_DMEIF ((uint32_t)0x00000004) -#define DMA_IT_FEIF ((uint32_t)0x00000001) +#define DMA_IT_TCIF ((uint32_t)0x00000020) +#define DMA_IT_HTIF ((uint32_t)0x00000010) +#define DMA_IT_TEIF ((uint32_t)0x00000008) +#define DMA_IT_DMEIF ((uint32_t)0x00000004) +#define DMA_IT_FEIF ((uint32_t)0x00000001) + +dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream); #else @@ -79,24 +95,22 @@ typedef enum { DMA1_CH5_HANDLER, DMA1_CH6_HANDLER, DMA1_CH7_HANDLER, +#if defined(STM32F3) || defined(STM32F10X_CL) DMA2_CH1_HANDLER, DMA2_CH2_HANDLER, DMA2_CH3_HANDLER, DMA2_CH4_HANDLER, DMA2_CH5_HANDLER, -} dmaHandlerIdentifier_e; +#endif + DMA_MAX_DESCRIPTORS +} dmaIdentifier_e; -typedef struct dmaChannelDescriptor_s { - DMA_TypeDef* dma; - DMA_Channel_TypeDef* channel; - dmaCallbackHandlerFuncPtr irqHandlerCallback; - uint8_t flagsShift; - IRQn_Type irqN; - uint32_t rcc; - uint32_t userParam; -} dmaChannelDescriptor_t; +#define DMA_MOD_VALUE 7 +#define DMA_MOD_OFFSET 1 +#define DMA_OUTPUT_INDEX 0 +#define DMA_OUTPUT_STRING "DMA%d Channel %d:" -#define DEFINE_DMA_CHANNEL(d, c, f, i, r) {.dma = d, .channel = c, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0} +#define DEFINE_DMA_CHANNEL(d, c, f, i, r) {.dma = d, .channel = c, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0, .owner = 0, .resourceIndex = 0 } #define DEFINE_DMA_IRQ_HANDLER(d, c, i) void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\ if (dmaDescriptors[i].irqHandlerCallback)\ dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ @@ -105,12 +119,16 @@ typedef struct dmaChannelDescriptor_s { #define DMA_CLEAR_FLAG(d, flag) d->dma->IFCR = (flag << d->flagsShift) #define DMA_GET_FLAG_STATUS(d, flag) (d->dma->ISR & (flag << d->flagsShift)) -#define DMA_IT_TCIF ((uint32_t)0x00000002) -#define DMA_IT_HTIF ((uint32_t)0x00000004) -#define DMA_IT_TEIF ((uint32_t)0x00000008) +#define DMA_IT_TCIF ((uint32_t)0x00000002) +#define DMA_IT_HTIF ((uint32_t)0x00000004) +#define DMA_IT_TEIF ((uint32_t)0x00000008) + +dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel); #endif -void dmaInit(void); -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam); +void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex); +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam); +resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier); +uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier); diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index 4147f29c07..afcd144286 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -23,11 +23,12 @@ #include "nvic.h" #include "dma.h" +#include "resource.h" /* * DMA descriptors. */ -static dmaChannelDescriptor_t dmaDescriptors[] = { +static dmaChannelDescriptor_t dmaDescriptors[DMA_MAX_DESCRIPTORS] = { DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream0, 0, DMA1_Stream0_IRQn, RCC_AHB1Periph_DMA1), DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream1, 6, DMA1_Stream1_IRQn, RCC_AHB1Periph_DMA1), DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream2, 16, DMA1_Stream2_IRQn, RCC_AHB1Periph_DMA1), @@ -67,12 +68,14 @@ DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER) -void dmaInit(void) +void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) { - // TODO: Do we need this? + RCC_AHB1PeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); + dmaDescriptors[identifier].owner = owner; + dmaDescriptors[identifier].resourceIndex = resourceIndex; } -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) { NVIC_InitTypeDef NVIC_InitStructure; @@ -100,4 +103,24 @@ uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream) RETURN_TCIF_FLAG(stream, 6); RETURN_TCIF_FLAG(stream, 7); return 0; +} + +resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].owner; +} + +uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].resourceIndex; +} + +dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream) +{ + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + if (dmaDescriptors[i].stream == stream) { + return i; + } + } + return 0; } \ No newline at end of file diff --git a/src/main/drivers/dma_stm32f7xx.c b/src/main/drivers/dma_stm32f7xx.c index f13c3a0619..998f5abe73 100644 --- a/src/main/drivers/dma_stm32f7xx.c +++ b/src/main/drivers/dma_stm32f7xx.c @@ -23,11 +23,12 @@ #include "drivers/nvic.h" #include "drivers/dma.h" +#include "resource.h" /* * DMA descriptors. */ -static dmaChannelDescriptor_t dmaDescriptors[] = { +static dmaChannelDescriptor_t dmaDescriptors[DMA_MAX_DESCRIPTORS] = { DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream0, 0, DMA1_Stream0_IRQn, RCC_AHB1ENR_DMA1EN), DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream1, 6, DMA1_Stream1_IRQn, RCC_AHB1ENR_DMA1EN), DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream2, 16, DMA1_Stream2_IRQn, RCC_AHB1ENR_DMA1EN), @@ -68,30 +69,50 @@ DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER) - -void dmaInit(void) +static void enableDmaClock(uint32_t rcc) { - // TODO: Do we need this? -} - -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) -{ - //clock - //RCC_AHB1PeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); - do { __IO uint32_t tmpreg; - SET_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc); + SET_BIT(RCC->AHB1ENR, rcc); /* Delay after an RCC peripheral clock enabling */ - tmpreg = READ_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc); + tmpreg = READ_BIT(RCC->AHB1ENR, rcc); UNUSED(tmpreg); } while(0); +} +void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) +{ + enableDmaClock(dmaDescriptors[identifier].rcc); + dmaDescriptors[identifier].owner = owner; + dmaDescriptors[identifier].resourceIndex = resourceIndex; +} + +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +{ + enableDmaClock(dmaDescriptors[identifier].rcc); dmaDescriptors[identifier].irqHandlerCallback = callback; dmaDescriptors[identifier].userParam = userParam; - HAL_NVIC_SetPriority(dmaDescriptors[identifier].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority)); HAL_NVIC_EnableIRQ(dmaDescriptors[identifier].irqN); } +resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].owner; +} + +uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].resourceIndex; +} + +dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream) +{ + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + if (dmaDescriptors[i].stream == stream) { + return i; + } + } + return 0; +} \ No newline at end of file diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 39de281fc7..1b272bf095 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -65,7 +65,7 @@ const struct ioPortDef_s ioPortDefs[] = { const char * const ownerNames[OWNER_TOTAL_COUNT] = { "FREE", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER", "SONAR_TRIGGER", "SONAR_ECHO", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", - "BARO", "MPU", "INVERTER", "LED_STRIP", "LED", "RX", "TX", "SOFT_SPI", "RX_SPI" + "BARO", "MPU", "INVERTER", "LED_STRIP", "LED", "RX", "TX", "SOFT_SPI", "RX_SPI", "MAX7456" }; const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index 5d8e4cd0c2..3c53cd03cd 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -106,6 +106,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) /* Link hdma_tim to hdma[x] (channelx) */ __HAL_LINKDMA(&TimHandle, hdma[dmaSource], hdma_tim); + dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaSource); /* Initialize TIMx DMA handle */ diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 1933e0a1c8..476fc6125f 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -114,6 +114,8 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); + + dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); const hsvColor_t hsv_white = { 0, 255, 255}; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index ffb6269d85..cb37a9cf62 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -124,6 +124,8 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); + + dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); const hsvColor_t hsv_white = { 0, 255, 255}; diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 4fd38325ee..13d00c0724 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -138,6 +138,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) DMA_ITConfig(stream, DMA_IT_TC, ENABLE); DMA_ClearITPendingBit(stream, dmaFlag_IT_TCIF(stream)); + dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); const hsvColor_t hsv_white = { 0, 255, 255 }; diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 8a14161062..d6e980b73e 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -173,6 +173,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_Channel_TypeDef *channel = timerHardware->dmaChannel; + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, motorIndex); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); DMA_Cmd(channel, DISABLE); diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 5e8287cfa8..9f732a4907 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -173,9 +173,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t } DMA_Stream_TypeDef *stream = timerHardware->dmaStream; + + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, motorIndex); + dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); DMA_Cmd(stream, DISABLE); DMA_DeInit(stream); + DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_Channel = timerHardware->dmaChannel; DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware); @@ -197,8 +201,6 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_ITConfig(stream, DMA_IT_TC, ENABLE); DMA_ClearITPendingBit(stream, dmaFlag_IT_TCIF(timerHardware->dmaStream)); - - dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); } #endif diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index fa03d52215..e192557c01 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -202,6 +202,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t /* Link hdma_tim to hdma[x] (channelx) */ __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], motor->hdma_tim); + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, motorIndex); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); /* Initialize TIMx DMA handle */ diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 44ec615660..7eb040e7d7 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -33,6 +33,7 @@ typedef enum { OWNER_TX, OWNER_SOFTSPI, OWNER_RX_SPI, + OWNER_MAX7456, OWNER_TOTAL_COUNT } resourceOwner_e; diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index 8b422df008..b4602ae020 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -109,8 +109,8 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART1; - #ifdef USE_UART1_RX_DMA + dmaInit(DMA1_CH5_HANDLER, OWNER_SERIAL, 1); s->rxDMAChannel = DMA1_Channel5; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; #endif @@ -118,7 +118,6 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; RCC_ClockCmd(RCC_APB2(USART1), ENABLE); - RCC_ClockCmd(RCC_AHB(DMA1), ENABLE); // UART1_TX PA9 // UART1_RX PA10 @@ -138,6 +137,7 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option } // DMA TX Interrupt + dmaInit(DMA1_CH4_HANDLER, OWNER_SERIAL, 1); dmaSetHandler(DMA1_CH4_HANDLER, uart_tx_dma_IRQHandler, NVIC_PRIO_SERIALUART1_TXDMA, (uint32_t)&uartPort1); #ifndef USE_UART1_RX_DMA @@ -189,7 +189,6 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; RCC_ClockCmd(RCC_APB1(USART2), ENABLE); - RCC_ClockCmd(RCC_AHB(DMA1), ENABLE); // UART2_TX PA2 // UART2_RX PA3 diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 41b222fd1b..f37f85790c 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -160,6 +160,7 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART1; #ifdef USE_UART1_RX_DMA + dmaInit(DMA1_CH5_HANDLER, OWNER_SERIAL, 1); s->rxDMAChannel = DMA1_Channel5; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; #endif @@ -177,6 +178,7 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option serialUARTInit(IOGetByTag(IO_TAG(UART1_TX_PIN)), IOGetByTag(IO_TAG(UART1_RX_PIN)), mode, options, GPIO_AF_7, 1); #ifdef USE_UART1_TX_DMA + dmaInit(DMA1_CH4_HANDLER, OWNER_SERIAL, 1); dmaSetHandler(DMA1_CH4_HANDLER, handleUsartTxDma, NVIC_PRIO_SERIALUART1_TXDMA, (uint32_t)&uartPort1); #endif @@ -214,10 +216,12 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART2; #ifdef USE_UART2_RX_DMA + dmaInit(DMA1_CH6_HANDLER, OWNER_SERIAL, 2); s->rxDMAChannel = DMA1_Channel6; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; #endif #ifdef USE_UART2_TX_DMA + dmaInit(DMA1_CH7_HANDLER, OWNER_SERIAL, 2); s->txDMAChannel = DMA1_Channel7; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; #endif @@ -269,10 +273,12 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART3; #ifdef USE_UART3_RX_DMA + dmaInit(DMA1_CH3_HANDLER, OWNER_SERIAL, 3); s->rxDMAChannel = DMA1_Channel3; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; #endif #ifdef USE_UART3_TX_DMA + dmaInit(DMA1_CH2_HANDLER, OWNER_SERIAL, 3); s->txDMAChannel = DMA1_Channel2; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; #endif diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index eb20fc8ec8..c0aed43f73 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -315,9 +315,13 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po if (uart->rxDMAStream) { s->rxDMAChannel = uart->DMAChannel; s->rxDMAStream = uart->rxDMAStream; + dmaInit(dmaGetIdentifier(uart->rxDMAStream), OWNER_SERIAL, RESOURCE_INDEX(device)); + } + if (uart->txDMAStream) { + s->txDMAChannel = uart->DMAChannel; + s->txDMAStream = uart->txDMAStream; + dmaInit(dmaGetIdentifier(uart->txDMAStream), OWNER_SERIAL, RESOURCE_INDEX(device)); } - s->txDMAChannel = uart->DMAChannel; - s->txDMAStream = uart->txDMAStream; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index bcb7f1542f..b53c5881ac 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -50,6 +50,7 @@ uint8_t cliMode = 0; #include "drivers/flash.h" #include "drivers/io.h" #include "drivers/io_impl.h" +#include "drivers/dma.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/sdcard.h" @@ -1242,12 +1243,12 @@ static void printSerial(uint8_t dumpMask, master_t *defaultConfig) serialConfig_t *serialConfigDefault; bool equalsDefault; for (uint32_t i = 0; i < SERIAL_PORT_COUNT; i++) { - serialConfig = &masterConfig.serialConfig; + serialConfig = &masterConfig.serialConfig; if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { continue; }; - serialConfigDefault = &defaultConfig->serialConfig; - equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier + serialConfigDefault = &defaultConfig->serialConfig; + equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex @@ -1961,11 +1962,11 @@ static void cliServo(char *cmdline) static void printServoMix(uint8_t dumpMask, master_t *defaultConfig) { for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { - servoMixer_t customServoMixer = masterConfig.customServoMixer[i]; - servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i]; + servoMixer_t customServoMixer = masterConfig.customServoMixer[i]; + servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i]; if (customServoMixer.rate == 0) { break; - } + } bool equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel && customServoMixer.inputSource == customServoMixerDefault.inputSource @@ -1975,7 +1976,7 @@ static void printServoMix(uint8_t dumpMask, master_t *defaultConfig) && customServoMixer.max == customServoMixerDefault.max && customServoMixer.box == customServoMixerDefault.box; - const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; + const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; cliDefaultPrintf(dumpMask, equalsDefault, format, i, customServoMixerDefault.targetChannel, @@ -1998,7 +1999,7 @@ static void printServoMix(uint8_t dumpMask, master_t *defaultConfig) ); } - cliPrint("\r\n"); + cliPrint("\r\n"); // print servo directions for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -2026,7 +2027,7 @@ static void cliServoMix(char *cmdline) len = strlen(cmdline); if (len == 0) { - printServoMix(DUMP_MASTER, NULL); + printServoMix(DUMP_MASTER, NULL); } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer)); @@ -2371,7 +2372,7 @@ static void cliVtx(char *cmdline) static void printName(uint8_t dumpMask) { bool equalsDefault = strlen(masterConfig.name) == 0; - cliDumpPrintf(dumpMask, equalsDefault, "name %s\r\n", equalsDefault ? emptyName : masterConfig.name); + cliDumpPrintf(dumpMask, equalsDefault, "name %s\r\n", equalsDefault ? emptyName : masterConfig.name); } static void cliName(char *cmdline) @@ -2599,7 +2600,7 @@ static void cliMap(char *cmdline) parseRcChannels(cmdline, &masterConfig.rxConfig); } cliPrint("Map: "); - uint32_t i; + uint32_t i; for (i = 0; i < 8; i++) out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; out[i] = '\0'; @@ -3834,7 +3835,7 @@ static void cliResource(char *cmdline) #ifndef CLI_MINIMAL_VERBOSITY cliPrintf("Currently active IO resource assignments:\r\n(reboot to update)\r\n----------------------\r\n"); #endif - for (uint32_t i = 0; i < DEFIO_IO_USED_COUNT; i++) { + for (int i = 0; i < DEFIO_IO_USED_COUNT; i++) { const char* owner; owner = ownerNames[ioRecs[i].owner]; @@ -3847,6 +3848,21 @@ static void cliResource(char *cmdline) cliPrintf("%c%02d: %s %s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, resource); } } + + cliPrintf("\r\n\r\nCurrently active DMA:\r\n"); + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + const char* owner; + owner = ownerNames[dmaGetOwner(i)]; + + cliPrintf(DMA_OUTPUT_STRING, i / DMA_MOD_VALUE + 1, (i % DMA_MOD_VALUE) + DMA_MOD_OFFSET); + uint8_t resourceIndex = dmaGetResourceIndex(i); + if (resourceIndex > 0) { + cliPrintf(" %s%d\r\n", owner, resourceIndex); + } else { + cliPrintf(" %s\r\n", owner); + } + } + #ifndef CLI_MINIMAL_VERBOSITY cliPrintf("\r\nUse: 'resource' to see how to change resources.\r\n"); #endif diff --git a/src/main/main.c b/src/main/main.c index 6a0cdf2514..6b0d6d6ede 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -233,10 +233,6 @@ void init(void) timerInit(); // timer must be initialized before any channel is allocated -#if !defined(USE_HAL_DRIVER) - dmaInit(); -#endif - #if defined(AVOID_UART1_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); From bf5430a2bd6def572238b029fdee6c62e6f76f8e Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 8 Nov 2016 12:14:27 +0900 Subject: [PATCH 148/188] Remove historical OSD_CALLS_CMS This was a part of the original code, and was left here so there will be an option to call CMS code from OSD task so that there is no need for a separate CMS task. It was eventually unused; there are couples of important logics around cmsOpenMenu so doing this will require the same amount of code. --- src/main/io/osd.c | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ce01260dc9..fc7eea3a03 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -608,13 +608,6 @@ 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)) { if (ARMING_FLAG(ARMED)) From b7342b98e2b3eaa3fcda10323b368a22ffb49e21 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 8 Nov 2016 13:18:28 +0900 Subject: [PATCH 149/188] Cosmetics around masterConfig.osdProfile.item_pos[] --- src/main/io/osd.c | 49 +++++++++++++++++++++++++++-------------------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fc7eea3a03..1b112cc418 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -453,47 +453,54 @@ int32_t osdGetAltitude(int32_t alt) void osdUpdateAlarms(void) { + osd_profile_t *pOsdProfile = &masterConfig.osdProfile; + + // This is overdone? + // uint16_t *itemPos = masterConfig.osdProfile.item_pos; + int32_t alt = osdGetAltitude(BaroAlt) / 100; statRssi = rssi * 100 / 1024; - if (statRssi < masterConfig.osdProfile.rssi_alarm) - masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; + if (statRssi < pOsdProfile->rssi_alarm) + pOsdProfile->item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; else - masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; if (vbat <= (batteryWarningVoltage - 1)) - masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; else - masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; if (STATE(GPS_FIX) == 0) - masterConfig.osdProfile.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; + pOsdProfile->item_pos[OSD_GPS_SATS] |= BLINK_FLAG; else - masterConfig.osdProfile.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; - if (flyTime / 60 >= masterConfig.osdProfile.time_alarm && ARMING_FLAG(ARMED)) - masterConfig.osdProfile.item_pos[OSD_FLYTIME] |= BLINK_FLAG; + if (flyTime / 60 >= pOsdProfile->time_alarm && ARMING_FLAG(ARMED)) + pOsdProfile->item_pos[OSD_FLYTIME] |= BLINK_FLAG; else - masterConfig.osdProfile.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - if (mAhDrawn >= masterConfig.osdProfile.cap_alarm) - masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; + if (mAhDrawn >= pOsdProfile->cap_alarm) + pOsdProfile->item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; else - masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; - if (alt >= masterConfig.osdProfile.alt_alarm) - masterConfig.osdProfile.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; + if (alt >= pOsdProfile->alt_alarm) + pOsdProfile->item_pos[OSD_ALTITUDE] |= BLINK_FLAG; else - masterConfig.osdProfile.item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; } void osdResetAlarms(void) { - 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; + osd_profile_t *pOsdProfile = &masterConfig.osdProfile; + + pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; } void osdResetStats(void) From 9fc0f97359388310bcfa69e0f59d631801311a84 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 8 Nov 2016 15:48:11 +1100 Subject: [PATCH 150/188] Adding missing licence headers --- src/main/drivers/io.c | 19 ++++++++++++++++++- src/main/drivers/io.h | 19 ++++++++++++++++++- src/main/drivers/io_def.h | 17 +++++++++++++++++ src/main/drivers/io_def_generated.h | 19 ++++++++++++++++++- src/main/drivers/io_impl.h | 19 ++++++++++++++++++- src/main/drivers/io_types.h | 19 ++++++++++++++++++- 6 files changed, 107 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 39de281fc7..7ce5f4c1ed 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -1,4 +1,21 @@ -#include "common/utils.h" +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + #include "common/utils.h" #include "io.h" #include "io_impl.h" diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index 23e43b92a1..2326db0288 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -1,4 +1,21 @@ -#pragma once +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + #pragma once #include #include diff --git a/src/main/drivers/io_def.h b/src/main/drivers/io_def.h index 7600054ece..5360dfbc96 100644 --- a/src/main/drivers/io_def.h +++ b/src/main/drivers/io_def.h @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + #pragma once #include "common/utils.h" diff --git a/src/main/drivers/io_def_generated.h b/src/main/drivers/io_def_generated.h index a1f0fc3114..975f6721c3 100644 --- a/src/main/drivers/io_def_generated.h +++ b/src/main/drivers/io_def_generated.h @@ -1,4 +1,21 @@ -#pragma once +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + #pragma once // this file is automatically generated by def_generated.pl script // do not modify this file directly, your changes will be lost diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h index 0db154b852..9f7a8f01c3 100644 --- a/src/main/drivers/io_impl.h +++ b/src/main/drivers/io_impl.h @@ -1,4 +1,21 @@ -#pragma once +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + #pragma once // TODO - GPIO_TypeDef include #include "io.h" diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h index ccd7e0a960..ebf2be7ce8 100644 --- a/src/main/drivers/io_types.h +++ b/src/main/drivers/io_types.h @@ -1,4 +1,21 @@ -#pragma once +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + #pragma once #include From 7861603c96a39090f63fec3b8a4df2bb34e5706a Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 8 Nov 2016 14:20:12 +0900 Subject: [PATCH 151/188] Optional string for OME_Label OME_Label now has an optional string (data). Profile number is now displayed PID menu using this facility. --- src/main/cms/cms.c | 12 ++++++++++-- src/main/cms/cms_menu_imu.c | 15 +++++++++++++-- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index cbf6ae56bd..82ff00fd1a 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -333,8 +333,14 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; - case OME_OSD_Exit: case OME_Label: + if (IS_PRINTVALUE(p) && p->data) { + // A label with optional string, immediately following text + cnt = displayWrite(pDisplay, LEFT_MENU_COLUMN + 2 + strlen(p->text), row, p->data); + CLR_PRINTVALUE(p); + } + break; + case OME_OSD_Exit: case OME_END: case OME_Back: break; @@ -417,7 +423,9 @@ static void cmsDrawMenu(displayPort_t *pDisplay) // Print text labels for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { if (IS_PRINTLABEL(p)) { - room -= displayWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text); + uint8_t coloff = LEFT_MENU_COLUMN; + coloff += (p->type == OME_Label) ? 1 : 2; + room -= displayWrite(pDisplay, coloff, i + top, p->text); CLR_PRINTLABEL(p); if (room < 30) return; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 279a265490..996b70345e 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -52,12 +52,14 @@ // static uint8_t tmpProfileIndex; static uint8_t profileIndex; +static char profileIndexString[] = " PROF n"; static uint8_t tempPid[4][3]; static long cmsx_menuImu_onEnter(void) { profileIndex = masterConfig.current_profile_index; tmpProfileIndex = profileIndex + 1; + profileIndexString[6] = '0' + tmpProfileIndex; return 0; } @@ -86,6 +88,15 @@ static long cmsx_PidRead(void) return 0; } +static long cmsx_PidOnEnter(void) +{ + profileIndexString[6] = '0' + tmpProfileIndex; + cmsx_PidRead(); + + return 0; +} + + static long cmsx_PidWriteback(OSD_Entry *self) { UNUSED(self); @@ -105,7 +116,7 @@ static long cmsx_PidWriteback(OSD_Entry *self) static OSD_Entry cmsx_menuPidEntries[] = { - { "-- PID --", OME_Label, NULL, NULL, 0}, + { "-- PID --", OME_Label, NULL, profileIndexString, 0}, { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][0], 0, 200, 1 }, 0 }, { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][1], 0, 200, 1 }, 0 }, @@ -126,7 +137,7 @@ static OSD_Entry cmsx_menuPidEntries[] = static CMS_Menu cmsx_menuPid = { "XPID", OME_MENU, - cmsx_PidRead, + cmsx_PidOnEnter, cmsx_PidWriteback, NULL, cmsx_menuPidEntries, From 30dfe0c3a5c08b8d7de02fa7bdf84980d9b33fe2 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 8 Nov 2016 05:25:30 +0000 Subject: [PATCH 152/188] Tidy of osd.c --- src/main/fc/config.c | 2 +- src/main/fc/fc_tasks.c | 2 +- src/main/io/osd.c | 218 +++++++++++++++++++++-------------------- src/main/io/osd.h | 21 ++-- 4 files changed, 120 insertions(+), 123 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index a55c774164..ec087fa6f3 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -498,7 +498,7 @@ void createDefaultConfig(master_t *config) #ifdef OSD intFeatureSet(FEATURE_OSD, config); - resetOsdConfig(&config->osdProfile); + osdResetConfig(&config->osdProfile); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index c88ef2eebe..94fc3f8d56 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -266,7 +266,7 @@ static void taskTransponder(uint32_t currentTime) static void taskUpdateOsd(uint32_t currentTime) { if (feature(FEATURE_OSD)) { - updateOsd(currentTime); + osdUpdate(currentTime); } } #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1b112cc418..9c677ccf20 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -65,6 +65,12 @@ #include "build/debug.h" +// Character coordinate and attributes + +#define OSD_POS(x,y) (x | (y << 5)) +#define OSD_X(x) (x & 0x001F) +#define OSD_Y(x) ((x >> 5) & 0x001F) + // Things in both OSD and CMS #define IS_HI(X) (rcData[X] > 1750) @@ -95,67 +101,40 @@ static uint8_t armState; static displayPort_t *osd7456DisplayPort; -// OSD forwards - -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); #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) +/** + * Gets the correct altitude symbol for the current unit system + */ +static char osdGetAltitudeSymbol() { - max7456ClearScreen(); - -#if 0 - if (currentElement) - osdDrawElementPositioningHelp(); -#else - if (false) - ; -#endif -#ifdef CMS - else if (sensors(SENSOR_ACC) || displayIsGrabbed(osd7456DisplayPort)) -#else - else if (sensors(SENSOR_ACC)) -#endif - { - osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); - osdDrawSingleElement(OSD_CROSSHAIRS); + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return 0xF; + default: + return 0xC; } - - osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); - osdDrawSingleElement(OSD_RSSI_VALUE); - osdDrawSingleElement(OSD_FLYTIME); - osdDrawSingleElement(OSD_ONTIME); - osdDrawSingleElement(OSD_FLYMODE); - osdDrawSingleElement(OSD_THROTTLE_POS); - osdDrawSingleElement(OSD_VTX_CHANNEL); - osdDrawSingleElement(OSD_CURRENT_DRAW); - osdDrawSingleElement(OSD_MAH_DRAWN); - osdDrawSingleElement(OSD_CRAFT_NAME); - osdDrawSingleElement(OSD_ALTITUDE); - -#ifdef GPS -#ifdef CMS - if (sensors(SENSOR_GPS) || displayIsGrabbed(osd7456DisplayPort)) -#else - if (sensors(SENSOR_GPS)) -#endif - { - osdDrawSingleElement(OSD_GPS_SATS); - osdDrawSingleElement(OSD_GPS_SPEED); - } -#endif // GPS } -void osdDrawSingleElement(uint8_t item) +/** + * Converts altitude based on the current unit system. + * @param alt Raw altitude (i.e. as taken from BaroAlt) + */ +static 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 + } +} + +static void osdDrawSingleElement(uint8_t item) { if (!VISIBLE(masterConfig.osdProfile.item_pos[item]) || BLINK(masterConfig.osdProfile.item_pos[item])) return; @@ -361,7 +340,53 @@ void osdDrawSingleElement(uint8_t item) max7456Write(elemPosX, elemPosY, buff); } -void resetOsdConfig(osd_profile_t *osdProfile) +void osdDrawElements(void) +{ + max7456ClearScreen(); + +#if 0 + if (currentElement) + osdDrawElementPositioningHelp(); +#else + if (false) + ; +#endif +#ifdef CMS + else if (sensors(SENSOR_ACC) || displayIsGrabbed(osd7456DisplayPort)) +#else + else if (sensors(SENSOR_ACC)) +#endif + { + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + osdDrawSingleElement(OSD_CROSSHAIRS); + } + + osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); + osdDrawSingleElement(OSD_RSSI_VALUE); + osdDrawSingleElement(OSD_FLYTIME); + osdDrawSingleElement(OSD_ONTIME); + osdDrawSingleElement(OSD_FLYMODE); + osdDrawSingleElement(OSD_THROTTLE_POS); + osdDrawSingleElement(OSD_VTX_CHANNEL); + osdDrawSingleElement(OSD_CURRENT_DRAW); + osdDrawSingleElement(OSD_MAH_DRAWN); + osdDrawSingleElement(OSD_CRAFT_NAME); + osdDrawSingleElement(OSD_ALTITUDE); + +#ifdef GPS +#ifdef CMS + if (sensors(SENSOR_GPS) || displayIsGrabbed(osd7456DisplayPort)) +#else + if (sensors(SENSOR_GPS)) +#endif + { + osdDrawSingleElement(OSD_GPS_SATS); + osdDrawSingleElement(OSD_GPS_SPEED); + } +#endif // GPS +} + +void osdResetConfig(osd_profile_t *osdProfile) { osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; @@ -424,33 +449,6 @@ void osdInit(void) #endif } -/** - * 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) { osd_profile_t *pOsdProfile = &masterConfig.osdProfile; @@ -503,7 +501,7 @@ void osdResetAlarms(void) pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; } -void osdResetStats(void) +static void osdResetStats(void) { stats.max_current = 0; stats.max_speed = 0; @@ -513,7 +511,7 @@ void osdResetStats(void) stats.max_altitude = 0; } -void osdUpdateStats(void) +static void osdUpdateStats(void) { int16_t value; @@ -535,7 +533,7 @@ void osdUpdateStats(void) stats.max_altitude = BaroAlt; } -void osdShowStats(void) +static void osdShowStats(void) { uint8_t top = 2; char buff[10]; @@ -579,7 +577,7 @@ void osdShowStats(void) } // called when motors armed -void osdArmMotors(void) +static void osdArmMotors(void) { max7456ClearScreen(); max7456Write(12, 7, "ARMED"); @@ -587,30 +585,7 @@ void osdArmMotors(void) 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(); - -#ifdef CMS - // do not allow ARM if we are in menu - if (displayIsGrabbed(osd7456DisplayPort)) { - DISABLE_ARMING_FLAG(OK_TO_ARM); - } -#endif -} - -void osdUpdate(uint32_t currentTime) +static void osdRefresh(uint32_t currentTime) { static uint8_t lastSec = 0; uint8_t sec; @@ -643,7 +618,7 @@ void osdUpdate(uint32_t currentTime) return; } - blinkState = (millis() / 200) % 2; + blinkState = (currentTime / 200000) % 2; #ifdef CMS if (!displayIsGrabbed(osd7456DisplayPort)) { @@ -657,4 +632,31 @@ void osdUpdate(uint32_t currentTime) #endif } +/* + * Called periodically by the scheduler + */ +void osdUpdate(uint32_t currentTime) +{ + static uint32_t counter = 0; +#ifdef MAX7456_DMA_CHANNEL_TX + // don't touch buffers if DMA transaction is in progress + if (max7456DmaInProgres()) + return; +#endif // MAX7456_DMA_CHANNEL_TX + + // redraw values in buffer + if (counter++ % 5 == 0) + osdRefresh(currentTime); + else // rest of time redraw screen 10 chars per idle to don't lock the main idle + max7456DrawScreen(); + +#ifdef CMS + // do not allow ARM if we are in menu + if (displayIsGrabbed(osd7456DisplayPort)) { + DISABLE_ARMING_FLAG(OK_TO_ARM); + } +#endif +} + + #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b6600953cc..e1c542ca77 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -17,6 +17,12 @@ #pragma once +#define VISIBLE_FLAG 0x0800 +#define BLINK_FLAG 0x0400 +#define VISIBLE(x) (x & VISIBLE_FLAG) +#define BLINK(x) ((x & BLINK_FLAG) && blinkState) +#define BLINK_OFF(x) (x & ~BLINK_FLAG) + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -57,18 +63,7 @@ typedef struct osd_profile_s { osd_unit_e units; } osd_profile_t; -void updateOsd(uint32_t currentTime); void osdInit(void); -void resetOsdConfig(osd_profile_t *osdProfile); +void osdResetConfig(osd_profile_t *osdProfile); void osdResetAlarms(void); - -// 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) +void osdUpdate(uint32_t currentTime); From 1bbc0420ad2b88099a74579efd46cf41a0824662 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 7 Nov 2016 15:37:14 +0000 Subject: [PATCH 153/188] CMS currentTime tidy --- src/main/cms/cms.c | 59 +++++++++++++++++++++++----------------------- 1 file changed, 29 insertions(+), 30 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 82ff00fd1a..c82b549e9d 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -99,8 +99,8 @@ static displayPort_t *cmsDisplayPortSelectNext(void) return cmsDisplayPorts[cmsCurrentDevice]; } -#define CMS_UPDATE_INTERVAL 50 // Interval of key scans (msec) -#define CMS_POLL_INTERVAL 100 // Interval of polling dynamic values (msec) +#define CMS_UPDATE_INTERVAL_US 50000 // Interval of key scans (microsec) +#define CMS_POLL_INTERVAL_US 100000 // Interval of polling dynamic values (microsec) // XXX LEFT_MENU_COLUMN and RIGHT_MENU_COLUMN must be adjusted // dynamically depending on size of the active output device, @@ -357,7 +357,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) return cnt; } -static void cmsDrawMenu(displayPort_t *pDisplay) +static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) { if (!pageTop) return; @@ -369,12 +369,11 @@ static void cmsDrawMenu(displayPort_t *pDisplay) // Polled (dynamic) value display denominator. bool drawPolled = false; - static uint32_t lastPolled = 0; - uint32_t now = millis(); // Argh... + static uint32_t lastPolledUs = 0; - if (now > lastPolled + CMS_POLL_INTERVAL) { + if (currentTimeUs > lastPolledUs + CMS_POLL_INTERVAL_US) { drawPolled = true; - lastPolled = now; + lastPolledUs = currentTimeUs; } uint32_t room = displayTxBytesFree(pDisplay); @@ -771,68 +770,69 @@ static uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) return res; } -static void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) +static void cmsUpdate(uint32_t currentTimeUs) { - static int16_t rcDelay = BUTTON_TIME; - static uint32_t lastCalled = 0; - static uint32_t lastCmsHeartBeat = 0; + static int16_t rcDelayMs = BUTTON_TIME; + static uint32_t lastCalledMs = 0; + static uint32_t lastCmsHeartBeatMs = 0; - uint8_t key = 0; + const uint32_t currentTimeMs = currentTimeUs / 1000; if (!cmsInMenu) { // Detect menu invocation if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { cmsMenuOpen(); - rcDelay = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME + rcDelayMs = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME } } else { - if (rcDelay > 0) { - rcDelay -= (currentTime - lastCalled); + uint8_t key = 0; + if (rcDelayMs > 0) { + rcDelayMs -= (currentTimeMs - lastCalledMs); } else if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { // Double enter = display switching cmsMenuOpen(); - rcDelay = BUTTON_PAUSE; + rcDelayMs = BUTTON_PAUSE; } else if (IS_HI(PITCH)) { key = KEY_UP; - rcDelay = BUTTON_TIME; + rcDelayMs = BUTTON_TIME; } else if (IS_LO(PITCH)) { key = KEY_DOWN; - rcDelay = BUTTON_TIME; + rcDelayMs = BUTTON_TIME; } else if (IS_LO(ROLL)) { key = KEY_LEFT; - rcDelay = BUTTON_TIME; + rcDelayMs = BUTTON_TIME; } else if (IS_HI(ROLL)) { key = KEY_RIGHT; - rcDelay = BUTTON_TIME; + rcDelayMs = BUTTON_TIME; } else if (IS_HI(YAW) || IS_LO(YAW)) { key = KEY_ESC; - rcDelay = BUTTON_TIME; + rcDelayMs = BUTTON_TIME; } //lastCalled = currentTime; if (key) { - rcDelay = cmsHandleKey(pCurrentDisplay, key); + rcDelayMs = cmsHandleKey(pCurrentDisplay, key); return; } - cmsDrawMenu(pDisplay); + cmsDrawMenu(pCurrentDisplay, currentTimeUs); - if (currentTime > lastCmsHeartBeat + 500) { + if (currentTimeMs > lastCmsHeartBeatMs + 500) { // Heart beat for external CMS display device @ 500msec // (Timeout @ 1000msec) displayHeartbeat(pCurrentDisplay); - lastCmsHeartBeat = currentTime; + lastCmsHeartBeatMs = currentTimeMs; } } - lastCalled = currentTime; + lastCalledMs = currentTimeMs; } void cmsHandler(uint32_t currentTime) @@ -841,11 +841,10 @@ void cmsHandler(uint32_t currentTime) return; static uint32_t lastCalled = 0; - const uint32_t now = currentTime / 1000; - if (now - lastCalled >= CMS_UPDATE_INTERVAL) { - cmsUpdate(pCurrentDisplay, now); - lastCalled = now; + if (currentTime >= lastCalled + CMS_UPDATE_INTERVAL_US) { + lastCalled = currentTime; + cmsUpdate(currentTime); } } From 2a2960f584b93891345c7881c743cb4bf7e1d841 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 8 Nov 2016 06:11:32 +0000 Subject: [PATCH 154/188] Tidied up CMS related #defines --- src/main/fc/fc_tasks.c | 3 +-- src/main/io/dashboard.c | 4 ++-- src/main/target/OMNIBUS/target.h | 5 +---- src/main/target/SIRINFPV/target.h | 5 +---- src/main/target/SPRACINGF3/target.h | 5 +---- 5 files changed, 6 insertions(+), 16 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 94fc3f8d56..13b93388ff 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -332,8 +332,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif #ifdef CMS - // XXX Should check FEATURE - setTaskEnabled(TASK_CMS, true); + setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); #endif } diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 8675ec5ef8..932af2e859 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -589,7 +589,7 @@ void dashboardUpdate(uint32_t currentTime) { static uint8_t previousArmedState = 0; -#ifdef OLEDCMS +#ifdef CMS if (displayIsGrabbed(displayPort)) { return; } @@ -707,7 +707,7 @@ void dashboardInit(rxConfig_t *rxConfigToUse) delay(200); displayPort = displayPortOledInit(); -#if defined(CMS) && defined(OLEDCMS) +#if defined(CMS) cmsDisplayPortRegister(displayPort); #endif diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index e8b78ee868..135b9ed3f6 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -97,12 +97,9 @@ #define CMS #define CMS_MAX_DEVICE 4 -// Use external display to run CMS +// Use external display connected by MSP to run CMS #define USE_MSP_DISPLAYPORT -// 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 diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index f32419f0f7..8f311405d7 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -144,12 +144,9 @@ // Configuratoin Menu System #define CMS -// Use external display to run CMS +// Use external display connected by MSP to run CMS #define USE_MSP_DISPLAYPORT -// 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/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 5cf175633e..89921a3cde 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -145,8 +145,5 @@ // Configuratoin Menu System #define CMS -// Use external display to run CMS +// Use external display connected by MSP to run CMS #define USE_MSP_DISPLAYPORT - -// USE I2C OLED display to run CMS -#define OLEDCMS From c9b7ac21c13dbd9bd0b85b6381589f6aea2db3df Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 8 Nov 2016 18:22:00 +0900 Subject: [PATCH 155/188] Added onChange function call to scalar data types MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This will make changed values to take effect immediately in the profile related menus. It’s only used for profile selection now; will be used more through out the menu contents. --- src/main/cms/cms.c | 12 ++++++++++++ src/main/cms/cms_menu_imu.c | 20 +++++++++++++++----- 2 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 40747b6fb2..636c2e48bb 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -698,6 +698,9 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) *ptr->val -= ptr->step; } SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } } break; case OME_TAB: @@ -729,6 +732,9 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) *ptr->val -= ptr->step; } SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } } break; case OME_UINT16: @@ -743,6 +749,9 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) *ptr->val -= ptr->step; } SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } } break; case OME_INT16: @@ -757,6 +766,9 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) *ptr->val -= ptr->step; } SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } } break; case OME_String: diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 996b70345e..19131c1bba 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -264,15 +264,25 @@ CMS_Menu menuImuMisc = { menuImuMiscEntries, }; +static long onProfileChange(displayPort_t *pDisplay, void *ptr) +{ + UNUSED(pDisplay); + UNUSED(ptr); + + masterConfig.current_profile_index = tmpProfileIndex - 1; + + return 0; +} + static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, NULL, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, - {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, - {"RATE&RXPO", OME_Submenu, cmsMenuChange, &cmsx_menuRateExpo, 0}, - {"RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, - {"MISC", OME_Submenu, cmsMenuChange, &menuImuMisc, 0}, + {"PID PROF", OME_UINT8, onProfileChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, + {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, + {"RATE&RXPO", OME_Submenu, cmsMenuChange, &cmsx_menuRateExpo, 0}, + {"RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, + {"MISC", OME_Submenu, cmsMenuChange, &menuImuMisc, 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} From ea3291c40496fd68263ec5fc69dd5dce0245b986 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 8 Nov 2016 22:38:30 +1300 Subject: [PATCH 156/188] Changed scaling of axisPID to be local to mixer only, since the servos need unscaled axisPID. --- src/main/flight/mixer.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index aea7b68adc..e023eed8af 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -452,9 +452,10 @@ void mixTable(pidProfile_t *pidProfile) throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f); throttleRange = motorOutputMax - motorOutputMin; + float scaledAxisPIDf[3]; // Limit the PIDsum for (int axis = 0; axis < 3; axis++) - axisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); // Calculate voltage compensation if (batteryConfig && pidProfile->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); @@ -462,9 +463,9 @@ void mixTable(pidProfile_t *pidProfile) // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { motorMix[i] = - axisPIDf[PITCH] * currentMixer[i].pitch + - axisPIDf[ROLL] * currentMixer[i].roll + - -mixerConfig->yaw_motor_direction * axisPIDf[YAW] * currentMixer[i].yaw; + scaledAxisPIDf[PITCH] * currentMixer[i].pitch + + scaledAxisPIDf[ROLL] * currentMixer[i].roll + + -mixerConfig->yaw_motor_direction * scaledAxisPIDf[YAW] * currentMixer[i].yaw; if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor; // Add voltage compensation From f783ef401562e8e525db3adc2b6b462fcec86ab7 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 9 Nov 2016 00:18:38 +0900 Subject: [PATCH 157/188] Added a TASK_CMS start condition for MSP_DISPLAYPORT only case --- src/main/fc/fc_tasks.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 13b93388ff..2ffbdc91ec 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -332,8 +332,12 @@ void fcTasksInit(void) setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif #ifdef CMS +#ifdef USE_MSP_DISPLAYPORT + setTaskEnabled(TASK_CMS, true); +#else setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); #endif +#endif } cfTask_t cfTasks[TASK_COUNT] = { From b5a29955f14f09d0330755abee1ce54facbd7047 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 9 Nov 2016 00:18:53 +0900 Subject: [PATCH 158/188] REVO target for testing --- src/main/target/REVO/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index dacaff8980..f0f429c83b 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -178,3 +178,6 @@ #endif #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) + +#define CMS +#define USE_MSP_DISPLAYPORT From f68e6a4d2f51b68db846f54ceb3aae63835854a9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 8 Nov 2016 15:46:05 +0000 Subject: [PATCH 159/188] Improved CMS const correctness. Added CMS and USE_MSP_DISPLAYPORT to common.h --- src/main/cms/cms.c | 25 +++++++------- src/main/cms/cms.h | 4 +-- src/main/cms/cms_menu_blackbox.c | 14 ++++---- src/main/cms/cms_menu_builtin.c | 36 ++++++++++---------- src/main/cms/cms_menu_imu.c | 58 ++++++++++++++++---------------- src/main/cms/cms_menu_ledstrip.c | 12 +++---- src/main/cms/cms_menu_osd.c | 37 ++++++++++---------- src/main/cms/cms_menu_vtx.c | 14 ++++---- src/main/cms/cms_types.h | 18 +++++----- src/main/target/common.h | 2 ++ 10 files changed, 110 insertions(+), 110 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 636c2e48bb..b8965ee9e3 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -125,11 +125,11 @@ static displayPort_t *cmsDisplayPortSelectNext(void) static bool cmsInMenu = false; -STATIC_UNIT_TESTED CMS_Menu *currentMenu; // Points to top entry of the current page +STATIC_UNIT_TESTED const CMS_Menu *currentMenu; // Points to top entry of the current page // XXX Does menu backing support backing into second page??? -static CMS_Menu *menuStack[10]; // Stack to save menu transition +static const CMS_Menu *menuStack[10]; // Stack to save menu transition static uint8_t menuStackHistory[10];// cursorRow in a stacked menu static uint8_t menuStackIdx = 0; @@ -179,14 +179,15 @@ static CMS_Menu menuErr = { static void cmsUpdateMaxRow(displayPort_t *instance) { - OSD_Entry *ptr; - maxRow = 0; - for (ptr = pageTop; ptr->type != OME_END; ptr++) - maxRow++; - if (maxRow > MAX_MENU_ITEMS(instance)) + for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) { + maxRow++; + } + + if (maxRow > MAX_MENU_ITEMS(instance)) { maxRow = MAX_MENU_ITEMS(instance); + } maxRow--; } @@ -446,7 +447,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) } } -long cmsMenuChange(displayPort_t *pDisplay, void *ptr) +long cmsMenuChange(displayPort_t *pDisplay, const void *ptr) { CMS_Menu *pMenu = (CMS_Menu *)ptr; @@ -544,13 +545,11 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void) cmsMenuChange(pCurrentDisplay, currentMenu); } -static void cmsTraverseGlobalExit(CMS_Menu *pMenu) +static void cmsTraverseGlobalExit(const CMS_Menu *pMenu) { - OSD_Entry *p; - debug[0]++; - for (p = pMenu->entries; p->type != OME_END ; p++) { + for (const OSD_Entry *p = pMenu->entries; p->type != OME_END ; p++) { if (p->type == OME_Submenu) { cmsTraverseGlobalExit(p->data); } @@ -562,7 +561,7 @@ static void cmsTraverseGlobalExit(CMS_Menu *pMenu) } } -long cmsMenuExit(displayPort_t *pDisplay, void *ptr) +long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) { if (ptr) { displayClear(pDisplay); diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index ff3fa46bf2..8e5110af22 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -9,8 +9,8 @@ bool cmsDisplayPortRegister(displayPort_t *pDisplay); void cmsInit(void); void cmsHandler(uint32_t currentTime); -long cmsMenuChange(displayPort_t *pPort, void *ptr); -long cmsMenuExit(displayPort_t *pPort, void *ptr); +long cmsMenuChange(displayPort_t *pPort, const void *ptr); +long cmsMenuExit(displayPort_t *pPort, const void *ptr); #define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" #define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 661ba9c4cc..0d65249fbc 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -43,7 +43,7 @@ #include "io/flashfs.h" #ifdef USE_FLASHFS -static long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) +static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) { UNUSED(ptr); @@ -101,11 +101,11 @@ static OSD_Entry cmsx_menuBlackboxEntries[] = }; CMS_Menu cmsx_menuBlackbox = { - "MENUBB", - OME_MENU, - cmsx_Blackbox_FeatureRead, - NULL, - cmsx_Blackbox_FeatureWriteback, - cmsx_menuBlackboxEntries, + .GUARD_text = "MENUBB", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Blackbox_FeatureRead, + .onExit = NULL, + .onGlobalExit = cmsx_Blackbox_FeatureWriteback, + .entries = cmsx_menuBlackboxEntries }; #endif diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 72365d4ae2..2d7ee7e787 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -75,12 +75,12 @@ static OSD_Entry menuInfoEntries[] = { }; static CMS_Menu menuInfo = { - "MENUINFO", - OME_MENU, - cmsx_InfoInit, - NULL, - NULL, - menuInfoEntries, + .GUARD_text = "MENUINFO", + .GUARD_type = OME_MENU, + .onEnter = cmsx_InfoInit, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuInfoEntries }; // Features @@ -100,12 +100,12 @@ static OSD_Entry menuFeaturesEntries[] = }; static CMS_Menu menuFeatures = { - "MENUFEATURES", - OME_MENU, - NULL, - NULL, - NULL, - menuFeaturesEntries, + .GUARD_text = "MENUFEATURES", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuFeaturesEntries, }; // Main @@ -129,11 +129,11 @@ static OSD_Entry menuMainEntries[] = }; CMS_Menu menuMain = { - "MENUMAIN", - OME_MENU, - NULL, - NULL, - NULL, - menuMainEntries, + .GUARD_text = "MENUMAIN", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuMainEntries, }; #endif diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 19131c1bba..4985e1f430 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -64,7 +64,7 @@ static long cmsx_menuImu_onEnter(void) return 0; } -static long cmsx_menuImu_onExit(OSD_Entry *self) +static long cmsx_menuImu_onExit(const OSD_Entry *self) { UNUSED(self); @@ -97,7 +97,7 @@ static long cmsx_PidOnEnter(void) } -static long cmsx_PidWriteback(OSD_Entry *self) +static long cmsx_PidWriteback(const OSD_Entry *self) { UNUSED(self); @@ -135,12 +135,12 @@ static OSD_Entry cmsx_menuPidEntries[] = }; static CMS_Menu cmsx_menuPid = { - "XPID", - OME_MENU, - cmsx_PidOnEnter, - cmsx_PidWriteback, - NULL, - cmsx_menuPidEntries, + .GUARD_text = "XPID", + .GUARD_type = OME_MENU, + .onEnter = cmsx_PidOnEnter, + .onExit = cmsx_PidWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuPidEntries }; // @@ -155,7 +155,7 @@ static long cmsx_RateExpoRead(void) return 0; } -static long cmsx_RateExpoWriteback(OSD_Entry *self) +static long cmsx_RateExpoWriteback(const OSD_Entry *self) { UNUSED(self); @@ -164,7 +164,7 @@ static long cmsx_RateExpoWriteback(OSD_Entry *self) return 0; } -static long cmsx_menuRcConfirmBack(OSD_Entry *self) +static long cmsx_menuRcConfirmBack(const OSD_Entry *self) { if (self && self->type == OME_Back) return 0; @@ -196,12 +196,12 @@ static OSD_Entry cmsx_menuRateExpoEntries[] = }; CMS_Menu cmsx_menuRateExpo = { - "MENURATE", - OME_MENU, - cmsx_RateExpoRead, - cmsx_RateExpoWriteback, - NULL, - cmsx_menuRateExpoEntries, + .GUARD_text = "MENURATE", + .GUARD_type = OME_MENU, + .onEnter = cmsx_RateExpoRead, + .onExit = cmsx_RateExpoWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuRateExpoEntries }; @@ -227,12 +227,12 @@ static OSD_Entry cmsx_menuRcEntries[] = }; CMS_Menu cmsx_menuRcPreview = { - "XRCPREV", - OME_MENU, - NULL, - cmsx_menuRcConfirmBack, - NULL, - cmsx_menuRcEntries, + .GUARD_text = "XRCPREV", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = cmsx_menuRcConfirmBack, + .onGlobalExit = NULL, + .entries = cmsx_menuRcEntries }; @@ -256,15 +256,15 @@ static OSD_Entry menuImuMiscEntries[]= }; CMS_Menu menuImuMisc = { - "XIMUMISC", - OME_MENU, - NULL, - NULL, - NULL, - menuImuMiscEntries, + .GUARD_text = "XIMUMISC", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuImuMiscEntries }; -static long onProfileChange(displayPort_t *pDisplay, void *ptr) +static long onProfileChange(displayPort_t *pDisplay, const void *ptr) { UNUSED(pDisplay); UNUSED(ptr); diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index 00ff866bf2..bd7efe2cd2 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -71,12 +71,12 @@ static OSD_Entry cmsx_menuLedstripEntries[] = }; CMS_Menu cmsx_menuLedstrip = { - "MENULED", - OME_MENU, - cmsx_Ledstrip_FeatureRead, - NULL, - cmsx_Ledstrip_FeatureWriteback, - cmsx_menuLedstripEntries, + .GUARD_text = "MENULED", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Ledstrip_FeatureRead, + .onExit = NULL, + .onGlobalExit = cmsx_Ledstrip_FeatureWriteback, + .entries = cmsx_menuLedstripEntries }; #endif // LED_STRIP #endif // CMS diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index f64c3a777f..1ef32700df 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -50,12 +50,12 @@ OSD_Entry cmsx_menuAlarmsEntries[] = }; CMS_Menu cmsx_menuAlarms = { - "MENUALARMS", - OME_MENU, - NULL, - NULL, - NULL, - cmsx_menuAlarmsEntries, + .GUARD_text = "MENUALARMS", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuAlarmsEntries, }; OSD_Entry menuOsdActiveElemsEntries[] = @@ -85,12 +85,12 @@ OSD_Entry menuOsdActiveElemsEntries[] = }; CMS_Menu menuOsdActiveElems = { - "MENUOSDACT", - OME_MENU, - NULL, - NULL, - NULL, - menuOsdActiveElemsEntries, + .GUARD_text = "MENUOSDACT", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuOsdActiveElemsEntries }; OSD_Entry cmsx_menuOsdLayoutEntries[] = @@ -102,12 +102,11 @@ OSD_Entry cmsx_menuOsdLayoutEntries[] = }; CMS_Menu cmsx_menuOsdLayout = { - "MENULAYOUT", - OME_MENU, - NULL, - NULL, - NULL, - cmsx_menuOsdLayoutEntries, + .GUARD_text = "MENULAYOUT", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuOsdLayoutEntries }; - #endif // CMS diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 32d921d62f..48f8bd6f1d 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -102,7 +102,7 @@ static long cmsx_Vtx_onEnter(void) return 0; } -static long cmsx_Vtx_onExit(OSD_Entry *self) +static long cmsx_Vtx_onExit(const OSD_Entry *self) { UNUSED(self); @@ -134,12 +134,12 @@ static OSD_Entry cmsx_menuVtxEntries[] = }; CMS_Menu cmsx_menuVtx = { - "MENUVTX", - OME_MENU, - cmsx_Vtx_onEnter, - cmsx_Vtx_onExit, - cmsx_Vtx_FeatureWriteback, - cmsx_menuVtxEntries, + .GUARD_text = "MENUVTX", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Vtx_onEnter, + .onExit= cmsx_Vtx_onExit, + .onGlobalExit = cmsx_Vtx_FeatureWriteback, + .entries = cmsx_menuVtxEntries }; #endif // VTX || USE_RTC6705 diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index fd60cb5480..427e1d38e6 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -22,8 +22,6 @@ #pragma once -typedef long (*CMSEntryFuncPtr)(displayPort_t *, void *); - //type of elements typedef enum @@ -53,11 +51,13 @@ typedef enum OME_MAX = OME_MENU } OSD_MenuElement; +typedef long (*CMSEntryFuncPtr)(displayPort_t *displayPort, const void *ptr); + typedef struct { const char *text; - OSD_MenuElement type; - CMSEntryFuncPtr func; + const OSD_MenuElement type; + const CMSEntryFuncPtr func; void *data; uint8_t flags; } OSD_Entry; @@ -87,17 +87,17 @@ onExit function is called with self: (2) NULL if called from menu exit (forced exit at top level). */ -typedef long (*CMSMenuOnExitPtr)(OSD_Entry *self); +typedef long (*CMSMenuOnExitPtr)(const OSD_Entry *self); typedef struct { // These two are debug aids for menu content creators. const char *GUARD_text; - OSD_MenuElement GUARD_type; + const OSD_MenuElement GUARD_type; - CMSMenuFuncPtr onEnter; - CMSMenuOnExitPtr onExit; - CMSMenuFuncPtr onGlobalExit; + const CMSMenuFuncPtr onEnter; + const CMSMenuOnExitPtr onExit; + const CMSMenuFuncPtr onGlobalExit; OSD_Entry *entries; } CMS_Menu; diff --git a/src/main/target/common.h b/src/main/target/common.h index 42dfed36de..c6b1ecefa3 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -55,7 +55,9 @@ #endif #if (FLASH_SIZE > 128) +#define CMS #define USE_DASHBOARD +#define USE_MSP_DISPLAYPORT #define TELEMETRY_MAVLINK #else #define SKIP_CLI_COMMAND_HELP From 657eee6617e5c808b1ca3f835cb842b74a629eb9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B6glund?= Date: Tue, 8 Nov 2016 19:41:20 +0100 Subject: [PATCH 160/188] Travis optimizations (#1496) * add travis directorie cache for arm sdk * Travis. Test if make all in one job would work. And how slow/fast. * Travis. Check that the old method with a couple specific targets still works. * Rebased master and build all targets again. * Make verbosity level 0 even less verbose, was still too much for travis in one mega-jumbo-all-target build. * Even less output from make in verbosity level 0. Some Makefile symbol name cleanup too. * arm_sdk_install did not check if allready downloaded and installed. Added checks and deps. Travis caching might work better now... * Build all 56 targets to test Travis caching perf. * Build all 56 targets to test Travis caching perf. Again. Now with correct yml arrsy syntax. * Added installation of ccache and also to Travis cache. Caching the cache. * Travis. Remove some unused APT libs, to see if this affects caching and build times. * Travis. YAPT (Yet another perf trial). Remove all apt package install. * Disaster. Put back installation if build-essential. * Disaster still. Put back installation if zliblg-dev. * Disaster, again. Put back installation of libc6 . * Travis. Minimizing APT installation as it not be cached, issue 5876 on travis-ci github. * Travis. Added comment on target specification. --- .travis.sh | 5 ++++- .travis.yml | 61 ++++++++++++++++++++++++++++----------------------- Makefile | 14 ++++++++---- make/tools.mk | 33 ++++++++++++++++++---------- 4 files changed, 70 insertions(+), 43 deletions(-) diff --git a/.travis.sh b/.travis.sh index ee310af88b..ad4274d112 100755 --- a/.travis.sh +++ b/.travis.sh @@ -50,7 +50,7 @@ elif [ $PUBLISHMETA ] ; then curl -k "${CURL_BASEOPTS[@]}" "${CURL_PUB_BASEOPTS[@]}" --form "recent_commits=${RECENT_COMMITS}" ${PUBLISH_URL} || true fi -else +elif [ $TARGET ] ; then if [ $PUBLISH_URL ] ; then make -j2 $MAKEFILE if [ -f ${TARGET_FILE}.bin ] ; then @@ -67,4 +67,7 @@ else else make -j2 $MAKEFILE fi +else +# No target specified, build all with very low verbosity. + make V=0 all fi diff --git a/.travis.yml b/.travis.yml index 8ba47b575c..14b7fbbf43 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,20 +1,23 @@ + env: +# Specify target(s) to build, or none to build all. # - RUNTESTS=True # - PUBLISHMETA=True # - PUBLISHDOCS=True # - TARGET=AFROMINI -# - TARGET=BEEBRAIN # - TARGET=AIORACERF3 # - TARGET=AIR32 +# - TARGET=AIRBOTF4 # - TARGET=AIRHEROF3 # - TARGET=ALIENFLIGHTF1 - - TARGET=ALIENFLIGHTF3 - - TARGET=ALIENFLIGHTF4 - - TARGET=ANYFCF7 - - TARGET=BETAFLIGHTF3 - - TARGET=BLUEJAYF4 - - TARGET=CC3D - - TARGET=CC3D_OPBL +# - TARGET=ALIENFLIGHTF3 +# - TARGET=ALIENFLIGHTF4 +# - TARGET=ANYFCF7 +# - TARGET=BEEBRAIN +# - TARGET=BETAFLIGHTF3 +# - TARGET=BLUEJAYF4 +# - TARGET=CC3D +# - TARGET=CC3D_OPBL # - TARGET=CHEBUZZF3 # - TARGET=CJMCU # - TARGET=COLIBRI @@ -23,50 +26,51 @@ env: # - TARGET=DOGE # - TARGET=F4BY # - TARGET=FURYF3 - - TARGET=FURYF4 +# - TARGET=FURYF4 +# - TARGET=FURYF7 +# - TARGET=IMPULSERCF3 # - TARGET=IRCFUSIONF3 # - TARGET=ISHAPEDF3 # - TARGET=KISSFC +# - TARGET=LUXV2_RACE # - TARGET=LUX_RACE # - TARGET=MICROSCISKY # - TARGET=MOTOLAB - - TARGET=NAZE +# - TARGET=NAZE # - TARGET=OMNIBUS # - TARGET=OMNIBUSF4 # - TARGET=PIKOBLX # - TARGET=RACEBASE - - TARGET=REVO +# - TARGET=RCEXPLORERF3 +# - TARGET=REVO +# - TARGET=REVOLT # - TARGET=REVONANO # - TARGET=REVO_OPBL # - TARGET=RMDO # - TARGET=SINGULARITY - - TARGET=SIRINFPV - - TARGET=SPARKY +# - TARGET=SIRINFPV +# - TARGET=SOULF4 +# - TARGET=SPARKY # - TARGET=SPARKY2 -# - TARGET=SPARKY_OPBL - - TARGET=SPRACINGF3 - - TARGET=SPRACINGF3EVO +# - TARGET=SPRACINGF3 +# - TARGET=SPRACINGF3EVO # - TARGET=SPRACINGF3MINI - - TARGET=STM32F3DISCOVERY +# - TARGET=STM32F3DISCOVERY # - TARGET=VRRACE # - TARGET=X_RACERSPI +# - TARGET=YUPIF4 # - TARGET=ZCOREF3 -# - TARGET=RCEXPLORERF3 # use new docker environment sudo: false +git: + depth: 5 + addons: apt: packages: - - build-essential - - git - libc6-i386 - - zlib1g-dev - - libssl-dev - - wkhtmltopdf - - libxml2-dev - - libxslt-dev # We use cpp for unit tests, and c for the main project. language: cpp @@ -78,8 +82,11 @@ install: before_script: tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version script: ./.travis.sh -cache: apt - +cache: + directories: + - downloads + - tools + #notifications: # irc: "chat.freenode.net#cleanflight" # use_notice: true diff --git a/Makefile b/Makefile index fa5bed9106..f20832a712 100644 --- a/Makefile +++ b/Makefile @@ -45,10 +45,16 @@ export AT := @ ifndef V export V0 := export V1 := $(AT) +export STDOUT := else ifeq ($(V), 0) export V0 := $(AT) export V1 := $(AT) +export STDOUT:= "> /dev/null" +export MAKE := $(MAKE) --no-print-directory else ifeq ($(V), 1) +export V0 := +export V1 := +export STDOUT := endif ############################################################################### @@ -813,25 +819,25 @@ $(TARGET_BIN): $(TARGET_ELF) $(V0) $(OBJCOPY) -O binary $< $@ $(TARGET_ELF): $(TARGET_OBJS) - $(V1) echo LD $(notdir $@) + $(V1) echo Linking $(TARGET) $(V1) $(CC) -o $@ $^ $(LDFLAGS) $(V0) $(SIZE) $(TARGET_ELF) # Compile $(OBJECT_DIR)/$(TARGET)/%.o: %.c $(V1) mkdir -p $(dir $@) - $(V1) echo %% $(notdir $<) + $(V1) echo "%% $(notdir $<)" "$(STDOUT)" $(V1) $(CC) -c -o $@ $(CFLAGS) $< # Assemble $(OBJECT_DIR)/$(TARGET)/%.o: %.s $(V1) mkdir -p $(dir $@) - $(V1) echo %% $(notdir $<) + $(V1) echo "%% $(notdir $<)" "$(STDOUT)" $(V1) $(CC) -c -o $@ $(ASFLAGS) $< $(OBJECT_DIR)/$(TARGET)/%.o: %.S $(V1) mkdir -p $(dir $@) - $(V1) echo %% $(notdir $<) + $(V1) echo "%% $(notdir $<)" "$(STDOUT)" $(V1) $(CC) -c -o $@ $(ASFLAGS) $< ## sample : Build all sample (travis) targets diff --git a/make/tools.mk b/make/tools.mk index 8d0b24d145..1e4323c111 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -25,36 +25,47 @@ ARM_SDK_URL_BASE := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update # source: https://launchpad.net/gcc-arm-embedded/5.0/ ifdef LINUX - arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2 + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2 endif ifdef MACOSX - arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-mac.tar.bz2 + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-mac.tar.bz2 endif ifdef WINDOWS - arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip endif -arm_sdk_install: ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) -# order-only prereq on directory existance: -arm_sdk_install: | $(DL_DIR) $(TOOLS_DIR) -arm_sdk_install: arm_sdk_clean -ifneq ($(OSFAMILY), windows) - # download the source only if it's newer than what we already have - $(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)" +ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) +SDK_INSTALL_MARKER := $(ARM_SDK_DIR)/bin/arm-none-eabi-gcc-$(GCC_REQUIRED_VERSION) + +# order-only prereq on directory existance: +arm_sdk_install: | $(TOOLS_DIR) + +arm_sdk_install: arm_sdk_download $(SDK_INSTALL_MARKER) + +$(SDK_INSTALL_MARKER): +ifneq ($(OSFAMILY), windows) # binary only release so just extract it $(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)" else - $(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)" $(V1) unzip -q -d $(ARM_SDK_DIR) "$(DL_DIR)/$(ARM_SDK_FILE)" endif +.PHONY: arm_sdk_download +arm_sdk_download: | $(DL_DIR) +arm_sdk_download: $(DL_DIR)/$(ARM_SDK_FILE) +$(DL_DIR)/$(ARM_SDK_FILE): + # download the source only if it's newer than what we already have + $(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" -z "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)" + + ## arm_sdk_clean : Uninstall Arm SDK .PHONY: arm_sdk_clean arm_sdk_clean: $(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR) + $(V1) [ ! -d "$(DL_DIR)" ] || $(RM) -r $(DL_DIR) .PHONY: openocd_win_install From 8530cd4ba15f4c580b73b6527be47ed5e9b89597 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 9 Nov 2016 05:42:22 +0900 Subject: [PATCH 161/188] Menu restructuring - Rate profiles are handled correctly - Added FILTER GLB (Global) and FILTER PP (Per Profile) sub menus - Moved MISC menu under top level. --- Makefile | 1 + src/main/cms/cms_menu_builtin.c | 22 ++-- src/main/cms/cms_menu_imu.c | 209 +++++++++++++++++++------------- src/main/cms/cms_menu_misc.c | 104 ++++++++++++++++ src/main/cms/cms_menu_misc.h | 20 +++ 5 files changed, 261 insertions(+), 95 deletions(-) create mode 100644 src/main/cms/cms_menu_misc.c create mode 100644 src/main/cms/cms_menu_misc.h diff --git a/Makefile b/Makefile index 62a455084c..63989117b4 100644 --- a/Makefile +++ b/Makefile @@ -556,6 +556,7 @@ HIGHEND_SRC = \ cms/cms_menu_builtin.c \ cms/cms_menu_imu.c \ cms/cms_menu_ledstrip.c \ + cms/cms_menu_misc.c \ cms/cms_menu_osd.c \ cms/cms_menu_vtx.c \ common/colorconversion.c \ diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 2d7ee7e787..50a85a4c08 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -43,6 +43,7 @@ #include "cms/cms_menu_vtx.h" #include "cms/cms_menu_osd.h" #include "cms/cms_menu_ledstrip.h" +#include "cms/cms_menu_misc.h" // Info @@ -112,19 +113,22 @@ static CMS_Menu menuFeatures = { static OSD_Entry menuMainEntries[] = { - {"-- MAIN MENU --", OME_Label, NULL, NULL, 0}, - {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0}, - {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, + {"-- MAIN --", OME_Label, NULL, NULL, 0}, + + {"PROFILE", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0}, + {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, #ifdef OSD - {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0}, - {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, + {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, #endif - {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, - {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, - {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, + {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, + {"MISC", OME_Submenu, cmsMenuChange, &cmsx_menuMisc, 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, #ifdef CMS_MENU_DEBUG - {"ERR SAMPLE", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0}, + {"ERR SAMPLE", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0}, #endif + {NULL,OME_END, NULL, NULL, 0} }; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 4985e1f430..7d846f6600 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -52,14 +52,21 @@ // static uint8_t tmpProfileIndex; static uint8_t profileIndex; -static char profileIndexString[] = " PROF n"; +static char profileIndexString[] = " p"; static uint8_t tempPid[4][3]; +static uint8_t tmpRateProfileIndex; +static uint8_t rateProfileIndex; +static char rateProfileIndexString[] = " p-r"; +static controlRateConfig_t rateProfile; + static long cmsx_menuImu_onEnter(void) { profileIndex = masterConfig.current_profile_index; tmpProfileIndex = profileIndex + 1; - profileIndexString[6] = '0' + tmpProfileIndex; + + rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile; + tmpRateProfileIndex = rateProfileIndex + 1; return 0; } @@ -68,7 +75,28 @@ static long cmsx_menuImu_onExit(const OSD_Entry *self) { UNUSED(self); - masterConfig.current_profile_index = tmpProfileIndex - 1; + masterConfig.current_profile_index = profileIndex; + masterConfig.profile[profileIndex].activeRateProfile = rateProfileIndex; + + return 0; +} + +static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + profileIndex = tmpProfileIndex - 1; + + return 0; +} + +static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + rateProfileIndex = tmpRateProfileIndex - 1; return 0; } @@ -90,13 +118,12 @@ static long cmsx_PidRead(void) static long cmsx_PidOnEnter(void) { - profileIndexString[6] = '0' + tmpProfileIndex; + profileIndexString[1] = '0' + tmpProfileIndex; cmsx_PidRead(); return 0; } - static long cmsx_PidWriteback(const OSD_Entry *self) { UNUSED(self); @@ -146,35 +173,35 @@ static CMS_Menu cmsx_menuPid = { // // Rate & Expo // -static controlRateConfig_t rateProfile; -static long cmsx_RateExpoRead(void) +static long cmsx_RateProfileRead(void) { - memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); + memcpy(&rateProfile, &masterConfig.profile[profileIndex].controlRateProfile[rateProfileIndex], sizeof(controlRateConfig_t)); return 0; } -static long cmsx_RateExpoWriteback(const OSD_Entry *self) +static long cmsx_RateProfileWriteback(const OSD_Entry *self) { UNUSED(self); - memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); + memcpy(&masterConfig.profile[profileIndex].controlRateProfile[rateProfileIndex], &rateProfile, sizeof(controlRateConfig_t)); return 0; } -static long cmsx_menuRcConfirmBack(const OSD_Entry *self) +static long cmsx_RateProfileOnEnter(void) { - if (self && self->type == OME_Back) - return 0; - else - return -1; + rateProfileIndexString[1] = '0' + tmpProfileIndex; + rateProfileIndexString[3] = '0' + tmpRateProfileIndex; + cmsx_RateProfileRead(); + + return 0; } -static OSD_Entry cmsx_menuRateExpoEntries[] = +static OSD_Entry cmsx_menuRateProfileEntries[] = { - { "-- RATE&EXPO --", OME_Label, NULL, NULL, 0 }, + { "-- RATE --", OME_Label, NULL, rateProfileIndexString, 0 }, { "RC RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRate8, 0, 255, 1, 10 }, 0 }, { "RC YAW RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawRate8, 0, 255, 1, 10 }, 0 }, @@ -195,105 +222,115 @@ static OSD_Entry cmsx_menuRateExpoEntries[] = { NULL, OME_END, NULL, NULL, 0 } }; -CMS_Menu cmsx_menuRateExpo = { +CMS_Menu cmsx_menuRateProfile = { .GUARD_text = "MENURATE", .GUARD_type = OME_MENU, - .onEnter = cmsx_RateExpoRead, - .onExit = cmsx_RateExpoWriteback, + .onEnter = cmsx_RateProfileOnEnter, + .onExit = cmsx_RateProfileWriteback, .onGlobalExit = NULL, - .entries = cmsx_menuRateExpoEntries + .entries = cmsx_menuRateProfileEntries }; - -// -// RC preview -// -static OSD_Entry cmsx_menuRcEntries[] = +static OSD_Entry cmsx_menuFilterGlobalEntries[] = { - { "-- RC PREV --", OME_Label, NULL, NULL, 0}, + { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, - { "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[ROLL], 1, 2500, 0 }, DYNAMIC }, - { "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[PITCH], 1, 2500, 0 }, DYNAMIC }, - { "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[THROTTLE], 1, 2500, 0 }, DYNAMIC }, - { "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[YAW], 1, 2500, 0 }, DYNAMIC }, + { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, + { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, + { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, + { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, + { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, - { "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX1], 1, 2500, 0 }, DYNAMIC }, - { "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX2], 1, 2500, 0 }, DYNAMIC }, - { "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX3], 1, 2500, 0 }, DYNAMIC }, - { "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX4], 1, 2500, 0 }, DYNAMIC }, - - { "BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; -CMS_Menu cmsx_menuRcPreview = { - .GUARD_text = "XRCPREV", - .GUARD_type = OME_MENU, - .onEnter = NULL, - .onExit = cmsx_menuRcConfirmBack, - .onGlobalExit = NULL, - .entries = cmsx_menuRcEntries -}; - - -// -// Misc -// -static OSD_Entry menuImuMiscEntries[]= -{ - { "-- MISC --", OME_Label, NULL, NULL, 0 }, - - { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, - { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 1 }, 0 }, - { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 1 }, 0 }, - { "YAW P LIM", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 1 }, 0 }, - { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1000, 2000, 1 }, 0 }, - { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 }, - { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 }, - - { "BACK", OME_Back, NULL, NULL, 0}, - { NULL, OME_END, NULL, NULL, 0} -}; - -CMS_Menu menuImuMisc = { - .GUARD_text = "XIMUMISC", +static CMS_Menu cmsx_menuFilterGlobal = { + .GUARD_text = "XFLTGLB", .GUARD_type = OME_MENU, .onEnter = NULL, .onExit = NULL, .onGlobalExit = NULL, - .entries = menuImuMiscEntries + .entries = cmsx_menuFilterGlobalEntries, }; -static long onProfileChange(displayPort_t *pDisplay, const void *ptr) -{ - UNUSED(pDisplay); - UNUSED(ptr); +static uint16_t cmsx_dterm_lpf_hz; +static uint16_t cmsx_dterm_notch_hz; +static uint16_t cmsx_dterm_notch_cutoff; +static uint16_t cmsx_yaw_lpf_hz; +static uint16_t cmsx_yaw_p_limit; - masterConfig.current_profile_index = tmpProfileIndex - 1; +static long cmsx_FilterPerProfileRead(void) +{ + cmsx_dterm_lpf_hz = masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz; + cmsx_dterm_notch_hz = masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz; + cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff; + cmsx_yaw_lpf_hz = masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz; + cmsx_yaw_p_limit = masterConfig.profile[profileIndex].pidProfile.yaw_p_limit; return 0; } +static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz; + masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz = cmsx_dterm_notch_hz; + masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff; + masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz; + masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit; + + return 0; +} + +static OSD_Entry cmsx_menuFilterPerProfileEntries[] = +{ + { "-- FILTER PP --", OME_Label, NULL, NULL, 0 }, + + { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf_hz, 0, 500, 1 }, 0 }, + { "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 }, + { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 }, + { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lpf_hz, 0, 500, 1 }, 0 }, + { "YAW P LIM", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_p_limit, 100, 500, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuFilterPerProfile = { + .GUARD_text = "XFLTPP", + .GUARD_type = OME_MENU, + .onEnter = cmsx_FilterPerProfileRead, + .onExit = cmsx_FilterPerProfileWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuFilterPerProfileEntries, +}; + static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, onProfileChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, - {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, - {"RATE&RXPO", OME_Submenu, cmsMenuChange, &cmsx_menuRateExpo, 0}, - {"RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, - {"MISC", OME_Submenu, cmsMenuChange, &menuImuMisc, 0}, + {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, + {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, + + {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, MAX_RATEPROFILES, 1}, 0}, + {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, + + {"FLT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, + + {"FLT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; CMS_Menu cmsx_menuImu = { - "XIMU", - OME_MENU, - cmsx_menuImu_onEnter, - cmsx_menuImu_onExit, - NULL, - cmsx_menuImuEntries, + .GUARD_text = "XIMU", + .GUARD_type = OME_MENU, + .onEnter = cmsx_menuImu_onEnter, + .onExit = cmsx_menuImu_onExit, + .onGlobalExit = NULL, + .entries = cmsx_menuImuEntries, }; #endif // CMS diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c new file mode 100644 index 0000000000..684fbf8b62 --- /dev/null +++ b/src/main/cms/cms_menu_misc.c @@ -0,0 +1,104 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_ledstrip.h" + +// +// Misc +// + +static long cmsx_menuRcConfirmBack(const OSD_Entry *self) +{ + if (self && self->type == OME_Back) + return 0; + else + return -1; +} + +// +// RC preview +// +static OSD_Entry cmsx_menuRcEntries[] = +{ + { "-- RC PREV --", OME_Label, NULL, NULL, 0}, + + { "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[ROLL], 1, 2500, 0 }, DYNAMIC }, + { "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[PITCH], 1, 2500, 0 }, DYNAMIC }, + { "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[THROTTLE], 1, 2500, 0 }, DYNAMIC }, + { "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[YAW], 1, 2500, 0 }, DYNAMIC }, + + { "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX1], 1, 2500, 0 }, DYNAMIC }, + { "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX2], 1, 2500, 0 }, DYNAMIC }, + { "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX3], 1, 2500, 0 }, DYNAMIC }, + { "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX4], 1, 2500, 0 }, DYNAMIC }, + + { "BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuRcPreview = { + .GUARD_text = "XRCPREV", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = cmsx_menuRcConfirmBack, + .onGlobalExit = NULL, + .entries = cmsx_menuRcEntries +}; + + +static OSD_Entry menuMiscEntries[]= +{ + { "-- MISC --", OME_Label, NULL, NULL, 0 }, + + { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1000, 2000, 1 }, 0 }, + { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 }, + { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 }, + { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, + + { "BACK", OME_Back, NULL, NULL, 0}, + { NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuMisc = { + .GUARD_text = "XMISC", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuMiscEntries +}; + +#endif // CMS diff --git a/src/main/cms/cms_menu_misc.h b/src/main/cms/cms_menu_misc.h new file mode 100644 index 0000000000..8b0ed9fc67 --- /dev/null +++ b/src/main/cms/cms_menu_misc.h @@ -0,0 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +extern CMS_Menu cmsx_menuMisc; From 8c9ed2d43786943e3bff4063906aba41e934bdaa Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 09:58:21 +1100 Subject: [PATCH 162/188] Small tidy of LED code --- src/main/drivers/light_ws2811strip.c | 5 +++ src/main/drivers/light_ws2811strip.h | 33 ++++++++++++------- src/main/drivers/light_ws2811strip_hal.c | 6 ++-- .../drivers/light_ws2811strip_stm32f10x.c | 6 ---- .../drivers/light_ws2811strip_stm32f30x.c | 6 ---- .../drivers/light_ws2811strip_stm32f4xx.c | 6 ---- 6 files changed, 29 insertions(+), 33 deletions(-) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 149c9b6129..15607cd346 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -89,6 +89,11 @@ void ws2811LedStripInit(ioTag_t ioTag) { memset(&ledStripDMABuffer, 0, WS2811_DMA_BUFFER_SIZE); ws2811LedStripHardwareInit(ioTag); + + const hsvColor_t hsv_white = { 0, 255, 255}; + setStripColor(&hsv_white); + ws2811UpdateStrip(); + ws2811UpdateStrip(); } diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 894ef4b1d6..2a1f708e7f 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -19,23 +19,34 @@ #include "io_types.h" -#define WS2811_LED_STRIP_LENGTH 32 -#define WS2811_BITS_PER_LED 24 -#define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay +#define WS2811_LED_STRIP_LENGTH 32 +#define WS2811_BITS_PER_LED 24 +// for 50us delay +#define WS2811_DELAY_BUFFER_LENGTH 42 #define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH) - -#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) +// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) +#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) #if defined(STM32F40_41xxx) -#define BIT_COMPARE_1 67 // timer compare value for logical 1 -#define BIT_COMPARE_0 33 // timer compare value for logical 0 +#define WS2811_TIMER_HZ 84000000 +#define WS2811_TIMER_PERIOD 104 +// timer compare value for logical 1 +#define BIT_COMPARE_1 67 +// timer compare value for logical 0 +#define BIT_COMPARE_0 33 #elif defined(STM32F7) -#define BIT_COMPARE_1 76 // timer compare value for logical 1 -#define BIT_COMPARE_0 38 // timer compare value for logical 0 +// timer compare value for logical 1 +#define BIT_COMPARE_1 76 +// timer compare value for logical 0 +#define BIT_COMPARE_0 38 #else -#define BIT_COMPARE_1 17 // timer compare value for logical 1 -#define BIT_COMPARE_0 9 // timer compare value for logical 0 +#define WS2811_TIMER_HZ 24000000 +#define WS2811_TIMER_PERIOD 29 +// timer compare value for logical 1 +#define BIT_COMPARE_1 17 +// timer compare value for logical 0 +#define BIT_COMPARE_0 9 #endif void ws2811LedStripInit(ioTag_t ioTag); diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index 5d8e4cd0c2..260b9b374d 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -131,9 +131,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) return; } - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; - setStripColor(&hsv_white); } @@ -145,9 +143,9 @@ void ws2811LedStripDMAEnable(void) return; } - if( HAL_TIM_PWM_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) + if (HAL_TIM_PWM_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { - /* Starting PWM generation Error */ + /* Starting PWM generation Error */ ws2811LedDataTransferInProgress = 0; return; } diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 1933e0a1c8..e0bb9656fc 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -30,9 +30,6 @@ #include "rcc.h" #include "timer.h" -#define WS2811_TIMER_HZ 24000000 -#define WS2811_TIMER_PERIOD 29 - static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; static DMA_Channel_TypeDef *dmaChannel = NULL; @@ -116,10 +113,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; - setStripColor(&hsv_white); - ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index ffb6269d85..0165241f53 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -31,9 +31,6 @@ #include "rcc.h" #include "timer.h" -#define WS2811_TIMER_HZ 24000000 -#define WS2811_TIMER_PERIOD 29 - static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; static DMA_Channel_TypeDef *dmaChannel = NULL; @@ -126,10 +123,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; - setStripColor(&hsv_white); - ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 4fd38325ee..e2b7d1dbfa 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -33,9 +33,6 @@ #include "timer_stm32f4xx.h" #include "io.h" -#define WS2811_TIMER_HZ 84000000 -#define WS2811_TIMER_PERIOD 104 - static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; static DMA_Stream_TypeDef *stream = NULL; @@ -140,10 +137,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - const hsvColor_t hsv_white = { 0, 255, 255 }; ws2811Initialised = true; - setStripColor(&hsv_white); - ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) From 7e548374bfad5d154fcb040a9b98758fd4660251 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 9 Nov 2016 01:54:48 +0100 Subject: [PATCH 163/188] correct AF --- src/main/target/BETAFLIGHTF3/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index af3f4daa69..084c65576c 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -29,7 +29,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 From dabb7d64368f917990c44f0f40117e20a9cb8d8f Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Tue, 8 Nov 2016 19:21:58 -0600 Subject: [PATCH 164/188] Fix FURYF3 Timers-Dshot --- src/main/target/FURYF3/target.c | 19 +++++++++++-------- src/main/target/FURYF3/target.h | 6 +++--- src/main/target/SPRACINGF3/target.c | 2 +- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 33bcf44f4a..a897bf38a0 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -25,12 +25,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_6, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM4 - S1 - { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 - { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM_USE_MOTOR, (1 | TIMER_OUTPUT_N_CHANNEL), GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO TIMER - LED_STRIP + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel4, DMA1_CH4_HANDLER }, // PWM4 - S1 + { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 + { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM6 - S3 + { TIM16, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM7 - S4 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO TIMER - LED_STRIP + }; diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index abbf99371b..52c673af86 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -162,12 +162,12 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT +#define REMAP_TIM17_DMA #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 8 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index 150389f054..b09058af7c 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -43,5 +43,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; From 9c4bedb7d44fd7b1f4ddbd7dd07cc981f2763b64 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 9 Nov 2016 11:10:44 +0900 Subject: [PATCH 165/188] Fix for SINGULARITY build --- src/main/cms/cms_menu_vtx.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 48f8bd6f1d..3b0cb021a4 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -72,7 +72,7 @@ static OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; static void cmsx_Vtx_ConfigRead(void) { #ifdef VTX - cmsx_vtxBand = masterConfig.vtxBand; + cmsx_vtxBand = masterConfig.vtx_band; cmsx_vtxChannel = masterConfig.vtx_channel + 1; #endif // VTX @@ -85,7 +85,7 @@ static void cmsx_Vtx_ConfigRead(void) static void cmsx_Vtx_ConfigWriteback(void) { #ifdef VTX - masterConfig.vtxBand = cmsx_vtxBand; + masterConfig.vtx_band = cmsx_vtxBand; masterConfig.vtx_channel = cmsx_vtxChannel - 1; #endif // VTX From 99cc39dcc449bd59cf1a8b8596510c90cf09647d Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 9 Nov 2016 12:11:35 +0900 Subject: [PATCH 166/188] Menu restructuring MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Created “OTHER PP†menu entry in PROFILE for “Other Per Profile†items. - Moved dterm set point weight and set point relax to OTHER PP. - Added missing angle strength, horizon strength and transition. --- src/main/cms/cms_menu_imu.c | 80 +++++++++++++++++++++++++++++-------- 1 file changed, 64 insertions(+), 16 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 7d846f6600..24b988cb07 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -53,7 +53,7 @@ static uint8_t tmpProfileIndex; static uint8_t profileIndex; static char profileIndexString[] = " p"; -static uint8_t tempPid[4][3]; +static uint8_t tempPid[3][3]; static uint8_t tmpRateProfileIndex; static uint8_t rateProfileIndex; @@ -109,9 +109,6 @@ static long cmsx_PidRead(void) tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i]; tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i]; } - tempPid[3][0] = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL]; return 0; } @@ -134,10 +131,6 @@ static long cmsx_PidWriteback(const OSD_Entry *self) masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2]; } - masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; - return 0; } @@ -215,14 +208,12 @@ static OSD_Entry cmsx_menuRateProfileEntries[] = { "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID, 0, 100, 1, 10}, 0 }, { "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10}, 0 }, - { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, - { "SETPT RLX", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; -CMS_Menu cmsx_menuRateProfile = { +static CMS_Menu cmsx_menuRateProfile = { .GUARD_text = "MENURATE", .GUARD_type = OME_MENU, .onEnter = cmsx_RateProfileOnEnter, @@ -231,6 +222,62 @@ CMS_Menu cmsx_menuRateProfile = { .entries = cmsx_menuRateProfileEntries }; +static uint8_t cmsx_dtermSetpointWeight; +static uint8_t cmsx_setpointRelaxRatio; +static uint8_t cmsx_angleStrength; +static uint8_t cmsx_horizonStrength; +static uint8_t cmsx_horizonTransition; + +static long cmsx_profileOtherOnEnter(void) +{ + profileIndexString[1] = '0' + tmpProfileIndex; + + cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight; + cmsx_setpointRelaxRatio = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio; + + cmsx_angleStrength = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL]; + cmsx_horizonStrength = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL]; + cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL]; + + return 0; +} + +static long cmsx_profileOtherOnExit(const OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight; + masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio; + + masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength; + masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength; + masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition; + + return 0; +} + +static OSD_Entry cmsx_menuProfileOtherEntries[] = { + { "-- OTHER PP --", OME_Label, NULL, profileIndexString, 0 }, + + { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, + { "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, + { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, + { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 }, + { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuProfileOther = { + .GUARD_text = "XPROFOTHER", + .GUARD_type = OME_MENU, + .onEnter = cmsx_profileOtherOnEnter, + .onExit = cmsx_profileOtherOnExit, + .onGlobalExit = NULL, + .entries = cmsx_menuProfileOtherEntries, +}; + static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, @@ -311,15 +358,16 @@ static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, - {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, + {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, + {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, + {"OTHER PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0}, {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, MAX_RATEPROFILES, 1}, 0}, - {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, + {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, - {"FLT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, + {"FLT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, - {"FLT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, + {"FLT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} From 1dbd7ee732d70fd10ee3f923107b02fa963fed5d Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 8 Nov 2016 15:47:25 +1100 Subject: [PATCH 167/188] Simplified target timer definitions --- src/main/drivers/tim_def.h | 357 +++++++++++++++++++++++++++++ src/main/drivers/timer.h | 1 + src/main/target/BLUEJAYF4/target.c | 14 ++ 3 files changed, 372 insertions(+) create mode 100644 src/main/drivers/tim_def.h diff --git a/src/main/drivers/tim_def.h b/src/main/drivers/tim_def.h new file mode 100644 index 0000000000..51090375ce --- /dev/null +++ b/src/main/drivers/tim_def.h @@ -0,0 +1,357 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include "common/utils.h" + +#if defined(STM32F3) + +#define DEF_TIM(tim, chan, pin, flags, out) {\ + tim,\ + IO_TAG(pin),\ + EXPAND(DEF_CHAN_ ## chan),\ + flags,\ + (DEF_CHAN_ ## chan ## _OUTPUT | out),\ + EXPAND(GPIO_AF__ ## pin ## _ ## tim ## _ ## chan),\ + CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _TYPE),\ + CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _HANDLER)\ + } + +#define DEF_TIM_CHAN(chan) DEF_CHAN_ ## chan +#define DEF_TIM_OUTPUT(chan, out) ( DEF_CHAN_ ## chan ## _OUTPUT | out ) + +#define DEF_DMA_TYPE(tim, chan) CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _TYPE) +#define DEF_DMA_HANDLER(tim, chan) CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _HANDLER) + +/* add the DMA mappings here */ +#define DEF_TIM_DMACHAN_TIM1_CH1 DMA1_CH2 +#define DEF_TIM_DMACHAN_TIM1_CH2 DMA1_CH3 +#define DEF_TIM_DMACHAN_TIM1_CH4 DMA1_CH4 +#define DEF_TIM_DMACHAN_TIM1_TRIG DMA1_CH4 +#define DEF_TIM_DMACHAN_TIM1_COM DMA1_CH4 +#define DEF_TIM_DMACHAN_TIM1_UP DMA1_CH5 +#define DEF_TIM_DMACHAN_TIM1_CH3 DMA1_CH6 + +#define DEF_TIM_DMACHAN_TIM2_CH3 DMA1_CH1 +#define DEF_TIM_DMACHAN_TIM2_UP DMA1_CH2 +#define DEF_TIM_DMACHAN_TIM2_CH1 DMA1_CH5 +#define DEF_TIM_DMACHAN_TIM2_CH2 DMA1_CH7 +#define DEF_TIM_DMACHAN_TIM2_CH4 DMA1_CH7 + +#define DEF_TIM_DMACHAN_TIM3_CH2 DMA_NONE +#define DEF_TIM_DMACHAN_TIM3_CH3 DMA1_CH2 +#define DEF_TIM_DMACHAN_TIM3_CH4 DMA1_CH3 +#define DEF_TIM_DMACHAN_TIM3_UP DMA1_CH3 +#define DEF_TIM_DMACHAN_TIM3_CH1 DMA1_CH6 +#define DEF_TIM_DMACHAN_TIM3_TRIG DMA1_CH6 + +#define DEF_TIM_DMACHAN_TIM4_CH1 DMA1_CH1 +#define DEF_TIM_DMACHAN_TIM4_CH2 DMA1_CH4 +#define DEF_TIM_DMACHAN_TIM4_CH3 DMA1_CH5 +#define DEF_TIM_DMACHAN_TIM4_UP DMA1_CH7 +#define DEF_TIM_DMACHAN_TIM4_CH4 DMA_NONE + +#define DEF_TIM_DMACHAN_TIM15_CH1 DMA1_CH5 +#define DEF_TIM_DMACHAN_TIM15_CH2 DMA_NONE +#define DEF_TIM_DMACHAN_TIM15_UP DMA1_CH5 +#define DEF_TIM_DMACHAN_TIM15_TRIG DMA1_CH5 +#define DEF_TIM_DMACHAN_TIM15_COM DMA1_CH5 + +#ifdef DMA_TIMER16_REMAP +#define DEF_TIM_DMACHAN_TIM16_CH1 DMA1_CH6 +#define DEF_TIM_DMACHAN_TIM16_UP DMA1_CH6 +#else +#define DEF_TIM_DMACHAN_TIM16_CH1 DMA1_CH3 +#define DEF_TIM_DMACHAN_TIM16_UP DMA1_CH3 +#endif + +#ifdef DMA_TIMER17_REMAP +#define DEF_TIM_DMACHAN_TIM17_CH1 DMA1_CH7 +#define DEF_TIM_DMACHAN_TIM17_UP DMA1_CH7 +#else +#define DEF_TIM_DMACHAN_TIM17_CH1 DMA1_CH1 +#define DEF_TIM_DMACHAN_TIM17_UP DMA1_CH1 +#endif + +#define DEF_TIM_DMACHAN_TIM8_CH3 DMA2_CH1 +#define DEF_TIM_DMACHAN_TIM8_UP DMA2_CH1 +#define DEF_TIM_DMACHAN_TIM8_CH4 DMA2_CH2 +#define DEF_TIM_DMACHAN_TIM8_TRIG DMA2_CH2 +#define DEF_TIM_DMACHAN_TIM8_COM DMA2_CH2 +#define DEF_TIM_DMACHAN_TIM8_CH1 DMA2_CH3 +#define DEF_TIM_DMACHAN_TIM8_CH2 DMA2_CH5 + + +#define DMA1_CH1_TYPE DMA1_Channel1 +#define DMA1_CH2_TYPE DMA1_Channel2 +#define DMA1_CH3_TYPE DMA1_Channel3 +#define DMA1_CH4_TYPE DMA1_Channel4 +#define DMA1_CH5_TYPE DMA1_Channel5 +#define DMA1_CH6_TYPE DMA1_Channel6 +#define DMA1_CH7_TYPE DMA1_Channel7 +#define DMA2_CH1_TYPE DMA2_Channel1 +#define DMA2_CH2_TYPE DMA2_Channel2 +#define DMA2_CH3_TYPE DMA2_Channel3 +#define DMA2_CH4_TYPE DMA2_Channel4 +#define DMA2_CH5_TYPE DMA2_Channel5 +#define DMA2_CH6_TYPE DMA2_Channel6 +#define DMA2_CH7_TYPE DMA2_Channel7 + +#define GPIO_AF(p, t) CONCAT(GPIO_AF__, p, _, t) + +#define GPIO_AF__PA0_TIM2_CH1 GPIO_AF_1 +#define GPIO_AF__PA1_TIM2_CH2 GPIO_AF_1 +#define GPIO_AF__PA2_TIM2_CH3 GPIO_AF_1 +#define GPIO_AF__PA3_TIM2_CH3 GPIO_AF_1 +#define GPIO_AF__PA5_TIM2_CH1 GPIO_AF_1 +#define GPIO_AF__PA6_TIM16_CH1 GPIO_AF_1 +#define GPIO_AF__PA7_TIM17_CH1 GPIO_AF_1 +#define GPIO_AF__PA12_TIM16_CH1 GPIO_AF_1 +#define GPIO_AF__PA13_TIM16_CH1N GPIO_AF_1 +#define GPIO_AF__PA15_TIM2_CH1 GPIO_AF_1 + +#define GPIO_AF__PA4_TIM3_CH2 GPIO_AF_2 +#define GPIO_AF__PA6_TIM3_CH1 GPIO_AF_2 +#define GPIO_AF__PA7_TIM3_CH2 GPIO_AF_2 +#define GPIO_AF__PA15_TIM8_CH1 GPIO_AF_2 + +#define GPIO_AF__PA7_TIM8_CH1N GPIO_AF_4 + +#define GPIO_AF__PA14_TIM4_CH2 GPIO_AF_5 + +#define GPIO_AF__PA7_TIM1_CH1N GPIO_AF_6 +#define GPIO_AF__PA8_TIM1_CH1 GPIO_AF_6 +#define GPIO_AF__PA9_TIM1_CH2 GPIO_AF_6 +#define GPIO_AF__PA10_TIM1_CH3 GPIO_AF_6 +#define GPIO_AF__PA11_TIM1_CH1N GPIO_AF_6 +#define GPIO_AF__PA12_TIM1_CH2N GPIO_AF_6 + +#define GPIO_AF__PA1_TIM15_CH1N GPIO_AF_9 +#define GPIO_AF__PA2_TIM15_CH1 GPIO_AF_9 +#define GPIO_AF__PA3_TIM15_CH2 GPIO_AF_9 + +#define GPIO_AF__PA9_TIM2_CH3 GPIO_AF_10 +#define GPIO_AF__PA10_TIM2_CH4 GPIO_AF_10 +#define GPIO_AF__PA11_TIM4_CH1 GPIO_AF_10 +#define GPIO_AF__PA12_TIM4_CH2 GPIO_AF_10 +#define GPIO_AF__PA13_TIM4_CH3 GPIO_AF_10 +#define GPIO_AF__PA11_TIM1_CH4 GPIO_AF_11 + +#define GPIO_AF__PB3_TIM2_CH2 GPIO_AF_1 +#define GPIO_AF__PB4_TIM16_CH1 GPIO_AF_1 +#define GPIO_AF__PB6_TIM16_CH1N GPIO_AF_1 +#define GPIO_AF__PB7_TIM17_CH1N GPIO_AF_1 +#define GPIO_AF__PB8_TIM16_CH1 GPIO_AF_1 +#define GPIO_AF__PB9_TIM17_CH1 GPIO_AF_1 +#define GPIO_AF__PB10_TIM2_CH3 GPIO_AF_1 +#define GPIO_AF__PB11_TIM2_CH4 GPIO_AF_1 +#define GPIO_AF__PB14_TIM15_CH1 GPIO_AF_1 +#define GPIO_AF__PB15_TIM15_CH2 GPIO_AF_1 + +#define GPIO_AF__PB0_TIM3_CH3 GPIO_AF_2 +#define GPIO_AF__PB1_TIM3_CH4 GPIO_AF_2 +#define GPIO_AF__PB4_TIM3_CH1 GPIO_AF_2 +#define GPIO_AF__PB5_TIM3_CH2 GPIO_AF_2 +#define GPIO_AF__PB6_TIM4_CH1 GPIO_AF_2 +#define GPIO_AF__PB7_TIM4_CH2 GPIO_AF_2 +#define GPIO_AF__PB8_TIM4_CH3 GPIO_AF_2 +#define GPIO_AF__PB9_TIM4_CH4 GPIO_AF_2 +#define GPIO_AF__PB15_TIM15_CH1N GPIO_AF_2 + +#define GPIO_AF__PB0_TIM8_CH2N GPIO_AF_4 +#define GPIO_AF__PB1_TIM8_CH3N GPIO_AF_4 +#define GPIO_AF__PB3_TIM8_CH1N GPIO_AF_4 +#define GPIO_AF__PB4_TIM8_CH2N GPIO_AF_4 +#define GPIO_AF__PB15_TIM1_CH3N GPIO_AF_4 + +#define GPIO_AF__PB6_TIM8_CH1 GPIO_AF_5 + +#define GPIO_AF__PB0_TIM1_CH2N GPIO_AF_6 +#define GPIO_AF__PB1_TIM1_CH3N GPIO_AF_6 +#define GPIO_AF__PB13_TIM1_CH1N GPIO_AF_6 +#define GPIO_AF__PB14_TIM1_CH2N GPIO_AF_6 + +#define GPIO_AF__PB5_TIM17_CH1 GPIO_AF_10 +#define GPIO_AF__PB7_TIM3_CH4 GPIO_AF_10 +#define GPIO_AF__PB8_TIM8_CH2 GPIO_AF_10 +#define GPIO_AF__PB9_TIM8_CH3 GPIO_AF_10 + +#define GPIO_AF__PC6_TIM3_CH1 GPIO_AF_2 +#define GPIO_AF__PC7_TIM3_CH2 GPIO_AF_2 +#define GPIO_AF__PC8_TIM3_CH3 GPIO_AF_2 +#define GPIO_AF__PC9_TIM3_CH4 GPIO_AF_2 + +#define GPIO_AF__PC6_TIM8_CH1 GPIO_AF_4 +#define GPIO_AF__PC7_TIM8_CH2 GPIO_AF_4 +#define GPIO_AF__PC8_TIM8_CH3 GPIO_AF_4 +#define GPIO_AF__PC9_TIM8_CH4 GPIO_AF_4 + +#define GPIO_AF__PC10_TIM8_CH1N GPIO_AF_4 +#define GPIO_AF__PC11_TIM8_CH2N GPIO_AF_4 +#define GPIO_AF__PC12_TIM8_CH3N GPIO_AF_4 +#define GPIO_AF__PC13_TIM8_CH1N GPIO_AF_4 + +#define GPIO_AF__PD3_TIM2_CH1 GPIO_AF_2 +#define GPIO_AF__PD4_TIM2_CH2 GPIO_AF_2 +#define GPIO_AF__PD6_TIM2_CH4 GPIO_AF_2 +#define GPIO_AF__PD7_TIM2_CH3 GPIO_AF_2 + +#define GPIO_AF__PD12_TIM4_CH1 GPIO_AF_2 +#define GPIO_AF__PD13_TIM4_CH2 GPIO_AF_2 +#define GPIO_AF__PD14_TIM4_CH3 GPIO_AF_2 +#define GPIO_AF__PD15_TIM4_CH4 GPIO_AF_2 + +#define GPIO_AF__PD1_TIM8_CH4 GPIO_AF_4 + +#elif defined(STM32F4) + +#define DEF_TIM(tim, chan, pin, flags, out, dmaopt) {\ + tim,\ + IO_TAG(pin),\ + EXPAND(DEF_CHAN_ ## chan),\ + flags,\ + (DEF_CHAN_ ## chan ## _OUTPUT | out),\ + EXPAND(GPIO_AF_## tim),\ + CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## _ ## tim ## _ ## chan), _TYPE),\ + EXPAND(DEF_TIM_DMA_CHN_ ## dmaopt ## _ ## tim ## _ ## chan),\ + CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## _ ## tim ## _ ## chan), _HANDLER)\ + } + +/* F4 Stream Mappings */ + +#define DEF_TIM_DMA_STR_0_TIM1_CH1 DMA2_ST6 +#define DEF_TIM_DMA_STR_1_TIM1_CH1 DMA2_ST1 +#define DEF_TIM_DMA_STR_2_TIM1_CH1 DMA2_ST3 +#define DEF_TIM_DMA_STR_0_TIM1_CH2 DMA2_ST6 +#define DEF_TIM_DMA_STR_1_TIM1_CH2 DMA2_ST2 +#define DEF_TIM_DMA_STR_0_TIM1_CH3 DMA2_ST6 +#define DEF_TIM_DMA_STR_1_TIM1_CH3 DMA2_ST6 +#define DEF_TIM_DMA_STR_0_TIM1_CH4 DMA2_ST4 + +#define DEF_TIM_DMA_STR_0_TIM2_CH1 DMA1_ST5 +#define DEF_TIM_DMA_STR_0_TIM2_CH2 DMA1_ST6 +#define DEF_TIM_DMA_STR_0_TIM2_CH3 DMA1_ST1 +#define DEF_TIM_DMA_STR_0_TIM2_CH4 DMA1_ST7 +#define DEF_TIM_DMA_STR_1_TIM2_CH4 DMA1_ST6 + +#define DEF_TIM_DMA_STR_0_TIM3_CH1 DMA1_ST4 +#define DEF_TIM_DMA_STR_0_TIM3_CH2 DMA1_ST5 +#define DEF_TIM_DMA_STR_0_TIM3_CH3 DMA1_ST7 +#define DEF_TIM_DMA_STR_0_TIM3_CH4 DMA1_ST2 + +#define DEF_TIM_DMA_STR_0_TIM4_CH1 DMA1_ST0 +#define DEF_TIM_DMA_STR_0_TIM4_CH2 DMA1_ST4 +#define DEF_TIM_DMA_STR_0_TIM4_CH3 DMA1_ST7 +#define DEF_TIM_DMA_STR_0_TIM4_CH4 DMA1_ST3 + +#define DEF_TIM_DMA_STR_0_TIM5_CH1 DMA1_ST2 +#define DEF_TIM_DMA_STR_0_TIM5_CH2 DMA1_ST4 +#define DEF_TIM_DMA_STR_0_TIM5_CH3 DMA1_ST0 +#define DEF_TIM_DMA_STR_0_TIM5_CH4 DMA1_ST1 +#define DEF_TIM_DMA_STR_1_TIM5_CH4 DMA1_ST3 + +#define DEF_TIM_DMA_STR_0_TIM8_CH1 DMA2_ST2 +#define DEF_TIM_DMA_STR_1_TIM8_CH1 DMA2_ST2 +#define DEF_TIM_DMA_STR_0_TIM8_CH2 DMA2_ST3 +#define DEF_TIM_DMA_STR_1_TIM8_CH2 DMA2_ST2 +#define DEF_TIM_DMA_STR_0_TIM8_CH3 DMA2_ST2 +#define DEF_TIM_DMA_STR_0_TIM8_CH4 DMA2_ST7 + +/* F4 Channel Mappings */ + +#define DEF_TIM_DMA_CHN_0_TIM1_CH1 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1_TIM1_CH1 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_2_TIM1_CH1 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0_TIM1_CH2 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1_TIM1_CH2 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0_TIM1_CH3 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1_TIM1_CH3 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0_TIM1_CH4 DMA_Channel_6 + +#define DEF_TIM_DMA_CHN_0_TIM2_CH1 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0_TIM2_CH2 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0_TIM2_CH3 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0_TIM2_CH4 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_1_TIM2_CH4 DMA_Channel_3 + +#define DEF_TIM_DMA_CHN_0_TIM3_CH1 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0_TIM3_CH2 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0_TIM3_CH3 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0_TIM3_CH4 DMA_Channel_5 + +#define DEF_TIM_DMA_CHN_0_TIM4_CH1 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0_TIM4_CH2 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0_TIM4_CH3 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0_TIM4_CH4 DMA_Channel_2 + +#define DEF_TIM_DMA_CHN_0_TIM5_CH1 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0_TIM5_CH2 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0_TIM5_CH3 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0_TIM5_CH4 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_1_TIM5_CH4 DMA_Channel_3 + +#define DEF_TIM_DMA_CHN_0_TIM8_CH1 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1_TIM8_CH1 DMA_Channel_7 +#define DEF_TIM_DMA_CHN_0_TIM8_CH2 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1_TIM8_CH2 DMA_Channel_7 +#define DEF_TIM_DMA_CHN_0_TIM8_CH3 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_0_TIM8_CH4 DMA_Channel_7 + +#define DMA1_ST0_TYPE DMA1_Stream0 +#define DMA1_ST1_TYPE DMA1_Stream1 +#define DMA1_ST2_TYPE DMA1_Stream2 +#define DMA1_ST3_TYPE DMA1_Stream3 +#define DMA1_ST4_TYPE DMA1_Stream4 +#define DMA1_ST5_TYPE DMA1_Stream5 +#define DMA1_ST6_TYPE DMA1_Stream6 +#define DMA1_ST7_TYPE DMA1_Stream7 +#define DMA2_ST0_TYPE DMA2_Stream0 +#define DMA2_ST1_TYPE DMA2_Stream1 +#define DMA2_ST2_TYPE DMA2_Stream2 +#define DMA2_ST3_TYPE DMA2_Stream3 +#define DMA2_ST4_TYPE DMA2_Stream4 +#define DMA2_ST5_TYPE DMA2_Stream5 +#define DMA2_ST6_TYPE DMA2_Stream6 +#define DMA2_ST7_TYPE DMA2_Stream7 + +#endif + +/**** Common Defines across all targets ****/ + +#define NONE 0 +#define DMA_NONE_TYPE NULL +#define DMA_NONE_HANDLER 0 + +#define DEF_CHAN_CH1 TIM_Channel_1 +#define DEF_CHAN_CH2 TIM_Channel_2 +#define DEF_CHAN_CH3 TIM_Channel_3 +#define DEF_CHAN_CH4 TIM_Channel_4 +#define DEF_CHAN_CH1N TIM_Channel_1 +#define DEF_CHAN_CH2N TIM_Channel_2 +#define DEF_CHAN_CH3N TIM_Channel_3 +#define DEF_CHAN_CH4N TIM_Channel_4 + +#define DEF_CHAN_CH1_OUTPUT TIMER_OUTPUT_NONE +#define DEF_CHAN_CH2_OUTPUT TIMER_OUTPUT_NONE +#define DEF_CHAN_CH3_OUTPUT TIMER_OUTPUT_NONE +#define DEF_CHAN_CH4_OUTPUT TIMER_OUTPUT_NONE +#define DEF_CHAN_CH1N_OUTPUT TIMER_OUTPUT_N_CHANNEL +#define DEF_CHAN_CH2N_OUTPUT TIMER_OUTPUT_N_CHANNEL +#define DEF_CHAN_CH3N_OUTPUT TIMER_OUTPUT_N_CHANNEL +#define DEF_CHAN_CH4N_OUTPUT TIMER_OUTPUT_N_CHANNEL diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index a6fa48f8a9..bb7a6c5bc1 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -105,6 +105,7 @@ typedef struct timerHardware_s { } timerHardware_t; typedef enum { + TIMER_OUTPUT_NONE = 0x00, TIMER_INPUT_ENABLED = 0x00, TIMER_OUTPUT_ENABLED = 0x01, TIMER_OUTPUT_INVERTED = 0x02, diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index d0d2b450de..6ab553f5fa 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -22,6 +22,9 @@ #include "drivers/timer.h" #include "drivers/dma.h" +#include "drivers/tim_def.h" + +/* const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT @@ -31,4 +34,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT }; +*/ + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0 ), // S1_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 0 ), // S4_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR | TIM_USE_LED, 0, 0 ), // S5_OUT + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S6_OUT +}; From 1a8f11261e3ac39e0bd532d148f4a30bdef20191 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 08:47:43 +1100 Subject: [PATCH 168/188] Updates following review --- src/main/drivers/tim_def.h | 308 +++++++++++++++++++------------------ 1 file changed, 156 insertions(+), 152 deletions(-) diff --git a/src/main/drivers/tim_def.h b/src/main/drivers/tim_def.h index 51090375ce..e41260f86e 100644 --- a/src/main/drivers/tim_def.h +++ b/src/main/drivers/tim_def.h @@ -33,85 +33,82 @@ CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _HANDLER)\ } -#define DEF_TIM_CHAN(chan) DEF_CHAN_ ## chan -#define DEF_TIM_OUTPUT(chan, out) ( DEF_CHAN_ ## chan ## _OUTPUT | out ) - -#define DEF_DMA_TYPE(tim, chan) CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _TYPE) -#define DEF_DMA_HANDLER(tim, chan) CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _HANDLER) +#define DEF_DMA_CHANNEL(tim, chan) CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _CHANNEL) +#define DEF_DMA_HANDLER(tim, chan) CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _HANDLER) /* add the DMA mappings here */ -#define DEF_TIM_DMACHAN_TIM1_CH1 DMA1_CH2 -#define DEF_TIM_DMACHAN_TIM1_CH2 DMA1_CH3 -#define DEF_TIM_DMACHAN_TIM1_CH4 DMA1_CH4 -#define DEF_TIM_DMACHAN_TIM1_TRIG DMA1_CH4 -#define DEF_TIM_DMACHAN_TIM1_COM DMA1_CH4 -#define DEF_TIM_DMACHAN_TIM1_UP DMA1_CH5 -#define DEF_TIM_DMACHAN_TIM1_CH3 DMA1_CH6 +#define DEF_TIM_DMA__TIM1_CH1 DMA1_CH2 +#define DEF_TIM_DMA__TIM1_CH2 DMA1_CH3 +#define DEF_TIM_DMA__TIM1_CH4 DMA1_CH4 +#define DEF_TIM_DMA__TIM1_TRIG DMA1_CH4 +#define DEF_TIM_DMA__TIM1_COM DMA1_CH4 +#define DEF_TIM_DMA__TIM1_UP DMA1_CH5 +#define DEF_TIM_DMA__TIM1_CH3 DMA1_CH6 -#define DEF_TIM_DMACHAN_TIM2_CH3 DMA1_CH1 -#define DEF_TIM_DMACHAN_TIM2_UP DMA1_CH2 -#define DEF_TIM_DMACHAN_TIM2_CH1 DMA1_CH5 -#define DEF_TIM_DMACHAN_TIM2_CH2 DMA1_CH7 -#define DEF_TIM_DMACHAN_TIM2_CH4 DMA1_CH7 +#define DEF_TIM_DMA__TIM2_CH3 DMA1_CH1 +#define DEF_TIM_DMA__TIM2_UP DMA1_CH2 +#define DEF_TIM_DMA__TIM2_CH1 DMA1_CH5 +#define DEF_TIM_DMA__TIM2_CH2 DMA1_CH7 +#define DEF_TIM_DMA__TIM2_CH4 DMA1_CH7 -#define DEF_TIM_DMACHAN_TIM3_CH2 DMA_NONE -#define DEF_TIM_DMACHAN_TIM3_CH3 DMA1_CH2 -#define DEF_TIM_DMACHAN_TIM3_CH4 DMA1_CH3 -#define DEF_TIM_DMACHAN_TIM3_UP DMA1_CH3 -#define DEF_TIM_DMACHAN_TIM3_CH1 DMA1_CH6 -#define DEF_TIM_DMACHAN_TIM3_TRIG DMA1_CH6 +#define DEF_TIM_DMA__TIM3_CH2 DMA_NONE +#define DEF_TIM_DMA__TIM3_CH3 DMA1_CH2 +#define DEF_TIM_DMA__TIM3_CH4 DMA1_CH3 +#define DEF_TIM_DMA__TIM3_UP DMA1_CH3 +#define DEF_TIM_DMA__TIM3_CH1 DMA1_CH6 +#define DEF_TIM_DMA__TIM3_TRIG DMA1_CH6 -#define DEF_TIM_DMACHAN_TIM4_CH1 DMA1_CH1 -#define DEF_TIM_DMACHAN_TIM4_CH2 DMA1_CH4 -#define DEF_TIM_DMACHAN_TIM4_CH3 DMA1_CH5 -#define DEF_TIM_DMACHAN_TIM4_UP DMA1_CH7 -#define DEF_TIM_DMACHAN_TIM4_CH4 DMA_NONE +#define DEF_TIM_DMA__TIM4_CH1 DMA1_CH1 +#define DEF_TIM_DMA__TIM4_CH2 DMA1_CH4 +#define DEF_TIM_DMA__TIM4_CH3 DMA1_CH5 +#define DEF_TIM_DMA__TIM4_UP DMA1_CH7 +#define DEF_TIM_DMA__TIM4_CH4 DMA_NONE -#define DEF_TIM_DMACHAN_TIM15_CH1 DMA1_CH5 -#define DEF_TIM_DMACHAN_TIM15_CH2 DMA_NONE -#define DEF_TIM_DMACHAN_TIM15_UP DMA1_CH5 -#define DEF_TIM_DMACHAN_TIM15_TRIG DMA1_CH5 -#define DEF_TIM_DMACHAN_TIM15_COM DMA1_CH5 +#define DEF_TIM_DMA__TIM15_CH1 DMA1_CH5 +#define DEF_TIM_DMA__TIM15_CH2 DMA_NONE +#define DEF_TIM_DMA__TIM15_UP DMA1_CH5 +#define DEF_TIM_DMA__TIM15_TRIG DMA1_CH5 +#define DEF_TIM_DMA__TIM15_COM DMA1_CH5 #ifdef DMA_TIMER16_REMAP -#define DEF_TIM_DMACHAN_TIM16_CH1 DMA1_CH6 -#define DEF_TIM_DMACHAN_TIM16_UP DMA1_CH6 +#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH6 +#define DEF_TIM_DMA__TIM16_UP DMA1_CH6 #else -#define DEF_TIM_DMACHAN_TIM16_CH1 DMA1_CH3 -#define DEF_TIM_DMACHAN_TIM16_UP DMA1_CH3 +#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH3 +#define DEF_TIM_DMA__TIM16_UP DMA1_CH3 #endif #ifdef DMA_TIMER17_REMAP -#define DEF_TIM_DMACHAN_TIM17_CH1 DMA1_CH7 -#define DEF_TIM_DMACHAN_TIM17_UP DMA1_CH7 +#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH7 +#define DEF_TIM_DMA__TIM17_UP DMA1_CH7 #else -#define DEF_TIM_DMACHAN_TIM17_CH1 DMA1_CH1 -#define DEF_TIM_DMACHAN_TIM17_UP DMA1_CH1 +#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH1 +#define DEF_TIM_DMA__TIM17_UP DMA1_CH1 #endif -#define DEF_TIM_DMACHAN_TIM8_CH3 DMA2_CH1 -#define DEF_TIM_DMACHAN_TIM8_UP DMA2_CH1 -#define DEF_TIM_DMACHAN_TIM8_CH4 DMA2_CH2 -#define DEF_TIM_DMACHAN_TIM8_TRIG DMA2_CH2 -#define DEF_TIM_DMACHAN_TIM8_COM DMA2_CH2 -#define DEF_TIM_DMACHAN_TIM8_CH1 DMA2_CH3 -#define DEF_TIM_DMACHAN_TIM8_CH2 DMA2_CH5 +#define DEF_TIM_DMA__TIM8_CH3 DMA2_CH1 +#define DEF_TIM_DMA__TIM8_UP DMA2_CH1 +#define DEF_TIM_DMA__TIM8_CH4 DMA2_CH2 +#define DEF_TIM_DMA__TIM8_TRIG DMA2_CH2 +#define DEF_TIM_DMA__TIM8_COM DMA2_CH2 +#define DEF_TIM_DMA__TIM8_CH1 DMA2_CH3 +#define DEF_TIM_DMA__TIM8_CH2 DMA2_CH5 -#define DMA1_CH1_TYPE DMA1_Channel1 -#define DMA1_CH2_TYPE DMA1_Channel2 -#define DMA1_CH3_TYPE DMA1_Channel3 -#define DMA1_CH4_TYPE DMA1_Channel4 -#define DMA1_CH5_TYPE DMA1_Channel5 -#define DMA1_CH6_TYPE DMA1_Channel6 -#define DMA1_CH7_TYPE DMA1_Channel7 -#define DMA2_CH1_TYPE DMA2_Channel1 -#define DMA2_CH2_TYPE DMA2_Channel2 -#define DMA2_CH3_TYPE DMA2_Channel3 -#define DMA2_CH4_TYPE DMA2_Channel4 -#define DMA2_CH5_TYPE DMA2_Channel5 -#define DMA2_CH6_TYPE DMA2_Channel6 -#define DMA2_CH7_TYPE DMA2_Channel7 +#define DMA1_CH1_CHANNEL DMA1_Channel1 +#define DMA1_CH2_CHANNEL DMA1_Channel2 +#define DMA1_CH3_CHANNEL DMA1_Channel3 +#define DMA1_CH4_CHANNEL DMA1_Channel4 +#define DMA1_CH5_CHANNEL DMA1_Channel5 +#define DMA1_CH6_CHANNEL DMA1_Channel6 +#define DMA1_CH7_CHANNEL DMA1_Channel7 +#define DMA2_CH1_CHANNEL DMA2_Channel1 +#define DMA2_CH2_CHANNEL DMA2_Channel2 +#define DMA2_CH3_CHANNEL DMA2_Channel3 +#define DMA2_CH4_CHANNEL DMA2_Channel4 +#define DMA2_CH5_CHANNEL DMA2_Channel5 +#define DMA2_CH6_CHANNEL DMA2_Channel6 +#define DMA2_CH7_CHANNEL DMA2_Channel7 #define GPIO_AF(p, t) CONCAT(GPIO_AF__, p, _, t) @@ -212,12 +209,12 @@ #define GPIO_AF__PD6_TIM2_CH4 GPIO_AF_2 #define GPIO_AF__PD7_TIM2_CH3 GPIO_AF_2 -#define GPIO_AF__PD12_TIM4_CH1 GPIO_AF_2 -#define GPIO_AF__PD13_TIM4_CH2 GPIO_AF_2 -#define GPIO_AF__PD14_TIM4_CH3 GPIO_AF_2 -#define GPIO_AF__PD15_TIM4_CH4 GPIO_AF_2 +#define GPIO_AF__PD12_TIM4_CH1 GPIO_AF_2 +#define GPIO_AF__PD13_TIM4_CH2 GPIO_AF_2 +#define GPIO_AF__PD14_TIM4_CH3 GPIO_AF_2 +#define GPIO_AF__PD15_TIM4_CH4 GPIO_AF_2 -#define GPIO_AF__PD1_TIM8_CH4 GPIO_AF_4 +#define GPIO_AF__PD1_TIM8_CH4 GPIO_AF_4 #elif defined(STM32F4) @@ -228,114 +225,121 @@ flags,\ (DEF_CHAN_ ## chan ## _OUTPUT | out),\ EXPAND(GPIO_AF_## tim),\ - CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## _ ## tim ## _ ## chan), _TYPE),\ - EXPAND(DEF_TIM_DMA_CHN_ ## dmaopt ## _ ## tim ## _ ## chan),\ - CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## _ ## tim ## _ ## chan), _HANDLER)\ + CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## __ ## tim ## _ ## chan), _STREAM),\ + EXPAND(DEF_TIM_DMA_CHN_ ## dmaopt ## __ ## tim ## _ ## chan),\ + CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## __ ## tim ## _ ## chan), _HANDLER)\ } +#define DEF_DMA_CHANNEL(tim, chan, dmaopt) EXPAND(DEF_TIM_DMA_CHN_ ## dmaopt ## __ ## tim ## _ ## chan) +#define DEF_DMA_STREAM(tim, chan, dmaopt) CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## __ ## tim ## _ ## chan), _STREAM) +#define DEF_DMA_HANDLER(tim, chan, dmaopt) CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## __ ## tim ## _ ## chan), _HANDLER) + /* F4 Stream Mappings */ -#define DEF_TIM_DMA_STR_0_TIM1_CH1 DMA2_ST6 -#define DEF_TIM_DMA_STR_1_TIM1_CH1 DMA2_ST1 -#define DEF_TIM_DMA_STR_2_TIM1_CH1 DMA2_ST3 -#define DEF_TIM_DMA_STR_0_TIM1_CH2 DMA2_ST6 -#define DEF_TIM_DMA_STR_1_TIM1_CH2 DMA2_ST2 -#define DEF_TIM_DMA_STR_0_TIM1_CH3 DMA2_ST6 -#define DEF_TIM_DMA_STR_1_TIM1_CH3 DMA2_ST6 -#define DEF_TIM_DMA_STR_0_TIM1_CH4 DMA2_ST4 +#define DEF_TIM_DMA_STR_0__TIM1_CH1 DMA2_ST6 +#define DEF_TIM_DMA_STR_1__TIM1_CH1 DMA2_ST1 +#define DEF_TIM_DMA_STR_2__TIM1_CH1 DMA2_ST3 +#define DEF_TIM_DMA_STR_0__TIM1_CH2 DMA2_ST6 +#define DEF_TIM_DMA_STR_1__TIM1_CH2 DMA2_ST2 +#define DEF_TIM_DMA_STR_0__TIM1_CH3 DMA2_ST6 +#define DEF_TIM_DMA_STR_1__TIM1_CH3 DMA2_ST6 +#define DEF_TIM_DMA_STR_0__TIM1_CH4 DMA2_ST4 -#define DEF_TIM_DMA_STR_0_TIM2_CH1 DMA1_ST5 -#define DEF_TIM_DMA_STR_0_TIM2_CH2 DMA1_ST6 -#define DEF_TIM_DMA_STR_0_TIM2_CH3 DMA1_ST1 -#define DEF_TIM_DMA_STR_0_TIM2_CH4 DMA1_ST7 -#define DEF_TIM_DMA_STR_1_TIM2_CH4 DMA1_ST6 +#define DEF_TIM_DMA_STR_0__TIM2_CH1 DMA1_ST5 +#define DEF_TIM_DMA_STR_0__TIM2_CH2 DMA1_ST6 +#define DEF_TIM_DMA_STR_0__TIM2_CH3 DMA1_ST1 +#define DEF_TIM_DMA_STR_0__TIM2_CH4 DMA1_ST7 +#define DEF_TIM_DMA_STR_1__TIM2_CH4 DMA1_ST6 -#define DEF_TIM_DMA_STR_0_TIM3_CH1 DMA1_ST4 -#define DEF_TIM_DMA_STR_0_TIM3_CH2 DMA1_ST5 -#define DEF_TIM_DMA_STR_0_TIM3_CH3 DMA1_ST7 -#define DEF_TIM_DMA_STR_0_TIM3_CH4 DMA1_ST2 +#define DEF_TIM_DMA_STR_0__TIM3_CH1 DMA1_ST4 +#define DEF_TIM_DMA_STR_0__TIM3_CH2 DMA1_ST5 +#define DEF_TIM_DMA_STR_0__TIM3_CH3 DMA1_ST7 +#define DEF_TIM_DMA_STR_0__TIM3_CH4 DMA1_ST2 -#define DEF_TIM_DMA_STR_0_TIM4_CH1 DMA1_ST0 -#define DEF_TIM_DMA_STR_0_TIM4_CH2 DMA1_ST4 -#define DEF_TIM_DMA_STR_0_TIM4_CH3 DMA1_ST7 -#define DEF_TIM_DMA_STR_0_TIM4_CH4 DMA1_ST3 +#define DEF_TIM_DMA_STR_0__TIM4_CH1 DMA1_ST0 +#define DEF_TIM_DMA_STR_0__TIM4_CH2 DMA1_ST4 +#define DEF_TIM_DMA_STR_0__TIM4_CH3 DMA1_ST7 +#define DEF_TIM_DMA_STR_0__TIM4_CH4 DMA1_ST3 -#define DEF_TIM_DMA_STR_0_TIM5_CH1 DMA1_ST2 -#define DEF_TIM_DMA_STR_0_TIM5_CH2 DMA1_ST4 -#define DEF_TIM_DMA_STR_0_TIM5_CH3 DMA1_ST0 -#define DEF_TIM_DMA_STR_0_TIM5_CH4 DMA1_ST1 -#define DEF_TIM_DMA_STR_1_TIM5_CH4 DMA1_ST3 +#define DEF_TIM_DMA_STR_0__TIM5_CH1 DMA1_ST2 +#define DEF_TIM_DMA_STR_0__TIM5_CH2 DMA1_ST4 +#define DEF_TIM_DMA_STR_0__TIM5_CH3 DMA1_ST0 +#define DEF_TIM_DMA_STR_0__TIM5_CH4 DMA1_ST1 +#define DEF_TIM_DMA_STR_1__TIM5_CH4 DMA1_ST3 -#define DEF_TIM_DMA_STR_0_TIM8_CH1 DMA2_ST2 -#define DEF_TIM_DMA_STR_1_TIM8_CH1 DMA2_ST2 -#define DEF_TIM_DMA_STR_0_TIM8_CH2 DMA2_ST3 -#define DEF_TIM_DMA_STR_1_TIM8_CH2 DMA2_ST2 -#define DEF_TIM_DMA_STR_0_TIM8_CH3 DMA2_ST2 -#define DEF_TIM_DMA_STR_0_TIM8_CH4 DMA2_ST7 +#define DEF_TIM_DMA_STR_0__TIM8_CH1 DMA2_ST2 +#define DEF_TIM_DMA_STR_1__TIM8_CH1 DMA2_ST2 +#define DEF_TIM_DMA_STR_0__TIM8_CH2 DMA2_ST3 +#define DEF_TIM_DMA_STR_1__TIM8_CH2 DMA2_ST2 +#define DEF_TIM_DMA_STR_0__TIM8_CH3 DMA2_ST2 +#define DEF_TIM_DMA_STR_0__TIM8_CH4 DMA2_ST7 /* F4 Channel Mappings */ -#define DEF_TIM_DMA_CHN_0_TIM1_CH1 DMA_Channel_0 -#define DEF_TIM_DMA_CHN_1_TIM1_CH1 DMA_Channel_6 -#define DEF_TIM_DMA_CHN_2_TIM1_CH1 DMA_Channel_6 -#define DEF_TIM_DMA_CHN_0_TIM1_CH2 DMA_Channel_0 -#define DEF_TIM_DMA_CHN_1_TIM1_CH2 DMA_Channel_6 -#define DEF_TIM_DMA_CHN_0_TIM1_CH3 DMA_Channel_0 -#define DEF_TIM_DMA_CHN_1_TIM1_CH3 DMA_Channel_6 -#define DEF_TIM_DMA_CHN_0_TIM1_CH4 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0__TIM1_CH1 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM1_CH1 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_2__TIM1_CH1 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0__TIM1_CH2 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM1_CH2 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0__TIM1_CH3 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM1_CH3 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0__TIM1_CH4 DMA_Channel_6 -#define DEF_TIM_DMA_CHN_0_TIM2_CH1 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_0_TIM2_CH2 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_0_TIM2_CH3 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_0_TIM2_CH4 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_1_TIM2_CH4 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM2_CH1 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM2_CH2 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM2_CH3 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM2_CH4 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_1__TIM2_CH4 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_0_TIM3_CH1 DMA_Channel_5 -#define DEF_TIM_DMA_CHN_0_TIM3_CH2 DMA_Channel_5 -#define DEF_TIM_DMA_CHN_0_TIM3_CH3 DMA_Channel_5 -#define DEF_TIM_DMA_CHN_0_TIM3_CH4 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0__TIM3_CH1 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0__TIM3_CH2 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0__TIM3_CH3 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0__TIM3_CH4 DMA_Channel_5 -#define DEF_TIM_DMA_CHN_0_TIM4_CH1 DMA_Channel_2 -#define DEF_TIM_DMA_CHN_0_TIM4_CH2 DMA_Channel_2 -#define DEF_TIM_DMA_CHN_0_TIM4_CH3 DMA_Channel_2 -#define DEF_TIM_DMA_CHN_0_TIM4_CH4 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0__TIM4_CH1 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0__TIM4_CH2 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0__TIM4_CH3 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0__TIM4_CH4 DMA_Channel_2 -#define DEF_TIM_DMA_CHN_0_TIM5_CH1 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_0_TIM5_CH2 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_0_TIM5_CH3 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_0_TIM5_CH4 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_1_TIM5_CH4 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM5_CH1 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM5_CH2 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM5_CH3 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM5_CH4 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_1__TIM5_CH4 DMA_Channel_3 -#define DEF_TIM_DMA_CHN_0_TIM8_CH1 DMA_Channel_0 -#define DEF_TIM_DMA_CHN_1_TIM8_CH1 DMA_Channel_7 -#define DEF_TIM_DMA_CHN_0_TIM8_CH2 DMA_Channel_0 -#define DEF_TIM_DMA_CHN_1_TIM8_CH2 DMA_Channel_7 -#define DEF_TIM_DMA_CHN_0_TIM8_CH3 DMA_Channel_0 -#define DEF_TIM_DMA_CHN_0_TIM8_CH4 DMA_Channel_7 +#define DEF_TIM_DMA_CHN_0__TIM8_CH1 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM8_CH1 DMA_Channel_7 +#define DEF_TIM_DMA_CHN_0__TIM8_CH2 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM8_CH2 DMA_Channel_7 +#define DEF_TIM_DMA_CHN_0__TIM8_CH3 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_0__TIM8_CH4 DMA_Channel_7 -#define DMA1_ST0_TYPE DMA1_Stream0 -#define DMA1_ST1_TYPE DMA1_Stream1 -#define DMA1_ST2_TYPE DMA1_Stream2 -#define DMA1_ST3_TYPE DMA1_Stream3 -#define DMA1_ST4_TYPE DMA1_Stream4 -#define DMA1_ST5_TYPE DMA1_Stream5 -#define DMA1_ST6_TYPE DMA1_Stream6 -#define DMA1_ST7_TYPE DMA1_Stream7 -#define DMA2_ST0_TYPE DMA2_Stream0 -#define DMA2_ST1_TYPE DMA2_Stream1 -#define DMA2_ST2_TYPE DMA2_Stream2 -#define DMA2_ST3_TYPE DMA2_Stream3 -#define DMA2_ST4_TYPE DMA2_Stream4 -#define DMA2_ST5_TYPE DMA2_Stream5 -#define DMA2_ST6_TYPE DMA2_Stream6 -#define DMA2_ST7_TYPE DMA2_Stream7 +#define DMA1_ST0_STREAM DMA1_Stream0 +#define DMA1_ST1_STREAM DMA1_Stream1 +#define DMA1_ST2_STREAM DMA1_Stream2 +#define DMA1_ST3_STREAM DMA1_Stream3 +#define DMA1_ST4_STREAM DMA1_Stream4 +#define DMA1_ST5_STREAM DMA1_Stream5 +#define DMA1_ST6_STREAM DMA1_Stream6 +#define DMA1_ST7_STREAM DMA1_Stream7 +#define DMA2_ST0_STREAM DMA2_Stream0 +#define DMA2_ST1_STREAM DMA2_Stream1 +#define DMA2_ST2_STREAM DMA2_Stream2 +#define DMA2_ST3_STREAM DMA2_Stream3 +#define DMA2_ST4_STREAM DMA2_Stream4 +#define DMA2_ST5_STREAM DMA2_Stream5 +#define DMA2_ST6_STREAM DMA2_Stream6 +#define DMA2_ST7_STREAM DMA2_Stream7 #endif /**** Common Defines across all targets ****/ -#define NONE 0 -#define DMA_NONE_TYPE NULL +#define DEF_TIM_CHAN(chan) DEF_CHAN_ ## chan +#define DEF_TIM_OUTPUT(chan, out) ( DEF_CHAN_ ## chan ## _OUTPUT | out ) + +#define DMA_NONE_CHANNEL NULL +#define DMA_NONE_STREAM NULL #define DMA_NONE_HANDLER 0 #define DEF_CHAN_CH1 TIM_Channel_1 From 2cacc9671010f38a01a0268dcacf0cf9cc21b618 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 16:31:36 +1100 Subject: [PATCH 169/188] Rename file, and fix F3 support. Added DSHOT support to DOGE (requires testing) --- src/main/drivers/{tim_def.h => timer_def.h} | 4 ++-- src/main/target/BLUEJAYF4/target.c | 2 +- src/main/target/DOGE/target.c | 21 +++++++++++---------- src/main/target/DOGE/target.h | 1 + 4 files changed, 15 insertions(+), 13 deletions(-) rename src/main/drivers/{tim_def.h => timer_def.h} (99%) diff --git a/src/main/drivers/tim_def.h b/src/main/drivers/timer_def.h similarity index 99% rename from src/main/drivers/tim_def.h rename to src/main/drivers/timer_def.h index e41260f86e..81d1c810f3 100644 --- a/src/main/drivers/tim_def.h +++ b/src/main/drivers/timer_def.h @@ -29,8 +29,8 @@ flags,\ (DEF_CHAN_ ## chan ## _OUTPUT | out),\ EXPAND(GPIO_AF__ ## pin ## _ ## tim ## _ ## chan),\ - CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _TYPE),\ - CONCAT(EXPAND(DEF_TIM_DMACHAN_ ## tim ## _ ## chan), _HANDLER)\ + CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _CHANNEL),\ + CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _HANDLER)\ } #define DEF_DMA_CHANNEL(tim, chan) CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _CHANNEL) diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index 6ab553f5fa..8bbbe806a9 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -22,7 +22,7 @@ #include "drivers/timer.h" #include "drivers/dma.h" -#include "drivers/tim_def.h" +#include "drivers/timer_def.h" /* const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 243de4dc18..92957613d5 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -22,16 +22,17 @@ #include "drivers/timer.h" #include "drivers/dma.h" +#include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6 , NULL, 0 }, // PWM1 - PA8 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM2 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM3 - PB9 - { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 }, // PMW4 - PA10 - { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 }, // PWM5 - PA9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0 }, // PWM7 - PA1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM8 - PB1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0 }, // PWM9 - PB0 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM9 - PB0 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0), // PWM1 - PA8 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1), // PWM2 - PB8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1), // PWM3 - PB9 + DEF_TIM(TIM2, CH4, PA10, TIM_USE_MOTOR, 1), // PMW4 - PA10 + DEF_TIM(TIM2, CH3, PA9, TIM_USE_MOTOR, 1), // PWM5 - PA9 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 1), // PWM6 - PA0 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1), // PWM7 - PA1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM8 - PB1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM9 - PB0 + DEF_TIM(TIM16, CH1, PA6, TIM_USE_LED, 1), // PWM9 - PB0 }; diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index ff03fbe1bd..ee10bfd70f 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -124,6 +124,7 @@ #define ENSURE_MPU_DATA_READY_IS_LOW #define LED_STRIP +#define USE_DSHOT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM From 97a83abab31107665148b726cd156c28cc3f2fd4 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 17:13:00 +1100 Subject: [PATCH 170/188] Added more targets for DSHOT --- src/main/drivers/timer_def.h | 33 ++++++++++++++++++++++++++++-- src/main/target/MOTOLAB/target.c | 21 ++++++++++--------- src/main/target/MOTOLAB/target.h | 1 + src/main/target/RMDO/target.c | 35 ++++++++++++++++---------------- src/main/target/RMDO/target.h | 1 + src/main/target/SOULF4/target.c | 25 ++++++++++++----------- src/main/target/SOULF4/target.h | 1 + src/main/target/SPARKY2/target.c | 24 ++++++++++++---------- src/main/target/SPARKY2/target.h | 3 ++- 9 files changed, 91 insertions(+), 53 deletions(-) diff --git a/src/main/drivers/timer_def.h b/src/main/drivers/timer_def.h index 81d1c810f3..1ddf0bc771 100644 --- a/src/main/drivers/timer_def.h +++ b/src/main/drivers/timer_def.h @@ -274,6 +274,20 @@ #define DEF_TIM_DMA_STR_0__TIM8_CH3 DMA2_ST2 #define DEF_TIM_DMA_STR_0__TIM8_CH4 DMA2_ST7 +#define DEF_TIM_DMA_STR_0__TIM9_CH1 DMA_NONE +#define DEF_TIM_DMA_STR_0__TIM9_CH2 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM10_CH1 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM11_CH1 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM12_CH1 DMA_NONE +#define DEF_TIM_DMA_STR_0__TIM12_CH2 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM13_CH1 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM14_CH1 DMA_NONE + /* F4 Channel Mappings */ #define DEF_TIM_DMA_CHN_0__TIM1_CH1 DMA_Channel_0 @@ -314,6 +328,20 @@ #define DEF_TIM_DMA_CHN_0__TIM8_CH3 DMA_Channel_0 #define DEF_TIM_DMA_CHN_0__TIM8_CH4 DMA_Channel_7 +#define DEF_TIM_DMA_CHN_0__TIM9_CH1 0 +#define DEF_TIM_DMA_CHN_0__TIM9_CH2 0 + +#define DEF_TIM_DMA_CHN_0__TIM10_CH1 0 + +#define DEF_TIM_DMA_CHN_0__TIM11_CH1 0 + +#define DEF_TIM_DMA_CHN_0__TIM12_CH1 0 +#define DEF_TIM_DMA_CHN_0__TIM12_CH2 0 + +#define DEF_TIM_DMA_CHN_0__TIM13_CH1 0 + +#define DEF_TIM_DMA_CHN_0__TIM14_CH1 0 + #define DMA1_ST0_STREAM DMA1_Stream0 #define DMA1_ST1_STREAM DMA1_Stream1 #define DMA1_ST2_STREAM DMA1_Stream2 @@ -334,12 +362,13 @@ #endif /**** Common Defines across all targets ****/ +#define DMA_NONE_CHANNEL NULL +#define DMA_NONE_STREAM NULL + #define DEF_TIM_CHAN(chan) DEF_CHAN_ ## chan #define DEF_TIM_OUTPUT(chan, out) ( DEF_CHAN_ ## chan ## _OUTPUT | out ) -#define DMA_NONE_CHANNEL NULL -#define DMA_NONE_STREAM NULL #define DMA_NONE_HANDLER 0 #define DEF_CHAN_CH1 TIM_Channel_1 diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index 4a9603edec..c9adecf16f 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -21,18 +21,19 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1 ), // PWM1 - PA4 - *TIM3_CH2 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1 ), // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1 ), // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1 ), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1 ), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0 ), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0 ), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index a5429e9683..a2739251a7 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -102,6 +102,7 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP +#define USE_DSHOT #define SPEKTRUM_BIND // USART2, PB4 diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index ebaeeaf24a..6e71f3cd0d 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -21,24 +21,25 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0}, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0 ), // RC_CH1 - PA0 - *TIM2_CH1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0 ), // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0 ), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0 ), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM, 0 ), // RC_CH5 - PB4 - *TIM3_CH1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0 ), // RC_CH6 - PB5 - *TIM3_CH2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0 ), // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0 ), // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MOTOR, 1 ), // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, 1 ), // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM4, CH1, PA11, TIM_USE_MOTOR, 1 ), // PWM3 - PA11 + DEF_TIM(TIM4, CH2, PA12, TIM_USE_MOTOR, 1 ), // PWM4 - PA12 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1 ), // PWM5 - PB8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1 ), // PWM6 - PB9 + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MOTOR, 1 ), // PWM7 - PA2 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM8 - PA3 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR | TIM_USE_LED, 1 ), // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 69d3575f58..658f6528e9 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -95,6 +95,7 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP +#define USE_DSHOT #undef GPS diff --git a/src/main/target/SOULF4/target.c b/src/main/target/SOULF4/target.c index f1aeb50ff6..3428d2e6a4 100644 --- a/src/main/target/SOULF4/target.c +++ b/src/main/target/SOULF4/target.c @@ -20,19 +20,20 @@ #include #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 , NULL, 0, 0 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 , NULL, 0, 0 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 , DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM (5th pin on FlexiIO port) + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1, 0 ), // S1_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1, 0 ), // S2_OUT + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 1, 0 ), // S3_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1, 0 ), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 1, 0 ), // S5_OUT - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR | TIM_USE_LED, 1, 0 ), // S6_OUT }; diff --git a/src/main/target/SOULF4/target.h b/src/main/target/SOULF4/target.h index 84752a82ce..410841e747 100644 --- a/src/main/target/SOULF4/target.h +++ b/src/main/target/SOULF4/target.h @@ -104,6 +104,7 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define LED_STRIP +#define USE_DSHOT #define SPEKTRUM_BIND // USART3, diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c index 22a00fa936..1bf8f23511 100644 --- a/src/main/target/SPARKY2/target.c +++ b/src/main/target/SPARKY2/target.c @@ -21,18 +21,20 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM8 }, // PPM IN - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S2_IN - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S5_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM, 0, 0 ), // S2_IN + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S3_IN - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S4_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S5_IN + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1, 0 ), // S1_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1, 0 ), // S2_OUT + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 1, 0 ), // S3_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1, 0 ), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 1, 0 ), // S5_OUT - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 1, 0 ), // S6_OUT }; diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 48a01c39a4..69a120a06b 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -32,10 +32,11 @@ #define BEEPER PC9 #define BEEPER_INVERTED - #define INVERTER PC6 #define INVERTER_USART USART6 +#define USE_DSHOT + // MPU9250 interrupt #define USE_EXTI #define MPU_INT_EXTI PC5 From 4f84f907df1dff431a98cf4c95eec454ec5eb41d Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 17:34:37 +1100 Subject: [PATCH 171/188] Adding DSHOT to EVO - needs testing. --- src/main/target/SPRACINGF3EVO/target.c | 25 +++++++++++++------------ src/main/target/SPRACINGF3EVO/target.h | 1 + 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 247082280f..526fa029e9 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -21,20 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM8, CH1, PA15, TIM_USE_PPM, 0 ), // PPM + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 1 ), // PWM1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1 ), // PWM2 + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MOTOR, 1 ), // PWM3 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM4 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1 ), // PWM5 + DEF_TIM(TIM3, CH2, PA7, TIM_USE_MOTOR, 1 ), // PWM6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // PWM7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // PWM8 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 1 ), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 1 ), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 1 ), // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 280af8c8a5..516d023dba 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -35,6 +35,7 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH +#define USE_DSHOT #define GYRO #define USE_GYRO_SPI_MPU6500 From 9df456cbd79dd2f29382a4103fb57a7ea0a16c09 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 17:40:01 +1100 Subject: [PATCH 172/188] Using defines from @boris.b. for remapping F3 T16 and 17 --- src/main/drivers/timer_def.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/timer_def.h b/src/main/drivers/timer_def.h index 1ddf0bc771..bfc1f1c7ed 100644 --- a/src/main/drivers/timer_def.h +++ b/src/main/drivers/timer_def.h @@ -70,7 +70,7 @@ #define DEF_TIM_DMA__TIM15_TRIG DMA1_CH5 #define DEF_TIM_DMA__TIM15_COM DMA1_CH5 -#ifdef DMA_TIMER16_REMAP +#ifdef REMAP_TIM16_DMA #define DEF_TIM_DMA__TIM16_CH1 DMA1_CH6 #define DEF_TIM_DMA__TIM16_UP DMA1_CH6 #else @@ -78,7 +78,7 @@ #define DEF_TIM_DMA__TIM16_UP DMA1_CH3 #endif -#ifdef DMA_TIMER17_REMAP +#ifdef REMAP_TIM17_DMA #define DEF_TIM_DMA__TIM17_CH1 DMA1_CH7 #define DEF_TIM_DMA__TIM17_UP DMA1_CH7 #else From 3fbe5cf8a643e59440fa661003d2a31993b91ffa Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 17:47:10 +1100 Subject: [PATCH 173/188] Fix NAZE LED hard fault --- src/main/drivers/light_ws2811strip_stm32f10x.c | 1 + src/main/drivers/light_ws2811strip_stm32f30x.c | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index e0bb9656fc..bb191b226f 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -90,6 +90,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) /* configure DMA */ /* DMA1 Channel6 Config */ + dmaChannel = timerHardware->dmaChannel; DMA_DeInit(dmaChannel); DMA_StructInit(&DMA_InitStructure); diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 0165241f53..5de0390dbf 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -97,10 +97,9 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) TIM_CtrlPWMOutputs(timer, ENABLE); - dmaChannel = timerHardware->dmaChannel; - /* configure DMA */ /* DMA1 Channel Config */ + dmaChannel = timerHardware->dmaChannel; DMA_DeInit(dmaChannel); DMA_StructInit(&DMA_InitStructure); From 7ea5c6ba420f47dd62ef7bd3e61139704f52a4df Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 9 Nov 2016 07:33:14 +0000 Subject: [PATCH 174/188] Added fine-grained TELEMETRY build #defines as per iNav --- src/main/target/common.h | 6 +++++ src/main/telemetry/telemetry.c | 47 +++++++++++++++++++++++++++++----- 2 files changed, 46 insertions(+), 7 deletions(-) diff --git a/src/main/target/common.h b/src/main/target/common.h index c6b1ecefa3..e5de703ab8 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -51,6 +51,11 @@ #define BLACKBOX #define GPS #define TELEMETRY +#define TELEMETRY_FRSKY +#define TELEMETRY_HOTT +#define TELEMETRY_IBUS +#define TELEMETRY_LTM +#define TELEMETRY_SMARTPORT #define USE_SERVOS #endif @@ -58,6 +63,7 @@ #define CMS #define USE_DASHBOARD #define USE_MSP_DISPLAYPORT +#define TELEMETRY_JETIEXBUS #define TELEMETRY_MAVLINK #else #define SKIP_CLI_COMMAND_HELP diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d4a1519825..305d4df229 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -23,6 +23,8 @@ #ifdef TELEMETRY +#include "common/utils.h" + #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" @@ -42,7 +44,6 @@ #include "telemetry/ltm.h" #include "telemetry/jetiexbus.h" #include "telemetry/mavlink.h" -#include "rx/jetiexbus.h" static telemetryConfig_t *telemetryConfig; @@ -53,13 +54,22 @@ void telemetryUseConfig(telemetryConfig_t *telemetryConfigToUse) void telemetryInit(void) { +#ifdef TELEMETRY_FRSKY initFrSkyTelemetry(telemetryConfig); +#endif +#ifdef TELEMETRY_HOTT initHoTTTelemetry(telemetryConfig); +#endif +#ifdef TELEMETRY_SMARTPORT initSmartPortTelemetry(telemetryConfig); +#endif +#ifdef TELEMETRY_LTM initLtmTelemetry(telemetryConfig); +#endif +#ifdef TELEMETRY_JETIEXBUS initJetiExBusTelemetry(telemetryConfig); - -#if defined(TELEMETRY_MAVLINK) +#endif +#ifdef TELEMETRY_MAVLINK initMAVLinkTelemetry(); #endif @@ -89,26 +99,49 @@ serialPort_t *telemetrySharedPort = NULL; void telemetryCheckState(void) { +#ifdef TELEMETRY_FRSKY checkFrSkyTelemetryState(); +#endif +#ifdef TELEMETRY_HOTT checkHoTTTelemetryState(); +#endif +#ifdef TELEMETRY_SMARTPORT checkSmartPortTelemetryState(); +#endif +#ifdef TELEMETRY_LTM checkLtmTelemetryState(); +#endif +#ifdef TELEMETRY_JETIEXBUS checkJetiExBusTelemetryState(); - -#if defined(TELEMETRY_MAVLINK) +#endif +#ifdef TELEMETRY_MAVLINK checkMAVLinkTelemetryState(); #endif } void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { +#ifdef TELEMETRY_FRSKY handleFrSkyTelemetry(rxConfig, deadband3d_throttle); +#else + UNUSED(rxConfig); + UNUSED(deadband3d_throttle); +#endif +#ifdef TELEMETRY_HOTT handleHoTTTelemetry(currentTime); +#else + UNUSED(currentTime); +#endif +#ifdef TELEMETRY_SMARTPORT handleSmartPortTelemetry(); +#endif +#ifdef TELEMETRY_LTM handleLtmTelemetry(); +#endif +#ifdef TELEMETRY_JETIEXBUS handleJetiExBusTelemetry(); - -#if defined(TELEMETRY_MAVLINK) +#endif +#ifdef TELEMETRY_MAVLINK handleMAVLinkTelemetry(); #endif } From b1844eb5f90bd2d8ea5eff8c50499acf5f99843e Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 18:46:01 +1100 Subject: [PATCH 175/188] Removed double up, and remove 4way interface so BEEBRAIN fits. --- src/main/drivers/light_ws2811strip.c | 2 -- src/main/target/NAZE/target.h | 2 ++ 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 15607cd346..ad10af7173 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -93,8 +93,6 @@ void ws2811LedStripInit(ioTag_t ioTag) const hsvColor_t hsv_white = { 0, 255, 255}; setStripColor(&hsv_white); ws2811UpdateStrip(); - - ws2811UpdateStrip(); } bool isWS2811LedStripReady(void) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index d1d09f6043..8a37117e10 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -157,7 +157,9 @@ // USART2, PA3 #define BIND_PIN PA3 +#if !defined(BEEBRAIN) #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM From 3b93caa3ab79e701a5cb9172eb4fd6c0c63892f1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 9 Nov 2016 08:20:51 +0000 Subject: [PATCH 176/188] Fix ANYFCF7 'unused' build warnings --- .../ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c | 3 +++ .../ST/STM32_USB_Device_Library/Core/Src/usbd_core.c | 6 ++++++ .../ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c | 1 + lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c | 1 + lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c | 4 ++++ lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c | 1 + lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c | 5 ++++- 7 files changed, 20 insertions(+), 1 deletion(-) diff --git a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index b2ca5f1648..d9befc711c 100644 --- a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -475,6 +475,7 @@ __ALIGN_BEGIN uint8_t USBD_CDC_OtherSpeedCfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIG static uint8_t USBD_CDC_Init (USBD_HandleTypeDef *pdev, uint8_t cfgidx) { + (void)cfgidx; uint8_t ret = 0; USBD_CDC_HandleTypeDef *hcdc; @@ -563,6 +564,7 @@ static uint8_t USBD_CDC_Init (USBD_HandleTypeDef *pdev, static uint8_t USBD_CDC_DeInit (USBD_HandleTypeDef *pdev, uint8_t cfgidx) { + (void)cfgidx; uint8_t ret = 0; /* Open EP IN */ @@ -663,6 +665,7 @@ static uint8_t USBD_CDC_Setup (USBD_HandleTypeDef *pdev, */ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum) { + (void)epnum; USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData; if(pdev->pClassData != NULL) diff --git a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c index 0158829c52..7ebb1873a1 100644 --- a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c +++ b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c @@ -212,6 +212,7 @@ USBD_StatusTypeDef USBD_Stop (USBD_HandleTypeDef *pdev) */ USBD_StatusTypeDef USBD_RunTestMode (USBD_HandleTypeDef *pdev) { + (void)pdev; return USBD_OK; } @@ -508,6 +509,8 @@ USBD_StatusTypeDef USBD_LL_SOF(USBD_HandleTypeDef *pdev) */ USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum) { + (void)pdev; + (void)epnum; return USBD_OK; } @@ -519,6 +522,8 @@ USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, uint8_t ep */ USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum) { + (void)pdev; + (void)epnum; return USBD_OK; } @@ -530,6 +535,7 @@ USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, uint8_t e */ USBD_StatusTypeDef USBD_LL_DevConnected(USBD_HandleTypeDef *pdev) { + (void)pdev; return USBD_OK; } diff --git a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c index 49330c667b..0f38e9af3e 100644 --- a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c +++ b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c @@ -716,6 +716,7 @@ void USBD_ParseSetupRequest(USBD_SetupReqTypedef *req, uint8_t *pdata) void USBD_CtlError( USBD_HandleTypeDef *pdev , USBD_SetupReqTypedef *req) { + (void)req; USBD_LL_StallEP(pdev , 0x80); USBD_LL_StallEP(pdev , 0); } diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c index 4cb8ca8c5d..64416398e0 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c @@ -756,6 +756,7 @@ HAL_StatusTypeDef HAL_ADCEx_MultiModeStop_DMA(ADC_HandleTypeDef* hadc) */ uint32_t HAL_ADCEx_MultiModeGetValue(ADC_HandleTypeDef* hadc) { + (void)hadc; /* Return the multi mode conversion value */ return ADC->CDR; } diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c index f761655d00..1e164b883a 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c @@ -2845,6 +2845,7 @@ HAL_StatusTypeDef HAL_I2C_DisableListen_IT(I2C_HandleTypeDef *hi2c) */ HAL_StatusTypeDef HAL_I2C_Master_Abort_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress) { + (void)DevAddress; if(hi2c->Mode == HAL_I2C_MODE_MASTER) { /* Process Locked */ @@ -3684,6 +3685,7 @@ static HAL_StatusTypeDef I2C_RequestMemoryRead(I2C_HandleTypeDef *hi2c, uint16_t */ static void I2C_ITAddrCplt(I2C_HandleTypeDef *hi2c, uint32_t ITFlags) { + (void)ITFlags; uint8_t transferdirection = 0; uint16_t slaveaddrcode = 0; uint16_t ownadd1code = 0; @@ -4254,6 +4256,7 @@ static void I2C_DMASlaveTransmitCplt(DMA_HandleTypeDef *hdma) /* No specific action, Master fully manage the generation of STOP condition */ /* Mean that this generation can arrive at any time, at the end or during DMA process */ /* So STOP condition should be manage through Interrupt treatment */ + (void)hdma; } /** @@ -4308,6 +4311,7 @@ static void I2C_DMASlaveReceiveCplt(DMA_HandleTypeDef *hdma) /* No specific action, Master fully manage the generation of STOP condition */ /* Mean that this generation can arrive at any time, at the end or during DMA process */ /* So STOP condition should be manage through Interrupt treatment */ + (void)hdma; } /** diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c index 9d59c0124f..6811622868 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c @@ -404,6 +404,7 @@ void HAL_PWR_DisableWakeUpPin(uint32_t WakeUpPinx) */ void HAL_PWR_EnterSLEEPMode(uint32_t Regulator, uint8_t SLEEPEntry) { + (void)Regulator; /* Check the parameters */ assert_param(IS_PWR_REGULATOR(Regulator)); assert_param(IS_PWR_SLEEP_ENTRY(SLEEPEntry)); diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c index 4776fb4e34..de97e1feb3 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c @@ -2118,7 +2118,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Start(TIM_HandleTypeDef *htim, uint32_t Outpu No need to enable the counter, it's enabled automatically by hardware (the counter starts in response to a stimulus and generate a pulse */ - + (void)OutputChannel; TIM_CCxChannelCmd(htim->Instance, TIM_CHANNEL_1, TIM_CCx_ENABLE); TIM_CCxChannelCmd(htim->Instance, TIM_CHANNEL_2, TIM_CCx_ENABLE); @@ -2150,6 +2150,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Stop(TIM_HandleTypeDef *htim, uint32_t Output if TIM_CHANNEL_1 is used as input, the TIM_CHANNEL_2 will be used as output in all combinations, the TIM_CHANNEL_1 and TIM_CHANNEL_2 should be disabled together */ + (void)OutputChannel; TIM_CCxChannelCmd(htim->Instance, TIM_CHANNEL_1, TIM_CCx_DISABLE); TIM_CCxChannelCmd(htim->Instance, TIM_CHANNEL_2, TIM_CCx_DISABLE); @@ -2187,6 +2188,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Start_IT(TIM_HandleTypeDef *htim, uint32_t Ou No need to enable the counter, it's enabled automatically by hardware (the counter starts in response to a stimulus and generate a pulse */ + (void)OutputChannel; /* Enable the TIM Capture/Compare 1 interrupt */ __HAL_TIM_ENABLE_IT(htim, TIM_IT_CC1); @@ -2218,6 +2220,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Start_IT(TIM_HandleTypeDef *htim, uint32_t Ou */ HAL_StatusTypeDef HAL_TIM_OnePulse_Stop_IT(TIM_HandleTypeDef *htim, uint32_t OutputChannel) { + (void)OutputChannel; /* Disable the TIM Capture/Compare 1 interrupt */ __HAL_TIM_DISABLE_IT(htim, TIM_IT_CC1); From 4ee932a6ce993a766436332010ff126b45902a34 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 20:03:06 +1100 Subject: [PATCH 177/188] Changed output of OC to be more inline with DSHOT - so the same flags can be utilised. --- src/main/drivers/pwm_output.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index b98367c53a..e4e04cbaa8 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -42,17 +42,18 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCStructInit(&TIM_OCInitStructure); - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + if (output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = value; - TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_High : TIM_OCPolarity_Low; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; timerOCInit(tim, channel, &TIM_OCInitStructure); timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable); From 0a1f940bcb926d998afd444c58c98d15efcf7d40 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 9 Nov 2016 20:07:13 +1100 Subject: [PATCH 178/188] Adjusted as per suggestion - could be other future brushed motor FCs based on NAZE --- src/main/target/NAZE/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 8a37117e10..f048215e40 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -157,7 +157,7 @@ // USART2, PA3 #define BIND_PIN PA3 -#if !defined(BEEBRAIN) +#if !defined(BRUSHED_MOTORS) #define USE_SERIAL_4WAY_BLHELI_INTERFACE #endif From ae3170bcd35c6850b82e02de0432ae1102f6a386 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 9 Nov 2016 10:43:06 +0100 Subject: [PATCH 179/188] Add N channel --- src/main/target/BETAFLIGHTF3/target.c | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 084c65576c..7c2631c5d4 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -24,16 +24,16 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // RC PPM - PB7 - TIM17_CH1N AF1, TIM4_CH2 AF2, TIM8_BKIN AF5, TIM3_CH4 AF10 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_PPM, TIMER_INPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // RC PPM - PB7 - TIM17_CH1N AF1, TIM4_CH2 AF2, TIM8_BKIN AF5, TIM3_CH4 AF10 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, TIMER_OUTPUT_ENABLED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP --requires resource remap with dshot-- }; From c5b9dd93684aa36f39fa600e6bc95594f9a63840 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 9 Nov 2016 10:47:29 +0100 Subject: [PATCH 180/188] Correct timer --- src/main/target/BETAFLIGHTF3/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 7c2631c5d4..9e0e5d964e 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -27,9 +27,9 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_PPM, TIMER_INPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // RC PPM - PB7 - TIM17_CH1N AF1, TIM4_CH2 AF2, TIM8_BKIN AF5, TIM3_CH4 AF10 { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 + { TIM1, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 From f35afc4586ddb9562349e3bfd714cc5d325094a7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 9 Nov 2016 11:26:02 +0100 Subject: [PATCH 181/188] Remap to new timer mapping macro --- src/main/drivers/timer_def.h | 1 + src/main/target/BETAFLIGHTF3/target.c | 24 ++++++++++++------------ 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/drivers/timer_def.h b/src/main/drivers/timer_def.h index bfc1f1c7ed..75029ccd0f 100644 --- a/src/main/drivers/timer_def.h +++ b/src/main/drivers/timer_def.h @@ -40,6 +40,7 @@ #define DEF_TIM_DMA__TIM1_CH1 DMA1_CH2 #define DEF_TIM_DMA__TIM1_CH2 DMA1_CH3 #define DEF_TIM_DMA__TIM1_CH4 DMA1_CH4 +#define DEF_TIM_DMA__TIM1_CH1N DMA1_CH2 #define DEF_TIM_DMA__TIM1_TRIG DMA1_CH4 #define DEF_TIM_DMA__TIM1_COM DMA1_CH4 #define DEF_TIM_DMA__TIM1_UP DMA1_CH5 diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 9e0e5d964e..7a1a904534 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -20,20 +20,20 @@ #include #include "drivers/io.h" + #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_PPM, TIMER_INPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // RC PPM - PB7 - TIM17_CH1N AF1, TIM4_CH2 AF2, TIM8_BKIN AF5, TIM3_CH4 AF10 - - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 - { TIM1, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, TIMER_OUTPUT_ENABLED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP --requires resource remap with dshot-- + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, TIMER_INPUT_ENABLED), // PPM IN + DEF_TIM(TIM16,CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM1 + DEF_TIM(TIM1, CH1N,PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM2 + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3 + DEF_TIM(TIM17,CH1, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM6 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM7 + DEF_TIM(TIM15,CH1, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM8 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED), // LED_STRIP --requires resource remap with dshot (remap to motor 5??)-- }; From c7ea30d425a2c7c8d3f739039d49893a7877d934 Mon Sep 17 00:00:00 2001 From: DTF UHF Date: Wed, 9 Nov 2016 11:16:04 -0500 Subject: [PATCH 182/188] Remap output timers to remove DMA conflict - should allow DSHOT to work. --- src/main/target/DOGE/target.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 92957613d5..19c10eefd4 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -26,10 +26,10 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0), // PWM1 - PA8 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1), // PWM2 - PB8 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1), // PWM3 - PB9 - DEF_TIM(TIM2, CH4, PA10, TIM_USE_MOTOR, 1), // PMW4 - PA10 - DEF_TIM(TIM2, CH3, PA9, TIM_USE_MOTOR, 1), // PWM5 - PA9 + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, 1), // PWM2 - PB8, DMA2ch5 + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, 1), // PWM3 - PB9, DMA2ch1 + DEF_TIM(TIM2, CH4, PA10, TIM_USE_MOTOR, 1), // PMW4 - PA10, DMA1ch7 + DEF_TIM(TIM2, CH3, PA9, TIM_USE_MOTOR, 1), // PWM5 - PA9, DMA1ch1 DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 1), // PWM6 - PA0 DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1), // PWM7 - PA1 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM8 - PB1 From 760a2b2e6724b953deac66f64fac23b415beca4e Mon Sep 17 00:00:00 2001 From: DTF UHF Date: Wed, 9 Nov 2016 13:25:29 -0500 Subject: [PATCH 183/188] Fix additional DMA conflict with DMA2. --- src/main/drivers/adc_stm32f30x.c | 7 +++++++ src/main/target/DOGE/target.h | 1 + 2 files changed, 8 insertions(+) diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 599ab4296e..f17fb892f1 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -40,7 +40,11 @@ const adcDevice_t adcHardware[] = { { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA1_Channel1 }, +#ifdef ADC24_DMA_REMAP + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 } +#else { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 } +#endif }; const adcTagMap_t adcTagMap[] = { @@ -134,6 +138,9 @@ void adcInit(drv_adc_config_t *init) if (device == ADCINVALID) return; +#ifdef ADC24_DMA_REMAP + SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_ADC2ADC4, ENABLE); +#endif adcDevice_t adc = adcHardware[device]; for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index ee10bfd70f..fd2ea81494 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -113,6 +113,7 @@ #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC #define ADC_INSTANCE ADC2 +#define ADC24_DMA_REMAP // moves ADC2 DMA from DMA2ch1 to DMA2ch3. #define VBAT_ADC_PIN PA4 #define CURRENT_METER_ADC_PIN PA5 From 446959e8df9d04d39db527f7413971c7e9d04939 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B6glund?= Date: Wed, 9 Nov 2016 21:28:14 +0100 Subject: [PATCH 184/188] Travis. Run both unit tests and target builds. (#1511) * Travis. Run both unit tests and target builds. --- .travis.sh | 16 +++++----------- .travis.yml | 13 +++++++++---- Makefile | 14 +++++++------- 3 files changed, 21 insertions(+), 22 deletions(-) diff --git a/.travis.sh b/.travis.sh index ad4274d112..0d6a3cdc1a 100755 --- a/.travis.sh +++ b/.travis.sh @@ -8,7 +8,6 @@ TARGET_FILE=obj/betaflight_${TARGET} TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/undefined} BUILDNAME=${BUILDNAME:=travis} TRAVIS_BUILD_NUMBER=${TRAVIS_BUILD_NUMBER:=undefined} -MAKEFILE="-f Makefile" CURL_BASEOPTS=( "--retry" "10" @@ -22,12 +21,8 @@ CURL_PUB_BASEOPTS=( "--form" "github_repo=${TRAVIS_REPO_SLUG}" "--form" "build_name=${BUILDNAME}" ) -# A hacky way of running the unit tests at the same time as the normal builds. -if [ $RUNTESTS ] ; then - cd ./src/test && make test - # A hacky way of building the docs at the same time as the normal builds. -elif [ $PUBLISHDOCS ] ; then +if [ $PUBLISHDOCS ] ; then if [ $PUBLISH_URL ] ; then # Patch Gimli to fix underscores_inside_words @@ -51,8 +46,8 @@ elif [ $PUBLISHMETA ] ; then fi elif [ $TARGET ] ; then + make $TARGET if [ $PUBLISH_URL ] ; then - make -j2 $MAKEFILE if [ -f ${TARGET_FILE}.bin ] ; then TARGET_FILE=${TARGET_FILE}.bin elif [ -f ${TARGET_FILE}.hex ] ; then @@ -64,10 +59,9 @@ elif [ $TARGET ] ; then curl -k "${CURL_BASEOPTS[@]}" "${CURL_PUB_BASEOPTS[@]}" --form "file=@${TARGET_FILE}" ${PUBLISH_URL} || true exit 0; - else - make -j2 $MAKEFILE fi -else -# No target specified, build all with very low verbosity. +elif [ $GOAL ] ; then + make V=0 $GOAL +else make V=0 all fi diff --git a/.travis.yml b/.travis.yml index 14b7fbbf43..5072324926 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,9 +1,11 @@ env: -# Specify target(s) to build, or none to build all. -# - RUNTESTS=True # - PUBLISHMETA=True # - PUBLISHDOCS=True +# Specify the main Mafile supported goals. + - GOAL=test + - GOAL=all +# Or specify targets to run. # - TARGET=AFROMINI # - TARGET=AIORACERF3 # - TARGET=AIR32 @@ -79,7 +81,11 @@ compiler: clang install: - make arm_sdk_install -before_script: tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version +before_script: + - tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version + - clang --version + - clang++ --version + script: ./.travis.sh cache: @@ -100,4 +106,3 @@ notifications: on_success: always # options: [always|never|change] default: always on_failure: always # options: [always|never|change] default: always on_start: always # options: [always|never|change] default: always - diff --git a/Makefile b/Makefile index ba4fd13707..1a8ea3a565 100644 --- a/Makefile +++ b/Makefile @@ -739,8 +739,8 @@ CCACHE := endif # Tool names -CC := $(CCACHE) $(ARM_SDK_PREFIX)gcc -CPP := $(CCACHE) $(ARM_SDK_PREFIX)g++ +CROSS_CC := $(CCACHE) $(ARM_SDK_PREFIX)gcc +CROSS_CXX := $(CCACHE) $(ARM_SDK_PREFIX)g++ OBJCOPY := $(ARM_SDK_PREFIX)objcopy SIZE := $(ARM_SDK_PREFIX)size @@ -833,25 +833,25 @@ $(TARGET_BIN): $(TARGET_ELF) $(TARGET_ELF): $(TARGET_OBJS) $(V1) echo Linking $(TARGET) - $(V1) $(CC) -o $@ $^ $(LDFLAGS) + $(V1) $(CROSS_CC) -o $@ $^ $(LDFLAGS) $(V0) $(SIZE) $(TARGET_ELF) # Compile $(OBJECT_DIR)/$(TARGET)/%.o: %.c $(V1) mkdir -p $(dir $@) $(V1) echo "%% $(notdir $<)" "$(STDOUT)" - $(V1) $(CC) -c -o $@ $(CFLAGS) $< + $(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $< # Assemble $(OBJECT_DIR)/$(TARGET)/%.o: %.s $(V1) mkdir -p $(dir $@) $(V1) echo "%% $(notdir $<)" "$(STDOUT)" - $(V1) $(CC) -c -o $@ $(ASFLAGS) $< + $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< $(OBJECT_DIR)/$(TARGET)/%.o: %.S $(V1) mkdir -p $(dir $@) $(V1) echo "%% $(notdir $<)" "$(STDOUT)" - $(V1) $(CC) -c -o $@ $(ASFLAGS) $< + $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< ## sample : Build all sample (travis) targets sample: $(SAMPLE_TARGETS) @@ -963,7 +963,7 @@ targets: ## test : run the cleanflight test suite ## junittest : run the cleanflight test suite, producing Junit XML result files. test junittest: - $(V0) cd src/test && $(MAKE) $@ || true + $(V0) cd src/test && $(MAKE) $@ # rebuild everything when makefile changes $(TARGET_OBJS) : Makefile From 9f79dcc10a9ec650a95b6a402f02a9a9ca5c23c6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 9 Nov 2016 21:29:07 +0100 Subject: [PATCH 185/188] Fix TIM inversion issue --- src/main/drivers/pwm_output_stm32f3xx.c | 4 +++- src/main/drivers/timer_def.h | 3 +++ src/main/target/KISSFC/target.c | 27 +++++++++++-------------- 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index d6e980b73e..58b785463d 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -20,6 +20,8 @@ #include "platform.h" +#include "build/debug.h" + #include "io.h" #include "timer.h" #include "pwm_output.h" @@ -150,7 +152,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; diff --git a/src/main/drivers/timer_def.h b/src/main/drivers/timer_def.h index 75029ccd0f..52a5eb1f78 100644 --- a/src/main/drivers/timer_def.h +++ b/src/main/drivers/timer_def.h @@ -41,6 +41,7 @@ #define DEF_TIM_DMA__TIM1_CH2 DMA1_CH3 #define DEF_TIM_DMA__TIM1_CH4 DMA1_CH4 #define DEF_TIM_DMA__TIM1_CH1N DMA1_CH2 +#define DEF_TIM_DMA__TIM1_CH2N DMA1_CH3 #define DEF_TIM_DMA__TIM1_TRIG DMA1_CH4 #define DEF_TIM_DMA__TIM1_COM DMA1_CH4 #define DEF_TIM_DMA__TIM1_UP DMA1_CH5 @@ -70,6 +71,7 @@ #define DEF_TIM_DMA__TIM15_UP DMA1_CH5 #define DEF_TIM_DMA__TIM15_TRIG DMA1_CH5 #define DEF_TIM_DMA__TIM15_COM DMA1_CH5 +#define DEF_TIM_DMA__TIM15_CH1N DMA1_CH5 #ifdef REMAP_TIM16_DMA #define DEF_TIM_DMA__TIM16_CH1 DMA1_CH6 @@ -94,6 +96,7 @@ #define DEF_TIM_DMA__TIM8_COM DMA2_CH2 #define DEF_TIM_DMA__TIM8_CH1 DMA2_CH3 #define DEF_TIM_DMA__TIM8_CH2 DMA2_CH5 +#define DEF_TIM_DMA__TIM8_CH2N DMA2_CH5 #define DMA1_CH1_CHANNEL DMA1_Channel1 diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index f276f5845e..a48fc93a02 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -19,23 +19,20 @@ #include #include "drivers/io.h" -#include "drivers/dma.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel3, DMA1_CH3_HANDLER }, - { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel5, DMA1_CH5_HANDLER }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, - - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, - //{ TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_10, NULL, 0}, - //{ TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_5, NULL, 0}, + DEF_TIM(TIM1, CH2N,PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM8, CH2N,PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM15,CH1N,PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM17,CH1, PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, TIMER_INPUT_ENABLED), }; - From 4cd3329f9aeba1c63c53dc70e3322a22da9387ba Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 9 Nov 2016 21:38:04 +0000 Subject: [PATCH 186/188] Spektrum RX fix --- src/main/rx/spektrum.c | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 0ca7f0b32d..9530450208 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -108,20 +108,16 @@ static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT]; uint8_t spektrumFrameStatus(void) { - uint8_t b; - uint16_t fade; - uint32_t current_secs; - if (!rcFrameComplete) { return RX_FRAME_PENDING; } - + rcFrameComplete = false; // Fetch the fade count - fade = (spekFrame[0] << 8) + spekFrame[1]; - current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC); - + const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1]; + const uint32_t current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC); + if (spek_fade_last_sec == 0) { // This is the first frame status received. spek_fade_last_sec_count = fade; @@ -140,12 +136,11 @@ uint8_t spektrumFrameStatus(void) spek_fade_last_sec_count = fade; spek_fade_last_sec = current_secs; } - - - for (b = 3; b < SPEK_FRAME_SIZE; b += 2) { - uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); + + for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) { + const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) { - if(rssi_channel != 0 && spekChannel != rssi_channel) { + if(rssi_channel == 0 || spekChannel != rssi_channel) { spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; } } From cf7e7f1dc3e3060225c54bf9e34cbea2d7f43fa9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Thu, 10 Nov 2016 09:00:42 +1100 Subject: [PATCH 187/188] LED strip F3 fix attempt. Fix for RESOURCE_INDEX issue on motors with DMA --- src/main/drivers/light_ws2811strip.c | 6 +++--- src/main/drivers/light_ws2811strip.h | 6 +++--- src/main/drivers/light_ws2811strip_stm32f30x.c | 7 +++---- src/main/drivers/light_ws2811strip_stm32f4xx.c | 3 +-- src/main/drivers/pwm_output_stm32f3xx.c | 2 +- src/main/drivers/pwm_output_stm32f4xx.c | 2 +- src/main/drivers/pwm_output_stm32f7xx.c | 8 ++++---- 7 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index ad10af7173..b61ea977a1 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -40,10 +40,10 @@ #include "io.h" #include "light_ws2811strip.h" -#if defined(STM32F4) || defined(STM32F7) -uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#else +#if defined(STM32F1) uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#else +uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #endif volatile uint8_t ws2811LedDataTransferInProgress = 0; diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 2a1f708e7f..1f65a58d09 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -67,9 +67,9 @@ void setStripColors(const hsvColor_t *colors); bool isWS2811LedStripReady(void); -#if defined(STM32F4) || defined(STM32F7) -extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#else +#if defined(STM32F1) extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#else +extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #endif extern volatile uint8_t ws2811LedDataTransferInProgress; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 7fe643af4e..854cc171ce 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -85,12 +85,11 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; TIM_OCInitStructure.TIM_Pulse = 0; TIM_OC1Init(timer, &TIM_OCInitStructure); TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable); @@ -109,8 +108,8 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index c7e01f7e2b..46671ef256 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -91,12 +91,11 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 58b785463d..72fc09043f 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -175,7 +175,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_Channel_TypeDef *channel = timerHardware->dmaChannel; - dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, motorIndex); + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); DMA_Cmd(channel, DISABLE); diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 9f732a4907..10e841f853 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -174,7 +174,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_Stream_TypeDef *stream = timerHardware->dmaStream; - dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, motorIndex); + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); DMA_Cmd(stream, DISABLE); diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index e192557c01..e7db17f738 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -182,8 +182,8 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motor->hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; motor->hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; motor->hdma_tim.Init.MemInc = DMA_MINC_ENABLE; - motor->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ; - motor->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ; + motor->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + motor->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; motor->hdma_tim.Init.Mode = DMA_NORMAL; motor->hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; motor->hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; @@ -202,7 +202,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t /* Link hdma_tim to hdma[x] (channelx) */ __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], motor->hdma_tim); - dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, motorIndex); + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); /* Initialize TIMx DMA handle */ @@ -218,7 +218,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH; TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; - TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; + TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; TIM_OCInitStructure.Pulse = 0; From 6aca7cfb0ea984c9673e41dc9930f24804cfadd5 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 10 Nov 2016 00:37:05 +0100 Subject: [PATCH 188/188] Add DSHOT to IMPULSERCF3 (tested) --- src/main/drivers/timer_def.h | 1 + src/main/target/IMPULSERCF3/target.c | 18 ++++++++++-------- src/main/target/IMPULSERCF3/target.h | 3 +++ 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/timer_def.h b/src/main/drivers/timer_def.h index 52a5eb1f78..71ebb89972 100644 --- a/src/main/drivers/timer_def.h +++ b/src/main/drivers/timer_def.h @@ -95,6 +95,7 @@ #define DEF_TIM_DMA__TIM8_TRIG DMA2_CH2 #define DEF_TIM_DMA__TIM8_COM DMA2_CH2 #define DEF_TIM_DMA__TIM8_CH1 DMA2_CH3 +#define DEF_TIM_DMA__TIM8_CH1N DMA2_CH3 #define DEF_TIM_DMA__TIM8_CH2 DMA2_CH5 #define DEF_TIM_DMA__TIM8_CH2N DMA2_CH5 diff --git a/src/main/target/IMPULSERCF3/target.c b/src/main/target/IMPULSERCF3/target.c index a5674d80de..d60b119917 100644 --- a/src/main/target/IMPULSERCF3/target.c +++ b/src/main/target/IMPULSERCF3/target.c @@ -19,16 +19,18 @@ #include #include "drivers/io.h" + #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0}, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0}, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0}, // PWM6 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER}, // LED_STRIP + DEF_TIM(TIM2, CH1,PA15, TIM_USE_PPM, TIMER_INPUT_ENABLED), // PPM IN + DEF_TIM(TIM8,CH2N, PB4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM1 + DEF_TIM(TIM17,CH1, PB5, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM4 + DEF_TIM(TIM16,CH1, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM5 + DEF_TIM(TIM17,CH1, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM6 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, TIMER_OUTPUT_ENABLED), // LED_STRIP }; diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index c8e6d50425..291605e0c4 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -46,6 +46,9 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 +#define USE_DSHOT +#define REMAP_TIM17_DMA + #define USE_VCP #define USE_UART1 #define USE_UART2