Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Add actuator output slew time to multicopters

Browse Source 
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.
This commit is contained in:
Matt 2018-11-30 09:34:14 -05:00 committed by Randy Mackay
parent 106d4058b4
commit 3d3f06cacb
7 changed files with 149 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -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<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
}
rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
set_actuator_with_slew(_actuator[5], actuator_spin_up());
set_actuator_with_slew(_actuator[6], actuator_spin_up());
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
@ -93,8 +95,10 @@ void AP_MotorsCoax::output_to_motors()
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
}
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_yt_ccw));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_yt_cw));
set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_yt_ccw));
set_actuator_with_slew(_actuator[6], thrust_to_actuator(_thrust_yt_cw));
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
break;
}
}

18
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -72,19 +72,13 @@ void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, mo
void AP_MotorsMatrix::output_to_motors()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
switch (_spool_mode) {
case SHUT_DOWN: {
// sends minimum values out to the motors
// set motor output based on thrust requests
// no output
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (_disarm_disable_pwm && _disarm_safety_timer == 0 && !armed()) {
motor_out[i] = 0;
} else {
motor_out[i] = get_pwm_output_min();
}
_actuator[i] = 0.0f;
}
}
break;
@ -93,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]) {
motor_out[i] = calc_spin_up_to_pwm();
set_actuator_with_slew(_actuator[i], actuator_spin_up());
}
}
break;
@ -103,16 +97,16 @@ void AP_MotorsMatrix::output_to_motors()
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
}
}
break;
}
// send output to each motor
// convert output to PWM and send to each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, motor_out[i]);
rc_write(i, output_to_pwm(_actuator[i]));
}
}
}

90
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -185,6 +185,24 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @User: Advanced
AP_GROUPINFO("BAT_IDX", 39, AP_MotorsMulticopter, _batt_idx, 0),
// @Param: SLEW_UP_TIME
// @DisplayName: Output slew time for increasing throttle
// @Description: Time in seconds to slew output from zero to full. For medium size copter such as a Solo, a value of 0.25 is a good starting point. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
// @Range: 0 .5
// @Units: s
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("SLEW_UP_TIME", 40, AP_MotorsMulticopter, _slew_up_time, AP_MOTORS_SLEW_TIME_DEFAULT),
// @Param: SLEW_DN_TIME
// @DisplayName: Output slew time for decreasing throttle
// @Description: Time in seconds to slew output from full to zero. For medium size copter such as a Solo, a value of 0.275 is a good starting point. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
// @Range: 0 .5
// @Units: s
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("SLEW_DN_TIME", 41, AP_MotorsMulticopter, _slew_dn_time, AP_MOTORS_SLEW_TIME_DEFAULT),
AP_GROUPEND
};
@ -363,17 +381,69 @@ float AP_MotorsMulticopter::get_compensation_gain() const
return ret;
}
int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
// convert actuator output (0~1) range to pwm range
int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
{
thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
return get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * (_spin_min + (_spin_max-_spin_min)*apply_thrust_curve_and_volt_scaling(thrust_in));
float pwm_output;
if (_spool_mode == SHUT_DOWN) {
// in shutdown mode, use PWM 0 or minimum PWM
if (_disarm_disable_pwm && _disarm_safety_timer == 0 && !armed()) {
pwm_output = 0;
} else {
pwm_output = get_pwm_output_min();
}
} else {
// in all other spool modes, covert to desired PWM
pwm_output = get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * actuator;
}
return pwm_output;
}
int16_t AP_MotorsMulticopter::calc_spin_up_to_pwm() const
// converts desired thrust to linearized actuator output in a range of 0~1
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in)
{
return get_pwm_output_min() + constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min * (get_pwm_output_max()-get_pwm_output_min());
thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
return _spin_min + (_spin_max-_spin_min)*apply_thrust_curve_and_volt_scaling(thrust_in);
}
// get minimum or maximum pwm value that can be output to motors
// adds slew rate limiting to actuator output
void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float input)
{
/*
If MOT_SLEW_UP_TIME is 0 (default), no slew limit is applied to increasing output.
If MOT_SLEW_DN_TIME is 0 (default), no slew limit is applied to decreasing output.
MOT_SLEW_UP_TIME and MOT_SLEW_DN_TIME are constrained to 0.0~0.5 for sanity.
If spool mode is shutdown, no slew limit is applied to allow immediate disarming of motors.
*/
// Output limits with no slew time applied
float output_slew_limit_up = 1.0f;
float output_slew_limit_dn = 0.0f;
// If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0
if (is_positive(_slew_up_time)) {
float output_delta_up_max = 1.0f/(constrain_float(_slew_up_time,0.0f,0.5f) * _loop_rate);
output_slew_limit_up = constrain_float(actuator_output + output_delta_up_max, 0.0f, 1.0f);
}
// If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0
if (is_positive(_slew_dn_time)) {
float output_delta_dn_max = 1.0f/(constrain_float(_slew_dn_time,0.0f,0.5f) * _loop_rate);
output_slew_limit_dn = constrain_float(actuator_output - output_delta_dn_max, 0.0f, 1.0f);
}
// Constrain change in output to within the above limits
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
{
return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
}
// get minimum pwm value that can be output to motors
int16_t AP_MotorsMulticopter::get_pwm_output_min() const
{
// return _pwm_min if both PWM_MIN and PWM_MAX parameters are defined and valid
@ -629,7 +699,6 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float r
{
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
int16_t motor_out;
if (mask & (1U<<i)) {
/*
apply rudder mixing differential thrust
@ -637,12 +706,11 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float r
apples to either tilted motors or tailsitters
*/
float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
motor_out = calc_thrust_to_pwm(thrust + diff_thrust);
set_actuator_with_slew(_actuator[i], thrust_to_actuator(thrust + diff_thrust));
rc_write(i, output_to_pwm(_actuator[i]));
} else {
motor_out = get_pwm_output_min();
rc_write(i, get_pwm_output_min());
}
rc_write(i, motor_out);
}
}
}

