Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsMulticopter: SPIN_MIN replaces min_throttle

Browse Source
This commit is contained in:
Leonard Hall 2016-06-09 15:34:47 +09:00 committed by Randy Mackay
parent 2c811364d3
commit 5cace33ca5
2 changed files with 20 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -102,6 +102,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @User: Advanced
AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0),
// @Param: SPIN_MIN
// @DisplayName: Motor Spin minimum
// @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range
// @Values: 0.0:Low, 0.15:Default, 0.3:High
// @User: Advanced
AP_GROUPINFO("SPIN_MIN", 18, AP_MotorsMulticopter, _thrust_curve_min, AP_MOTORS_SPIN_MIN_DEFAULT),
// @Param: SPIN_ARM
// @DisplayName: Motor Spin armed
// @Description: Point at which the motors start to spin expressed as a number from 0 to 1 in the entire output range
@ -145,8 +152,8 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz
_batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
_batt_voltage_filt.reset(1.0f);
// default throttle ranges (i.e. _min_throttle, _throttle_radio_min, _throttle_radio_max)
set_throttle_range(130, 1100, 1900);
// default throttle ranges (i.e. _throttle_radio_min, _throttle_radio_max)
set_throttle_range(1100, 1900);
};
// output - sends commands to the motors
@ -309,8 +316,8 @@ float AP_MotorsMulticopter::get_compensation_gain() const
int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
{
thrust_in = constrain_float(thrust_in, 0, 1);
return constrain_int16((get_pwm_output_min() + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) *
(get_pwm_output_max() - (get_pwm_output_min() + _min_throttle))), get_pwm_output_min() + _min_throttle, get_pwm_output_max());
return get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * (_thrust_curve_min + (_thrust_curve_max-_thrust_curve_min)*apply_thrust_curve_and_volt_scaling(thrust_in));
}
int16_t AP_MotorsMulticopter::calc_spin_up_to_pwm() const
{
@ -338,15 +345,13 @@ int16_t AP_MotorsMulticopter::get_pwm_output_max() const
// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets throttle channel minimum and maximum pwm
void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max)
void AP_MotorsMulticopter::set_throttle_range(int16_t radio_min, int16_t radio_max)
{
// sanity check
if ((radio_max > radio_min) && (min_throttle < (radio_max - radio_min))) {
if ((radio_max > radio_min)) {
_throttle_radio_min = radio_min;
_throttle_radio_max = radio_max;
}
// update _min_throttle
_min_throttle = (float)min_throttle * ((get_pwm_output_max() - get_pwm_output_min()) / 1000.0f);
}
// update the throttle input filter. should be called at 100hz
@ -415,8 +420,8 @@ void AP_MotorsMulticopter::output_logic()
}
} else { // _spool_desired == SPIN_WHEN_ARMED
float spin_up_armed_ratio = 0.0f;
if (_min_throttle > 0) {
spin_up_armed_ratio = _thrust_curve_arm / _min_throttle;
if (_thrust_curve_min > 0.0f) {
spin_up_armed_ratio = _thrust_curve_arm / _thrust_curve_min;
}
_spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step);
}

9
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -17,6 +17,7 @@
#define AP_MOTORS_THST_EXPO_DEFAULT 0.65f // set to 0 for linear and 1 for second order approximation
#define AP_MOTORS_THST_HOVER_DEFAULT 0.5f // the estimated hover throttle, 0 ~ 1
#define AP_MOTORS_THST_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1
#define AP_MOTORS_SPIN_MIN_DEFAULT 0.15f // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define AP_MOTORS_SPIN_MAX_DEFAULT 0.95f // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define AP_MOTORS_SPIN_ARM_DEFAULT 0.10f // throttle out ratio which produces the armed spin rate. (i.e. 0 ~ 1 ) of the full throttle range
#define AP_MOTORS_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default
@ -49,7 +50,7 @@ public:
// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets minimum and maximum pwm values that will be sent to the motors
void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max);
void set_throttle_range(int16_t radio_min, int16_t radio_max);
// update estimated throttle required to hover
void update_throttle_hover(float dt);
@ -146,7 +147,8 @@ 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 _thrust_curve_max; // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _thrust_curve_min; // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _thrust_curve_max; // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _thrust_curve_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
@ -158,10 +160,8 @@ protected:
// internal variables
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled
int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
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)
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 _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min
@ -174,6 +174,7 @@ protected:
int16_t _batt_timer; // timer used in battery resistance calcs
float _lift_max; // maximum lift ratio from battery voltage
float _throttle_limit; // ratio of throttle limit between hover and maximum
float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
// vehicle supplied callback for thrust compensation. Used for tiltrotors and tiltwings
thrust_compensation_fn_t _thrust_compensation_callback;