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

AP_Motors: Rename and move Throttle Mix / Comp

Browse Source
This commit is contained in:
Leonard Hall 2015-05-03 23:51:05 +09:30 committed by Randy Mackay
parent a2a0d834aa
commit 0f894ac1a8
3 changed files with 33 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -240,7 +240,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it
int16_t motor_mid = (rpy_low+rpy_high)/2;
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_low_comp)+get_hover_throttle_as_pwm()*_throttle_low_comp));
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_thr_mix)+get_hover_throttle_as_pwm()*_throttle_thr_mix));
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller

29
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -93,6 +93,13 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("CURR_MAX", 12, AP_Motors, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT),
// @Param: THR_MIX_MIN
// @DisplayName: Throttle Mix Minimum
// @Description: Minimum ratio that the average throttle can increase above the desired throttle after roll, pitch and yaw are mixed
// @Range: 0.1 0.25
// @User: Advanced
AP_GROUPINFO("THR_MIX_MIN", 13, AP_Motors, _thr_mix_min, AP_MOTORS_THR_MIX_MIN_DEFAULT),
AP_GROUPEND
};
@ -108,8 +115,8 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_spin_when_armed_ramped(0),
_throttle_low_comp(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_throttle_low_comp_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_throttle_thr_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_throttle_thr_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_batt_voltage(0.0f),
_batt_voltage_resting(0.0f),
_batt_current(0.0f),
@ -187,7 +194,7 @@ void AP_Motors::output()
update_lift_max_from_batt_voltage();
// move throttle_low_comp towards desired throttle low comp
update_throttle_low_comp();
update_throttle_thr_mix();
if (_flags.armed) {
if (!_flags.interlock) {
@ -354,18 +361,18 @@ void AP_Motors::update_battery_resistance()
}
}
// update_throttle_low_comp - slew set_throttle_low_comp to requested value
void AP_Motors::update_throttle_low_comp()
// update_throttle_thr_mix - slew set_throttle_thr_mix to requested value
void AP_Motors::update_throttle_thr_mix()
{
// slew _throttle_low_comp to _throttle_low_comp_desired
if (_throttle_low_comp < _throttle_low_comp_desired) {
// slew _throttle_thr_mix to _throttle_thr_mix_desired
if (_throttle_thr_mix < _throttle_thr_mix_desired) {
// increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
_throttle_low_comp += min(2.0f/_loop_rate, _throttle_low_comp_desired-_throttle_low_comp);
} else if (_throttle_low_comp > _throttle_low_comp_desired) {
_throttle_thr_mix += min(2.0f/_loop_rate, _throttle_thr_mix_desired-_throttle_thr_mix);
} else if (_throttle_thr_mix > _throttle_thr_mix_desired) {
// reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
_throttle_low_comp -= min(0.5f/_loop_rate, _throttle_low_comp-_throttle_low_comp_desired);
_throttle_thr_mix -= min(0.5f/_loop_rate, _throttle_thr_mix-_throttle_thr_mix_desired);
}
_throttle_low_comp = constrain_float(_throttle_low_comp, 0.1f, 1.0f);
_throttle_thr_mix = constrain_float(_throttle_thr_mix, 0.1f, 1.0f);
}
float AP_Motors::get_compensation_gain() const

22
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -63,6 +63,9 @@
#define AP_MOTORS_THST_BAT_MIN_DEFAULT 0.0f
#define AP_MOTORS_CURR_MAX_DEFAULT 0.0f // current limiting max default
#define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5f // battery voltage filtered at 0.5hz
#define AP_MOTORS_THR_MIX_MIN_DEFAULT 0.1f // minimum throttle mix
#define AP_MOTORS_THR_MIX_MID_DEFAULT 0.5f // manual throttle mix
#define AP_MOTORS_THR_MIX_MAX_DEFAULT 0.9f // maximum throttle mix
// bit mask for recording which limits we have reached when outputting to motors
#define AP_MOTOR_NO_LIMITS_REACHED 0x00
@ -165,13 +168,15 @@ public:
// set_density_ratio - sets air density as a proportion of sea level density
void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; }
// set_throttle_low_comp - set desired throttle_low_comp (actual throttle_low_comp is slewed towards this value over 1~2 seconds)
// set_throttle_thr_mix - set desired throttle_thr_mix (actual throttle_thr_mix is slewed towards this value over 1~2 seconds)
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void set_throttle_low_comp(float throttle_low_comp) { _throttle_low_comp_desired = throttle_low_comp; }
void set_throttle_mix_min() { _throttle_thr_mix_desired = _thr_mix_min; }
void set_throttle_mix_mid() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MID_DEFAULT; }
void set_throttle_mix_max() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MAX_DEFAULT; }
// get_throttle_low_comp - get low throttle compensation value
float get_throttle_low_comp() { return _throttle_low_comp; }
// get_throttle_thr_mix - get low throttle compensation value
float is_throttle_mix_min() { return (_throttle_thr_mix < 1.25f*_thr_mix_min); }
// get_lift_max - get maximum lift ratio
float get_lift_max() { return _lift_max; }
@ -235,8 +240,8 @@ protected:
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void update_battery_resistance();
// update_throttle_low_comp - updates thr_low_comp value towards the target
void update_throttle_low_comp();
// update_throttle_thr_mix - updates thr_low_comp value towards the target
void update_throttle_thr_mix();
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
@ -266,6 +271,7 @@ protected:
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
AP_Float _thr_mix_min; // current over which maximum throttle is limited
// internal variables
RC_Channel& _rc_roll; // roll input in from users is held in servo_out
@ -278,8 +284,8 @@ protected:
int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start)
int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero
float _throttle_low_comp; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_low_comp_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
// battery voltage compensation variables
float _batt_voltage; // latest battery voltage reading