apm 3.4.6 四轴混控代码解析
AP_MotorsMatrix::output_armed_stabilizing：
1、针对每个舵机，计算roll和pitch占用的控制量，并按油门量最大不超过0.5计算yaw最大可使用的控制量
// calculate roll and pitch for each motor
// calculate the amount of yaw input that each motor can accept
for (i=0; i
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
if (!is_zero(_yaw_factor[i])){
if (yaw_thrust * _yaw_factor[i] > 0.0f) {
unused_range = fabsf((1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]))/_yaw_factor[i]);
if (yaw_allowed > unused_range) {
yaw_allowed = unused_range;
}
} else {
unused_range = fabsf((throttle_thrust_best_rpy + _thrust_rpyt_out[i])/_yaw_factor[i]);
if (yaw_allowed > unused_range) {
yaw_allowed = unused_range;
}
}
}
}
}
2、针对每个舵机，计算roll、pitch和yaw占用的控制量,并统计rpy_low,rpy_high。 rpy_low <=0:为最小的负控制量；rpy_high>=0:最大的正控制量
// add yaw to intermediate numbers for each motor
rpy_low = 0.0f;
rpy_high = 0.0f;
for (i=0; i
_thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i];
}
// record highest roll+pitch+yaw command
if (_thrust_rpyt_out[i] > rpy_high) {
rpy_high = _thrust_rpyt_out[i];
}
}
}
3、throttle_thrust_best_rpy为正；
rpy_scale为1.0f的情况下：保持前面计算的rpy控制不变，加上油门控制量，这个油门控制量基本上为throttle_thrust，但要修改满足最后算出的每个舵机的控制量在(0.0,1.0)范围内；
rpy_scale<1.0:这种情况发生在计算出来的油门量 throttle_thrust_best_rpy<(-rpy_low)。假设throttle_thrust_best_rpy=0.5f - (rpy_low+rpy_high)/2.0，此时会导致
(throttle_thrust_best_rpy+rpy_high)>1.0，所以需要将前面计算出来的rpy控制量乘以rpy_scale比例较小，满足每个舵机的控制量在(0.0,1.0)。
// check everything fits
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 {
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
}
// calculate how close the motors can come to the desired throttle
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f){
// Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f) {
limit.throttle_upper = true;
}
thr_adj = 0.0f;
} else {
if (thr_adj < -(throttle_thrust_best_rpy+rpy_low)){
// Throttle can't be reduced to desired value
thr_adj = -(throttle_thrust_best_rpy+rpy_low);
} else if (thr_adj > 1.0f - (throttle_thrust_best_rpy+rpy_high)){
// Throttle can't be increased to desired value
thr_adj = 1.0f - (throttle_thrust_best_rpy+rpy_high);
limit.throttle_upper = true;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + rpy_scale*_thrust_rpyt_out[i];
}
}