From c05ad2ec9b7d3a0915ef9748a160a0cc28931069 Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Tue, 11 Jan 2022 20:09:55 +0000
Subject: [PATCH] Fix issues impacting RX_STATE_MODES state duration and add
 DEBUG_RX_STATE_TIME

---
 src/main/build/debug.c         |  1 +
 src/main/build/debug.h         |  1 +
 src/main/fc/rc_adjustments.c   |  3 +++
 src/main/fc/tasks.c            | 15 +++++++++++++--
 src/main/osd/osd.c             |  7 +++++++
 src/main/scheduler/scheduler.c |  5 +++++
 src/main/scheduler/scheduler.h |  1 +
 7 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 2ac1b2cbb2..d3ae5fb3f4 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -99,4 +99,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "TIMING_ACCURACY",
     "RX_EXPRESSLRS_SPI",
     "RX_EXPRESSLRS_PHASELOCK",
+    "RX_STATE_TIME"
 };
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index acde8b9003..8494550257 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -97,6 +97,7 @@ typedef enum {
     DEBUG_TIMING_ACCURACY,
     DEBUG_RX_EXPRESSLRS_SPI,
     DEBUG_RX_EXPRESSLRS_PHASELOCK,
+    DEBUG_RX_STATE_TIME,
     DEBUG_COUNT
 } debugType_e;
 
diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c
index 51b7a0e439..ad4cbf0529 100644
--- a/src/main/fc/rc_adjustments.c
+++ b/src/main/fc/rc_adjustments.c
@@ -659,6 +659,9 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui
 
 static void calcActiveAdjustmentRanges(void)
 {
+    // This initialisation upsets the scheduler task duration estimation
+    schedulerIgnoreTaskExecTime();
+
     adjustmentRange_t defaultAdjustmentRange;
     memset(&defaultAdjustmentRange, 0, sizeof(defaultAdjustmentRange));
 
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index 0d8e2c1b5c..463954b4e0 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -182,9 +182,10 @@ bool taskUpdateRxMainInProgress()
 
 static void taskUpdateRxMain(timeUs_t currentTimeUs)
 {
-    static timeUs_t rxStateDurationFracUs[RX_STATE_COUNT];
-    timeUs_t executeTimeUs;
+    static timeDelta_t rxStateDurationFracUs[RX_STATE_COUNT];
+    timeDelta_t executeTimeUs;
     rxState_e oldRxState = rxState;
+    timeDelta_t anticipatedExecutionTime;
 
     // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
     if (rxState != RX_STATE_UPDATE) {
@@ -230,6 +231,12 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
 
     executeTimeUs = micros() - currentTimeUs + RX_TASK_MARGIN;
 
+    // If the scheduler has reduced the anticipatedExecutionTime due to task aging, pick that up
+    anticipatedExecutionTime = schedulerGetNextStateTime();
+    if (anticipatedExecutionTime != (rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) {
+        rxStateDurationFracUs[oldRxState] = anticipatedExecutionTime << RX_TASK_DECAY_SHIFT;
+    }
+
     if (executeTimeUs > (rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) {
         rxStateDurationFracUs[oldRxState] = executeTimeUs << RX_TASK_DECAY_SHIFT;
     } else {
@@ -237,6 +244,10 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
         rxStateDurationFracUs[oldRxState]--;
     }
 
+    if (debugMode == DEBUG_RX_STATE_TIME) {
+        debug[oldRxState] = rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT;
+    }
+
     schedulerSetNextStateTime(rxStateDurationFracUs[rxState] >> RX_TASK_DECAY_SHIFT);
 }
 
diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c
index cf9f04935b..7a86267d57 100644
--- a/src/main/osd/osd.c
+++ b/src/main/osd/osd.c
@@ -299,6 +299,13 @@ void changeOsdProfileIndex(uint8_t profileIndex)
 
 void osdAnalyzeActiveElements(void)
 {
+    /* This code results in a total RX task RX_STATE_MODES state time of ~68us on an F411 overclocked to 108MHz
+     * This upsets the scheduler task duration estimation and will break SPI RX communication. This can
+     * occur in flight, but only when the OSD profile is changed by switch so can be ignored, only causing
+     * one late task instance.
+     */
+    schedulerIgnoreTaskExecTime();
+
     osdAddActiveElements();
     osdDrawActiveElementsBackground(osdDisplayPort);
 }
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index d1e552a943..a2e24de9c0 100644
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -356,6 +356,11 @@ FAST_CODE void schedulerSetNextStateTime(timeDelta_t nextStateTime)
     taskNextStateTime = nextStateTime;
 }
 
+FAST_CODE timeDelta_t schedulerGetNextStateTime()
+{
+    return currentTask->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
+}
+
 FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs)
 {
     timeUs_t taskExecutionTimeUs = 0;
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 2a8fbd662e..66330822ee 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -230,6 +230,7 @@ void schedulerResetTaskStatistics(taskId_e taskId);
 void schedulerResetTaskMaxExecutionTime(taskId_e taskId);
 void schedulerResetCheckFunctionMaxExecutionTime(void);
 void schedulerSetNextStateTime(timeDelta_t nextStateTime);
+timeDelta_t schedulerGetNextStateTime();
 void schedulerInit(void);
 void scheduler(void);
 timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs);