Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_Motors: Add multicopter thrust slew limit #9929

Merged
merged 1 commit into from Jan 21, 2019
Merged

AP_Motors: Add multicopter thrust slew limit #9929

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
16 changes: 10 additions & 6 deletions libraries/AP_Motors/AP_MotorsCoax.cpp
View file
Open in desktop
Expand Up @@ -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:
Expand All @@ -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;
}
}
Expand Down
18 changes: 6 additions & 12 deletions libraries/AP_Motors/AP_MotorsMatrix.cpp
View file
Open in desktop
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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]));
}
}
}
Expand Down
88 changes: 78 additions & 10 deletions libraries/AP_Motors/AP_MotorsMulticopter.cpp
View file
Open in desktop
Expand Up @@ -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
};

Expand Down Expand Up @@ -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)
{
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;
}

// converts desired thrust to linearized actuator output in a range of 0~1
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in)
{
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));
return _spin_min + (_spin_max-_spin_min)*apply_thrust_curve_and_volt_scaling(thrust_in);
}

// 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);
}

int16_t AP_MotorsMulticopter::calc_spin_up_to_pwm() const
// gradually increase actuator output maximum limit
float AP_MotorsMulticopter::actuator_spin_up() const
{
return get_pwm_output_min() + constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min * (get_pwm_output_max()-get_pwm_output_min());
return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
}
// get minimum or maximum pwm value that can be output to motors

// 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
Expand Down Expand Up @@ -629,20 +699,18 @@ 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
copter frame roll is plane frame yaw as this only
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);
}
}
}
Expand Down
31 changes: 22 additions & 9 deletions libraries/AP_Motors/AP_MotorsMulticopter.h
View file
Open in desktop
Expand Up @@ -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.
Expand Down Expand Up @@ -90,7 +91,7 @@ class AP_MotorsMulticopter : public AP_Motors {

// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

protected:

// run spool logic
Expand All @@ -114,11 +115,17 @@ class AP_MotorsMulticopter : public AP_Motors {
// 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;
Copy link
Contributor

@rmackay9 rmackay9 Jan 16, 2019 •
edited

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder if we could come up with a more easily understood name and/or comment for the "actuator_spin_up()" function. maybe it's "actuator_spin_up_to_ground_idle()". That's a bit long but "spin_up" alone doesn't really clarify that it's only spinning up to ground-idle. the comment as it is might make future developers think it spins up to throttle-unlimited.


// apply any thrust compensation for the frame
virtual void thrust_compensation(void) {}
Expand All @@ -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 {
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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];
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

it would be good to add a comment re what's held in this array

};
16 changes: 10 additions & 6 deletions libraries/AP_Motors/AP_MotorsSingle.cpp
View file
Open in desktop
Expand Up @@ -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:
Expand All @@ -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;
}
}
Expand Down
13 changes: 7 additions & 6 deletions libraries/AP_Motors/AP_MotorsTailsitter.cpp
View file
Open in desktop
Expand Up @@ -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;
}

Expand Down