電機輸出代碼邏輯整理（二）
待解決問題
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值，進而控制電機的運轉。
![PIXHAWK上安裝PX4Flow光流傳感器及調試過程[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)