diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index a4e775dc2d..b5010a464d 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -52,7 +52,7 @@ public: typedef enum { k_GPIO = -1, ///< used as GPIO pin (input or output) - k_none = 0, ///< disabled + k_none = 0, ///< general use PWM output used by do-set-servo commands and lua scripts k_manual = 1, ///< manual, just pass-thru the RC in signal k_flap = 2, ///< flap k_flap_auto = 3, ///< flap automated @@ -183,7 +183,7 @@ public: // check if a function is valid for indexing into functions static bool valid_function(Aux_servo_function_t fn) { - return fn > k_none && fn < k_nr_aux_servo_functions; + return fn >= k_none && fn < k_nr_aux_servo_functions; } bool valid_function(void) const { return valid_function(function); @@ -519,8 +519,9 @@ public: static void push(); - // disable output to a set of channels given by a mask. This is used by the AP_BLHeli code + // disable PWM output to a set of channels given by a mask. This is used by the AP_BLHeli code static void set_disabled_channel_mask(uint16_t mask) { disabled_mask = mask; } + static uint16_t get_disabled_channel_mask() { return disabled_mask; } // add to mask of outputs which use digital (non-PWM) output and optionally can reverse thrust, such as DShot static void set_digital_outputs(uint16_t dig_mask, uint16_t rev_mask); @@ -600,6 +601,7 @@ private: static AP_FETtecOneWire *fetteconwire_ptr; #endif // AP_FETTEC_ONEWIRE_ENABLED + // mask of disabled channels static uint16_t disabled_mask; // mask of outputs which use a digital output protocol, not diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 8df4e1ee59..118bbba1b6 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -206,7 +206,7 @@ void SRV_Channels::enable_aux_servos() for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { SRV_Channel &c = channels[i]; // see if it is a valid function - if (c.valid_function()) { + if (c.valid_function() && !(disabled_mask & (1U<enable_ch(c.ch_num); } else { hal.rcout->disable_ch(c.ch_num); @@ -248,6 +248,27 @@ void SRV_Channels::set_digital_outputs(uint16_t dig_mask, uint16_t rev_mask) { digital_mask |= dig_mask; reversible_mask |= rev_mask; + // add in NeoPixel and ProfiLED functions to digital array to determine anything else + // that should be disabled + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + SRV_Channel &c = channels[i]; + switch (c.function.get()) { + case SRV_Channel::k_LED_neopixel1: + case SRV_Channel::k_LED_neopixel2: + case SRV_Channel::k_LED_neopixel3: + case SRV_Channel::k_LED_neopixel4: + case SRV_Channel::k_ProfiLED_1: + case SRV_Channel::k_ProfiLED_2: + case SRV_Channel::k_ProfiLED_3: + case SRV_Channel::k_ProfiLED_Clock: + dig_mask |= 1U<get_disabled_channels(dig_mask); + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { SRV_Channel &c = channels[i]; if (digital_mask & (1U<