From 9fa0c3bb1e752b513890d333ac6d21b65cfbf06a Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Fri, 10 Apr 2020 09:51:38 -0400 Subject: [PATCH] Fix CMS task rate; correct cmsDeviceCount logic; coding style cleanup The CMS task was configured to run at 60hz but there was limiting logic in place to restrict it to every 50ms (20hz). Remove the extra logic and simply run the task at 20hz. There was incorrect logic that checked if `(cmsDeviceCount < 0)` to not run the task. Firstly `cmsDeviceCount` can never be < 0, and secondly the task should not run when it is 0 as that means there are no registered `displayPort` devices. Generally fix the logic to used `unsigned` for `cmsDeviceCount`. It can only ever be increaded (new device registered) and there is no mechanism to "unregister" a device. Lots of coding style cleanup. --- src/main/cms/cms.c | 98 ++++++++++++++++++++------------------------- src/main/fc/tasks.c | 2 +- 2 files changed, 45 insertions(+), 55 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 2ed813998..da5ca4209 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -48,33 +48,33 @@ #include "common/maths.h" #include "common/typeconversion.h" +#include "config/config.h" +#include "config/feature.h" + #include "drivers/system.h" #include "drivers/time.h" #include "drivers/motor.h" -// For rcData, stopAllMotors, stopPwmAllMotors -#include "config/feature.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" -#include "pg/rx.h" - -// For 'ARM' related -#include "config/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "flight/mixer.h" -// For VISIBLE* #include "io/rcdevice_cam.h" +#include "pg/pg.h" +#include "pg/pg_ids.h" +#include "pg/rx.h" +#ifdef USE_USB_CDC_HID +#include "pg/usb.h" +#endif + #include "osd/osd.h" #include "rx/rx.h" #ifdef USE_USB_CDC_HID #include "sensors/battery.h" -#include "pg/usb.h" #endif // DisplayPort management @@ -88,7 +88,7 @@ displayPort_t *pCurrentDisplay; static displayPort_t *cmsDisplayPorts[CMS_MAX_DEVICE]; -static int cmsDeviceCount; +static unsigned cmsDeviceCount; static int cmsCurrentDevice = -1; #ifdef USE_OSD static unsigned int osdProfileCursor = 1; @@ -98,8 +98,9 @@ int menuChainBack; bool cmsDisplayPortRegister(displayPort_t *pDisplay) { - if (cmsDeviceCount == CMS_MAX_DEVICE) + if (cmsDeviceCount >= CMS_MAX_DEVICE) { return false; + } cmsDisplayPorts[cmsDeviceCount++] = pDisplay; @@ -108,19 +109,22 @@ bool cmsDisplayPortRegister(displayPort_t *pDisplay) static displayPort_t *cmsDisplayPortSelectCurrent(void) { - if (cmsDeviceCount == 0) + if (cmsDeviceCount == 0) { return NULL; + } - if (cmsCurrentDevice < 0) + if (cmsCurrentDevice < 0) { cmsCurrentDevice = 0; + } return cmsDisplayPorts[cmsCurrentDevice]; } static displayPort_t *cmsDisplayPortSelectNext(void) { - if (cmsDeviceCount == 0) + if (cmsDeviceCount == 0) { return NULL; + } cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay @@ -129,10 +133,7 @@ static displayPort_t *cmsDisplayPortSelectNext(void) bool cmsDisplayPortSelect(displayPort_t *instance) { - if (cmsDeviceCount == 0) { - return false; - } - for (int i = 0; i < cmsDeviceCount; i++) { + for (unsigned i = 0; i < cmsDeviceCount; i++) { if (cmsDisplayPortSelectNext() == instance) { return true; } @@ -140,7 +141,6 @@ bool cmsDisplayPortSelect(displayPort_t *instance) return false; } -#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 @@ -289,15 +289,18 @@ static void cmsFormatFloat(int32_t value, char *floatString) // 03.450 // usuwam koncowe zera i kropke // Keep the first decimal place - for (k = 5; k > 3; k--) - if (floatString[k] == '0' || floatString[k] == '.') + for (k = 5; k > 3; k--) { + if (floatString[k] == '0' || floatString[k] == '.') { floatString[k] = 0; - else + } else { break; + } + } // oraz zero wiodonce - if (floatString[0] == '0') + if (floatString[0] == '0') { floatString[0] = ' '; + } } // CMS on OSD legacy was to use LEFT aligned values, not the RIGHT way ;-) @@ -311,8 +314,9 @@ static void cmsPadRightToSize(char *buf, int size) int i; for (i = 0 ; i < size ; i++) { - if (buf[i] == 0) + if (buf[i] == 0) { break; + } } for ( ; i < size ; i++) { @@ -683,8 +687,9 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) if (IS_PRINTVALUE(runtimeEntryFlags[i])) { bool selectedRow = i == currentCtx.cursorRow; room -= cmsDrawMenuEntry(pDisplay, p, top + i * linesPerMenuItem, selectedRow, &runtimeEntryFlags[i]); - if (room < 30) + if (room < 30) { return; + } } } } @@ -757,8 +762,9 @@ void cmsMenuOpen(void) if (!cmsInMenu) { // New open pCurrentDisplay = cmsDisplayPortSelectCurrent(); - if (!pCurrentDisplay) + if (!pCurrentDisplay) { return; + } cmsInMenu = true; currentCtx = (cmsCtx_t){ NULL, 0, 0 }; startMenu = &cmsx_menuMain; @@ -799,8 +805,7 @@ void cmsMenuOpen(void) maxMenuItems = pCurrentDisplay->rows - 2; } - if (pCurrentDisplay->useFullscreen) - { + if (pCurrentDisplay->useFullscreen) { leftMenuColumn = 0; rightMenuColumn = pCurrentDisplay->cols; maxMenuItems = pCurrentDisplay->rows; @@ -1033,8 +1038,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, cms_key_e key) if (*ptr->val < ptr->max) { *ptr->val += ptr->step; } - } - else { + } else { if (*ptr->val > ptr->min) { *ptr->val -= ptr->step; } @@ -1215,25 +1219,17 @@ static void cmsUpdate(uint32_t currentTimeUs) } else { if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { key = CMS_KEY_MENU; - } - else if (IS_HI(PITCH)) { + } else if (IS_HI(PITCH)) { key = CMS_KEY_UP; - } - else if (IS_LO(PITCH)) { + } else if (IS_LO(PITCH)) { key = CMS_KEY_DOWN; - } - else if (IS_LO(ROLL)) { + } else if (IS_LO(ROLL)) { key = CMS_KEY_LEFT; - } - else if (IS_HI(ROLL)) { + } else if (IS_HI(ROLL)) { key = CMS_KEY_RIGHT; - } - else if (IS_LO(YAW)) - { + } else if (IS_LO(YAW)) { key = CMS_KEY_ESC; - } - else if (IS_HI(YAW)) - { + } else if (IS_HI(YAW)) { key = CMS_KEY_SAVEMENU; } @@ -1274,8 +1270,9 @@ static void cmsUpdate(uint32_t currentTimeUs) // start calling handler multiple times. - if (repeatBase == 0) + if (repeatBase == 0) { repeatBase = holdCount; + } repeatCount = repeatCount + (holdCount - repeatBase) / 5; @@ -1305,14 +1302,7 @@ static void cmsUpdate(uint32_t currentTimeUs) void cmsHandler(timeUs_t currentTimeUs) { - if (cmsDeviceCount < 0) { - return; - } - - static timeUs_t lastCalledUs = 0; - - if (currentTimeUs >= lastCalledUs + CMS_UPDATE_INTERVAL_US) { - lastCalledUs = currentTimeUs; + if (cmsDeviceCount > 0) { cmsUpdate(currentTimeUs); } } diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index b103712b0..0df81eb6e 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -452,7 +452,7 @@ task_t tasks[TASK_COUNT] = { #endif #ifdef USE_CMS - [TASK_CMS] = DEFINE_TASK("CMS", NULL, NULL, cmsHandler, TASK_PERIOD_HZ(60), TASK_PRIORITY_LOW), + [TASK_CMS] = DEFINE_TASK("CMS", NULL, NULL, cmsHandler, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW), #endif #ifdef USE_VTX_CONTROL