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_Motorsclass 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_Motorsvoid Copter::loop()     |     Vvoid Copter::fast_loop()     |     Vvoid 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 motorsvoid 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