From efb1d3136d24e7db996811d60710d3a6fb6124db Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 30 Nov 2018 09:34:14 -0500 Subject: [PATCH] AP_Motors: Add actuator output slew time to multicopters Adds slew time limiting for throttling up and throttling down to multicopters. New parameters MOT_SLEW_UP_TIME and MOT_SLEW_DN_TIME added. 0 = disabled, no slew limiting. Valid values are 0 to 0.5 seconds. Also reworked functions related to linearization and PWM conversion to make more flexible throughout the code. --- libraries/AP_Motors/AP_MotorsCoax.cpp | 16 ++-- libraries/AP_Motors/AP_MotorsMatrix.cpp | 18 ++-- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 88 +++++++++++++++++--- libraries/AP_Motors/AP_MotorsMulticopter.h | 31 +++++-- libraries/AP_Motors/AP_MotorsSingle.cpp | 16 ++-- libraries/AP_Motors/AP_MotorsTailsitter.cpp | 13 +-- libraries/AP_Motors/AP_MotorsTri.cpp | 24 ++++-- 7 files changed, 148 insertions(+), 58 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 30605ef4d3256..78db05b7e4ba0 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -75,16 +75,18 @@ void AP_MotorsCoax::output_to_motors() rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_4, -_pitch_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE); - rc_write(AP_MOTORS_MOT_5, get_pwm_output_min()); - rc_write(AP_MOTORS_MOT_6, get_pwm_output_min()); + rc_write(AP_MOTORS_MOT_5, output_to_pwm(0)); + rc_write(AP_MOTORS_MOT_6, output_to_pwm(0)); break; case GROUND_IDLE: // sends output to motors when armed but not flying for (uint8_t i=0; i 0 and not shutdown + void set_actuator_with_slew(float& actuator_output, float input); - // calculate spin up to pwm range - int16_t calc_spin_up_to_pwm() const; + // gradually increase actuator output maximum limit + float actuator_spin_up() const; // apply any thrust compensation for the frame virtual void thrust_compensation(void) {} @@ -127,7 +134,7 @@ class AP_MotorsMulticopter : public AP_Motors { virtual void output_boost_throttle(void); // save parameters as part of disarming - void save_params_on_disarm() override; + void save_params_on_disarm() override; // enum values for HOVER_LEARN parameter enum HoverLearn { @@ -139,9 +146,11 @@ class AP_MotorsMulticopter : public AP_Motors { // parameters AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range AP_Float _thrust_curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation - AP_Float _spin_min; // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range - AP_Float _spin_max; // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range - AP_Float _spin_arm; // throttle out ratio which produces the armed spin rate. (i.e. 0 ~ 1 ) of the full throttle range + AP_Float _slew_up_time; // throttle increase slew limitting + AP_Float _slew_dn_time; // throttle decrease slew limitting + AP_Float _spin_min; // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range + AP_Float _spin_max; // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range + AP_Float _spin_arm; // throttle out ratio which produces the armed spin rate. (i.e. 0 ~ 1 ) of the full throttle range AP_Float _batt_voltage_max; // maximum voltage used to scale lift AP_Float _batt_voltage_min; // minimum voltage used to scale lift AP_Float _batt_current_max; // current over which maximum throttle is limited @@ -166,6 +175,7 @@ class AP_MotorsMulticopter : public AP_Motors { bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled int16_t _throttle_radio_min; // minimum PWM from RC input's throttle channel (i.e. minimum PWM input from receiver, RC3_MIN) int16_t _throttle_radio_max; // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX) + // spool variables // spool variables float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min @@ -179,4 +189,7 @@ class AP_MotorsMulticopter : public AP_Motors { // vehicle supplied callback for thrust compensation. Used for tiltrotors and tiltwings thrust_compensation_fn_t _thrust_compensation_callback; + + // array of motor output values + float _actuator[AP_MOTORS_MAX_NUM_MOTORS]; }; diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index c8a888bdb5533..a63d7af9dbe3d 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -78,16 +78,18 @@ void AP_MotorsSingle::output_to_motors() rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_4, -_pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); - rc_write(AP_MOTORS_MOT_5, get_pwm_output_min()); - rc_write(AP_MOTORS_MOT_6, get_pwm_output_min()); + rc_write(AP_MOTORS_MOT_5, output_to_pwm(0)); + rc_write(AP_MOTORS_MOT_6, output_to_pwm(0)); break; case GROUND_IDLE: // sends output to motors when armed but not flying for (uint8_t i=0; iget_trim()); break; case GROUND_IDLE: // sends output to motors when armed but not flying - 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()); + 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()); + 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])); rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim()); break; case SPOOL_UP: case THROTTLE_UNLIMITED: case SPOOL_DOWN: // set motor output based on thrust requests - rc_write(AP_MOTORS_MOT_1, calc_thrust_to_pwm(_thrust_right)); - rc_write(AP_MOTORS_MOT_2, calc_thrust_to_pwm(_thrust_left)); - rc_write(AP_MOTORS_MOT_4, calc_thrust_to_pwm(_thrust_rear)); + set_actuator_with_slew(_actuator[1], thrust_to_actuator(_thrust_right)); + set_actuator_with_slew(_actuator[2], thrust_to_actuator(_thrust_left)); + set_actuator_with_slew(_actuator[4], thrust_to_actuator(_thrust_rear)); + 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])); rc_write(AP_MOTORS_CH_TRI_YAW, calc_yaw_radio_output(_pivot_angle, radians(_yaw_servo_angle_max_deg))); break; }