电机输出代码逻辑整理（二）
待解决问题
1.roll、pitch、yaw等因子facor是怎么得来的；
2.筛选可接受的控制量是如何进行的；
3.如何计算得到最后的pwm值。
将在这篇文章中为大家带来分析。
这里我们先来看，最终的pwm值是如何计算的，发送到电机的的最终pwm值是在AP_MotorsMatrix这个类里的函数output_to_motors()定义的，在switch语句里面当是SPOOL_DOWN时，根据推力要求得到的pwm值是由motor_out[i] = calc_thrust_to_pwm (_thrust_rpyt_out[i])计算得到的。
void AP_MotorsMatrix::output_to_motors()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // 发送到电机的最终PWM值
switch (_spool_mode) {
case SHUT_DOWN: {
// 向电机发送最小值
// 根据推力要求设置电机输出
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (_disarm_disable_pwm && _disarm_safety_timer == 0 && !armed()) {
motor_out[i] = 0;
} else {
motor_out[i] = get_pwm_output_min();
}
}
}
break;
}
case SPIN_WHEN_ARMED:
// 当解锁启动但不飞行时，向电机发送输出
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = calc_spin_up_to_pwm();
}
}
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// 根据推力要求设置电机输出
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
}
}
break;
}
// 向每个电机发送输出
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, motor_out[i]);
}
}
}
对此我们先追踪到数组_thrust_rpyt_out[i])，这个数组它的作用是将俯仰、滚转、偏航以及油门这些输出到电机的量限制在0~1范围内，而它的计算是由AP_MotorsMatrix类里的函数output_armed_stabilizing()得来的，进入函数output_armed_stabilizing()：
void AP_MotorsMatrix::output_armed_stabilizing()
{
...
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
throttle_thrust_best_rpy = MIN(0.5f, _throttle_avg_max);
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[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;
}
}
}
}
}
}
在这段代码里，先是计算了最佳油门输出的rpy值，这是由下列公式计算得来的：
为什么会这样计算选择最佳油门输出的rpy值呢？选择这个油门值，为最大的偏航量提供最大的空间裕度，由于上述的计算公式，这将使最大可能的裕度高于最高电机并低于最低电机，换句话说，这里存在两种情况，第一种可能是0.5最小，此时这是偏航控制的最佳油门，当然这可能是以减少油门值为代价的；第二种可能是(rpy_low+rpy_high)/2最小，在这个情况下，确保我们永远不会增加油门高于油门的油门，除非飞控已经命令这个，允许我们将油门提高到飞控命令的水平值以上，但实际上并不会导致直升机上升，但是这个时候，倾向于减少油门为代价，已经是由指令和悬停油门二者的混控，已经不是情况一种更好的偏航控制油门。
之后计算了每个电机可以接受的偏航输入量，以及发送给每个电机的俯仰和滚转量，这里我们需要计算roll、pitch、yaw的factor因子，先进入setup_motors函数对构型进行相应的选择，此后进入add_motor函数，这里面有两种add_motor可以选择，不对称的可以自行选择更改数值。
//气动布局结构对称的无人机
void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order)
{
add_motor(motor_num, angle_degrees, angle_degrees, yaw_factor, testing_order);
}
//气动布局结构不对称的无人机
void AP_MotorsMatrix::add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order)
{
add_motor_raw(
motor_num,
cosf(radians(roll_factor_in_degrees + 90)),
cosf(radians(pitch_factor_in_degrees)),
yaw_factor,
testing_order);
}
这里我们进入函数add_motor_raw：
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
{
// 确保提供了有效的电机编号
if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
// 如果此电机是新启用的电机，则增加电机数量
if( !motor_enabled[motor_num] ) {
motor_enabled[motor_num] = true;
}
// 设置滚转、俯仰、油门系数和反向电机
_roll_factor[motor_num] = roll_fac;
_pitch_factor[motor_num] = pitch_fac;
_yaw_factor[motor_num] = yaw_fac;
// 设置电机出现在测试中的顺序
_test_order[motor_num] = testing_order;
// 调用父类方法
add_motor_num(motor_num);
}
}
然后我们就得到了各量的factor因子，从而计算得到_thrust_rpyt_out[i])，calc_thrust_to_pwm (_thrust_rpyt_out[i])，对于多旋翼我们进入AP_MotorsMulticopter类里的calc_thrust_to_pwm函数：
分析get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * (_spin_min + (_spin_max-_spin_min)*apply_thrust_curve_and_volt_scaling(thrust_in))
int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
{
thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
return get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * (_spin_min + (_spin_max-_spin_min)*apply_thrust_curve_and_volt_scaling(thrust_in));
}
对于constrain_float函数，是一个模板类，它的作用是将这三个值限定成一个范围，比如说，a,b,c，a的值在b、c之间，那么得到范围b~c。
对于函数返回式子中的每一项分别找到：
//获得输出到电机的最小pwm值
int16_t AP_MotorsMulticopter::get_pwm_output_min() const
{
// 如果PWM_MIN和PWM_MAX参数都已定义并有效，则返回_pwm_min
if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) {
return _pwm_min;
}
return _throttle_radio_min;
}
...
//获得输出到电机的最大pwm值
int16_t AP_MotorsMulticopter::get_pwm_output_max() const
{
// 如果PWM_MIN和PWM_MAX参数都已定义并有效，则返回_pwm_max
if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) {
return _pwm_max;
}
return _throttle_radio_max;
}
_spin_min是油门产生最小的推力，(即0 ~ 1)的全油门范围；
_spin_max是油门产生最大的推力，(即0 ~ 1)的全油门范围。
对于函数apply_thrust_curve_and_volt_scaling：
float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) const
{
float throttle_ratio = thrust;
// 应用推力曲线-域0.0至1.0，范围0.0至1.0
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
if (fabsf(thrust_curve_expo) < 0.001) {
// 零expo表示线性，避免小值的浮点异常
return thrust;
}
if(!is_zero(_batt_voltage_filt.get())) {
throttle_ratio = ((thrust_curve_expo-1.0f) + safe_sqrt((1.0f-thrust_curve_expo)*(1.0f-thrust_curve_expo) + 4.0f*thrust_curve_expo*_lift_max*thrust))/(2.0f*thrust_curve_expo*_batt_voltage_filt.get());
} else {
throttle_ratio = ((thrust_curve_expo-1.0f) + safe_sqrt((1.0f-thrust_curve_expo)*(1.0f-thrust_curve_expo) + 4.0f*thrust_curve_expo*_lift_max*thrust))/(2.0f*thrust_curve_expo);
}
return constrain_float(throttle_ratio, 0.0f, 1.0f);
}
这个函数的作用是应用推力曲线和电压缩放比例，将油门输出值限制在0~1范围内，最终返回得到输出到各个电机上的pwm值，从而控制电机的运转。
