diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index dbfb1db593..735950c56e 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -182,3 +182,9 @@ uint16_t SRV_Channel::get_limit_pwm(LimitValue limit) const } } +// return true if function is for a multicopter motor +bool SRV_Channel::is_motor(SRV_Channel::Aux_servo_function_t function) +{ + return ((function >= SRV_Channel::k_motor1 && function <= SRV_Channel::k_motor8) || + (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12)); +} diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 1a70f5665a..7a0da141aa 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -110,6 +110,10 @@ public: k_vtail_left = 79, k_vtail_right = 80, k_boost_throttle = 81, ///< vertical booster throttle + k_motor9 = 82, + k_motor10 = 83, + k_motor11 = 84, + k_motor12 = 85, k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; @@ -157,6 +161,9 @@ public: return servo_trim; } + // return true if function is for a multicopter motor + static bool is_motor(SRV_Channel::Aux_servo_function_t function); + private: AP_Int16 servo_min; AP_Int16 servo_max; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 7a86168c0e..d57edafaf9 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -39,6 +39,7 @@ void SRV_Channel::output_ch(void) passthrough_from = int8_t(function - k_rcin1); break; case k_motor1 ... k_motor8: + case k_motor9 ... k_motor12: // handled by AP_Motors::rc_write() return; }