Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsMatrix: formatting fixes

This commit is contained in:
Randy Mackay 2018-09-08 08:46:30 +09:00
parent 54a59f581f
commit 853a56dc9b
1 changed files with 15 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -223,7 +223,6 @@ void AP_MotorsMatrix::output_armed_stabilizing()
}
// check for roll and pitch saturation
rpy_scale = 1.0f;
if (rp_high-rp_low > 1.0f || throttle_avg_max < -rp_low) {
// Full range is being used by roll and pitch.
limit.roll_pitch = true;
@ -269,11 +268,10 @@ void AP_MotorsMatrix::output_armed_stabilizing()
}
// calculate any scaling needed to make the combined thrust outputs fit within the output range
rpy_scale = 1.0f;
if (rpy_high-rpy_low > 1.0f) {
rpy_scale = 1.0f / (rpy_high-rpy_low);
}
if (rpy_low < 0.0f) {
if (is_negative(rpy_low)) {
rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
}
@ -305,7 +303,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + rpy_scale*_thrust_rpyt_out[i];
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]);
}
}
@ -333,7 +331,7 @@ void AP_MotorsMatrix::check_for_failed_motor(float throttle_thrust_best_plus_adj
float rpyt_high = 0.0f;
float rpyt_sum = 0.0f;
float number_motors = 0.0f;
uint8_t number_motors = 0.0f;
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
number_motors += 1;
@ -341,7 +339,7 @@ void AP_MotorsMatrix::check_for_failed_motor(float throttle_thrust_best_plus_adj
// record highest thrust command
if (_thrust_rpyt_out_filt[i] > rpyt_high) {
rpyt_high = _thrust_rpyt_out_filt[i];
// hold motor lost pointer constant while thrust balance is true
// hold motor lost index constant while thrust balance is true
if (_thrust_balanced) {
_motor_lost_index = i;
}