diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index 36a191f981..cba5c84d69 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -318,11 +318,6 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc
     // ensure valid motor number is provided
     if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
 
-        // increment number of motors if this motor is being newly motor_enabled
-        if( !motor_enabled[motor_num] ) {
-            motor_enabled[motor_num] = true;
-        }
-
         // set roll, pitch, thottle factors and opposite motor (for stability patch)
         _roll_factor[motor_num] = roll_fac;
         _pitch_factor[motor_num] = pitch_fac;
@@ -332,7 +327,7 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc
         _test_order[motor_num] = testing_order;
 
         // call parent class method
-        add_motor_num(motor_num);
+        motor_enabled[motor_num] = add_motor_num(motor_num);
     }
 }
 
diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp
index b4c8785a7e..d8c4b4f0d3 100644
--- a/libraries/AP_Motors/AP_Motors_Class.cpp
+++ b/libraries/AP_Motors/AP_Motors_Class.cpp
@@ -193,17 +193,18 @@ int16_t AP_Motors::calc_pwm_output_0to1(float input, const SRV_Channel *servo)
 /*
   add a motor, setting up _motor_map and _motor_map_mask as needed
  */
-void AP_Motors::add_motor_num(int8_t motor_num)
+bool 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_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((SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+motor_num), chan)) {
             _motor_map[motor_num] = chan;
             _motor_map_mask |= 1U<<motor_num;
+            return true;
         }
     }
+    return false;
 }
diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h
index ec3cd6d381..497a97d068 100644
--- a/libraries/AP_Motors/AP_Motors_Class.h
+++ b/libraries/AP_Motors/AP_Motors_Class.h
@@ -166,7 +166,7 @@ protected:
     virtual uint32_t    rc_map_mask(uint32_t mask) const;
 
     // add a motor to the motor map
-    void add_motor_num(int8_t motor_num);
+    bool add_motor_num(int8_t motor_num);
     
     // update the throttle input filter
     virtual void update_throttle_filter() = 0;