2.3[Lib]AP_Motors

前言

之前在分析RCOutput的源码时候，其中的output_ch的成员函数实现过程中，对RC通道的Function进行了分类输出，具体的分成了4类：

• Function manual 下RC通道为通道直通；

• Function rc1～rc16 下RC通道则是映射到实际的ch0~ch15;

• Function motor1～motor8 则是通过AP_Motors::rc_write来实现控制；

• 其他,则是直接输出到当前SRV_Channel绑定的物理通道；

所以这里就对AP_Motors进行分析：

AP_Motors

// offsets for motors in motor_out and _motor_filtered arrays
#define AP_MOTORS_MOT_1 0U
#define AP_MOTORS_MOT_2 1U
#define AP_MOTORS_MOT_3 2U
#define AP_MOTORS_MOT_4 3U
#define AP_MOTORS_MOT_5 4U
#define AP_MOTORS_MOT_6 5U
#define AP_MOTORS_MOT_7 6U
#define AP_MOTORS_MOT_8 7U

#define AP_MOTORS_MAX_NUM_MOTORS 8

// motor update rate
#define AP_MOTORS_SPEED_DEFAULT 490 // default output rate to the motors

/// @class AP_Motors
class AP_Motors {
public:

  enum motor_frame_class {
  MOTOR_FRAME_UNDEFINED = 0,
  MOTOR_FRAME_QUAD = 1,
  MOTOR_FRAME_HEXA = 2,
  MOTOR_FRAME_OCTA = 3,
  MOTOR_FRAME_OCTAQUAD = 4,
  MOTOR_FRAME_Y6 = 5,
  MOTOR_FRAME_HELI = 6,
  MOTOR_FRAME_TRI = 7,
  MOTOR_FRAME_SINGLE = 8,
  MOTOR_FRAME_COAX = 9,
  MOTOR_FRAME_TAILSITTER = 10,
  };
  enum motor_frame_type {
  MOTOR_FRAME_TYPE_PLUS = 0,
  MOTOR_FRAME_TYPE_X = 1,
  MOTOR_FRAME_TYPE_V = 2,
  MOTOR_FRAME_TYPE_H = 3,
  MOTOR_FRAME_TYPE_VTAIL = 4,
  MOTOR_FRAME_TYPE_ATAIL = 5,
  MOTOR_FRAME_TYPE_Y6B = 10,
  MOTOR_FRAME_TYPE_Y6F = 11 // for FireFlyY6
  };

