mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: fix optimum thrust calcs
This commit is contained in:
parent
b47d575f65
commit
eb6ab53f6c
|
@ -136,6 +136,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
|
||||||
float pitch_thrust; // pitch thrust input value, +/- 1.0
|
float pitch_thrust; // pitch thrust input value, +/- 1.0
|
||||||
float yaw_thrust; // yaw thrust input value, +/- 1.0
|
float yaw_thrust; // yaw thrust input value, +/- 1.0
|
||||||
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
|
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
|
||||||
|
float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
|
||||||
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
|
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
|
||||||
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
|
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
|
||||||
float rpy_low = 0.0f; // lowest motor value
|
float rpy_low = 0.0f; // lowest motor value
|
||||||
|
@ -149,6 +150,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
|
||||||
pitch_thrust = _pitch_in * get_compensation_gain();
|
pitch_thrust = _pitch_in * get_compensation_gain();
|
||||||
yaw_thrust = _yaw_in * get_compensation_gain();
|
yaw_thrust = _yaw_in * get_compensation_gain();
|
||||||
throttle_thrust = get_throttle() * get_compensation_gain();
|
throttle_thrust = get_throttle() * get_compensation_gain();
|
||||||
|
throttle_avg_max = _throttle_avg_max * get_compensation_gain();
|
||||||
|
|
||||||
// sanity check throttle is above zero and below current limited throttle
|
// sanity check throttle is above zero and below current limited throttle
|
||||||
if (throttle_thrust <= 0.0f) {
|
if (throttle_thrust <= 0.0f) {
|
||||||
|
@ -160,7 +162,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
|
||||||
limit.throttle_upper = true;
|
limit.throttle_upper = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
_throttle_avg_max = constrain_float(_throttle_avg_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:
|
// 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
|
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
|
||||||
|
@ -175,7 +177,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
|
||||||
// calculate amount of yaw we can fit into the throttle range
|
// 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
|
// 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_avg_max);
|
throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
|
||||||
|
|
||||||
// calculate roll and pitch for each motor
|
// calculate roll and pitch for each motor
|
||||||
// calculate the amount of yaw input that each motor can accept
|
// calculate the amount of yaw input that each motor can accept
|
||||||
|
@ -184,12 +186,12 @@ void AP_MotorsMatrix::output_armed_stabilizing()
|
||||||
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
|
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
|
||||||
if (!is_zero(_yaw_factor[i])){
|
if (!is_zero(_yaw_factor[i])){
|
||||||
if (yaw_thrust * _yaw_factor[i] > 0.0f) {
|
if (yaw_thrust * _yaw_factor[i] > 0.0f) {
|
||||||
unused_range = fabsf((1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]))/_yaw_factor[i]);
|
unused_range = fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]);
|
||||||
if (yaw_allowed > unused_range) {
|
if (yaw_allowed > unused_range) {
|
||||||
yaw_allowed = unused_range;
|
yaw_allowed = unused_range;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
unused_range = fabsf((throttle_thrust_best_rpy + _thrust_rpyt_out[i])/_yaw_factor[i]);
|
unused_range = fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]);
|
||||||
if (yaw_allowed > unused_range) {
|
if (yaw_allowed > unused_range) {
|
||||||
yaw_allowed = unused_range;
|
yaw_allowed = unused_range;
|
||||||
}
|
}
|
||||||
|
@ -225,11 +227,15 @@ void AP_MotorsMatrix::output_armed_stabilizing()
|
||||||
}
|
}
|
||||||
|
|
||||||
// check everything fits
|
// check everything fits
|
||||||
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, _throttle_avg_max);
|
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_avg_max);
|
||||||
if (is_zero(rpy_low)){
|
if (is_zero(rpy_low) && is_zero(rpy_high)){
|
||||||
rpy_scale = 1.0f;
|
rpy_scale = 1.0f;
|
||||||
} else {
|
} else if (is_zero(rpy_low)) {
|
||||||
|
rpy_scale = constrain_float((1.0f-throttle_thrust_best_rpy)/rpy_high, 0.0f, 1.0f);
|
||||||
|
} else if (is_zero(rpy_high)) {
|
||||||
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
|
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
|
||||||
|
} else {
|
||||||
|
rpy_scale = constrain_float(MIN(-throttle_thrust_best_rpy/rpy_low, (1.0f-throttle_thrust_best_rpy)/rpy_high), 0.0f, 1.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
// calculate how close the motors can come to the desired throttle
|
// calculate how close the motors can come to the desired throttle
|
||||||
|
|
Loading…
Reference in New Issue