Merge pull request #7534 from etracer65/optimize_rc_modes_processing

Optimize rc modes activation conditions processing

src/main/cli/cli.c

@@ -942,7 +942,7 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi
         bool equalsDefault = false;
         if (defaultModeActivationConditions) {
             const modeActivationCondition_t *macDefault = &defaultModeActivationConditions[i];
-            equalsDefault = !memcmp(mac, macDefault, sizeof(*mac));
+            equalsDefault = !isModeActivationConditionConfigured(mac, macDefault);
             const box_t *box = findBoxByBoxId(macDefault->modeId);
             const box_t *linkedTo = findBoxByBoxId(macDefault->linkedTo);
             if (box) {
@@ -1028,6 +1028,7 @@ static void cliAux(char *cmdline)
             } else if (validArgumentCount != 6) {
                 memset(mac, 0, sizeof(modeActivationCondition_t));
             }
+            analyzeModeActivationConditions();
             cliPrintLinef( "aux %u %u %u %u %u %u %u",
                 i,
                 mac->modeId,

src/main/fc/rc_controls.c

@@ -393,5 +393,6 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
 
 void rcControlsInit(void)
 {
+    analyzeModeActivationConditions();
     isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
 }

src/main/fc/rc_modes.c

@@ -47,6 +47,11 @@ static boxBitmask_t stickyModesEverDisabled;
 
 static bool airmodeEnabled;
 
+static int activeMacCount = 0;
+static uint8_t activeMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT];
+static int activeLinkedMacCount = 0;
+static uint8_t activeLinkedMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT];
+
 PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 2);
 
 bool IS_RC_MODE_ACTIVE(boxId_e boxId)
@@ -108,13 +113,8 @@ void updateActivatedModes(void)
     bitArraySet(&stickyModes, BOXPARALYZE);
 
     // determine which conditions set/clear the mode
-    for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
+    for (int i = 0; i < activeMacCount; i++) {
-        const modeActivationCondition_t *mac = modeActivationConditions(i);
+        const modeActivationCondition_t *mac = modeActivationConditions(activeMacArray[i]);
-
-        // Skip linked macs for now to fully determine target states
-        if (mac->linkedTo) {
-            continue;
-        }
 
         if (bitArrayGet(&stickyModes, mac->modeId)) {
             updateMasksForStickyModes(mac, &andMask, &newMask);
@@ -126,12 +126,8 @@ void updateActivatedModes(void)
     bitArrayXor(&newMask, sizeof(&newMask), &newMask, &andMask);
 
     // Update linked modes
-    for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
+    for (int i = 0; i < activeLinkedMacCount; i++) {
-        const modeActivationCondition_t *mac = modeActivationConditions(i);
+        const modeActivationCondition_t *mac = modeActivationConditions(activeLinkedMacArray[i]);
-
-        if (!mac->linkedTo) {
-            continue;
-        }
 
         bitArrayCopy(&newMask, mac->linkedTo, mac->modeId);
     }
@@ -177,3 +173,32 @@ void removeModeActivationCondition(const boxId_e modeId)
         }
     }
 }