  // Constructor
  AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);

  // check initialisation succeeded
  bool initialised_ok() const { return _flags.initialised_ok; }

  // arm, disarm or check status status of motors,通过AP_Notify传递解锁状态
  bool armed() const { return _flags.armed; }
  void armed(bool arm);

  // set motor interlock status
  void set_interlock(bool set) { _flags.interlock = set;}

  // get motor interlock status. true means motors run, false motors don't run
  bool get_interlock() const { return _flags.interlock; }

  // set_roll, set_pitch, set_yaw, set_throttle，设置各个通道输入值范围
  void set_roll(float roll_in) { _roll_in = roll_in; }; // range -1 ~ +1
  void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1
  void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1
  void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1
  void set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max,0.0f,1.0f); }; // range 0 ~ 1
  void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
  void set_forward(float forward_in) { _forward_in = forward_in; }; // range -1 ~ +1
  void set_lateral(float lateral_in) { _lateral_in = lateral_in; }; // range -1 ~ +1

  // accessors for roll, pitch, yaw and throttle inputs to motors，获取当前各个通道的输入值
  float get_roll() const { return _roll_in; }
  float get_pitch() const { return _pitch_in; }
  float get_yaw() const { return _yaw_in; }
  float get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); }
  float get_throttle_bidirectional() const { return constrain_float(2*(_throttle_filter.get()-0.5f),-1.0f,1.0f); }
  float get_forward() const { return _forward_in; }
  float get_lateral() const { return _lateral_in; }
  virtual float get_throttle_hover() const = 0;

  // spool up states
  enum spool_up_down_desired {
  DESIRED_SHUT_DOWN = 0, // all motors stop
  DESIRED_SPIN_WHEN_ARMED = 1, // all motors at spin when armed
  DESIRED_THROTTLE_UNLIMITED = 2, // motors are no longer constrained by start up procedure
  };

  virtual void set_desired_spool_state(enum spool_up_down_desired spool) { _spool_desired = spool; };

  enum spool_up_down_desired get_desired_spool_state(void) const { return _spool_desired; }

  //
  // voltage, current and air pressure compensation or limiting features - multicopters only
  //
  // set_voltage - set voltage to be used for output scaling
  void set_voltage(float volts){ _batt_voltage = volts; }

  // set_current - set current to be used for output scaling
  void set_current(float current){ _batt_current = current; }

  // set_density_ratio - sets air density as a proportion of sea level density
  void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; }

  // structure for holding motor limit flags
  struct AP_Motors_limit {
  uint8_t roll_pitch : 1; // we have reached roll or pitch limit
  uint8_t yaw : 1; // we have reached yaw limit
  uint8_t throttle_lower : 1; // we have reached throttle's lower limit
  uint8_t throttle_upper : 1; // we have reached throttle's upper limit
  } limit;

  //
  // virtual functions that should be implemented by child classes
  //

  // set update rate to motors - a value in hertz，设置Motors的更新频率
  virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; }

  // init，设置机身类型[6轴、8轴等轴数]和机身布局类型[V、H等布局类型]
  virtual void init(motor_frame_class frame_class, motor_frame_type frame_type) = 0;

  // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)，设置机身类型和机身布局类型
  virtual void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) = 0;

  // enable - starts allowing signals to be sent to motors，使能hal对应通道
  virtual void enable() = 0;

  // output - sends commands to the motors
  virtual void output() = 0;

  // output_min - sends minimum values out to the motors
  virtual void output_min() = 0;

  // output_test - spin a motor at the pwm value specified
  // motor_seq is the motor's sequence number from 1 to the number of motors on the frame
  // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
  // 指定电机以固定的频率旋转
  virtual void output_test(uint8_t motor_seq, int16_t pwm) = 0;

  // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
  // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
  // 返回当前正在运行的电机编号
  virtual uint16_t get_motor_mask() = 0;

  // pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
  // RC通道直通为姿态输入，直接控制电机
  void set_radio_passthrough(float roll_input, float pitch_input, float throttle_input, float yaw_input);

  // set loop rate. Used to support loop rate as a parameter
  void set_loop_rate(uint16_t loop_rate) { _loop_rate = loop_rate; }

  enum pwm_type { PWM_TYPE_NORMAL=0, PWM_TYPE_ONESHOT=1, PWM_TYPE_ONESHOT125=2, PWM_TYPE_BRUSHED16kHz=3 };
  pwm_type get_pwm_type(void) const { return (pwm_type)_pwm_type.get(); }

