From d00725b2edb2f28cd5eb3935db32d738de64f205 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 18 May 2017 23:16:23 +0900 Subject: [PATCH] AP_Motors: rc_write handles motors 9 to 12 --- libraries/AP_Motors/AP_Motors_Class.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index b4c8785a7e..69e8f53ad0 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -198,10 +198,14 @@ void AP_Motors::add_motor_num(int8_t motor_num) // ensure valid motor number is provided if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { uint8_t chan; - SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+motor_num); + SRV_Channel::Aux_servo_function_t function; + if (motor_num < 8) { + function = (SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+motor_num); + } else { + function = (SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor9+(motor_num-8)); + } SRV_Channels::set_aux_channel_default(function, motor_num); - if (SRV_Channels::find_channel((SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+motor_num), - chan) && chan != motor_num) { + if (SRV_Channels::find_channel(function, chan) && chan != motor_num) { _motor_map[motor_num] = chan; _motor_map_mask |= 1U<