diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index b1d1fcc55f..75c9625925 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -277,3 +277,19 @@ bool SRV_Channel::is_control_surface(SRV_Channel::Aux_servo_function_t function) return false; } + +// return the motor number of a channel, or -1 if not a motor +// return 0 for first motor +int8_t SRV_Channel::get_motor_num(void) const +{ + const auto k_function = get_function(); + switch (k_function) { + case k_motor1 ... k_motor8: + return int8_t(uint16_t(k_function) - k_motor1); + case k_motor9 ... k_motor12: + return 8 + int8_t(uint16_t(k_function) - k_motor9); + default: + break; + } + return -1; +} diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index eb7d8e9102..20ee360b23 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -240,6 +240,9 @@ public: return (SRV_Channel::Aux_servo_function_t)function.get(); } + // return the motor number of a channel, or -1 if not a motor + int8_t get_motor_num(void) const; + // set and save function for channel. Used in upgrade of parameters in plane void function_set_and_save(SRV_Channel::Aux_servo_function_t f) { function.set_and_save(int8_t(f));