protected:
  // output functions that should be overloaded by child classes
  // 下面这些就是用于派生类 多态实现，具体分析我们参照实际的例子来
  virtual void output_armed_stabilizing()=0;
  virtual void rc_write(uint8_t chan, uint16_t pwm);
  virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz);
  virtual void rc_enable_ch(uint8_t chan);
  virtual uint32_t rc_map_mask(uint32_t mask) const;

  // add a motor to the motor map，增加一个电机
  void add_motor_num(int8_t motor_num);

  // update the throttle input filter，更新油门的滤波器
  virtual void update_throttle_filter() = 0;

  // save parameters as part of disarming，在上锁的时候保存参数
  virtual void save_params_on_disarm() {}

  // convert input in -1 to +1 range to pwm output,转换输入pwm范围到[-1,1]
  int16_t calc_pwm_output_1to1(float input, const SRV_Channel *servo);

  // convert input in 0 to +1 range to pwm output,转换输入pwm到[0,1]
  int16_t calc_pwm_output_0to1(float input, const SRV_Channel *servo);

  // flag bitmask
  struct AP_Motors_flags {
  uint8_t armed : 1; // 0 if disarmed, 1 if armed
  uint8_t interlock : 1; // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run)
  uint8_t initialised_ok : 1; // 1 if initialisation was successful
  } _flags;

  // internal variables 内部变量
  uint16_t _loop_rate; // rate in Hz at which output() function is called (normally 400hz)AP_Motor的执行频率
  uint16_t _speed_hz; // speed in hz to send updates to motors pwm输出的更新频率
  float _roll_in; // desired roll control from attitude controllers, -1 ~ +1
  float _pitch_in; // desired pitch control from attitude controller, -1 ~ +1
  float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1
  float _throttle_in; // last throttle input from set_throttle caller
  float _forward_in; // last forward input from set_forward caller
  float _lateral_in; // last lateral input from set_lateral caller
  float _throttle_avg_max; // last throttle input from set_throttle_avg_max
  LowPassFilterFloat _throttle_filter; // throttle input filter，油门低通滤波器
  spool_up_down_desired _spool_desired; // desired spool state

  // battery voltage, current and air pressure compensation variables，电池电压，电流和气压补偿变量
  float _batt_voltage; // latest battery voltage reading
  float _batt_current; // latest battery current reading
  float _air_density_ratio; // air density / sea level density - decreases in altitude

  // mapping to output channels，电机通道映射
  uint8_t _motor_map[AP_MOTORS_MAX_NUM_MOTORS];
  uint16_t _motor_map_mask;
  uint16_t _motor_fast_mask;

  // pass through variables，这个不明白意思
  float _roll_radio_passthrough = 0.0f; // roll input from pilot in -1 ~ +1 range. used for setup and providing servo feedback while landed
  float _pitch_radio_passthrough = 0.0f; // pitch input from pilot in -1 ~ +1 range. used for setup and providing servo feedback while landed
  float _throttle_radio_passthrough = 0.0f; // throttle/collective input from pilot in 0 ~ 1 range. used for setup and providing servo feedback while landed
  float _yaw_radio_passthrough = 0.0f; // yaw input from pilot in -1 ~ +1 range. used for setup and providing servo feedback while landed

  AP_Int8 _pwm_type; // PWM output type，PWM输出模式
};

从上面的数据结构来看，整个AP_Motors是定义了一个面向电机组从输入到输出的框架，从数据结构上来看，分为两个部分：
1.方法，自身实现了的方法包括了对姿态期望自输入的归一化，以及电压、电流、气压计补偿值等参数的设置；虚函数则是主要对实际rc输出的实现，这里不同的机型和布局，是存在区别；
2.变量，变量上主要也是对姿态控制器出来的数值的读入、电流、电压等一些数据的读入、电机通道的映射…
简单来说AP_Motors就是读取并保存姿态控制器计算出来的姿态各个方向的值，并归一化，然后依据motor_map的映射关系对应通道输出；

下面来找一个实际的多旋翼的例子进行说明：

void Copter::init_ardupilot()下面调用Copter::allocate_motors(void)，
/*
  allocate the motors class
 */
