From 1fa335a77b34d0eee75d7960417a3a8c4644eb30 Mon Sep 17 00:00:00 2001 From: Randy Mackay <rmackay9@yahoo.com> Date: Mon, 21 Jan 2019 20:13:00 +0900 Subject: [PATCH] AP_Motors: actuator_spin_up renamed to include _to_ground_idle --- libraries/AP_Motors/AP_MotorsCoax.cpp | 4 ++-- libraries/AP_Motors/AP_MotorsMatrix.cpp | 2 +- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 4 ++-- libraries/AP_Motors/AP_MotorsMulticopter.h | 4 ++-- libraries/AP_Motors/AP_MotorsSingle.cpp | 4 ++-- libraries/AP_Motors/AP_MotorsTailsitter.cpp | 8 ++++---- libraries/AP_Motors/AP_MotorsTri.cpp | 6 +++--- 7 files changed, 16 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 78db05b7e4..0ad6623ff3 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -83,8 +83,8 @@ void AP_MotorsCoax::output_to_motors() for (uint8_t i=0; i<NUM_ACTUATORS; i++) { rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE); } - set_actuator_with_slew(_actuator[5], actuator_spin_up()); - set_actuator_with_slew(_actuator[6], actuator_spin_up()); + set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle()); + set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle()); rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5])); rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6])); break; diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 18a1425202..37d3ef64b8 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -87,7 +87,7 @@ void AP_MotorsMatrix::output_to_motors() // sends output to motors when armed but not flying for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { - set_actuator_with_slew(_actuator[i], actuator_spin_up()); + set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle()); } } break; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index e6a422acd1..c2388dd72b 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -437,8 +437,8 @@ void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float actuator_output = constrain_float(input, output_slew_limit_dn, output_slew_limit_up); } -// gradually increase actuator output maximum limit -float AP_MotorsMulticopter::actuator_spin_up() const +// gradually increase actuator output to spin_min +float AP_MotorsMulticopter::actuator_spin_up_to_ground_idle() const { return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min; } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 9762ae3e80..5934b966cd 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -124,8 +124,8 @@ protected: // adds slew rate limiting to actuator output if MOT_SLEW_TIME > 0 and not shutdown void set_actuator_with_slew(float& actuator_output, float input); - // gradually increase actuator output maximum limit - float actuator_spin_up() const; + // gradually increase actuator output to ground idle + float actuator_spin_up_to_ground_idle() const; // apply any thrust compensation for the frame virtual void thrust_compensation(void) {} diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index a63d7af9db..54dd8e1714 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -86,8 +86,8 @@ void AP_MotorsSingle::output_to_motors() for (uint8_t i=0; i<NUM_ACTUATORS; i++) { rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); } - set_actuator_with_slew(_actuator[5], actuator_spin_up()); - set_actuator_with_slew(_actuator[6], actuator_spin_up()); + set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle()); + set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle()); rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5])); rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6])); break; diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp index 928406fa7e..03fbd9fb56 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.cpp +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -90,10 +90,10 @@ void AP_MotorsTailsitter::output_to_motors() SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, get_pwm_output_min()); break; case GROUND_IDLE: - throttle_pwm = output_to_pwm(actuator_spin_up()); - set_actuator_with_slew(_actuator[1], actuator_spin_up()); - SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, output_to_pwm(actuator_spin_up())); - SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, output_to_pwm(actuator_spin_up())); + throttle_pwm = output_to_pwm(actuator_spin_up_to_ground_idle()); + set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, output_to_pwm(actuator_spin_up_to_ground_idle())); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, output_to_pwm(actuator_spin_up_to_ground_idle())); break; case SPOOL_UP: case THROTTLE_UNLIMITED: diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 441d2d0239..29cda819b5 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -87,9 +87,9 @@ void AP_MotorsTri::output_to_motors() break; case GROUND_IDLE: // sends output to motors when armed but not flying - set_actuator_with_slew(_actuator[1], actuator_spin_up()); - set_actuator_with_slew(_actuator[2], actuator_spin_up()); - set_actuator_with_slew(_actuator[4], actuator_spin_up()); + set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle()); + set_actuator_with_slew(_actuator[2], actuator_spin_up_to_ground_idle()); + set_actuator_with_slew(_actuator[4], actuator_spin_up_to_ground_idle()); rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1])); rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2])); rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4]));