31
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -22,6 +22,7 @@
#define AP_MOTORS_BAT_CURR_MAX_DEFAULT 0.0f // current limiting max default
#define AP_MOTORS_BAT_CURR_TC_DEFAULT 5.0f // Time constant used to limit the maximum current
#define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5f // battery voltage filtered at 0.5hz
#define AP_MOTORS_SLEW_TIME_DEFAULT 0.0f // slew rate limit for thrust output
// spool definition
#define AP_MOTORS_SPOOL_UP_TIME_DEFAULT 0.5f // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle.
@ -90,7 +91,7 @@ public:
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
// run spool logic
@ -114,11 +115,17 @@ protected:
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
// convert thrust (0~1) range back to pwm range
int16_t calc_thrust_to_pwm(float thrust_in) const;
// convert actuator output (0~1) range to pwm range
int16_t output_to_pwm(float _actuator_output);
// converts desired thrust to linearized actuator output in a range of 0~1
float thrust_to_actuator(float thrust_in);
// 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);
// 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 @@ protected:
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 @@ protected:
// 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 @@ protected:
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 @@ protected:
// 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];
};

16
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -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; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
}
rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
set_actuator_with_slew(_actuator[5], actuator_spin_up());
set_actuator_with_slew(_actuator[6], actuator_spin_up());
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
@ -96,8 +98,10 @@ void AP_MotorsSingle::output_to_motors()
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
}
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_out));
set_actuator_with_slew(_actuator[6], thrust_to_actuator(_thrust_out));
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
break;
}
}

13
libraries/AP_Motors/AP_MotorsTailsitter.cpp
View File

@ -90,16 +90,17 @@ 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 = calc_spin_up_to_pwm();
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, calc_spin_up_to_pwm());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, calc_spin_up_to_pwm());
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()));
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
throttle_pwm = calc_thrust_to_pwm(_throttle);
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, calc_thrust_to_pwm(_thrust_left));
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, calc_thrust_to_pwm(_thrust_right));
throttle_pwm = output_to_pwm(thrust_to_actuator(_throttle));
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, output_to_pwm(thrust_to_actuator(_thrust_left)));
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, output_to_pwm(thrust_to_actuator(_thrust_right)));
break;
}

24
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -80,25 +80,31 @@ void AP_MotorsTri::output_to_motors()
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_2, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_4, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_1, output_to_pwm(0));
rc_write(AP_MOTORS_MOT_2, output_to_pwm(0));
rc_write(AP_MOTORS_MOT_4, output_to_pwm(0));
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_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;
}