From 295e36d7d6c4d21002d6e2f8cbe19a260f8ceb37 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Mar 2022 10:45:43 +0000 Subject: [PATCH] SRV_Channel: don't count disabled channels in mask and setup functions disable channels that are not in use --- libraries/SRV_Channel/SRV_Channel.h | 2 +- libraries/SRV_Channel/SRV_Channel_aux.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 2affe9eb19..a4e775dc2d 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -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 >= 0 && 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); diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 0c94f9f9ce..8df4e1ee59 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -208,6 +208,8 @@ void SRV_Channels::enable_aux_servos() // see if it is a valid function if (c.valid_function()) { hal.rcout->enable_ch(c.ch_num); + } else { + hal.rcout->disable_ch(c.ch_num); } // output some servo functions before we fiddle with the