From e1ffd34d8f3575aae146e5a830068b4af4a1741c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 15 Feb 2017 07:07:51 +0000 Subject: [PATCH 01/20] Further work on making gyro reentrant --- src/main/sensors/gyro.c | 66 ++++++++++++++++++++--------------------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index ac374c7a14..30f1560c47 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -74,7 +74,13 @@ gyro_t gyro; STATIC_UNIT_TESTED gyroDev_t gyroDev0; static int16_t gyroTemperature0; -static uint16_t calibratingG = 0; +typedef struct gyroCalibration_s { + int32_t g[XYZ_AXIS_COUNT]; + stdev_t var[XYZ_AXIS_COUNT]; + uint16_t calibratingG; +} gyroCalibration_t; + +STATIC_UNIT_TESTED gyroCalibration_t gyroCalibration; static filterApplyFnPtr softLpfFilterApplyFn; static void *softLpfFilter[3]; @@ -416,12 +422,12 @@ void gyroInitFilters(void) bool isGyroCalibrationComplete(void) { - return calibratingG == 0; + return gyroCalibration.calibratingG == 0; } -static bool isOnFinalGyroCalibrationCycle(void) +static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) { - return calibratingG == 1; + return gyroCalibration->calibratingG == 1; } static uint16_t gyroCalculateCalibratingCycles(void) @@ -429,56 +435,52 @@ static uint16_t gyroCalculateCalibratingCycles(void) return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } -static bool isOnFirstGyroCalibrationCycle(void) +static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) { - return calibratingG == gyroCalculateCalibratingCycles(); + return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles(); } void gyroSetCalibrationCycles(void) { - calibratingG = gyroCalculateCalibratingCycles(); + gyroCalibration.calibratingG = gyroCalculateCalibratingCycles(); } -STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, uint8_t gyroMovementCalibrationThreshold) +STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold) { - static int32_t g[3]; - static stdev_t var[3]; - for (int axis = 0; axis < 3; axis++) { - // Reset g[axis] at start of calibration - if (isOnFirstGyroCalibrationCycle()) { - g[axis] = 0; - devClear(&var[axis]); + if (isOnFirstGyroCalibrationCycle(gyroCalibration)) { + gyroCalibration->g[axis] = 0; + devClear(&gyroCalibration->var[axis]); } // Sum up CALIBRATING_GYRO_CYCLES readings - g[axis] += gyroDev->gyroADC[axis]; - devPush(&var[axis], gyroDev->gyroADC[axis]); + gyroCalibration->g[axis] += gyroDev->gyroADC[axis]; + devPush(&gyroCalibration->var[axis], gyroDev->gyroADC[axis]); // Reset global variables to prevent other code from using un-calibrated data gyroDev->gyroADC[axis] = 0; gyroDev->gyroZero[axis] = 0; - if (isOnFinalGyroCalibrationCycle()) { - const float dev = devStandardDeviation(&var[axis]); + if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { + const float stddev = devStandardDeviation(&gyroCalibration->var[axis]); - DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(dev)); + DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev)); // check deviation and startover in case the model was moved - if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { + if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) { gyroSetCalibrationCycles(); return; } - gyroDev->gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); + gyroDev->gyroZero[axis] = (gyroCalibration->g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); } } - if (isOnFinalGyroCalibrationCycle()) { + if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics beeper(BEEPER_GYRO_CALIBRATED); } - calibratingG--; + gyroCalibration->calibratingG--; } @@ -493,14 +495,13 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev) #endif gyroDev->dataReady = false; // move gyro data into 32-bit variables to avoid overflows in calculations - gyroDev->gyroADC[X] = gyroDev->gyroADCRaw[X]; - gyroDev->gyroADC[Y] = gyroDev->gyroADCRaw[Y]; - gyroDev->gyroADC[Z] = gyroDev->gyroADCRaw[Z]; + gyroDev->gyroADC[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X]; + gyroDev->gyroADC[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y]; + gyroDev->gyroADC[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z]; alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroDev->gyroADC[axis] -= gyroDev->gyroZero[axis]; // scale gyro output to degrees per second float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale; gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); @@ -527,9 +528,9 @@ void gyroUpdate(void) } gyroDev0.dataReady = false; // move gyro data into 32-bit variables to avoid overflows in calculations - gyroDev0.gyroADC[X] = gyroDev0.gyroADCRaw[X]; - gyroDev0.gyroADC[Y] = gyroDev0.gyroADCRaw[Y]; - gyroDev0.gyroADC[Z] = gyroDev0.gyroADCRaw[Z]; + gyroDev0.gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X]; + gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y]; + gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z]; alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign); @@ -546,11 +547,10 @@ void gyroUpdate(void) debug[3] = (uint16_t)(micros() & 0xffff); #endif } else { - performGyroCalibration(&gyroDev0, gyroConfig()->gyroMovementCalibrationThreshold); + performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold); } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroDev0.gyroADC[axis] -= gyroDev0.gyroZero[axis]; // scale gyro output to degrees per second float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale; From 602b656b01881953c0ae1be2ebf2da505e49fa73 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 5 Apr 2017 17:32:00 +0900 Subject: [PATCH 02/20] Refactor CMS - Introduce 'page' - Handle more than 2 pages correctly --- src/main/cms/cms.c | 274 ++++++++++++++++------------- src/main/cms/cms.h | 6 + src/main/cms/cms_menu_builtin.c | 5 +- src/main/target/OMNIBUSF4/target.h | 3 + 4 files changed, 164 insertions(+), 124 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index f7e106a3e1..c90482600e 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -130,19 +130,20 @@ static displayPort_t *cmsDisplayPortSelectNext(void) static bool cmsInMenu = false; -STATIC_UNIT_TESTED const CMS_Menu *currentMenu; // Points to top entry of the current page +typedef struct cmsCtx_s { + const CMS_Menu *menu; // menu for this context + uint8_t page; // page in the menu + int8_t cursorRow; // cursorRow in the page +} cmsCtx_t; -// XXX Does menu backing support backing into second page??? - -static const CMS_Menu *menuStack[10]; // Stack to save menu transition -static uint8_t menuStackHistory[10];// cursorRow in a stacked menu +static cmsCtx_t menuStack[10]; static uint8_t menuStackIdx = 0; -static OSD_Entry *pageTop; // Points to top entry of the current page -static OSD_Entry *pageTopAlt; // Only 2 pages are allowed (for now) -static uint8_t maxRow; // Max row in the current page +static int8_t pageCount; // Number of pages in the current menu +static OSD_Entry *pageTop; // First entry for the current page +static uint8_t pageMaxRow; // Max row in the current page -static int8_t cursorRow; +static cmsCtx_t currentCtx; #ifdef CMS_MENU_DEBUG // For external menu content creators @@ -165,19 +166,54 @@ static CMS_Menu menuErr = { }; #endif +#define CMS_PAGE_DEBUG + +#ifdef CMS_PAGE_DEBUG +#define cmsPageDebug() { \ + debug[0] = pageCount; \ + debug[1] = currentCtx.page; \ + debug[2] = pageMaxRow; \ + debug[3] = currentCtx.cursorRow; } struct _dummy +#else +#define cmsPageDebug() +#endif + static void cmsUpdateMaxRow(displayPort_t *instance) { - maxRow = 0; + pageMaxRow = 0; for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) { - maxRow++; + pageMaxRow++; } - if (maxRow > MAX_MENU_ITEMS(instance)) { - maxRow = MAX_MENU_ITEMS(instance); + if (pageMaxRow > MAX_MENU_ITEMS(instance)) { + pageMaxRow = MAX_MENU_ITEMS(instance); } - maxRow--; + pageMaxRow--; +} + +static uint8_t cmsCursorAbsolute(displayPort_t *instance) +{ + return currentCtx.cursorRow + currentCtx.page * MAX_MENU_ITEMS(instance); +} + +static void cmsPageSelect(displayPort_t *instance, int8_t newpage) +{ + currentCtx.page = (newpage + pageCount) % pageCount; + pageTop = ¤tCtx.menu->entries[currentCtx.page * MAX_MENU_ITEMS(instance)]; + cmsUpdateMaxRow(instance); + displayClearScreen(instance); +} + +static void cmsPageNext(displayPort_t *instance) +{ + cmsPageSelect(instance, currentCtx.page + 1); +} + +static void cmsPagePrev(displayPort_t *instance) +{ + cmsPageSelect(instance, currentCtx.page - 1); } static void cmsFormatFloat(int32_t value, char *floatString) @@ -378,7 +414,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) uint8_t i; OSD_Entry *p; - uint8_t top = (pDisplay->rows - maxRow) / 2 - 1; + uint8_t top = (pDisplay->rows - pageMaxRow) / 2 - 1; // Polled (dynamic) value display denominator. @@ -397,17 +433,9 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) SET_PRINTLABEL(p); SET_PRINTVALUE(p); } - - if (i > MAX_MENU_ITEMS(pDisplay)) // max per page - { - pageTopAlt = pageTop + MAX_MENU_ITEMS(pDisplay); - if (pageTopAlt->type == OME_END) - pageTopAlt = NULL; - } - pDisplay->cleared = false; } else if (drawPolled) { - for (p = pageTop ; p <= pageTop + maxRow ; p++) { + for (p = pageTop ; p <= pageTop + pageMaxRow ; p++) { if (IS_DYNAMIC(p)) SET_PRINTVALUE(p); } @@ -415,19 +443,21 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) // Cursor manipulation - while ((pageTop + cursorRow)->type == OME_Label) // skip label - cursorRow++; + while ((pageTop + currentCtx.cursorRow)->type == OME_Label) // skip label + currentCtx.cursorRow++; - if (pDisplay->cursorRow >= 0 && cursorRow != pDisplay->cursorRow) { + cmsPageDebug(); + + if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) { room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, " "); } if (room < 30) return; - if (pDisplay->cursorRow != cursorRow) { - room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, cursorRow + top, " >"); - pDisplay->cursorRow = cursorRow; + if (pDisplay->cursorRow != currentCtx.cursorRow) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCtx.cursorRow + top, " >"); + pDisplay->cursorRow = currentCtx.cursorRow; } if (room < 30) @@ -459,46 +489,57 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) } } +static void cmsMenuCountPage(displayPort_t *pDisplay) +{ + OSD_Entry *p; + for (p = currentCtx.menu->entries; p->type != OME_END; p++); + pageCount = (p - currentCtx.menu->entries) / MAX_MENU_ITEMS(pDisplay) + 1; +} + long cmsMenuChange(displayPort_t *pDisplay, const void *ptr) { CMS_Menu *pMenu = (CMS_Menu *)ptr; - if (pMenu) { + if (!pMenu) + return 0; + #ifdef CMS_MENU_DEBUG - if (pMenu->GUARD_type != OME_MENU) { - // ptr isn't pointing to a CMS_Menu. - if (pMenu->GUARD_type <= OME_MAX) { - strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1); - } else { - strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1); - } - pMenu = &menuErr; + if (pMenu->GUARD_type != OME_MENU) { + // ptr isn't pointing to a CMS_Menu. + if (pMenu->GUARD_type <= OME_MAX) { + strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1); + } else { + strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1); } + pMenu = &menuErr; + } #endif + if (pMenu != currentCtx.menu) { // Stack the current menu and move to a new menu. - // The (pMenu == curretMenu) case occurs when reopening for display sw - if (pMenu != currentMenu) { - menuStack[menuStackIdx] = currentMenu; - cursorRow += pageTop - currentMenu->entries; // Convert cursorRow to absolute value - menuStackHistory[menuStackIdx] = cursorRow; - menuStackIdx++; + menuStack[menuStackIdx++] = currentCtx; - currentMenu = pMenu; - cursorRow = 0; + currentCtx.menu = pMenu; + currentCtx.cursorRow = 0; - if (pMenu->onEnter) - pMenu->onEnter(); - } + if (pMenu->onEnter) + pMenu->onEnter(); - pageTop = currentMenu->entries; - pageTopAlt = NULL; + cmsMenuCountPage(pDisplay); + cmsPageSelect(pDisplay, 0); + } else { + // The (pMenu == curretMenu) case occurs when reopening for display cycling + // currentCtx.cursorRow has been saved as absolute; convert it back to page + relative - displayClearScreen(pDisplay); - cmsUpdateMaxRow(pDisplay); + int8_t cursorAbs = currentCtx.cursorRow; + currentCtx.cursorRow = cursorAbs % MAX_MENU_ITEMS(pDisplay); + cmsMenuCountPage(pDisplay); + cmsPageSelect(pDisplay, cursorAbs / MAX_MENU_ITEMS(pDisplay)); } + cmsPageDebug(); + return 0; } @@ -506,29 +547,19 @@ STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay) { // Let onExit function decide whether to allow exit or not. - if (currentMenu->onExit && currentMenu->onExit(pageTop + cursorRow) < 0) + if (currentCtx.menu->onExit && currentCtx.menu->onExit(pageTop + currentCtx.cursorRow) < 0) return -1; - if (menuStackIdx) { - displayClearScreen(pDisplay); - menuStackIdx--; - currentMenu = menuStack[menuStackIdx]; - cursorRow = menuStackHistory[menuStackIdx]; + if (!menuStackIdx) + return 0; - // cursorRow is absolute offset of a focused entry when stacked. - // Convert it back to page and relative offset. + currentCtx = menuStack[--menuStackIdx]; - pageTop = currentMenu->entries; // Temporary for cmsUpdateMaxRow() - cmsUpdateMaxRow(pDisplay); + cmsMenuCountPage(pDisplay); - if (cursorRow > maxRow) { - // Cursor was in the second page. - pageTopAlt = currentMenu->entries; - pageTop = pageTopAlt + maxRow + 1; - cursorRow -= (maxRow + 1); - cmsUpdateMaxRow(pDisplay); // Update maxRow for the second page - } - } + cmsPageSelect(pDisplay, currentCtx.page); + + cmsPageDebug(); return 0; } @@ -541,12 +572,15 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void) if (!pCurrentDisplay) return; cmsInMenu = true; - currentMenu = &menuMain; + currentCtx = (cmsCtx_t){ &menuMain, 0, 0 }; DISABLE_ARMING_FLAG(OK_TO_ARM); } else { // Switch display displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); if (pNextDisplay != pCurrentDisplay) { + // DisplayPort has been changed. + // Convert cursorRow to absolute value + currentCtx.cursorRow = cmsCursorAbsolute(pCurrentDisplay); displayRelease(pCurrentDisplay); pCurrentDisplay = pNextDisplay; } else { @@ -554,7 +588,7 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void) } } displayGrab(pCurrentDisplay); // grab the display for use by the CMS - cmsMenuChange(pCurrentDisplay, currentMenu); + cmsMenuChange(pCurrentDisplay, currentCtx.menu); } static void cmsTraverseGlobalExit(const CMS_Menu *pMenu) @@ -571,39 +605,47 @@ static void cmsTraverseGlobalExit(const CMS_Menu *pMenu) } long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) -{ - if (ptr) { +{ + int exitType = (int)ptr; + switch (exitType) { + case CMS_EXIT_SAVE: + case CMS_EXIT_SAVEREBOOT: + + cmsTraverseGlobalExit(&menuMain); + + if (currentCtx.menu->onExit) + currentCtx.menu->onExit((OSD_Entry *)NULL); // Forced exit + + saveConfigAndNotify(); + break; + + case CMS_EXIT: + break; + } + + cmsInMenu = false; + + displayRelease(pDisplay); + currentCtx.menu = NULL; + + if (exitType == CMS_EXIT_SAVEREBOOT) { displayClearScreen(pDisplay); - displayWrite(pDisplay, 5, 3, "REBOOTING..."); + displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? - + stopMotors(); stopPwmAllMotors(); delay(200); - - cmsTraverseGlobalExit(&menuMain); - - if (currentMenu->onExit) - currentMenu->onExit((OSD_Entry *)NULL); // Forced exit - - saveConfigAndNotify(); - } - - cmsInMenu = false; - - displayRelease(pDisplay); - currentMenu = NULL; - - if (ptr) + systemReset(); - + } + ENABLE_ARMING_FLAG(OK_TO_ARM); - + return 0; } - // Stick/key detection and key codes #define IS_HI(X) (rcData[X] > 1750) @@ -626,7 +668,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) uint16_t res = BUTTON_TIME; OSD_Entry *p; - if (!currentMenu) + if (!currentCtx.menu) return res; if (key == KEY_MENU) { @@ -640,42 +682,32 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } if (key == KEY_DOWN) { - if (cursorRow < maxRow) { - cursorRow++; + if (currentCtx.cursorRow < pageMaxRow) { + currentCtx.cursorRow++; } else { - if (pageTopAlt) { // we have another page - displayClearScreen(pDisplay); - p = pageTopAlt; - pageTopAlt = pageTop; - pageTop = (OSD_Entry *)p; - cmsUpdateMaxRow(pDisplay); - } - cursorRow = 0; // Goto top in any case + cmsPageNext(pDisplay); + currentCtx.cursorRow = 0; // Goto top in any case } } if (key == KEY_UP) { - cursorRow--; + currentCtx.cursorRow--; - if ((pageTop + cursorRow)->type == OME_Label && cursorRow > 0) - cursorRow--; + // Skip non-title labels + if ((pageTop + currentCtx.cursorRow)->type == OME_Label && currentCtx.cursorRow > 0) + currentCtx.cursorRow--; - if (cursorRow == -1 || (pageTop + cursorRow)->type == OME_Label) { - if (pageTopAlt) { - displayClearScreen(pDisplay); - p = pageTopAlt; - pageTopAlt = pageTop; - pageTop = (OSD_Entry *)p; - cmsUpdateMaxRow(pDisplay); - } - cursorRow = maxRow; // Goto bottom in any case + if (currentCtx.cursorRow == -1 || (pageTop + currentCtx.cursorRow)->type == OME_Label) { + // Goto previous page + cmsPagePrev(pDisplay); + currentCtx.cursorRow = pageMaxRow; } } if (key == KEY_DOWN || key == KEY_UP) return res; - p = pageTop + cursorRow; + p = pageTop + currentCtx.cursorRow; switch (p->type) { case OME_Submenu: @@ -970,8 +1002,6 @@ void cmsHandler(timeUs_t currentTimeUs) } } -// Is initializing with menuMain better? -// Can it be done with the current main()? void cmsInit(void) { cmsDeviceCount = 0; diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index 8bd35c47b5..6667c3d3cb 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -18,3 +18,9 @@ void cmsUpdate(uint32_t currentTimeUs); #define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" #define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" #define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" + +// cmsMenuExit special ptr values +#define CMS_EXIT (0) +#define CMS_EXIT_SAVE (1) +#define CMS_EXIT_SAVEREBOOT (2) + diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 1827655268..72196e4a5c 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -135,8 +135,9 @@ static OSD_Entry menuMainEntries[] = #endif {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, {"MISC", OME_Submenu, cmsMenuChange, &cmsx_menuMisc, 0}, - {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, - {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT, 0}, + {"SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVEREBOOT, 0}, #ifdef CMS_MENU_DEBUG {"ERR SAMPLE", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0}, #endif diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 735a66982f..ec5577bac2 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -161,6 +161,9 @@ #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) + #define USE_SPI #define USE_SPI_DEVICE_1 From f15d5dd12c522d6caed15e57c1423f85b5a8a8e7 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 7 Apr 2017 12:40:51 +0900 Subject: [PATCH 03/20] More strict stick commands --- src/main/cms/cms.c | 11 +++++------ src/main/target/OMNIBUSF4/target.h | 4 ++-- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index c90482600e..933fddd1fb 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -556,7 +556,6 @@ STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay) currentCtx = menuStack[--menuStackIdx]; cmsMenuCountPage(pDisplay); - cmsPageSelect(pDisplay, currentCtx.page); cmsPageDebug(); @@ -911,19 +910,19 @@ void cmsUpdate(uint32_t currentTimeUs) if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { key = KEY_MENU; } - else if (IS_HI(PITCH)) { + else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_MID(ROLL) && IS_HI(PITCH)) { key = KEY_UP; } - else if (IS_LO(PITCH)) { + else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_MID(ROLL) && IS_LO(PITCH)) { key = KEY_DOWN; } - else if (IS_LO(ROLL)) { + else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_LO(ROLL) && IS_MID(PITCH)) { key = KEY_LEFT; } - else if (IS_HI(ROLL)) { + else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_HI(ROLL) && IS_MID(PITCH)) { key = KEY_RIGHT; } - else if (IS_HI(YAW) || IS_LO(YAW)) + else if ((IS_HI(YAW) || IS_LO(YAW)) && IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH)) { key = KEY_ESC; } diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index ec5577bac2..6b6e631a69 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -161,8 +161,8 @@ #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 -#define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +//#define USE_I2C +//#define I2C_DEVICE (I2CDEV_2) #define USE_SPI #define USE_SPI_DEVICE_1 From b10412ac9b83468a6292bae7e49a2a23f146ff63 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 7 Apr 2017 18:04:56 +0900 Subject: [PATCH 04/20] Tidy --- src/main/cms/cms.c | 33 ++++++++++++++++-------------- src/main/target/OMNIBUSF4/target.h | 3 --- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 933fddd1fb..d2e1cd7c51 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -500,8 +500,9 @@ long cmsMenuChange(displayPort_t *pDisplay, const void *ptr) { CMS_Menu *pMenu = (CMS_Menu *)ptr; - if (!pMenu) + if (!pMenu) { return 0; + } #ifdef CMS_MENU_DEBUG if (pMenu->GUARD_type != OME_MENU) { @@ -547,11 +548,13 @@ STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay) { // Let onExit function decide whether to allow exit or not. - if (currentCtx.menu->onExit && currentCtx.menu->onExit(pageTop + currentCtx.cursorRow) < 0) + if (currentCtx.menu->onExit && currentCtx.menu->onExit(pageTop + currentCtx.cursorRow) < 0) { return -1; + } - if (!menuStackIdx) + if (!menuStackIdx) { return 0; + } currentCtx = menuStack[--menuStackIdx]; @@ -609,39 +612,39 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) switch (exitType) { case CMS_EXIT_SAVE: case CMS_EXIT_SAVEREBOOT: - + cmsTraverseGlobalExit(&menuMain); - + if (currentCtx.menu->onExit) currentCtx.menu->onExit((OSD_Entry *)NULL); // Forced exit - + saveConfigAndNotify(); break; - + case CMS_EXIT: break; } - + cmsInMenu = false; - + displayRelease(pDisplay); currentCtx.menu = NULL; - + if (exitType == CMS_EXIT_SAVEREBOOT) { displayClearScreen(pDisplay); displayWrite(pDisplay, 5, 3, "REBOOTING..."); - + displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? - + stopMotors(); stopPwmAllMotors(); delay(200); - + systemReset(); } - + ENABLE_ARMING_FLAG(OK_TO_ARM); - + return 0; } diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 6b6e631a69..735a66982f 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -161,9 +161,6 @@ #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 -//#define USE_I2C -//#define I2C_DEVICE (I2CDEV_2) - #define USE_SPI #define USE_SPI_DEVICE_1 From 86f3eb5fbf15ddea82cc14465e4f53be5215ad86 Mon Sep 17 00:00:00 2001 From: Cheng Lin Date: Wed, 26 Apr 2017 16:13:50 -0400 Subject: [PATCH 05/20] Add PWM beeper to CL_RACINGF4 TARGET --- src/main/target/OMNIBUSF4/target.c | 1 + src/main/target/OMNIBUSF4/target.h | 8 ++++---- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 6f495b5f49..3eaaa8592d 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -45,6 +45,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 1), // S3_OUT D1_ST6 DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S4_OUT D1_ST1 #if defined(CL_RACINGF4) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT #elif defined(OMNIBUSF4SD) DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 735a66982f..43caa05073 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -41,6 +41,7 @@ //#define LED1 PB4 // Remove this at the next major release #define BEEPER PB4 #define BEEPER_INVERTED +#define BEEPER_PWM_HZ 3800 // Beeper PWM frequency in Hz #ifdef OMNIBUSF4SD #define INVERTER_PIN_UART6 PC8 // Omnibus F4 V3 and later @@ -190,7 +191,6 @@ #else //#define RSSI_ADC_PIN PA0 #endif - #define USE_ESC_SENSOR #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define AVOID_UART1_FOR_PWM_PPM @@ -209,13 +209,13 @@ #define TARGET_IO_PORTD BIT(2) #ifdef CL_RACINGF4 -#define USABLE_TIMER_CHANNEL_COUNT 6 -#define USED_TIMERS ( TIM_N(4) | TIM_N(8) ) +#define USABLE_TIMER_CHANNEL_COUNT 7 +#define USED_TIMERS ( TIM_N(3) | TIM_N(4) | TIM_N(4) | TIM_N(8) ) #else #ifdef OMNIBUSF4SD #define USABLE_TIMER_CHANNEL_COUNT 13 #else #define USABLE_TIMER_CHANNEL_COUNT 12 #endif -#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(9)) +#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(9)) #endif From b8f4092bc6e33d0f0e5acb674e2fabb0fa856f5e Mon Sep 17 00:00:00 2001 From: Cheng Lin Date: Thu, 27 Apr 2017 13:24:33 -0400 Subject: [PATCH 06/20] remove conflit PPM, UART1 on CL_RACINGF4, ADD pwm beeper feature for CL_RACINGF4 --- src/main/target/OMNIBUSF4/target.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 43caa05073..5363d8ef22 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -193,13 +193,12 @@ #endif #define USE_ESC_SENSOR #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define AVOID_UART1_FOR_PWM_PPM #if defined(CL_RACINGF4) #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD ) #else #define DEFAULT_FEATURES (FEATURE_OSD) +#define AVOID_UART1_FOR_PWM_PPM #endif - #define SPEKTRUM_BIND_PIN UART1_RX_PIN #define USE_SERIAL_4WAY_BLHELI_INTERFACE From 09819aba26be019c01f1a4d1c7e41c8d263ea407 Mon Sep 17 00:00:00 2001 From: Cheng Lin Date: Thu, 27 Apr 2017 13:28:40 -0400 Subject: [PATCH 07/20] delete a tab spacing in target.c for CL_RACINGF4 --- src/main/target/OMNIBUSF4/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 3eaaa8592d..7fce82b65a 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -45,7 +45,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 1), // S3_OUT D1_ST6 DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S4_OUT D1_ST1 #if defined(CL_RACINGF4) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM + DEF_TIM(TIM3, CH1, PB4, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT #elif defined(OMNIBUSF4SD) DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT From 22147105fb1ac956b17a9561494a5a6d61b3ed90 Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 23 Apr 2017 20:39:24 +0100 Subject: [PATCH 08/20] CF/BF - First cut of MSP current sensor. How it works: 1. Current Meter Source on FC is set to MSP/OSD Slave by user. 2. On reboot FC sends MSP_ANALOG to OSD Slave. 3. OSD Slave listens for incoming MSP as usual. 4. OSD responds to MSP as usual. 6. The FC recevies the data from the OSD Slave and updates the MSP Current Meter. --- src/main/fc/fc_msp.c | 32 ++++++++++++++--- src/main/fc/fc_msp.h | 2 ++ src/main/fc/fc_tasks.c | 11 +++++- src/main/io/displayport_msp.c | 2 +- src/main/io/osd_slave.c | 3 ++ src/main/io/osd_slave.h | 2 ++ src/main/msp/msp.h | 6 ++++ src/main/msp/msp_protocol.h | 2 +- src/main/msp/msp_serial.c | 23 +++++++++--- src/main/msp/msp_serial.h | 3 +- src/main/sensors/battery.c | 15 ++++++++ src/main/sensors/current.c | 49 +++++++++++++++++++++++++- src/main/sensors/current.h | 23 ++++++++++++ src/main/sensors/current_ids.h | 3 ++ src/main/target/SPRACINGF3/target.h | 2 ++ src/main/target/SPRACINGF3EVO/target.h | 2 ++ src/main/target/SPRACINGF4EVO/target.h | 1 + 17 files changed, 166 insertions(+), 15 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7f967bf710..1f06610296 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -792,7 +792,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro switch (cmdMSP) { case MSP_STATUS_EX: - sbufWriteU16(dst, 0); // task delta + sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL)); #ifdef USE_I2C sbufWriteU16(dst, i2cGetErrorCounter()); #else @@ -807,7 +807,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro break; case MSP_STATUS: - sbufWriteU16(dst, 0); // task delta + sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL)); #ifdef USE_I2C sbufWriteU16(dst, i2cGetErrorCounter()); #else @@ -1375,7 +1375,6 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) static mspResult_e mspOsdSlaveProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { UNUSED(cmdMSP); UNUSED(src); - // Nothing OSD SLAVE specific yet. return MSP_RESULT_ERROR; } #endif @@ -2145,8 +2144,31 @@ void mspFcProcessReply(mspPacket_t *reply) UNUSED(src); // potentially unused depending on compile options. switch (reply->cmd) { - case MSP_DISPLAYPORT: { +#ifndef OSD_SLAVE + case MSP_ANALOG: + { + uint8_t batteryVoltage = sbufReadU8(src); + uint16_t mAhDrawn = sbufReadU16(src); + uint16_t rssi = sbufReadU16(src); + uint16_t amperage = sbufReadU16(src); + + UNUSED(rssi); + UNUSED(batteryVoltage); + UNUSED(amperage); + UNUSED(mAhDrawn); + +#ifdef USE_MSP_CURRENT_METER + currentMeterMSPSet(amperage, mAhDrawn); +#endif + break; + } +#endif + #ifdef USE_OSD_SLAVE + case MSP_DISPLAYPORT: + { + osdSlaveIsLocked = true; // lock it as soon as a MSP_DISPLAYPORT message is received to prevent accidental CLI/DFU mode. + int subCmd = sbufReadU8(src); switch (subCmd) { @@ -2184,9 +2206,9 @@ void mspFcProcessReply(mspPacket_t *reply) osdSlaveDrawScreen(); } } -#endif break; } +#endif } } diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index c5ccb1f1c4..577969c349 100644 --- a/src/main/fc/fc_msp.h +++ b/src/main/fc/fc_msp.h @@ -33,3 +33,5 @@ void mspFcInit(void); void mspOsdSlaveInit(void); mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); void mspFcProcessReply(mspPacket_t *reply); + +void mspSerialProcessStreamSchedule(void); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a01e833546..a34c582f8f 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -23,6 +23,8 @@ #include "cms/cms.h" +#include "build/debug.h" + #include "common/axis.h" #include "common/color.h" #include "common/utils.h" @@ -81,6 +83,8 @@ #include "telemetry/telemetry.h" +#include "io/osd_slave.h" + #ifdef USE_BST void taskBstMasterProcess(timeUs_t currentTimeUs); #endif @@ -115,7 +119,12 @@ static void taskHandleSerial(timeUs_t currentTimeUs) return; } #endif - mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand, mspFcProcessReply); +#ifndef OSD_SLAVE + bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA; +#else + bool evaluateMspData = osdSlaveIsLocked ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;; +#endif + mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply); } void taskBatteryAlerts(timeUs_t currentTimeUs) diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index d977ca75ca..dc8088ef4c 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -58,7 +58,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len return 0; } #endif - return mspSerialPush(cmd, buf, len); + return mspSerialPush(cmd, buf, len, MSP_DIRECTION_REPLY); } static int heartbeat(displayPort_t *displayPort) diff --git a/src/main/io/osd_slave.c b/src/main/io/osd_slave.c index ff148e83e8..b8a6ff6ca0 100644 --- a/src/main/io/osd_slave.c +++ b/src/main/io/osd_slave.c @@ -40,6 +40,9 @@ //#define OSD_SLAVE_DEBUG +// when locked the system ignores requests to enter cli or bootloader mode via serial connection. +bool osdSlaveIsLocked = false; + static displayPort_t *osdDisplayPort; static void osdDrawLogo(int x, int y) diff --git a/src/main/io/osd_slave.h b/src/main/io/osd_slave.h index c7974f84dc..7d9e945afc 100644 --- a/src/main/io/osd_slave.h +++ b/src/main/io/osd_slave.h @@ -22,6 +22,8 @@ struct displayPort_s; +extern bool osdSlaveIsLocked; + // init void osdSlaveInit(struct displayPort_s *osdDisplayPort); diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index 5e6a14ac58..9a9ddb0f4b 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -26,10 +26,16 @@ typedef enum { MSP_RESULT_NO_REPLY = 0 } mspResult_e; +typedef enum { + MSP_DIRECTION_REPLY = 0, + MSP_DIRECTION_REQUEST = 1 +} mspDirection_e; + typedef struct mspPacket_s { sbuf_t buf; int16_t cmd; int16_t result; + uint8_t direction; } mspPacket_t; struct serialPort_s; diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index f8c8eadf18..14d0e606af 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -101,7 +101,6 @@ #define MSP_NAME 10 //out message Returns user set board name - betaflight #define MSP_SET_NAME 11 //in message Sets board name - betaflight - // // MSP commands for Cleanflight original features // @@ -312,3 +311,4 @@ #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration #define MSP_SET_4WAY_IF 245 //in message Sets 4way interface + diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 7a233ca876..08cb7d7e4a 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -27,12 +27,13 @@ #include "io/serial.h" +#include "drivers/system.h" + #include "msp/msp.h" #include "msp/msp_serial.h" static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; - static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort) { memset(mspPortToReset, 0, sizeof(mspPort_t)); @@ -138,7 +139,13 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) serialBeginWrite(msp->port); const int len = sbufBytesRemaining(&packet->buf); const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT; - uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd}; + uint8_t hdr[8] = { + '$', + 'M', + packet->result == MSP_RESULT_ERROR ? '!' : packet->direction == MSP_DIRECTION_REPLY ? '>' : '<', + mspLen, + packet->cmd + }; int hdrLen = 5; #define CHECKSUM_STARTPOS 3 // checksum starts from mspLen field if (len >= JUMBO_FRAME_SIZE_LIMIT) { @@ -165,6 +172,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), }, .cmd = -1, .result = 0, + .direction = MSP_DIRECTION_REPLY, }; uint8_t *outBufHead = reply.buf.ptr; @@ -172,6 +180,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, }, .cmd = msp->cmdMSP, .result = 0, + .direction = MSP_DIRECTION_REQUEST, }; mspPostProcessFnPtr mspPostProcessFn = NULL; @@ -182,7 +191,6 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr mspSerialEncode(msp, &reply); } - msp->c_state = MSP_IDLE; return mspPostProcessFn; } @@ -215,7 +223,9 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm if (!mspPort->port) { continue; } + mspPostProcessFnPtr mspPostProcessFn = NULL; + while (serialRxBytesWaiting(mspPort->port)) { const uint8_t c = serialRead(mspPort->port); @@ -231,9 +241,12 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm } else if (mspPort->packetType == MSP_PACKET_REPLY) { mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn); } + + mspPort->c_state = MSP_IDLE; break; // process one command at a time so as not to block. } } + if (mspPostProcessFn) { waitForSerialPortToFinishTransmitting(mspPort->port); mspPostProcessFn(mspPort->port); @@ -262,7 +275,7 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } -int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) +int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction) { int ret = 0; @@ -281,6 +294,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) .buf = { .ptr = data, .end = data + datalen, }, .cmd = cmd, .result = 0, + .direction = direction, }; ret = mspSerialEncode(mspPort, &push); @@ -288,6 +302,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) return ret; // return the number of bytes written } + uint32_t mspSerialTxBytesFree() { uint32_t ret = UINT32_MAX; diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index e6136420cc..0032937363 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -67,11 +67,10 @@ typedef struct mspPort_s { uint8_t inBuf[MSP_PORT_INBUF_SIZE]; } mspPort_t; - void mspSerialInit(void); bool mspSerialWaiting(void); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); -int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen); +int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction); uint32_t mspSerialTxBytesFree(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index ed289aaecb..a0e7af7956 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -76,9 +76,13 @@ static batteryState_e consumptionState; #ifdef USE_VIRTUAL_CURRENT_METER #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL #else +#ifdef USE_MSP_CURRENT_METER +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_MSP +#else #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE #endif #endif +#endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC @@ -302,6 +306,11 @@ void batteryInit(void) case CURRENT_METER_ESC: #ifdef ESC_SENSOR currentMeterESCInit(); +#endif + break; + case CURRENT_METER_MSP: +#ifdef USE_MSP_CURRENT_METER + currentMeterMSPInit(); #endif break; @@ -362,6 +371,12 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) currentMeterESCRefresh(lastUpdateAt); currentMeterESCReadCombined(¤tMeter); } +#endif + break; + case CURRENT_METER_MSP: +#ifdef USE_MSP_CURRENT_METER + currentMeterMSPRefresh(currentTimeUs); + currentMeterMSPRead(¤tMeter); #endif break; diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index 67b170fd32..da7654357c 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -56,12 +56,15 @@ const uint8_t currentMeterIds[] = { CURRENT_METER_ID_ESC_MOTOR_11, CURRENT_METER_ID_ESC_MOTOR_12, #endif +#ifdef USE_MSP_CURRENT_METER + CURRENT_METER_ID_MSP_1, +#endif }; const uint8_t supportedCurrentMeterCount = ARRAYLEN(currentMeterIds); // -// ADC/Virtual/ESC shared +// ADC/Virtual/ESC/MSP shared // void currentMeterReset(currentMeter_t *meter) @@ -225,6 +228,45 @@ void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter) } #endif + +#ifdef USE_MSP_CURRENT_METER +#include "common/streambuf.h" +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +currentMeterMSPState_t currentMeterMSPState; + +void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn) +{ + // We expect the FC's MSP_ANALOG response handler to call this function + currentMeterMSPState.amperage = amperage; + currentMeterMSPState.mAhDrawn = mAhDrawn; +} + +void currentMeterMSPInit(void) +{ + memset(¤tMeterMSPState, 0, sizeof(currentMeterMSPState_t)); +} + +void currentMeterMSPRefresh(timeUs_t currentTimeUs) +{ + // periodically request MSP_ANALOG + static timeUs_t streamRequestAt = 0; + if (cmp32(currentTimeUs, streamRequestAt) > 0) { + streamRequestAt = currentTimeUs + ((1000 * 1000) / 10); // 10hz + + mspSerialPush(MSP_ANALOG, NULL, 0, MSP_DIRECTION_REQUEST); + } +} + +void currentMeterMSPRead(currentMeter_t *meter) +{ + meter->amperageLatest = currentMeterMSPState.amperage; + meter->amperage = currentMeterMSPState.amperage; + meter->mAhDrawn = currentMeterMSPState.mAhDrawn; +} +#endif + // // API for current meters using IDs // @@ -241,6 +283,11 @@ void currentMeterRead(currentMeterId_e id, currentMeter_t *meter) currentMeterVirtualRead(meter); } #endif +#ifdef USE_MSP_CURRENT_METER + else if (id == CURRENT_METER_ID_MSP_1) { + currentMeterMSPRead(meter); + } +#endif #ifdef USE_ESC_SENSOR else if (id == CURRENT_METER_ID_ESC_COMBINED_1) { currentMeterESCReadCombined(meter); diff --git a/src/main/sensors/current.h b/src/main/sensors/current.h index 7da02fa6b5..48c0db2bab 100644 --- a/src/main/sensors/current.h +++ b/src/main/sensors/current.h @@ -17,6 +17,7 @@ #pragma once +#include "common/time.h" #include "current_ids.h" typedef enum { @@ -24,6 +25,7 @@ typedef enum { CURRENT_METER_ADC, CURRENT_METER_VIRTUAL, CURRENT_METER_ESC, + CURRENT_METER_MSP, CURRENT_METER_MAX = CURRENT_METER_ESC } currentMeterSource_e; @@ -48,6 +50,7 @@ typedef enum { CURRENT_SENSOR_VIRTUAL = 0, CURRENT_SENSOR_ADC, CURRENT_SENSOR_ESC, + CURRENT_SENSOR_MSP } currentSensor_e; @@ -93,6 +96,21 @@ typedef struct currentMeterESCState_s { int32_t amperage; // current read by current sensor in centiampere (1/100th A) } currentMeterESCState_t; + +// +// MSP +// + +typedef struct currentMeterMSPState_s { + int32_t mAhDrawn; // milliampere hours drawn from the battery since start + int32_t amperage; // current read by current sensor in centiampere (1/100th A) +} currentMeterMSPState_t; + + +// +// Current Meter API +// + void currentMeterReset(currentMeter_t *meter); void currentMeterADCInit(void); @@ -108,6 +126,11 @@ void currentMeterESCRefresh(int32_t lastUpdateAt); void currentMeterESCReadCombined(currentMeter_t *meter); void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter); +void currentMeterMSPInit(void); +void currentMeterMSPRefresh(timeUs_t currentTimeUs); +void currentMeterMSPRead(currentMeter_t *meter); +void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn); + // // API for reading current meters by id. // diff --git a/src/main/sensors/current_ids.h b/src/main/sensors/current_ids.h index 2f366c6f1f..f7bb3029e3 100644 --- a/src/main/sensors/current_ids.h +++ b/src/main/sensors/current_ids.h @@ -66,4 +66,7 @@ typedef enum { CURRENT_METER_ID_VIRTUAL_1 = 80, // 80-89 for virtual meters CURRENT_METER_ID_VIRTUAL_2, + CURRENT_METER_ID_MSP_1 = 90, // 90-99 for MSP meters + CURRENT_METER_ID_MSP_2, + } currentMeterId_e; diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 12542977eb..c1ab6f9830 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -180,6 +180,8 @@ #define USE_OSD_OVER_MSP_DISPLAYPORT #define USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED +#define USE_MSP_CURRENT_METER + #define USE_ESC_SENSOR #define REMAP_TIM17_DMA diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index c9884f42f5..f4acbf5b98 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -163,6 +163,8 @@ #define OSD #define USE_OSD_OVER_MSP_DISPLAYPORT +#define USE_MSP_CURRENT_METER + #undef USE_DASHBOARD #define TRANSPONDER diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index f4a0bd2d8a..b3c70e5bac 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -162,6 +162,7 @@ #define OSD #define USE_OSD_OVER_MSP_DISPLAYPORT +#define USE_MSP_CURRENT_METER #define LED_STRIP #define TRANSPONDER From a59a3283222eb5ac2d9333e8c8637414b7b0fe75 Mon Sep 17 00:00:00 2001 From: Hydra Date: Sat, 29 Apr 2017 21:01:41 +0100 Subject: [PATCH 09/20] CF/BF - Bump MSP protocol, required for MSP current sensor support. --- src/main/msp/msp_protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 14d0e606af..3574f99ea8 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 35 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) +#define API_VERSION_MINOR 36 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) #define API_VERSION_LENGTH 2 From 4c114db666e4208f24110ed6eb4af2a4354caf0a Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 1 May 2017 10:40:44 +0900 Subject: [PATCH 10/20] Fix pageCount calculation --- 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 d2e1cd7c51..b5600db43e 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -493,7 +493,7 @@ static void cmsMenuCountPage(displayPort_t *pDisplay) { OSD_Entry *p; for (p = currentCtx.menu->entries; p->type != OME_END; p++); - pageCount = (p - currentCtx.menu->entries) / MAX_MENU_ITEMS(pDisplay) + 1; + pageCount = (p - currentCtx.menu->entries - 1) / MAX_MENU_ITEMS(pDisplay) + 1; } long cmsMenuChange(displayPort_t *pDisplay, const void *ptr) From f4c21d06a96dfa73660a3762126baec6574fe41e Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 1 May 2017 11:56:25 +0900 Subject: [PATCH 11/20] Disabled page debugging --- src/main/cms/cms.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index b5600db43e..a910aeb204 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -21,6 +21,7 @@ CMS-displayPort separation by jflyper and martinbudden */ +//#define CMS_PAGE_DEBUG // For multi-page/menu debugging //#define CMS_MENU_DEBUG // For external menu content creators #include @@ -166,8 +167,6 @@ static CMS_Menu menuErr = { }; #endif -#define CMS_PAGE_DEBUG - #ifdef CMS_PAGE_DEBUG #define cmsPageDebug() { \ debug[0] = pageCount; \ From acdc429eb34078c9630043c69480f40d0f2e438d Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 2 May 2017 07:54:29 +0200 Subject: [PATCH 12/20] Get HAL timer struture copmletely initialized --- src/main/drivers/light_ws2811strip_hal.c | 1 + src/main/drivers/pwm_output.c | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index 1a501abbed..10930142bc 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -129,6 +129,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; TIM_OCInitStructure.Pulse = 0; TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH; + TIM_OCInitStructure.OCNPolarity = TIM_OCPOLARITY_HIGH; TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 53ae693e95..035688529d 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -59,11 +59,15 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; if (output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; + TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH: TIM_OCPOLARITY_LOW; TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW; } else { TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET; - TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; + TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; + TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET; + TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH; } TIM_OCInitStructure.Pulse = value; From 50ac13d79b331c7f45f9fff65c2c96f88625af83 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 2 May 2017 07:13:55 +0100 Subject: [PATCH 13/20] Minor blackbox code tidy --- src/main/blackbox/blackbox.c | 88 ++++++++------------- src/main/common/maths.c | 9 +++ src/main/common/maths.h | 1 + src/main/flight/failsafe.c | 2 +- src/test/unit/blackbox_encoding_unittest.cc | 13 +-- 5 files changed, 48 insertions(+), 65 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ba21a2acea..ab1da3e480 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -449,6 +449,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_NEVER: return false; + default: return false; } @@ -456,11 +457,8 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) static void blackboxBuildConditionCache(void) { - FlightLogFieldCondition cond; - blackboxConditionCache = 0; - - for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { + for (FlightLogFieldCondition cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { if (testBlackboxConditionUncached(cond)) { blackboxConditionCache |= 1 << cond; } @@ -765,10 +763,10 @@ static void loadSlowState(blackboxSlowState_t *slow) * If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations * since the field was last logged. */ -static void writeSlowFrameIfNeeded(bool allowPeriodicWrite) +static void writeSlowFrameIfNeeded(void) { // Write the slow frame peridocially so it can be recovered if we ever lose sync - bool shouldWrite = allowPeriodicWrite && blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL; + bool shouldWrite = blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL; if (shouldWrite) { loadSlowState(&slowHistory); @@ -790,15 +788,6 @@ static void writeSlowFrameIfNeeded(bool allowPeriodicWrite) } } -static int gcd(int num, int denom) -{ - if (denom == 0) { - return num; - } - - return gcd(denom, num % denom); -} - void blackboxValidateConfig(void) { if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0 @@ -1011,50 +1000,36 @@ static void writeGPSFrame(timeUs_t currentTimeUs) static void loadMainState(timeUs_t currentTimeUs) { blackboxMainState_t *blackboxCurrent = blackboxHistory[0]; - int i; blackboxCurrent->time = currentTimeUs; - for (i = 0; i < XYZ_AXIS_COUNT; i++) { + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_P[i] = axisPID_P[i]; - } - for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_I[i] = axisPID_I[i]; - } - for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_D[i] = axisPID_D[i]; + blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); + blackboxCurrent->accSmooth[i] = acc.accSmooth[i]; +#ifdef MAG + blackboxCurrent->magADC[i] = mag.magADC[i]; +#endif } - for (i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) { blackboxCurrent->rcCommand[i] = rcCommand[i]; } - for (i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); - } - - for (i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->accSmooth[i] = acc.accSmooth[i]; - } - - for (i = 0; i < 4; i++) { + for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) { blackboxCurrent->debug[i] = debug[i]; } const int motorCount = getMotorCount(); - for (i = 0; i < motorCount; i++) { + for (int i = 0; i < motorCount; i++) { blackboxCurrent->motor[i] = motor[i]; } blackboxCurrent->vbatLatest = getBatteryVoltageLatest(); blackboxCurrent->amperageLatest = getAmperageLatest(); -#ifdef MAG - for (i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->magADC[i] = mag.magADC[i]; - } -#endif - #ifdef BARO blackboxCurrent->BaroAlt = baro.BaroAlt; #endif @@ -1425,6 +1400,22 @@ static bool blackboxShouldLogIFrame(void) return blackboxPFrameIndex == 0; } +/* + * If the GPS home point has been updated, or every 128 I-frames (~10 seconds), write the + * GPS home position. + * + * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can + * still be interpreted correctly. + */ +STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void) +{ + if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1] + || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) { + return true; + } + return false; +} + // Called once every FC loop in order to keep track of how many FC loop iterations have passed static void blackboxAdvanceIterationTimers(void) { @@ -1447,7 +1438,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice. */ - writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes()); + if (blackboxIsOnlyLoggingIntraframes()) { + writeSlowFrameIfNeeded(); + } loadMainState(currentTimeUs); writeIntraframe(); @@ -1460,23 +1453,14 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream. * So only log slow frames during loop iterations where we log a main frame. */ - writeSlowFrameIfNeeded(true); + writeSlowFrameIfNeeded(); loadMainState(currentTimeUs); writeInterframe(); } #ifdef GPS if (feature(FEATURE_GPS)) { - /* - * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the - * GPS home position. - * - * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can - * still be interpreted correctly. - */ - if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1] - || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) { - + if (blackboxShouldLogGpsHomeFrame()) { writeGPSHomeFrame(); writeGPSFrame(currentTimeUs); } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0] @@ -1497,8 +1481,6 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) */ void blackboxUpdate(timeUs_t currentTimeUs) { - int i; - switch (blackboxState) { case BLACKBOX_STATE_STOPPED: if (ARMING_FLAG(ARMED)) { @@ -1526,7 +1508,7 @@ void blackboxUpdate(timeUs_t currentTimeUs) */ if (millis() > xmitState.u.startTime + 100) { if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) { - for (i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) { + for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) { blackboxWrite(blackboxHeader[xmitState.headerIndex]); blackboxHeaderBudget--; } diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 97fc286798..acfbf0cc7a 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -99,6 +99,15 @@ float acos_approx(float x) } #endif +int gcd(int num, int denom) +{ + if (denom == 0) { + return num; + } + + return gcd(denom, num % denom); +} + float powerf(float base, int exp) { float result = base; for (int count = 1; count < exp; count++) result *= base; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index a7839a4c4e..3cdf33705a 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -76,6 +76,7 @@ typedef union { fp_angles_def angles; } fp_angles_t; +int gcd(int num, int denom); float powerf(float base, int exp); int32_t applyDeadband(int32_t value, int32_t deadband); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 8d27688460..1527ed4fc3 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -88,7 +88,7 @@ void failsafeInit(void) return; } -failsafePhase_e failsafePhase() +failsafePhase_e failsafePhase(void) { return failsafeState.phase; } diff --git a/src/test/unit/blackbox_encoding_unittest.cc b/src/test/unit/blackbox_encoding_unittest.cc index 836b219ee9..652ae161a4 100644 --- a/src/test/unit/blackbox_encoding_unittest.cc +++ b/src/test/unit/blackbox_encoding_unittest.cc @@ -29,15 +29,6 @@ extern "C" { #include "drivers/serial.h" #include "io/serial.h" - - PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); - - PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, - .device = DEFAULT_BLACKBOX_DEVICE, - .rate_num = 1, - .rate_denom = 1, - .on_motor_test = 0 // default off - ); } #include "unittest_macros.h" @@ -115,7 +106,7 @@ void serialTestResetBuffers() serialWritePos = 0; } -TEST(BlackboxTest, Test1) +TEST(BlackboxEncodingTest, TestWriteUnsignedVB) { blackboxPort = &serialTestInstance; blackboxWriteUnsignedVB(0); @@ -127,6 +118,7 @@ TEST(BlackboxTest, Test1) // STUBS extern "C" { +PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); int32_t blackboxHeaderBudget; void mspSerialAllocatePorts(void) {} void blackboxWrite(uint8_t value) {serialWrite(blackboxPort, value);} @@ -140,5 +132,4 @@ int blackboxPrint(const char *s) const int length = pos - (uint8_t*)s; return length; } - } From c48826a7393cd15d54370ec7d0f8f83052e3d83a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 2 May 2017 07:40:11 +0100 Subject: [PATCH 14/20] Minor tidy of blackbox --- src/main/blackbox/blackbox.c | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ab1da3e480..c3dbc1433f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -763,7 +763,7 @@ static void loadSlowState(blackboxSlowState_t *slow) * If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations * since the field was last logged. */ -static void writeSlowFrameIfNeeded(void) +static bool writeSlowFrameIfNeeded(void) { // Write the slow frame peridocially so it can be recovered if we ever lose sync bool shouldWrite = blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL; @@ -786,6 +786,7 @@ static void writeSlowFrameIfNeeded(void) if (shouldWrite) { writeSlowFrame(); } + return shouldWrite; } void blackboxValidateConfig(void) @@ -821,6 +822,13 @@ void blackboxValidateConfig(void) } } +static void blackboxResetIterationTimers(void) +{ + blackboxIteration = 0; + blackboxPFrameIndex = 0; + blackboxIFrameIndex = 0; +} + /** * Start Blackbox logging if it is not already running. Intended to be called upon arming. */ @@ -850,11 +858,9 @@ static void blackboxStart(void) */ blackboxBuildConditionCache(); - blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); - blackboxIteration = 0; - blackboxPFrameIndex = 0; - blackboxIFrameIndex = 0; + blackboxResetIterationTimers(); /* * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when From 015d848d733daa77acb9484095c9bd0cfafeaa7b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 25 Apr 2017 14:41:45 +0100 Subject: [PATCH 15/20] Added TAG2_3SVARIABLE encoding --- src/main/blackbox/blackbox_encoding.c | 123 +++++++++++++++++++ src/main/blackbox/blackbox_encoding.h | 1 + src/main/blackbox/blackbox_fielddefs.h | 3 +- src/test/unit/blackbox_encoding_unittest.cc | 126 +++++++++++++++++++- 4 files changed, 251 insertions(+), 2 deletions(-) diff --git a/src/main/blackbox/blackbox_encoding.c b/src/main/blackbox/blackbox_encoding.c index dfa708e145..d0893bca81 100644 --- a/src/main/blackbox/blackbox_encoding.c +++ b/src/main/blackbox/blackbox_encoding.c @@ -231,6 +231,129 @@ void blackboxWriteTag2_3S32(int32_t *values) } } +/** + * Write a 2 bit tag followed by 3 signed fields of 2, 554, 877 or 32 bits + */ +int blackboxWriteTag2_3SVariable(int32_t *values) +{ + static const int FIELD_COUNT = 3; + enum { + BITS_2 = 0, + BITS_554 = 1, + BITS_877 = 2, + BITS_32 = 3 + }; + + enum { + BYTES_1 = 0, + BYTES_2 = 1, + BYTES_3 = 2, + BYTES_4 = 3 + }; + + + /* + * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes + * below: + * + * Selector possibilities + * + * 2 bits per field ss11 2233, + * 554 bits per field ss11 1112 2222 3333 + * 877 bits per field ss11 1111 1122 2222 2333 3333 + * 32 bits per field sstt tttt followed by fields of various byte counts + */ + int selector = BITS_2; + int selector2 = 0; + // Require more than 877 bits? + if (values[0] >= 256 || values[0] < -256 + || values[1] >= 128 || values[1] < -128 + || values[2] >= 128 || values[2] < -128) { + selector = BITS_32; + // Require more than 554 bits? + } else if (values[0] >= 16 || values[0] < -16 + || values[1] >= 16 || values[1] < -16 + || values[2] >= 8 || values[2] < -8) { + selector = BITS_877; + // Require more than 2 bits? + } else if (values[0] >= 2 || values[0] < -2 + || values[1] >= 2 || values[1] < -2 + || values[2] >= 2 || values[2] < -2) { + selector = BITS_554; + } + + switch (selector) { + case BITS_2: + blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03)); + break; + case BITS_554: + // 554 bits per field ss11 1112 2222 3333 + blackboxWrite((selector << 6) | ((values[0] & 0x1F) << 1) | ((values[1] & 0x1F) >> 4)); + blackboxWrite(((values[1] & 0x0F) << 4) | (values[2] & 0x0F)); + break; + case BITS_877: + // 877 bits per field ss11 1111 1122 2222 2333 3333 + blackboxWrite((selector << 6) | ((values[0] & 0xFF) >> 2)); + blackboxWrite(((values[0] & 0x03) << 6) | ((values[1] & 0x7F) >> 1)); + blackboxWrite(((values[1] & 0x01) << 7) | (values[2] & 0x7F)); + break; + case BITS_32: + /* + * Do another round to compute a selector for each field, assuming that they are at least 8 bits each + * + * Selector2 field possibilities + * 0 - 8 bits + * 1 - 16 bits + * 2 - 24 bits + * 3 - 32 bits + */ + selector2 = 0; + //Encode in reverse order so the first field is in the low bits: + for (int x = FIELD_COUNT - 1; x >= 0; x--) { + selector2 <<= 2; + + if (values[x] < 128 && values[x] >= -128) { + selector2 |= BYTES_1; + } else if (values[x] < 32768 && values[x] >= -32768) { + selector2 |= BYTES_2; + } else if (values[x] < 8388608 && values[x] >= -8388608) { + selector2 |= BYTES_3; + } else { + selector2 |= BYTES_4; + } + } + + //Write the selectors + blackboxWrite((selector << 6) | selector2); + + //And now the values according to the selectors we picked for them + for (int x = 0; x < FIELD_COUNT; x++, selector2 >>= 2) { + switch (selector2 & 0x03) { + case BYTES_1: + blackboxWrite(values[x]); + break; + case BYTES_2: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + break; + case BYTES_3: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + break; + case BYTES_4: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + blackboxWrite(values[x] >> 24); + break; + } + } + break; + } + return selector; +} + /** * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits. */ diff --git a/src/main/blackbox/blackbox_encoding.h b/src/main/blackbox/blackbox_encoding.h index 1293ed45ce..2cdb6d8388 100644 --- a/src/main/blackbox/blackbox_encoding.h +++ b/src/main/blackbox/blackbox_encoding.h @@ -27,6 +27,7 @@ void blackboxWriteSignedVBArray(int32_t *array, int count); void blackboxWriteSigned16VBArray(int16_t *array, int count); void blackboxWriteS16(int16_t value); void blackboxWriteTag2_3S32(int32_t *values); +int blackboxWriteTag2_3SVariable(int32_t *values); void blackboxWriteTag8_4S16(int32_t *values); void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); void blackboxWriteU32(int32_t value); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 3d34216c5a..b1b9af001e 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -97,7 +97,8 @@ typedef enum FlightLogFieldEncoding { FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6, FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7, FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8, - FLIGHT_LOG_FIELD_ENCODING_NULL = 9 // Nothing is written to the file, take value to be zero + FLIGHT_LOG_FIELD_ENCODING_NULL = 9, // Nothing is written to the file, take value to be zero + FLIGHT_LOG_FIELD_ENCODING_TAG2_3SVARIABLE = 10 } FlightLogFieldEncoding; typedef enum FlightLogFieldSign { diff --git a/src/test/unit/blackbox_encoding_unittest.cc b/src/test/unit/blackbox_encoding_unittest.cc index 652ae161a4..16ba59aa65 100644 --- a/src/test/unit/blackbox_encoding_unittest.cc +++ b/src/test/unit/blackbox_encoding_unittest.cc @@ -99,6 +99,7 @@ bool isSerialTransmitBufferEmpty(const serialPort_t *instance) void serialTestResetBuffers() { + blackboxPort = &serialTestInstance; memset(&serialReadBuffer, 0, sizeof(serialReadBuffer)); serialReadPos = 0; serialReadEnd = 0; @@ -108,7 +109,8 @@ void serialTestResetBuffers() TEST(BlackboxEncodingTest, TestWriteUnsignedVB) { - blackboxPort = &serialTestInstance; + serialTestResetBuffers(); + blackboxWriteUnsignedVB(0); EXPECT_EQ(0, serialWriteBuffer[0]); blackboxWriteUnsignedVB(128); @@ -116,6 +118,128 @@ TEST(BlackboxEncodingTest, TestWriteUnsignedVB) EXPECT_EQ(1, serialWriteBuffer[2]); } +TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS2) +{ + serialTestResetBuffers(); + uint8_t *buf = &serialWriteBuffer[0]; + int selector; + int32_t v[3]; + + // 2 bits per field ss11 2233, + v[0] = 0; + v[1] = 0; + v[2] = 0; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0, selector); + EXPECT_EQ(0, buf[0]); + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; + + v[0] = 1; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0x10, buf[0]); // 00010000 + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; + + v[0] = 1; + v[1] = 1; + v[2] = 1; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0, selector); + EXPECT_EQ(0x15, buf[0]); // 00010101 + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; + + v[0] = -1; + v[1] = -1; + v[2] = -1; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0, selector); + EXPECT_EQ(0x3F, buf[0]); // 00111111 + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; + + v[0] = -2; + v[1] = -2; + v[2] = -2; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0, selector); + EXPECT_EQ(0x2A, buf[0]); // 00101010 + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; +} + +TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS554) +{ + serialTestResetBuffers(); + uint8_t *buf = &serialWriteBuffer[0]; + int selector; + int32_t v[3]; + + // 554 bits per field ss11 1112 2222 3333 + // 5 bits per field [-16, 15], 4 bits per field [-8, 7] + v[0] = 15; + v[1] = 15; + v[2] = 7; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(1, selector); + EXPECT_EQ(0x5E, buf[0]); // 0101 1110 + EXPECT_EQ(0xF7, buf[1]); // 1111 0111 + EXPECT_EQ(0, buf[2]); // ensure next byte has not been written + buf += 2; + + v[0] = -16; + v[1] = -16; + v[2] = -8; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(1, selector); + EXPECT_EQ(0x61, buf[0]); // 0110 0001 + EXPECT_EQ(0x08, buf[1]); // 0000 1000 + EXPECT_EQ(0, buf[2]); // ensure next byte has not been written + buf += 2; + + v[0] = 7; + v[1] = 8; + v[2] = 5; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(1, selector); + EXPECT_EQ(0x4E, buf[0]); // 0100 1110 + EXPECT_EQ(0x85, buf[1]); // 1000 0101 + EXPECT_EQ(0, buf[2]); // ensure next byte has not been written + buf += 2; +} + +TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS887) +{ + serialTestResetBuffers(); + uint8_t *buf = &serialWriteBuffer[0]; + int selector; + int32_t v[3]; + + // 877 bits per field ss11 1111 1122 2222 2333 3333 + // 8 bits per field [-128, 127], 7 bits per field [-64, 63] + v[0] = 127; + v[1] = 63; + v[2] = 63; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(2, selector); + EXPECT_EQ(0x9F, buf[0]); // 1001 1111 + EXPECT_EQ(0xDF, buf[1]); // 1101 1111 + EXPECT_EQ(0xBF, buf[2]); // 1011 1111 + EXPECT_EQ(0, buf[3]); // ensure next byte has not been written + buf += 3; + + v[0] = -128; + v[1] = -64; + v[2] = -64; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(2, selector); + EXPECT_EQ(0xA0, buf[0]); // 1010 0000 + EXPECT_EQ(0x20, buf[1]); // 0010 0000 + EXPECT_EQ(0x40, buf[2]); // 0100 0000 + EXPECT_EQ(0, buf[3]); // ensure next byte has not been written + buf += 3; +} // STUBS extern "C" { PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); From 49b698f09e42527dcfc4841f532ba1b3c261ed45 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 30 Mar 2017 07:26:17 +0100 Subject: [PATCH 16/20] Base gyro calibration on raw data --- src/main/sensors/gyro.c | 36 +++++++++++++++++------------------- 1 file changed, 17 insertions(+), 19 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 0d550ee735..88e728c8c9 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -448,15 +448,13 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, gyroCalibrati if (isOnFirstGyroCalibrationCycle(gyroCalibration)) { gyroCalibration->g[axis] = 0; devClear(&gyroCalibration->var[axis]); + // gyroZero is set to zero until calibration complete + gyroDev->gyroZero[axis] = 0; } // Sum up CALIBRATING_GYRO_CYCLES readings - gyroCalibration->g[axis] += gyroDev->gyroADC[axis]; - devPush(&gyroCalibration->var[axis], gyroDev->gyroADC[axis]); - - // Reset global variables to prevent other code from using un-calibrated data - gyroDev->gyroADC[axis] = 0; - gyroDev->gyroZero[axis] = 0; + gyroCalibration->g[axis] += gyroDev->gyroADCRaw[axis]; + devPush(&gyroCalibration->var[axis], gyroDev->gyroADCRaw[axis]); if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { const float stddev = devStandardDeviation(&gyroCalibration->var[axis]); @@ -523,15 +521,8 @@ void gyroUpdate(void) return; } gyroDev0.dataReady = false; - // move gyro data into 32-bit variables to avoid overflows in calculations - gyroDev0.gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X]; - gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y]; - gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z]; - alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign); - - const bool calibrationComplete = isGyroCalibrationComplete(); - if (calibrationComplete) { + if (isGyroCalibrationComplete()) { #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) // SPI-based gyro so can read and update in ISR if (gyroConfig()->gyro_isr_update) { @@ -542,8 +533,20 @@ void gyroUpdate(void) #ifdef DEBUG_MPU_DATA_READY_INTERRUPT debug[3] = (uint16_t)(micros() & 0xffff); #endif + // move gyro data into 32-bit variables to avoid overflows in calculations + gyroDev0.gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X]; + gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y]; + gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z]; + + alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign); } else { performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold); + // Reset gyro values to zero to prevent other code from using uncalibrated data + gyro.gyroADCf[X] = 0.0f; + gyro.gyroADCf[Y] = 0.0f; + gyro.gyroADCf[Z] = 0.0f; + // still calibrating, so no need to further process gyro data + return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -561,11 +564,6 @@ void gyroUpdate(void) gyro.gyroADCf[axis] = gyroADCf; } - if (!calibrationComplete) { - gyroDev0.gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale); - gyroDev0.gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale); - gyroDev0.gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale); - } #ifdef USE_GYRO_DATA_ANALYSE gyroDataAnalyse(&gyroDev0, &gyro); #endif From 5f24cf5a700db6d470092169ee8dcf24b2548f3e Mon Sep 17 00:00:00 2001 From: Cheng Lin Date: Tue, 2 May 2017 21:34:24 -0400 Subject: [PATCH 17/20] change the spektrum_bind_pin to RX6 CL_RACINGF4 change the spektrum_bind_pin to UART6_RX_PIN for CL_RACINGF4 --- src/main/target/OMNIBUSF4/target.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 5363d8ef22..b0e88fafc9 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -195,11 +195,13 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #if defined(CL_RACINGF4) #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD ) +#define SPEKTRUM_BIND_PIN UART6_RX_PIN #else #define DEFAULT_FEATURES (FEATURE_OSD) #define AVOID_UART1_FOR_PWM_PPM -#endif #define SPEKTRUM_BIND_PIN UART1_RX_PIN +#endif + #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) From abd03ab558363f281ee2626de73ec74662bedea0 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 3 May 2017 17:16:58 +0200 Subject: [PATCH 18/20] Fixed compiler warning on unused blackboxShouldLogGpsHomeFrame function. --- src/main/blackbox/blackbox.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c3dbc1433f..397ebf9273 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1413,6 +1413,7 @@ static bool blackboxShouldLogIFrame(void) * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can * still be interpreted correctly. */ +#ifdef GPS STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void) { if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1] @@ -1421,6 +1422,7 @@ STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void) } return false; } +#endif // Called once every FC loop in order to keep track of how many FC loop iterations have passed static void blackboxAdvanceIterationTimers(void) From 934c5904f0cc327abb0a56760de985972a8a6458 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 3 May 2017 17:24:29 +0200 Subject: [PATCH 19/20] Fix SERIAL_PORT_COUNT on ANYFCM7 and NUCLEOF722. Compiler warning. --- src/main/target/ANYFCM7/target.h | 2 +- src/main/target/NUCLEOF722/target.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index 7845ab59de..9f9e278d94 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -86,7 +86,7 @@ #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART2, USART3, UART4, UART5, USART6, USART7, USART8, SOFTSERIAL x 2 +#define SERIAL_PORT_COUNT 9 //VCP, USART1, USART2, USART3, UART4, UART5, USART6, SOFTSERIAL x 2 #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/NUCLEOF722/target.h b/src/main/target/NUCLEOF722/target.h index bfbaa605a8..82e09fe950 100644 --- a/src/main/target/NUCLEOF722/target.h +++ b/src/main/target/NUCLEOF722/target.h @@ -93,7 +93,7 @@ #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 4 //VCP, USART2, USART3, UART4, UART5, USART6, USART7, USART8, SOFTSERIAL x 2 +#define SERIAL_PORT_COUNT 6 //VCP, USART2, USART3, UART4,SOFTSERIAL x 2 #define USE_SPI #define USE_SPI_DEVICE_1 From 6b22fca1a570e123665a9fcd5e90e70c9d69c47e Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 24 Apr 2017 13:48:46 +1200 Subject: [PATCH 20/20] Globally enable ESC sensor support for F4 and F7 MCUs. --- src/main/target/ANYFCF7/target.h | 2 -- src/main/target/BLUEJAYF4/target.h | 2 -- src/main/target/FURYF4/target.h | 2 -- src/main/target/FURYF7/target.h | 2 -- src/main/target/NERO/target.h | 1 - src/main/target/OMNIBUSF4/target.h | 2 +- src/main/target/REVO/target.h | 2 -- src/main/target/common_fc_pre.h | 2 ++ 8 files changed, 3 insertions(+), 12 deletions(-) diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index bbf000f326..72cfb6a43a 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -21,8 +21,6 @@ #define USBD_PRODUCT_STRING "AnyFCF7" -#define USE_ESC_SENSOR - #define LED0 PB7 #define LED1 PB6 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index a807a6f538..027eb69797 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -144,8 +144,6 @@ #define VBAT_ADC_PIN PC3 #define CURRENT_METER_ADC_PIN PC2 -#define USE_ESC_SENSOR - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index f0e39218fa..fe691c1a71 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -183,8 +183,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define USE_ESC_SENSOR - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 458aad7d42..26577662d8 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -21,8 +21,6 @@ #define USBD_PRODUCT_STRING "FuryF7" -#define USE_ESC_SENSOR - #define LED0 PB5 #define LED1 PB4 diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index 67234d1913..a5ae8e5336 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -120,7 +120,6 @@ #define USE_ADC #define VBAT_ADC_PIN PC3 -//#define USE_ESC_SENSOR #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index b0e88fafc9..5895cb8e2b 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -191,7 +191,7 @@ #else //#define RSSI_ADC_PIN PA0 #endif -#define USE_ESC_SENSOR + #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #if defined(CL_RACINGF4) #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD ) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 665cc54ffa..979ca7fc01 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -48,8 +48,6 @@ #endif -#define USE_ESC_SENSOR - #define LED0 PB5 #if defined(PODIUMF4) #define LED1 PB4 diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 0d2054ea0f..112471a92b 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -45,12 +45,14 @@ #ifdef STM32F4 #define USE_DSHOT +#define USE_ESC_SENSOR #define I2C3_OVERCLOCK true #define TELEMETRY_IBUS #endif #ifdef STM32F7 #define USE_DSHOT +#define USE_ESC_SENSOR #define I2C3_OVERCLOCK true #define I2C4_OVERCLOCK true #define TELEMETRY_IBUS