void Copter::allocate_motors(void)
{
  const struct AP_Param::GroupInfo *var_info;

  switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
/*这是针对非直升机的模型，对应的motor的类型都是AP_MotorsMatrix*/
#if FRAME_CONFIG != HELI_FRAME
  case AP_Motors::MOTOR_FRAME_QUAD://四旋翼
  case AP_Motors::MOTOR_FRAME_HEXA://六旋翼
  case AP_Motors::MOTOR_FRAME_Y6:  //三旋翼上下桨
  case AP_Motors::MOTOR_FRAME_OCTA://八旋翼
  case AP_Motors::MOTOR_FRAME_OCTAQUAD://四旋翼上下桨
  default:
  motors = new AP_MotorsMatrix(MAIN_LOOP_RATE);
  var_info = AP_MotorsMatrix::var_info;
  break;

/*这是针对三旋翼的模型，对应的motor的类型是AP_MotorsTri*/
  case AP_Motors::MOTOR_FRAME_TRI:
  motors = new AP_MotorsTri(MAIN_LOOP_RATE);
  var_info = AP_MotorsTri::var_info;
  AP_Param::set_frame_type_flags(AP_PARAM_FRAME_TRICOPTER);
  break;

/*这是针对单轴的模型，对应AP_MotorsSingle*/
  case AP_Motors::MOTOR_FRAME_SINGLE:
  motors = new AP_MotorsSingle(MAIN_LOOP_RATE);
  var_info = AP_MotorsSingle::var_info;
  break;

/*这是针对单轴的模型，对应AP_MotorsCoax*/
  case AP_Motors::MOTOR_FRAME_COAX:
  motors = new AP_MotorsCoax(MAIN_LOOP_RATE);
  var_info = AP_MotorsCoax::var_info;
  break;

/*这是针对单轴的模型，对应AP_MotorsTailsitter*/
  case AP_Motors::MOTOR_FRAME_TAILSITTER:
  motors = new AP_MotorsTailsitter(MAIN_LOOP_RATE);
  var_info = AP_MotorsTailsitter::var_info;
  break;
#else // FRAME_CONFIG == HELI_FRAME直升机模型
  case AP_Motors::MOTOR_FRAME_HELI:
  default:
  motors = new AP_MotorsHeli_Single(MAIN_LOOP_RATE);
  var_info = AP_MotorsHeli::var_info;
  AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI);
  break;
#endif
  }
  if (motors == nullptr) {
  AP_HAL::panic("Unable to allocate FRAME_CLASS=%u", (unsigned)g2.frame_class.get());
  }
  AP_Param::load_object_from_eeprom(motors, var_info);

  AP_AHRS_View *ahrs_view = ahrs.create_view(ROTATION_NONE);
  if (ahrs_view == nullptr) {
  AP_HAL::panic("Unable to allocate AP_AHRS_View");
  }

/*下面是控制模型*/
/*非直升机的控制模型均为AC_AttitudeControl_Multi*/
#if FRAME_CONFIG != HELI_FRAME
  attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, MAIN_LOOP_SECONDS);
  var_info = AC_AttitudeControl_Multi::var_info;
#else
/*直升机的控制模型为AC_AttitudeControl_Heli*/
  attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors, MAIN_LOOP_SECONDS);
  var_info = AC_AttitudeControl_Heli::var_info;
#endif
  if (attitude_control == nullptr) {
  AP_HAL::panic("Unable to allocate AttitudeControl");
  }
  AP_Param::load_object_from_eeprom(attitude_control, var_info);

/*位置控制模型均为AC_PosControl*/
  pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control,
  g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
  g.p_pos_xy, g.pi_vel_xy);
  if (pos_control == nullptr) {
  AP_HAL::panic("Unable to allocate PosControl");
  }
  AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info);

/*航点导航均为AC_WPNav*/
  wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
  if (wp_nav == nullptr) {
  AP_HAL::panic("Unable to allocate WPNav");
  }
  AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info);

/*绕圈AC_Circle*/
  circle_nav = new AC_Circle(inertial_nav, *ahrs_view, *pos_control);
  if (wp_nav == nullptr) {
  AP_HAL::panic("Unable to allocate CircleNav");
  }
  AP_Param::load_object_from_eeprom(circle_nav, circle_nav->var_info);

  // reload lines from the defaults file that may now be accessible
  AP_Param::reload_defaults_file();

  // now setup some frame-class specific defaults
