From 216bab4de441a3974d472f8529feabad17ec191a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 May 2018 11:08:27 +1000 Subject: [PATCH] AP_Motors: fixed default motor functions for single and coax copter should be enabling motors 5 and 6 to match docs and SITL --- libraries/AP_Motors/AP_MotorsCoax.cpp | 4 ++++ libraries/AP_Motors/AP_MotorsSingle.cpp | 5 +++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 5a92488817..33f3404979 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -50,6 +50,10 @@ void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_t _servo3->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); _servo4->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + // allow mapping of motors 5 and 6 + add_motor_num(CH_5); + add_motor_num(CH_6); + // record successful initialisation if what we setup was the desired frame_class _flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX); } diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 7b7096f132..c6c5e697b7 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -53,8 +53,9 @@ void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame _servo3->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); _servo4->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); - // allow mapping of motor7 - add_motor_num(CH_7); + // allow mapping of motors 5 and 6 + add_motor_num(CH_5); + add_motor_num(CH_6); // record successful initialisation if what we setup was the desired frame_class _flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE);