From 488f90b39dd37cf61013297af65ef2d3fb977566 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 8 Jun 2016 18:10:54 +0900 Subject: [PATCH] AP_Motors: add calc_spin_up_to_pwm to reduce repeated code --- libraries/AP_Motors/AP_MotorsCoax.cpp | 12 ++++---- libraries/AP_Motors/AP_MotorsMatrix.cpp | 2 +- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 31 +++++++++++--------- libraries/AP_Motors/AP_MotorsMulticopter.h | 5 +++- libraries/AP_Motors/AP_MotorsSingle.cpp | 12 ++++---- libraries/AP_Motors/AP_MotorsTri.cpp | 6 ++-- 6 files changed, 37 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index e3a42e2abd..30f2f3226f 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -129,12 +129,12 @@ void AP_MotorsCoax::output_to_motors() case SPIN_WHEN_ARMED: // sends output to motors when armed but not flying hal.rcout->cork(); - rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1)); - 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(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle)); - rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle)); + rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1)); + rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2)); + rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3)); + rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4)); + rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm()); + rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm()); hal.rcout->push(); break; case SPOOL_UP: diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index a2e17273a9..7e1ddda81c 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -102,7 +102,7 @@ void AP_MotorsMatrix::output_to_motors() // sends output to motors when armed but not flying for (i=0; i= 1.0f) { - _throttle_low_end_pct = 1.0f; + if (_spin_up_ratio >= 1.0f) { + _spin_up_ratio = 1.0f; _multicopter_flags.spool_mode = SPOOL_UP; } } else { // _spool_desired == SPIN_WHEN_ARMED - float spin_when_armed_low_end_pct = 0.0f; + float spin_up_armed_ratio = 0.0f; if (_min_throttle > 0) { - spin_when_armed_low_end_pct = (float)_spin_when_armed / _min_throttle; + spin_up_armed_ratio = (float)_spin_when_armed / _min_throttle; } - _throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct-_throttle_low_end_pct, -spool_step, spool_step); + _spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step); } _throttle_thrust_max = 0.0f; break; @@ -436,7 +439,7 @@ void AP_MotorsMulticopter::output_logic() } // set and increment ramp variables - _throttle_low_end_pct = 1.0f; + _spin_up_ratio = 1.0f; _throttle_thrust_max += 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate); // constrain ramp value and update mode @@ -465,7 +468,7 @@ void AP_MotorsMulticopter::output_logic() } // set and increment ramp variables - _throttle_low_end_pct = 1.0f; + _spin_up_ratio = 1.0f; _throttle_thrust_max = get_current_limit_max_throttle(); break; @@ -486,7 +489,7 @@ void AP_MotorsMulticopter::output_logic() } // set and increment ramp variables - _throttle_low_end_pct = 1.0f; + _spin_up_ratio = 1.0f; _throttle_thrust_max -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate); // constrain ramp value and update mode diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index bf0c986448..b9ce18fae0 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -128,6 +128,9 @@ protected: // convert thrust (0~1) range back to pwm range int16_t calc_thrust_to_pwm(float thrust_in) const; + // calculate spin up to pwm range + int16_t calc_spin_up_to_pwm() const; + // apply any thrust compensation for the frame virtual void thrust_compensation(void) {} @@ -161,7 +164,7 @@ protected: float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max // spool variables - float _throttle_low_end_pct; // throttle percentage (0 ~ 1) between zero and throttle_min + float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min // battery voltage, current and air pressure compensation variables float _batt_voltage_resting; // battery voltage reading at minimum throttle diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index db5dfd788b..da1b922541 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -132,12 +132,12 @@ void AP_MotorsSingle::output_to_motors() case SPIN_WHEN_ARMED: // sends output to motors when armed but not flying hal.rcout->cork(); - rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1)); - 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(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle)); - rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle)); + rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1)); + rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2)); + rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3)); + rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4)); + rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm()); + rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm()); hal.rcout->push(); break; case SPOOL_UP: diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index eae3778919..4c10a42480 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -137,9 +137,9 @@ void AP_MotorsTri::output_to_motors() case SPIN_WHEN_ARMED: // sends output to motors when armed but not flying hal.rcout->cork(); - rc_write(AP_MOTORS_MOT_1, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle)); - rc_write(AP_MOTORS_MOT_2, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle)); - rc_write(AP_MOTORS_MOT_4, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle)); + rc_write(AP_MOTORS_MOT_1, calc_spin_up_to_pwm()); + rc_write(AP_MOTORS_MOT_2, calc_spin_up_to_pwm()); + rc_write(AP_MOTORS_MOT_4, calc_spin_up_to_pwm()); rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); hal.rcout->push(); break;