根据不同的机架类型，设定不同的PID参数
  switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {

  case AP_Motors::MOTOR_FRAME_Y6:
  attitude_control->get_rate_roll_pid().kP().set_default(0.1);
  attitude_control->get_rate_roll_pid().kD().set_default(0.006);
  attitude_control->get_rate_pitch_pid().kP().set_default(0.1);
  attitude_control->get_rate_pitch_pid().kD().set_default(0.006);
  attitude_control->get_rate_yaw_pid().kP().set_default(0.15);
  attitude_control->get_rate_yaw_pid().kI().set_default(0.015);
  break;

  case AP_Motors::MOTOR_FRAME_TRI:
  attitude_control->get_rate_yaw_pid().filt_hz().set_default(100);
  break;
  default:
  break;
  }

  if (upgrading_frame_params) {
  // do frame specific upgrade. This is only done the first time we run the new firmware
#if FRAME_CONFIG == HELI_FRAME
  SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 12, CH_1);
  SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 13, CH_2);
  SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 14, CH_3);
  SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 15, CH_4);
#else
  if (g2.frame_class == AP_Motors::MOTOR_FRAME_TRI) {
  const AP_Param::ConversionInfo tri_conversion_info[] = {
  { Parameters::k_param_motors, 32, AP_PARAM_INT16, "SERVO7_TRIM" },
  { Parameters::k_param_motors, 33, AP_PARAM_INT16, "SERVO7_MIN" },
  { Parameters::k_param_motors, 34, AP_PARAM_INT16, "SERVO7_MAX" },
  { Parameters::k_param_motors, 35, AP_PARAM_FLOAT, "MOT_YAW_SV_ANGLE" },
  };
  // we need to use CONVERT_FLAG_FORCE as the SERVO7_* parameters will already be set from RC7_*
  AP_Param::convert_old_parameters(tri_conversion_info, ARRAY_SIZE(tri_conversion_info), AP_Param::CONVERT_FLAG_FORCE);
  const AP_Param::ConversionInfo tri_conversion_info_rev { Parameters::k_param_motors, 31, AP_PARAM_INT8, "SERVO7_REVERSED" };
  AP_Param::convert_old_parameter(&tri_conversion_info_rev, 1, AP_Param::CONVERT_FLAG_REVERSE | AP_Param::CONVERT_FLAG_FORCE);
  // AP_MotorsTri was converted from having nested var_info to one level
  AP_Param::convert_parent_class(Parameters::k_param_motors, motors, motors->var_info);
  }
#endif
  }

  // upgrade parameters. This must be done after allocating the objects
  convert_pid_parameters();
}
从void Copter::allocate_motors中就会首先依据机架类型设定电机控制模型和姿态控制模型；

class AP_MotorsMulticopter : public AP_Motors

void Copter::loop()
     |
     V
void Copter::fast_loop()
     |
     V
void Copter::motors_output()

而在Copter这个类中就定义了：
#if FRAME_CONFIG == HELI_FRAME
 #define MOTOR_CLASS AP_MotorsHeli_Single
#else
 #define MOTOR_CLASS AP_MotorsMulticopter
#endif

  MOTOR_CLASS *motors;
所以可以明显看到整个AP_Motors的运行就开始了

void Copter::motors_output()
还是先从void Copter::motors_output()这个函数的开始，上层的调用逻辑暂时先不去管；
// motors_output - send output to motors library which will adjust and send to ESCs and servos
// 电机输出，其结果是发送给ESCs和伺服接口
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE == ENABLED
  // this is to allow the failsafe module to deliberately crash
  // the vehicle. Only used in extreme circumstances to meet the
  // OBC rules，我靠，这段是用来故意使机器坠毁的代码，感觉好恶意满满
  if (g2.afs.should_crash_vehicle()) {
  g2.afs.terminate_vehicle();
  return;
  }
