AP_Motors: rename throttle_ave_max to throttle_avg_max

This commit is contained in:
Randy Mackay 2016-06-08 17:48:54 +09:00
parent ef106e4b0f
commit a1b573ed0a
6 changed files with 18 additions and 18 deletions

View File

@ -196,7 +196,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
limit.throttle_upper = true;
}
_throttle_ave_max = constrain_float(_throttle_ave_max, throttle_thrust, _throttle_thrust_max);
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
@ -219,15 +219,15 @@ void AP_MotorsCoax::output_armed_stabilizing()
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(fabsf(rp_scale * rp_thrust_max), fabsf(yaw_thrust));
thr_adj = throttle_thrust - _throttle_ave_max;
if (thr_adj < (thrust_min_rpy - _throttle_ave_max)) {
thr_adj = throttle_thrust - _throttle_avg_max;
if (thr_adj < (thrust_min_rpy - _throttle_avg_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rpy, _throttle_ave_max) - _throttle_ave_max;
thr_adj = MIN(thrust_min_rpy, _throttle_avg_max) - _throttle_avg_max;
}
// calculate the throttle setting for the lift fan
thrust_out = _throttle_ave_max + thr_adj;
thrust_out = _throttle_avg_max + thr_adj;
if (fabsf(yaw_thrust) > thrust_out) {
yaw_thrust = constrain_float(yaw_thrust, -thrust_out, thrust_out);

View File

@ -175,7 +175,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
limit.throttle_upper = true;
}
_throttle_ave_max = constrain_float(_throttle_ave_max, throttle_thrust, _throttle_thrust_max);
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
// calculate throttle that gives most possible room for yaw which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
@ -190,7 +190,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// 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
throttle_thrust_best_rpy = MIN(0.5f, _throttle_ave_max);
throttle_thrust_best_rpy = MIN(0.5f, _throttle_avg_max);
// calculate roll and pitch for each motor
// calculate the amount of yaw input that each motor can accept
@ -240,7 +240,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
}
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, _throttle_ave_max);
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, _throttle_avg_max);
if (is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {

View File

@ -200,7 +200,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
limit.throttle_upper = true;
}
_throttle_ave_max = constrain_float(_throttle_ave_max, throttle_thrust, _throttle_thrust_max);
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
@ -234,15 +234,15 @@ void AP_MotorsSingle::output_armed_stabilizing()
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(MAX(fabsf(actuator[0]), fabsf(actuator[1])), MAX(fabsf(actuator[2]), fabsf(actuator[3])));
thr_adj = throttle_thrust - _throttle_ave_max;
if (thr_adj < (thrust_min_rpy - _throttle_ave_max)) {
thr_adj = throttle_thrust - _throttle_avg_max;
if (thr_adj < (thrust_min_rpy - _throttle_avg_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rpy, _throttle_ave_max) - _throttle_ave_max;
thr_adj = MIN(thrust_min_rpy, _throttle_avg_max) - _throttle_avg_max;
}
// calculate the throttle setting for the lift fan
_thrust_out = _throttle_ave_max + thr_adj;
_thrust_out = _throttle_avg_max + thr_adj;
if (is_zero(_thrust_out)) {
limit.roll_pitch = true;

View File

@ -211,7 +211,7 @@ void AP_MotorsTri::output_armed_stabilizing()
limit.throttle_upper = true;
}
_throttle_ave_max = constrain_float(_throttle_ave_max, throttle_thrust, _throttle_thrust_max);
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
// The following mix may be offer less coupling between axis but needs testing
//_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
@ -248,7 +248,7 @@ void AP_MotorsTri::output_armed_stabilizing()
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, _throttle_ave_max);
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, _throttle_avg_max);
if(is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {

View File

@ -32,7 +32,7 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
_pitch_in(0.0f),
_yaw_in(0.0f),
_throttle_in(0.0f),
_throttle_ave_max(0.0f),
_throttle_avg_max(0.0f),
_throttle_filter(),
_spool_desired(DESIRED_SHUT_DOWN),
_batt_voltage(0.0f),

View File

@ -63,7 +63,7 @@ public:
void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1
void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1
void set_throttle_ave_max(float throttle_ave_max) { _throttle_ave_max = constrain_float(throttle_ave_max,0.0f,1.0f); }; // range 0 ~ 1
void set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max,0.0f,1.0f); }; // range 0 ~ 1
void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
// accessors for roll, pitch, yaw and throttle inputs to motors
@ -170,7 +170,7 @@ protected:
float _pitch_in; // desired pitch control from attitude controller, -1 ~ +1
float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1
float _throttle_in; // last throttle input from set_throttle caller
float _throttle_ave_max; // last throttle input from set_throttle_ave_max
float _throttle_avg_max; // last throttle input from set_throttle_avg_max
LowPassFilterFloat _throttle_filter; // throttle input filter
spool_up_down_desired _spool_desired; // desired spool state