From 49b14b3cb6e46f105f8dc2b744b728f90abee3d6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 21 May 2016 15:47:46 +0900 Subject: [PATCH] AP_Motors: add PWM_MIN, MAX These parameters allow the motor PWM output range to be different from receiver's throttle input range Also renamed throttle_pass_through to set_throttle_passthrough_for_esc_calibration and change the method's input to the 0 to 1 range instead of direct PWM value --- libraries/AP_Motors/AP_MotorsCoax.cpp | 8 +-- libraries/AP_Motors/AP_MotorsMatrix.cpp | 4 +- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 67 ++++++++++++++++---- libraries/AP_Motors/AP_MotorsMulticopter.h | 13 ++-- libraries/AP_Motors/AP_MotorsSingle.cpp | 8 +-- libraries/AP_Motors/AP_MotorsTri.cpp | 12 ++-- 6 files changed, 78 insertions(+), 34 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index c4e0ffedeb..953f661abb 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -122,8 +122,8 @@ void AP_MotorsCoax::output_to_motors() rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough, _servo2)); rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_roll_radio_passthrough, _servo3)); rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_pitch_radio_passthrough, _servo4)); - rc_write(AP_MOTORS_MOT_5, _throttle_radio_min); - rc_write(AP_MOTORS_MOT_6, _throttle_radio_min); + rc_write(AP_MOTORS_MOT_5, get_pwm_min()); + rc_write(AP_MOTORS_MOT_6, get_pwm_min()); hal.rcout->push(); break; case SPIN_WHEN_ARMED: @@ -133,8 +133,8 @@ void AP_MotorsCoax::output_to_motors() rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2)); rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3)); rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4)); - rc_write(AP_MOTORS_MOT_5, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); - rc_write(AP_MOTORS_MOT_6, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); + rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle)); + rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle)); hal.rcout->push(); break; case SPOOL_UP: diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index da831824ab..fc581ba71b 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -94,7 +94,7 @@ void AP_MotorsMatrix::output_to_motors() // set motor output based on thrust requests for (i=0; i 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) { + return _pwm_min; + } + return _throttle_radio_min; +} + +// get maximum pwm value that can be output to motors +int16_t AP_MotorsMulticopter::get_pwm_max() const +{ + // return _pwm_max if both PWM_MIN and PWM_MAX parmeters are defined and valid + if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) { + return _pwm_max; + } + return _throttle_radio_max; } // set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) // also sets throttle channel minimum and maximum pwm void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max) { - _throttle_radio_min = radio_min; - _throttle_radio_max = radio_max; - _min_throttle = (float)min_throttle * ((_throttle_radio_max - _throttle_radio_min) / 1000.0f); + // sanity check + if ((radio_max > radio_min) && (min_throttle < (radio_max - radio_min))) { + _throttle_radio_min = radio_min; + _throttle_radio_max = radio_max; + } + // update _min_throttle + _min_throttle = (float)min_throttle * ((get_pwm_max() - get_pwm_min()) / 1000.0f); } void AP_MotorsMulticopter::output_logic() @@ -488,16 +526,17 @@ void AP_MotorsMulticopter::output_logic() } } -// 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_MotorsMulticopter::throttle_pass_through(int16_t pwm) +// passes throttle directly to all motors for ESC calibration. +// throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_min() and 1 will send get_pwm_max() +void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float throttle_input) { if (armed()) { + uint16_t pwm_out = get_pwm_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_max() - get_pwm_min()); // send the pilot's input directly to each enabled motor hal.rcout->cork(); for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { - rc_write(i, pwm); + rc_write(i, pwm_out); } } hal.rcout->push(); @@ -516,7 +555,7 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask) if (mask & (1U<push(); break; case SPIN_WHEN_ARMED: @@ -136,8 +136,8 @@ void AP_MotorsSingle::output_to_motors() rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2)); rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3)); rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4)); - rc_write(AP_MOTORS_MOT_5, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); - rc_write(AP_MOTORS_MOT_6, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); + rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle)); + rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle)); hal.rcout->push(); break; case SPOOL_UP: diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 0d3f226ac6..2a3844e8c1 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -128,18 +128,18 @@ void AP_MotorsTri::output_to_motors() case SHUT_DOWN: // sends minimum values out to the motors hal.rcout->cork(); - rc_write(AP_MOTORS_MOT_1, _throttle_radio_min); - rc_write(AP_MOTORS_MOT_2, _throttle_radio_min); - rc_write(AP_MOTORS_MOT_4, _throttle_radio_min); + rc_write(AP_MOTORS_MOT_1, get_pwm_min()); + rc_write(AP_MOTORS_MOT_2, get_pwm_min()); + rc_write(AP_MOTORS_MOT_4, get_pwm_min()); rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); hal.rcout->push(); break; case SPIN_WHEN_ARMED: // sends output to motors when armed but not flying hal.rcout->cork(); - rc_write(AP_MOTORS_MOT_1, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); - rc_write(AP_MOTORS_MOT_2, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); - rc_write(AP_MOTORS_MOT_4, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); + rc_write(AP_MOTORS_MOT_1, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle)); + rc_write(AP_MOTORS_MOT_2, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle)); + rc_write(AP_MOTORS_MOT_4, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle)); rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); hal.rcout->push(); break;