diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index be7b8408d8..17659bf38b 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -422,6 +422,9 @@ public: static void push(); + // disable 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; } + private: struct { bool k_throttle_reversible:1; @@ -449,6 +452,7 @@ private: // support for BLHeli protocol AP_BLHeli blheli; static AP_BLHeli *blheli_ptr; + static uint16_t disabled_mask; SRV_Channel obj_channels[NUM_SERVO_CHANNELS]; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index b6cc4445ca..d7330dbfda 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -50,7 +50,9 @@ void SRV_Channel::output_ch(void) } } } - hal.rcout->write(ch_num, output_pwm); + if (!(SRV_Channels::disabled_mask & (1U<write(ch_num, output_pwm); + } } /* diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 05ed7f7c80..1a1ea1587c 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -29,6 +29,7 @@ SRV_Channels *SRV_Channels::instance; AP_Volz_Protocol *SRV_Channels::volz_ptr; AP_SBusOut *SRV_Channels::sbus_ptr; AP_BLHeli *SRV_Channels::blheli_ptr; +uint16_t SRV_Channels::disabled_mask; bool SRV_Channels::disabled_passthrough; bool SRV_Channels::initialised;