Merge pull request #6796 from etracer65/adjustment_range_optimize

Optimize in-flight adjustments to only process configured ranges

src/main/fc/core.c

@@ -725,7 +725,6 @@ bool processRx(timeUs_t currentTimeUs)
 #endif
 
     if (!cliMode) {
-        updateAdjustmentStates();
         processRcAdjustments(currentControlRateProfile);
     }
 

src/main/fc/rc_adjustments.c

@@ -56,6 +56,8 @@
 
 #include "rx/rx.h"
 
+#define ADJUSTMENT_RANGE_COUNT_INVALID -1
+
 PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0);
 
 uint8_t pidAudioPositionToModeMap[7] = {
@@ -75,6 +77,11 @@ uint8_t pidAudioPositionToModeMap[7] = {
 
 static pidProfile_t *pidProfile;
 
+static int activeAdjustmentCount = ADJUSTMENT_RANGE_COUNT_INVALID;
+static uint8_t activeAdjustmentArray[MAX_ADJUSTMENT_RANGE_COUNT];
+static int activeAbsoluteAdjustmentCount;
+static uint8_t activeAbsoluteAdjustmentArray[MAX_ADJUSTMENT_RANGE_COUNT];
+
 static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
 {
 #ifndef USE_BLACKBOX
@@ -664,6 +671,38 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui
     return position;
 }
 
+static void calcActiveAdjustmentRanges(void)
+{
+    adjustmentRange_t defaultAdjustmentRange;
+    memset(&defaultAdjustmentRange, 0, sizeof(defaultAdjustmentRange));
+
+    activeAdjustmentCount = 0;
+    activeAbsoluteAdjustmentCount = 0;
+    for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
+        const adjustmentRange_t * const adjustmentRange = adjustmentRanges(i);
+        if (memcmp(adjustmentRange, &defaultAdjustmentRange, sizeof(defaultAdjustmentRange)) != 0) {
+            if (adjustmentRange->adjustmentCenter == 0) {
+                activeAdjustmentArray[activeAdjustmentCount++] = i;
+            } else {
+                activeAbsoluteAdjustmentArray[activeAbsoluteAdjustmentCount++] = i;
+            }
+        }
+    }
+}
+
+static void updateAdjustmentStates(void)
+{
+    for (int index = 0; index < activeAdjustmentCount; index++) {
+        const adjustmentRange_t * const adjustmentRange = adjustmentRanges(activeAdjustmentArray[index]);
+        // Only use slots if center value has not been specified, otherwise apply values directly (scaled) from aux channel
+        if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range) &&
+            (adjustmentRange->adjustmentCenter == 0)) {
+            const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
+            configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
+        }
+    }
+}
+
 #define RESET_FREQUENCY_2HZ (1000 / 2)
 
 void processRcAdjustments(controlRateConfig_t *controlRateConfig)
@@ -674,6 +713,13 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig)
 
     const bool canUseRxData = rxIsReceivingSignal();
 
+    // Recalculate the new active adjustments if required
+    if (activeAdjustmentCount == ADJUSTMENT_RANGE_COUNT_INVALID) {
+        calcActiveAdjustmentRanges();
+    }
+
+    updateAdjustmentStates();
+    
     // Process Increment/Decrement adjustments
     for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
         adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
@@ -741,9 +787,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig)
     }
 
     // Process Absolute adjustments
-    for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
+    for (int i = 0; i < activeAbsoluteAdjustmentCount; i++) {
         static int16_t lastRcData[MAX_ADJUSTMENT_RANGE_COUNT] = { 0 };
-
+        int index = activeAbsoluteAdjustmentArray[i];
         const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index);
         const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentRange->auxSwitchChannelIndex;
         const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
@@ -767,26 +813,14 @@ void resetAdjustmentStates(void)
     memset(adjustmentStates, 0, sizeof(adjustmentStates));
 }
 
-void updateAdjustmentStates(void)
-{
-    for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
-        const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index);
-        // Only use slots if center value has not been specified, otherwise apply values directly (scaled) from aux channel
-        if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range) &&
-            (adjustmentRange->adjustmentCenter == 0)) {
-            const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
-            configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
-        }
-    }
-}
-
 void useAdjustmentConfig(pidProfile_t *pidProfileToUse)
 {
     pidProfile = pidProfileToUse;
 }
 
 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
-const char *getAdjustmentsRangeName(void) {
+const char *getAdjustmentsRangeName(void)
+{
     if (adjustmentRangeNameIndex > 0) {
         return &adjustmentLabels[adjustmentRangeNameIndex - 1][0];
     } else {
@@ -794,7 +828,13 @@ const char *getAdjustmentsRangeName(void) {
     }
 }
 
-int getAdjustmentsRangeValue(void) {
+int getAdjustmentsRangeValue(void)
+{
     return adjustmentRangeValue;
 }
 #endif
+
+void activeAdjustmentRangeReset(void)
+{
+    activeAdjustmentCount = ADJUSTMENT_RANGE_COUNT_INVALID;
+}

src/main/fc/rc_adjustments.h

@@ -109,10 +109,10 @@ typedef struct adjustmentState_s {
 #endif
 
 void resetAdjustmentStates(void);
-void updateAdjustmentStates(void);
 struct controlRateConfig_s;
 void processRcAdjustments(struct controlRateConfig_s *controlRateConfig);
 struct pidProfile_s;
 void useAdjustmentConfig(struct pidProfile_s *pidProfileToUse);
 const char *getAdjustmentsRangeName(void);
 int getAdjustmentsRangeValue(void);
+void activeAdjustmentRangeReset(void);

src/main/interface/cli.c

@@ -1330,6 +1330,9 @@ static void cliAdjustmentRange(char *cmdline)
                 ar->adjustmentScale = val;
                 validArgumentCount++;
             }
+
+            activeAdjustmentRangeReset();
+
             cliDumpPrintLinef(0, false, format,
                 i,
                 ar->adjustmentIndex,

src/main/interface/msp.c

@@ -1623,6 +1623,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             } else {
                 return MSP_RESULT_ERROR;
             }
+            activeAdjustmentRangeReset();
         } else {
             return MSP_RESULT_ERROR;
         }

src/test/unit/cli_unittest.cc

@@ -282,4 +282,6 @@ bool boardInformationIsSet(void) { return true; };
 bool setBoardName(char *newBoardName) { UNUSED(newBoardName); return true; };
 bool setManufacturerId(char *newManufacturerId) { UNUSED(newManufacturerId); return true; };
 bool persistBoardInformation(void) { return true; };
+
+void activeAdjustmentRangeReset(void) {}
 }