From 5bb05828541332a7cee3b39e4f76f2fb452c0e8b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 14 May 2013 17:07:36 +0900 Subject: [PATCH] ACMotors: ESC Calibration only for enabled motors --- libraries/AP_Motors/AP_Motors_Class.cpp | 12 +++++++----- libraries/AP_Motors/AP_Motors_Class.h | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 32d8c1fad3..b0465a4a9c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -73,13 +73,15 @@ void AP_Motors::Init() setup_throttle_curve(); }; -// throttle_pass_through - passes throttle through to motors - dangerous but used for initialising ESCs +// throttle_pass_through - passes pilot's throttle input directly to all motors - dangerous but used for initialising ESCs void AP_Motors::throttle_pass_through() { - if( armed() ) { - // XXX - for( int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++ ) { - hal.rcout->write(_motor_to_channel_map[i], _rc_throttle->radio_in); + if (armed()) { + // send the pilot's input directly to each enabled motor + for (int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + if (motor_enabled[i]) { + hal.rcout->write(_motor_to_channel_map[i], _rc_throttle->radio_in); + } } } } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index db6a7da7ac..f7495f1e4f 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -126,7 +126,7 @@ public: virtual void output_test() { }; - // throttle_pass_through - passes throttle through to motors - dangerous but required for initialising ESCs + // throttle_pass_through - passes pilot's throttle input directly to all motors - dangerous but used for initialising ESCs virtual void throttle_pass_through(); // setup_throttle_curve - used to linearlise thrust output by motors