#endif

  // Update arming delay state，解锁延迟，也就是机器解锁后不立即启动。
  if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
  ap.in_arming_delay = false;
  }

  // check if we are performing the motor test，检测当前是否处于电机测试模式
  if (ap.motor_test) {
       motor_test_output();//执行电机测试输出
  } else {
  bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop;
  if (!motors->get_interlock() && interlock) {
  motors->set_interlock(true);
  Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
  } else if (motors->get_interlock() && !interlock) {
  motors->set_interlock(false);
  Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
        }

  // send output signals to motors，执行电机输出
  motors->output();
        }
}

void AP_MotorsMulticopter::output()
接下来就来分析output具体的实现过程了，这也是AP_Motors虚函数的第一个实现

// output - sends commands to the motors
void AP_MotorsMulticopter::output()
{
  // update throttle filter，对油门进行一阶低通滤波
  update_throttle_filter();

  // update battery resistance，更新电池电阻（确保大机动运动的时候，电池电压稳定）
  update_battery_resistance();

  // calc filtered battery voltage and lift_max，计算电池电压的最大升力
  update_lift_max_from_batt_voltage();

  // run spool logic
  //将电机输出设定为几个阶段：
  //SHUT_DOWN           ：Motors停转无输出，Servos输出保持中位或测试条件值
  //SPIN_WHEN_ARMED     ：Motors停转无输出或者开始输出旋转，Servos开始输出
  //SPOOL_UP            ：Motors最大油门输出，Servos正常输出
  //THROTTLE_UNLIMITED  ：Motors正常输出，Servos正常输出
  //SPOOL_DOWN          ：Motors最小输出，Servos正常输出
  output_logic();

  // calculate thrust
  //计算推力
  output_armed_stabilizing();

  // apply any thrust compensation for the frame
  // 对机架进行推力补偿
  thrust_compensation();

  // convert rpy_thrust values to pwm
  // 根据_spool_mode分别计算出motor_out[AP_MOTORS_MAX_NUM_MOTORS]的PWM值
  // 然后通过hal.rcout->write(chan, pwm);进行输出
  output_to_motors();
};

到这里面向电机的输入输出的代码梳理完成了，接下来分析这这个过程中用到的一些关键算法和逻辑处理代码

关键算法和逻辑处理

这里就从void AP_MotorsMulticopter::output()开始梳理

• 油门低通滤波器（一阶低通滤）

fl = 1/(2πRC)，fl为截止频率；-------①
w0 = 1/(RC)，w0为截止角频率；-------②
通过时域、频域的转换，最终得到采样系数 a = t/(RC),t为采样周期
a为与RC值有关的一个参数，称为滤波系数，其值决定新采样值在本次滤波结果中所占的权重，其值通常远小于1
而在最终得到a的过程中RC还需通过式①转换截止频率间接得到
以下:
{
  float rc = 1.0f/(M_2PI*cutoff_freq);
  alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
  _output += (sample - _output) * alpha;
  return _output;
}

 电池负载更新（计算的电池内阻+外阻）

这里计算了PWM到电机升力的近似线性转换：
_lift_max = batt_voltage_filt*(1-_thrust_curve_expo) + _thrust_curve_expo*batt_voltage_filt*batt_voltage_filt;
做这一步的目的是对后面每个电机的补偿（这个没懂原理）

- 电机输出逻辑（分步实现电机运行）

  //SHUT_DOWN                 ：Motors停转无输出，Servos输出保持中位或测试条件值
  //SPIN_WHEN_ARMED    ：Motors停转无输出或者开始输出旋转，Servos开始输出
  //SPOOL_UP                     ：Motors最大油门输出，Servos正常输出
  //THROTTLE_UNLIMITED ：Motors正常输出，Servos正常输出
  //SPOOL_DOWN               ：Motors最小输出，Servos正常输出
这几个模式之间的切换的结果是改变了_spin_up_ratio和_throttle_thrust_max