AP_Arming: add pre-arm check of disabled servo channels

hzi-braunschweig · Apr 25, 2022 · 902b895 · 902b895
1 parent cc6b907
commit 902b895
Showing 1 changed file with 17 additions and 0 deletions.
diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
@@ -811,6 +811,23 @@ bool AP_Arming::servo_checks(bool report) const
             check_failed(report, "SERVO%d_MAX is less than SERVO%d_TRIM", i + 1, i + 1);
             check_passed = false;
         }
+
+        // check functions using PWM are enabled
+        if (SRV_Channels::get_disabled_channel_mask() & 1U<<i) {
+            const SRV_Channel::Aux_servo_function_t ch_function = c->get_function();
+
+            // motors, e-stoppable functions, neopixels and ProfiLEDs may be digital outputs and thus can be disabled
+            const bool disabled_ok = SRV_Channel::is_motor(ch_function) ||
+                                     SRV_Channel::should_e_stop(ch_function) ||
+                                     (ch_function >= SRV_Channel::k_LED_neopixel1 && ch_function <= SRV_Channel::k_LED_neopixel4) ||
+                                     (ch_function >= SRV_Channel::k_ProfiLED_1 && ch_function <= SRV_Channel::k_ProfiLED_Clock);
+
+            // for all other functions raise a pre-arm failure
+            if (!disabled_ok) {
+                check_failed(report, "SERVO%u_FUNCTION=%u on disabled channel", i + 1, (unsigned)ch_function);
+                check_passed = false;
+            }
+        }
     }
 
 #if HAL_WITH_IO_MCU