+
+bool isModeActivationConditionConfigured(const modeActivationCondition_t *mac, const modeActivationCondition_t *emptyMac)
+{
+    if (memcmp(mac, emptyMac, sizeof(*emptyMac))) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+// Build the list of used modeActivationConditions indices
+// We can then use this to speed up processing by only evaluating used conditions
+void analyzeModeActivationConditions(void)
+{
+    modeActivationCondition_t emptyMac;
+    memset(&emptyMac, 0, sizeof(emptyMac));
+
+    activeMacCount = 0;
+    activeLinkedMacCount = 0;
+
+    for (uint8_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
+        const modeActivationCondition_t *mac = modeActivationConditions(i);
+        if (mac->linkedTo) {
+            activeLinkedMacArray[activeLinkedMacCount++] = i;
+        } else if (isModeActivationConditionConfigured(mac, &emptyMac)) {
+            activeMacArray[activeMacCount++] = i;
+        }
+    }
+}

src/main/fc/rc_modes.h

@@ -130,3 +130,5 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 void updateActivatedModes(void);
 bool isModeActivationConditionPresent(boxId_e modeId);
 void removeModeActivationCondition(boxId_e modeId);
+bool isModeActivationConditionConfigured(const modeActivationCondition_t *mac, const modeActivationCondition_t *emptyMac);
+void analyzeModeActivationConditions(void);

src/main/target/BEEBRAIN_V2F/config.c

@@ -158,6 +158,8 @@ void targetConfiguration(void)
     modeActivationConditionsMutable(0)->range.startStep  = CHANNEL_VALUE_TO_STEP(900);
     modeActivationConditionsMutable(0)->range.endStep    = CHANNEL_VALUE_TO_STEP(2100);
 
+    analyzeModeActivationConditions();
+
 #if defined(BEEBRAIN_V2D)
     // DSM version
     for (uint8_t rxRangeIndex = 0; rxRangeIndex < NON_AUX_CHANNEL_COUNT; rxRangeIndex++) {

src/main/target/MULTIFLITEPICO/config.c

@@ -74,6 +74,8 @@ void targetConfiguration(void)
     modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1425);
     modeActivationConditionsMutable(1)->range.endStep   = CHANNEL_VALUE_TO_STEP(1575);
 
+    analyzeModeActivationConditions();
+
     failsafeConfigMutable()->failsafe_delay = 2;
     failsafeConfigMutable()->failsafe_off_delay = 0;
 

src/test/unit/cli_unittest.cc

@@ -285,4 +285,6 @@ bool setManufacturerId(char *newManufacturerId) { UNUSED(newManufacturerId); ret
 bool persistBoardInformation(void) { return true; };
 
 void activeAdjustmentRangeReset(void) {}
+void analyzeModeActivationConditions(void) {}
+bool isModeActivationConditionConfigured(const modeActivationCondition_t *, const modeActivationCondition_t *) { return false; }
 }

src/test/unit/rc_controls_unittest.cc

@@ -87,6 +87,7 @@ TEST_F(RcControlsModesTest, updateActivatedModesWithAllInputsAtMidde)
     }
 
     // when
+    analyzeModeActivationConditions();
     updateActivatedModes();
 
     // then
@@ -178,6 +179,7 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV
     bitArraySet(&activeBoxIds, 5);
 
     // when
+    analyzeModeActivationConditions();
     updateActivatedModes();
 
     // then

src/test/unit/rcdevice_unittest.cc

@@ -301,6 +301,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready)
     modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1300);
     modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
 
+    analyzeModeActivationConditions();
+    
     // make the binded mode inactive
     rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1800;
     rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900;
@@ -362,6 +364,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady)
     modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
     modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
 
+    analyzeModeActivationConditions();
+
     rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
     rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000;
     rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
@@ -422,6 +426,8 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine)
     modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
     modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
 
+    analyzeModeActivationConditions();
+
     // // make the binded mode inactive
     rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
     rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000;

src/test/unit/rx_rx_unittest.cc

@@ -78,6 +78,8 @@ TEST(RxTest, TestValidFlightChannels)
     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
 
+    analyzeModeActivationConditions();
+
     // when
     rxInit();
 
@@ -113,6 +115,8 @@ TEST(RxTest, TestValidFlightChannelsHighArm)
     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1400);
     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 
+    analyzeModeActivationConditions();
+
     // when
     rxInit();
 
@@ -147,6 +151,8 @@ TEST(RxTest, TestInvalidFlightChannels)
     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1400);
     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 
+    analyzeModeActivationConditions();
+
     // and
     uint16_t channelPulses[MAX_SUPPORTED_RC_CHANNEL_COUNT];
     memset(&channelPulses, 1500, sizeof(channelPulses));

src/test/unit/vtx_unittest.cc

@@ -89,6 +89,8 @@ TEST(VtxTest, PitMode)
     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 
+    analyzeModeActivationConditions();
+
     // and
     vtxSettingsConfigMutable()->band = 0;
     vtxSettingsConfigMutable()->freq = 5800;