diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 19f09d5097..61aafc165f 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -744,11 +744,6 @@ bool isAirmodeActivated() */ bool processRx(timeUs_t currentTimeUs) { - static bool armedBeeperOn = false; -#ifdef USE_TELEMETRY - static bool sharedPortTelemetryEnabled = false; -#endif - timeDelta_t frameAgeUs; timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs); @@ -891,6 +886,17 @@ bool processRx(timeUs_t currentTimeUs) } #endif + return true; +} + +void processRxModes(timeUs_t currentTimeUs) +{ + static bool armedBeeperOn = false; +#ifdef USE_TELEMETRY + static bool sharedPortTelemetryEnabled = false; +#endif + const throttleStatus_e throttleStatus = calculateThrottleStatus(); + // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough @@ -1082,8 +1088,6 @@ bool processRx(timeUs_t currentTimeUs) #endif pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || featureIsEnabled(FEATURE_ANTI_GRAVITY)); - - return true; } static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) diff --git a/src/main/fc/core.h b/src/main/fc/core.h index 4072c0e53e..d955dfbe28 100644 --- a/src/main/fc/core.h +++ b/src/main/fc/core.h @@ -76,6 +76,7 @@ void disarm(flightLogDisarmReason_e reason); void tryArm(void); bool processRx(timeUs_t currentTimeUs); +void processRxModes(timeUs_t currentTimeUs); void updateArmingStatus(void); void taskGyroSample(timeUs_t currentTimeUs); diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 06eab31d18..b5a0f4120c 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -158,23 +158,54 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs) } #endif +static enum { + CHECK, PROCESS, MODES, UPDATE +} rxState = CHECK; + +bool taskUpdateRxMainInProgress() +{ + return (rxState != CHECK); +} + static void taskUpdateRxMain(timeUs_t currentTimeUs) { - if (!processRx(currentTimeUs)) { - return; - } + switch (rxState) { + default: + case CHECK: + ignoreTaskTime(); + rxState = PROCESS; + break; - // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState - updateRcCommands(); - updateArmingStatus(); + case PROCESS: + ignoreTaskTime(); + if (!processRx(currentTimeUs)) { + break; + } + rxState = MODES; + break; + + case MODES: + processRxModes(currentTimeUs); + rxState = UPDATE; + break; + + case UPDATE: + ignoreTaskTime(); + // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + updateRcCommands(); + updateArmingStatus(); #ifdef USE_USB_CDC_HID - if (!ARMING_FLAG(ARMED)) { - sendRcDataToHid(); - } + if (!ARMING_FLAG(ARMED)) { + sendRcDataToHid(); + } #endif + rxState = CHECK; + break; + } } + #ifdef USE_BARO static void taskUpdateBaro(timeUs_t currentTimeUs) { diff --git a/src/main/fc/tasks.h b/src/main/fc/tasks.h index 77eb21819e..b9c3dc1f24 100644 --- a/src/main/fc/tasks.h +++ b/src/main/fc/tasks.h @@ -24,3 +24,6 @@ void tasksInit(void); task_t *getTask(unsigned taskId); + +bool taskUpdateRxMainInProgress(); + diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 40d139deef..380f5e7dc2 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -43,6 +43,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" +#include "fc/tasks.h" #include "flight/failsafe.h" @@ -459,6 +460,11 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) bool signalReceived = false; bool useDataDrivenProcessing = true; + if (taskUpdateRxMainInProgress()) { + // There are more states to process + return true; + } + switch (rxRuntimeState.rxProvider) { default: diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 62e09cbbdf..de475dee92 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -50,6 +50,7 @@ // 3 - time spent executing check function static FAST_DATA_ZERO_INIT task_t *currentTask = NULL; +static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskTime; static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasks; static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasksSamples; @@ -210,6 +211,12 @@ timeDelta_t getTaskDeltaTimeUs(taskId_e taskId) } } +// Called by tasks executing what are known to be short states +void ignoreTaskTime() +{ + ignoreCurrentTaskTime = true; +} + void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse) { calculateTaskStatistics = calculateTaskStatisticsToUse; @@ -281,6 +288,7 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi if (selectedTask) { currentTask = selectedTask; + ignoreCurrentTaskTime = false; selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastExecutedAtUs); #if defined(USE_TASK_STATISTICS) float period = currentTimeUs - selectedTask->lastExecutedAtUs; @@ -295,10 +303,12 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi const timeUs_t currentTimeBeforeTaskCallUs = micros(); selectedTask->taskFunc(currentTimeBeforeTaskCallUs); taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs; - selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; - selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT; + if (!ignoreCurrentTaskTime) { + selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; + selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT; + selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs); + } selectedTask->totalExecutionTimeUs += taskExecutionTimeUs; // time consumed by scheduler + task - selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs); selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs); } else #endif diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 3324444802..1e86537f8a 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -187,6 +187,7 @@ void getTaskInfo(taskId_e taskId, taskInfo_t *taskInfo); void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs); void setTaskEnabled(taskId_e taskId, bool newEnabledState); timeDelta_t getTaskDeltaTimeUs(taskId_e taskId); +void ignoreTaskTime(); void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics); void schedulerResetTaskStatistics(taskId_e taskId); void schedulerResetTaskMaxExecutionTime(taskId_e taskId); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index f31a30c93c..0e7b34ed59 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -51,6 +51,8 @@ #include "sensors/sensors.h" +#include "scheduler/scheduler.h" + #include "barometer.h" baro_t baro; // barometer access functions @@ -349,7 +351,7 @@ static uint32_t recalculateBarometerTotal(uint32_t pressureTotal, int32_t newPre nextSampleIndex = 0; baroReady = true; } else { - nextSampleIndex = (currentSampleIndex + 1); + nextSampleIndex = (currentSampleIndex + 1); } barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading); @@ -385,6 +387,12 @@ uint32_t baroUpdate(void) debug[0] = state; } + // Tell the scheduler to ignore how long this task takes unless the pressure is being read + // as that takes the longest + if (state != BAROMETER_NEEDS_PRESSURE_READ) { + ignoreTaskTime(); + } + switch (state) { default: case BAROMETER_NEEDS_TEMPERATURE_START: @@ -414,6 +422,8 @@ uint32_t baroUpdate(void) case BAROMETER_NEEDS_PRESSURE_READ: if (baro.dev.read_up(&baro.dev)) { state = BAROMETER_NEEDS_PRESSURE_SAMPLE; + } else { + ignoreTaskTime(); } break; diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index 18d4bc5116..ee4320230e 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -459,6 +459,7 @@ extern "C" { void failsafeOnValidDataReceived(void) { } void failsafeOnValidDataFailed(void) { } void pinioBoxTaskControl(void) { } + bool taskUpdateRxMainInProgress() { return true; } void rxPwmInit(rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback) { diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 9ac70fd420..636178def0 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -107,6 +107,7 @@ extern "C" { void failsafeOnRxSuspend(uint32_t ) {} void failsafeOnRxResume(void) {} +bool taskUpdateRxMainInProgress() { return true; } uint32_t micros(void) { return 0; } uint32_t millis(void) { return 0; } diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index 03c008b584..c7daaab02f 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -235,6 +235,7 @@ extern "C" { void xBusInit(const rxConfig_t *, rxRuntimeState_t *) {} void rxMspInit(const rxConfig_t *, rxRuntimeState_t *) {} void rxPwmInit(const rxConfig_t *, rxRuntimeState_t *) {} + bool taskUpdateRxMainInProgress() { return true; } float pt1FilterGain(float f_cut, float dT) { UNUSED(f_cut);