Merge pull request #9366 from shota3527/sh_servo_fix

fixes servo output issue
iNavFlight · Oct 12, 2023 · 750e8c3 · 750e8c3
2 parents 96549fc + e5d9033
commit 750e8c3
Showing 1 changed file with 30 additions and 22 deletions.
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
@@ -143,6 +143,33 @@ void servoComputeScalingFactors(uint8_t servoIndex) {
     servoMetadata[servoIndex].scaleMin = (servoParams(servoIndex)->middle - servoParams(servoIndex)->min) / 500.0f;
 }
 
+void computeServoCount(void)
+{
+    static bool firstRun = true;
+    if (!firstRun) {
+        return;
+    }
+    minServoIndex = 255;
+    maxServoIndex = 0;
+    for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) {
+        for (int i = 0; i < MAX_SERVO_RULES; i++) {
+            // check if done
+            if (mixerServoMixersByIndex(j)[i].rate == 0){
+                break;
+            }
+            if (mixerServoMixersByIndex(j)[i].targetChannel < minServoIndex) {
+                minServoIndex = mixerServoMixersByIndex(j)[i].targetChannel;
+            }
+
+            if (mixerServoMixersByIndex(j)[i].targetChannel > maxServoIndex) {
+                maxServoIndex = mixerServoMixersByIndex(j)[i].targetChannel;
+            }
+            mixerUsesServos = true;
+        }
+    }
+    firstRun = false;
+}
+
 void servosInit(void)
 {
     // give all servos a default command
@@ -153,12 +180,12 @@ void servosInit(void)
     /*
      * load mixer
      */
+    computeServoCount();
     loadCustomServoMixer();
 
     // If there are servo rules after all, update variables
-    if (servoRuleCount > 0) {
+    if (mixerUsesServos) {
         servoOutputEnabled = true;
-        mixerUsesServos = true;
     }
 
     for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
@@ -168,26 +195,7 @@ void servosInit(void)
 
 int getServoCount(void)
 {   
-    bool servoRuleDetected = false;
-    minServoIndex = 0;
-    maxServoIndex = 255;
-    for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) {
-        for (int i = 0; i < MAX_SERVO_RULES; i++) {
-            // check if done
-            if (mixerServoMixersByIndex(j)[i].rate == 0){
-                break;
-            }
-            if (mixerServoMixersByIndex(j)[i].targetChannel < minServoIndex) {
-                minServoIndex = mixerServoMixersByIndex(j)[i].targetChannel;
-            }
-
-            if (mixerServoMixersByIndex(j)[i].targetChannel > maxServoIndex) {
-                maxServoIndex = mixerServoMixersByIndex(j)[i].targetChannel;
-            }
-            servoRuleDetected = true;
-        }
-    }
-    if (servoRuleDetected) {
+    if (mixerUsesServos) {
         return 1 + maxServoIndex - minServoIndex;
     }
     else {