From 37551d407b721ae9bfdafe39ecf57515835a6255 Mon Sep 17 00:00:00 2001 From: Michael <101.37584@germanynet.de> Date: Fri, 12 May 2023 19:55:40 +0200 Subject: [PATCH] Initial commit --- radio/src/gui/128x64/model_setup.cpp | 2 +- radio/src/gui/212x64/model_setup.cpp | 2 +- radio/src/gui/colorlcd/crossfire_settings.cpp | 4 +- radio/src/gui/colorlcd/radio_version.cpp | 17 ++-- radio/src/gui/common/stdlcd/radio_version.cpp | 3 +- radio/src/mixer_scheduler.cpp | 83 +++++++++++++++---- radio/src/mixer_scheduler.h | 40 +++++---- radio/src/tasks/mixer_task.cpp | 19 ++++- radio/src/tests/mixer_scheduler.cpp | 69 +++++++++++++++ 9 files changed, 189 insertions(+), 50 deletions(-) create mode 100644 radio/src/tests/mixer_scheduler.cpp diff --git a/radio/src/gui/128x64/model_setup.cpp b/radio/src/gui/128x64/model_setup.cpp index e31d2dbc73e..0cb71c1ec9d 100644 --- a/radio/src/gui/128x64/model_setup.cpp +++ b/radio/src/gui/128x64/model_setup.cpp @@ -1333,7 +1333,7 @@ void menuModelSetup(event_t event) case ITEM_MODEL_SETUP_EXTERNAL_MODULE_SERIALSTATUS: #endif lcdDrawText(INDENT_WIDTH, y, STR_STATUS); - lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, 1000000 / getMixerSchedulerPeriod(), LEFT | attr); + lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, 1000000 / getMixerSchedulerRealPeriod(moduleIdx), LEFT | attr); lcdDrawText(lcdNextPos, y, "Hz ", attr); // lcdDrawNumber(lcdNextPos, y, telemetryErrors, attr); // lcdDrawText(lcdNextPos + 1, y, "Err", attr); diff --git a/radio/src/gui/212x64/model_setup.cpp b/radio/src/gui/212x64/model_setup.cpp index 5d86a23f8c4..4e552b45215 100644 --- a/radio/src/gui/212x64/model_setup.cpp +++ b/radio/src/gui/212x64/model_setup.cpp @@ -1242,7 +1242,7 @@ void menuModelSetup(event_t event) case ITEM_MODEL_SETUP_EXTERNAL_MODULE_SERIALSTATUS: lcdDrawText(INDENT_WIDTH, y, STR_STATUS); - lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, 1000000 / getMixerSchedulerPeriod(), LEFT | attr); + lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, 1000000 / getMixerSchedulerRealPeriod(EXTERNAL_MODULE), LEFT | attr); lcdDrawText(lcdNextPos, y, "Hz ", attr); // lcdDrawNumber(lcdNextPos, y, telemetryErrors, attr); // lcdDrawText(lcdNextPos + 1, y, "Err", attr); diff --git a/radio/src/gui/colorlcd/crossfire_settings.cpp b/radio/src/gui/colorlcd/crossfire_settings.cpp index b7c3a64abc4..e4c89be3e86 100644 --- a/radio/src/gui/colorlcd/crossfire_settings.cpp +++ b/radio/src/gui/colorlcd/crossfire_settings.cpp @@ -61,9 +61,7 @@ CrossfireSettings::CrossfireSettings(Window* parent, const FlexGridLayout& g, new StaticText(line, rect_t{}, STR_STATUS, 0, COLOR_THEME_PRIMARY1); new DynamicText(line, rect_t{}, [=] { char msg[64] = ""; - // sprintf(msg, "%d Hz %" PRIu32 " Err", 1000000 / getMixerSchedulerPeriod(), - // telemetryErrors); - sprintf(msg, "%d Hz", 1000000 / getMixerSchedulerPeriod()); + snprintf(msg, 64, "%d Hz", 1000000 / getMixerSchedulerRealPeriod(moduleIdx)); return std::string(msg); }); } diff --git a/radio/src/gui/colorlcd/radio_version.cpp b/radio/src/gui/colorlcd/radio_version.cpp index 6c5f23c1d88..25376e50616 100644 --- a/radio/src/gui/colorlcd/radio_version.cpp +++ b/radio/src/gui/colorlcd/radio_version.cpp @@ -187,11 +187,10 @@ class VersionDialog : public Dialog #if defined(CROSSFIRE) // CRSF is able to provide status if (isModuleCrossfire(module)) { - char statusText[64]; + char statusText[64]; + + snprintf(statusText, 64, "%d Hz", 1000000 / getMixerSchedulerRealPeriod(module)); - auto hz = 1000000 / getMixerSchedulerPeriod(); - // snprintf(statusText, 64, "%d Hz %" PRIu32 " Err", hz, telemetryErrors); - snprintf(statusText, 64, "%d Hz", hz); status->setText(statusText); lv_obj_clear_flag(module_status_w->getLvObj(), LV_OBJ_FLAG_HIDDEN); } @@ -213,9 +212,12 @@ class VersionDialog : public Dialog #if defined(MULTIMODULE) // MPM is able to provide status if (isModuleMultimodule(module)) { - char statusText[64]; + char mpmStatusText[20]; + char statusText[40]; + + getMultiModuleStatus(module).getStatusString(mpmStatusText); + snprintf(statusText, 40, "%s %d Hz", mpmStatusText, 1000000 / getMixerSchedulerRealPeriod(module)); - getMultiModuleStatus(module).getStatusString(statusText); status->setText(statusText); lv_obj_clear_flag(module_status_w->getLvObj(), LV_OBJ_FLAG_HIDDEN); } @@ -248,6 +250,9 @@ class VersionDialog : public Dialog mod_ver += " "; mod_ver += variants[variant]; } + + snprintf(tmp, 20, " %d Hz", 1000000 / getMixerSchedulerRealPeriod(module)); + mod_ver += tmp; } status->setText(mod_ver); lv_obj_clear_flag(module_status_w->getLvObj(), LV_OBJ_FLAG_HIDDEN); diff --git a/radio/src/gui/common/stdlcd/radio_version.cpp b/radio/src/gui/common/stdlcd/radio_version.cpp index 74f730f49cd..d6fee62f9e3 100644 --- a/radio/src/gui/common/stdlcd/radio_version.cpp +++ b/radio/src/gui/common/stdlcd/radio_version.cpp @@ -162,8 +162,7 @@ void menuRadioModulesVersion(event_t event) #if defined(CROSSFIRE) if (isModuleCrossfire(module)) { char statusText[64] = ""; - sprintf(statusText, "%d Hz %zu Err", - 1000000 / getMixerSchedulerPeriod(), (size_t)0/*telemetryErrors*/); + snprintf(statusText, 64, "%d Hz", 1000000 / getMixerSchedulerRealPeriod(module)); lcdDrawText(COLUMN2_X, y, statusText); y += FH; continue; diff --git a/radio/src/mixer_scheduler.cpp b/radio/src/mixer_scheduler.cpp index 98da61da011..d3b5bb8ceda 100644 --- a/radio/src/mixer_scheduler.cpp +++ b/radio/src/mixer_scheduler.cpp @@ -49,42 +49,91 @@ bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs) #endif } -#if !defined(SIMU) - // Global trigger flag // Mixer schedule struct MixerSchedule { // period in us - volatile uint16_t period; + volatile uint16_t period = 0; + volatile uint16_t divider = 1; }; static MixerSchedule mixerSchedules[NUM_MODULES]; +static volatile uint8_t _syncedModule; +// Called from ISR uint16_t getMixerSchedulerPeriod() { -#if defined(HARDWARE_INTERNAL_MODULE) - if (mixerSchedules[INTERNAL_MODULE].period) { - return mixerSchedules[INTERNAL_MODULE].period; + bool hasActiveModules = false; + uint8_t synced_module = INTERNAL_MODULE; + uint16_t sync_period = MAX_REFRESH_RATE; + + // Find the fastest of the modules. It becomes + // master, i.e. the one getting synced + for(uint8_t module = 0; module < NUM_MODULES; module++) { + auto& sched = mixerSchedules[module]; + + if(!isModuleNone(module)) { + hasActiveModules = true; + + if(sched.period < sync_period) { + synced_module = module; + sync_period = sched.period; + } + } else + sched.period = MIXER_SCHEDULER_DEFAULT_PERIOD_US; } -#endif -#if defined(HARDWARE_EXTERNAL_MODULE) - if (mixerSchedules[EXTERNAL_MODULE].period) { - return mixerSchedules[EXTERNAL_MODULE].period; + + // Compute dividers for master and slave modules + for(uint8_t module = 0; module < NUM_MODULES; module++) { + auto& sched = mixerSchedules[module]; + + if(module == synced_module) + sched.divider = 1; + else + sched.divider = ((sched.period - 1) / sync_period) + 1; } -#endif + + _syncedModule = synced_module; + + // No active module + if(!hasActiveModules) { #if defined(STM32) && !defined(SIMU) - if (getSelectedUsbMode() == USB_JOYSTICK_MODE) { - return MIXER_SCHEDULER_JOYSTICK_PERIOD_US; - } + // no internal/external module and Joystick connected + if(getSelectedUsbMode() == USB_JOYSTICK_MODE) { + return MIXER_SCHEDULER_JOYSTICK_PERIOD_US; + } #endif - return MIXER_SCHEDULER_DEFAULT_PERIOD_US; + + return MIXER_SCHEDULER_DEFAULT_PERIOD_US; + } + + // at least one module is active + return sync_period; +} + +uint16_t getMixerSchedulerRealPeriod(uint8_t moduleIdx) { + return mixerSchedules[_syncedModule].period * getMixerSchedulerDivider(moduleIdx); +} + +uint16_t getMixerSchedulerDivider(uint8_t moduleIdx) { + return mixerSchedules[moduleIdx].divider; +} + +uint8_t getMixerSchedulerSyncedModule() { + return _syncedModule; } void mixerSchedulerInit() { - memset(mixerSchedules, 0, sizeof(mixerSchedules)); + _syncedModule = 0; + + // set default divider and period (for simu as sync not active) + for(uint8_t module = 0; module < NUM_MODULES; module++) { + mixerSchedules[module].period = MIXER_SCHEDULER_DEFAULT_PERIOD_US; + mixerSchedules[module].divider = 1; + } } void mixerSchedulerSetPeriod(uint8_t moduleIdx, uint16_t periodUs) @@ -104,6 +153,7 @@ uint16_t mixerSchedulerGetPeriod(uint8_t moduleIdx) return mixerSchedules[moduleIdx].period; } +#if !defined(SIMU) void mixerSchedulerISRTrigger() { BaseType_t xHigherPriorityTaskWoken = pdFALSE; @@ -123,5 +173,4 @@ void mixerSchedulerISRTrigger() called portEND_SWITCHING_ISR(). */ portYIELD_FROM_ISR( xHigherPriorityTaskWoken ); } - #endif diff --git a/radio/src/mixer_scheduler.h b/radio/src/mixer_scheduler.h index d7b10802ea1..75173603fbc 100644 --- a/radio/src/mixer_scheduler.h +++ b/radio/src/mixer_scheduler.h @@ -29,8 +29,6 @@ #define MIN_REFRESH_RATE 850 /* us */ #define MAX_REFRESH_RATE 50000 /* us */ -#if !defined(SIMU) - // Call once to initialize the mixer scheduler void mixerSchedulerInit(); @@ -58,27 +56,33 @@ void mixerSchedulerSoftTrigger(); // Fetch the current scheduling period uint16_t getMixerSchedulerPeriod(); +// fetch the mixer schedule divider +uint16_t getMixerSchedulerDivider(uint8_t moduleIdx); + +// Fetch the module index of the module responsible for synchro +uint8_t getMixerSchedulerSyncedModule(); + +// Fetch the real mixer task period +uint16_t getMixerSchedulerRealPeriod(uint8_t moduleIdx); + // Trigger mixer from an ISR void mixerSchedulerISRTrigger(); -#else +// Wait for the scheduler timer to trigger +// returns true if timeout, false otherwise +bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs); -#define mixerSchedulerInit() -#define mixerSchedulerStart() -#define mixerSchedulerStop() -#define mixerSchedulerSetPeriod(m,p) ((void)(p)) -#define mixerSchedulerGetPeriod(m) ((uint16_t)MIXER_SCHEDULER_DEFAULT_PERIOD_US) +#if !defined(SIMU) +// Configure and start the scheduler timer +void mixerSchedulerStart(); +// Enable the timer trigger +void mixerSchedulerEnableTrigger(); + +// Disable the timer trigger +void mixerSchedulerDisableTrigger(); +#else +#define mixerSchedulerStart() #define mixerSchedulerEnableTrigger() #define mixerSchedulerDisableTrigger() - -#define mixerSchedulerSoftTrigger() - -#define getMixerSchedulerPeriod() (MIXER_SCHEDULER_DEFAULT_PERIOD_US) -#define mixerSchedulerISRTrigger() - #endif - -// Wait for the scheduler timer to trigger -// returns true if timeout, false otherwise -bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs); diff --git a/radio/src/tasks/mixer_task.cpp b/radio/src/tasks/mixer_task.cpp index 962beea106f..b15fd7957cb 100644 --- a/radio/src/tasks/mixer_task.cpp +++ b/radio/src/tasks/mixer_task.cpp @@ -143,6 +143,8 @@ void execMixerFrequentActions() TASK_FUNCTION(mixerTask) { +static uint16_t syncCounter = 0; + #if defined(IMU) gyroInit(); #endif @@ -194,7 +196,21 @@ TASK_FUNCTION(mixerTask) mixerTaskLock(); doMixerCalculations(); - pulsesSendChannels(); + + syncCounter++; + + if(getMixerSchedulerSyncedModule() == EXTERNAL_MODULE) { + pulsesSendNextFrame(EXTERNAL_MODULE); + + if((syncCounter % getMixerSchedulerDivider(INTERNAL_MODULE)) == 0) + pulsesSendNextFrame(INTERNAL_MODULE); + } else { + pulsesSendNextFrame(INTERNAL_MODULE); + + if((syncCounter % getMixerSchedulerDivider(EXTERNAL_MODULE)) == 0) + pulsesSendNextFrame(EXTERNAL_MODULE); + } + doMixerPeriodicUpdates(); // TODO: what are these for??? @@ -219,7 +235,6 @@ TASK_FUNCTION(mixerTask) maxMixerDuration = t0; } } - TASK_RETURN(); } diff --git a/radio/src/tests/mixer_scheduler.cpp b/radio/src/tests/mixer_scheduler.cpp new file mode 100644 index 00000000000..60faabb6dd3 --- /dev/null +++ b/radio/src/tests/mixer_scheduler.cpp @@ -0,0 +1,69 @@ +/* + * Copyright (C) EdgeTX + * + * Based on code named + * opentx - https://github.com/opentx/opentx + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program 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. + */ + +#include "gtests.h" +#include "mixer_scheduler.h" + +TEST(MixerScheduler, MultiModules) +{ + // Init: both modules at 250Hz (4000us = MIXER_SCHEDULER_DEFAULT_PERIOD_US) + mixerSchedulerInit(); + g_model.moduleData[INTERNAL_MODULE].type = MODULE_TYPE_MULTIMODULE; + g_model.moduleData[EXTERNAL_MODULE].type = MODULE_TYPE_CROSSFIRE; + + EXPECT_EQ(getMixerSchedulerPeriod(), MIXER_SCHEDULER_DEFAULT_PERIOD_US); + EXPECT_EQ(getMixerSchedulerDivider(INTERNAL_MODULE), 1); + EXPECT_EQ(getMixerSchedulerDivider(EXTERNAL_MODULE), 1); + + // internal module 143Hz + // external module 500Hz + mixerSchedulerSetPeriod(INTERNAL_MODULE, 7000); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, 2000); + + EXPECT_EQ(getMixerSchedulerPeriod(), 2000); + EXPECT_EQ(getMixerSchedulerRealPeriod(INTERNAL_MODULE), 8000); + EXPECT_EQ(getMixerSchedulerRealPeriod(EXTERNAL_MODULE), 2000); + EXPECT_EQ(getMixerSchedulerDivider(INTERNAL_MODULE), 4); + EXPECT_EQ(getMixerSchedulerDivider(EXTERNAL_MODULE), 1); + + // internal module 143Hz + // external module 333Hz + mixerSchedulerSetPeriod(INTERNAL_MODULE, 7000); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, 3003); + + EXPECT_EQ(getMixerSchedulerPeriod(), 3003); + EXPECT_EQ(getMixerSchedulerRealPeriod(INTERNAL_MODULE), 9009); + EXPECT_EQ(getMixerSchedulerRealPeriod(EXTERNAL_MODULE), 3003); + EXPECT_EQ(getMixerSchedulerDivider(INTERNAL_MODULE), 3); + EXPECT_EQ(getMixerSchedulerDivider(EXTERNAL_MODULE), 1); + + + // internal module 143Hz + // external module 100Hz + mixerSchedulerSetPeriod(INTERNAL_MODULE, 7000); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, 10000); + + EXPECT_EQ(getMixerSchedulerPeriod(), 7000); + EXPECT_EQ(getMixerSchedulerRealPeriod(INTERNAL_MODULE), 7000); + EXPECT_EQ(getMixerSchedulerRealPeriod(EXTERNAL_MODULE), 14000); + EXPECT_EQ(getMixerSchedulerDivider(INTERNAL_MODULE), 1); + EXPECT_EQ(getMixerSchedulerDivider(EXTERNAL_MODULE), 2); +} \ No newline at end of file