From b57539a9adc7e15f8b480ef6667275e7d3b8130d Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 19 Sep 2014 22:06:29 +0900 Subject: [PATCH] AP_Motors: throttle_pass_through accepts pwm --- ArduCopter/compassmot.pde | 2 +- libraries/AP_Motors/AP_Motors_Class.cpp | 7 ++++--- libraries/AP_Motors/AP_Motors_Class.h | 5 +++-- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ArduCopter/compassmot.pde b/ArduCopter/compassmot.pde index 61cb11473a..9bb3a681f8 100644 --- a/ArduCopter/compassmot.pde +++ b/ArduCopter/compassmot.pde @@ -147,7 +147,7 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan) read_radio(); // pass through throttle to motors - motors.throttle_pass_through(); + motors.throttle_pass_through(g.rc_3.radio_in); // read some compass values compass.read(); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 9fc0cf2537..660396b40e 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -117,14 +117,15 @@ void AP_Motors::set_mid_throttle(uint16_t mid_throttle) _hover_out = _rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * mid_throttle / 1000.0f; } -// throttle_pass_through - passes pilot's throttle input directly to all motors - dangerous but used for initialising ESCs -void AP_Motors::throttle_pass_through() +// throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs +// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 +void AP_Motors::throttle_pass_through(int16_t pwm) { 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(pgm_read_byte(&_motor_to_channel_map[i]), _rc_throttle.radio_in); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm); } } } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index befa01a1d2..832200836e 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -117,8 +117,9 @@ public: // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 virtual void output_test(uint8_t motor_seq, int16_t pwm) = 0; - // throttle_pass_through - passes pilot's throttle input directly to all motors - dangerous but used for initialising ESCs - virtual void throttle_pass_through(); + // throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs + // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 + virtual void throttle_pass_through(int16_t pwm); // setup_throttle_curve - used to linearlise thrust output by motors // returns true if curve is created successfully