ardupilot PWM输出源代码分析
作者：互联网
这几天看apm源代码，看到pwm输出这块，做个笔记，方便日后回头看。有分析不对的，还望各位大神指出。
pwm输出从上往下看依次是SRV_Channels类（与SRV_Channel互为友元类，在成员变量上定义了SRV_Channel类数组，方法上更对是对SRV_Channel的补充）-SRV_Channel类（对输出通道的抽象出的数据结构，枚举了所有PWM通道；实现了单个SRV_Channel的从信号的输入到输出的计算，并且附加了对通道数值的类型和限幅操作 ）-到最后输入是调用底层AP_HAL::RCOutput类。
首先是入口函数Copter::init_ardupilot()初始一系列，里面调用了allocate_motors()函数，该函数详细代码如下：
/*
allocate the motors class
*/
void Copter::allocate_motors(void)
{
switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
#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:
case AP_Motors::MOTOR_FRAME_DODECAHEXA:
default:
motors = new AP_MotorsMatrix(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsMatrix::var_info;
break;
case AP_Motors::MOTOR_FRAME_TRI:
motors = new AP_MotorsTri(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsTri::var_info;
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_TRICOPTER);
break;
case AP_Motors::MOTOR_FRAME_SINGLE:
motors = new AP_MotorsSingle(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsSingle::var_info;
break;
case AP_Motors::MOTOR_FRAME_COAX:
motors = new AP_MotorsCoax(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsCoax::var_info;
break;
case AP_Motors::MOTOR_FRAME_TAILSITTER:
motors = new AP_MotorsTailsitter(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsTailsitter::var_info;
break;
#else // FRAME_CONFIG == HELI_FRAME
case AP_Motors::MOTOR_FRAME_HELI_DUAL:
motors = new AP_MotorsHeli_Dual(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsHeli_Dual::var_info;
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI);
break;
case AP_Motors::MOTOR_FRAME_HELI_QUAD:
motors = new AP_MotorsHeli_Quad(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsHeli_Quad::var_info;
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI);
break;
case AP_Motors::MOTOR_FRAME_HELI:
default:
motors = new AP_MotorsHeli_Single(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsHeli_Single::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, motors_var_info);
ahrs_view = ahrs.create_view(ROTATION_NONE);
if (ahrs_view == nullptr) {
AP_HAL::panic("Unable to allocate AP_AHRS_View");
}
const struct AP_Param::GroupInfo *ac_var_info;
#if FRAME_CONFIG != HELI_FRAME
attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
ac_var_info = AC_AttitudeControl_Multi::var_info;
#else
attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
ac_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, ac_var_info);
pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control);
if (pos_control == nullptr) {
AP_HAL::panic("Unable to allocate PosControl");
}
AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info);
#if AC_OAPATHPLANNER_ENABLED == ENABLED
wp_nav = new AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
#else
wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
#endif
if (wp_nav == nullptr) {
AP_HAL::panic("Unable to allocate WPNav");
}
AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info);
loiter_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
if (loiter_nav == nullptr) {
AP_HAL::panic("Unable to allocate LoiterNav");
}
AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info);
#if MODE_CIRCLE_ENABLED == ENABLED
circle_nav = new AC_Circle(inertial_nav, *ahrs_view, *pos_control);
if (circle_nav == nullptr) {
AP_HAL::panic("Unable to allocate CircleNav");
}
AP_Param::load_object_from_eeprom(circle_nav, circle_nav->var_info);
#endif
// reload lines from the defaults file that may now be accessible
AP_Param::reload_defaults_file(true);
// now setup some frame-class specific defaults
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_D_hz().set_default(100);
break;
default:
break;
}
// brushed 16kHz defaults to 16kHz pulses
if (motors->get_pwm_type() == AP_Motors::PWM_TYPE_BRUSHED) {
g.rc_speed.set_default(16000);
}
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();
#if FRAME_CONFIG == HELI_FRAME
convert_tradheli_parameters();
#endif
}
里面是根据不同机型来初始化motors对象。例如飞机类型为MOTOR_FRAME_Y6，该motors被初始化为AP_MotorsMatrix类。在该类初始函数init里面初始化pwm输出通道。该代码如下，我们主要分析setup_motors(frame_class, frame_type)函数，该函数主要进一步调用底层来初始化pwm输出通道。
// init
void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// record requested frame class and type
_last_frame_class = frame_class;
_last_frame_type = frame_type;
// setup the motors
setup_motors(frame_class, frame_type);
// enable fast channels or instant pwm
set_update_rate(_speed_hz);
}
setup_motors(frame_class, frame_type)函数详细代码如下，该代码根据不同的机型来添加不同的pwm输出顺序。里面主要的函数是add_motor_raw函数，该函数最终还是调用void AP_Motors::add_motor_num(int8_t motor_num)函数来初始化输出通道。
void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{
// remove existing motors
for (int8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
remove_motor(i);
}
bool success = true;
switch (frame_class) {
case MOTOR_FRAME_QUAD:
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_X:
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
break;
case MOTOR_FRAME_TYPE_BF_X:
// betaflight quad X order
// see: https://fpvfrenzy.com/betaflight-motor-order/
add_motor(AP_MOTORS_MOT_1, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,1);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,3);
add_motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
break;
case MOTOR_FRAME_TYPE_DJI_X:
// DJI quad X order
// see https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
break;
case MOTOR_FRAME_TYPE_CW_X:
// "clockwise X" motor order. Motors are ordered clockwise from front right
// matching test order
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
break;
case MOTOR_FRAME_TYPE_V:
add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
add_motor(AP_MOTORS_MOT_2, -135, 1.0000f, 3);
add_motor(AP_MOTORS_MOT_3, -45, -0.7981f, 4);
add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 2);
break;
case MOTOR_FRAME_TYPE_H:
// H frame set-up - same as X but motors spin in opposite directiSons
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
break;
case MOTOR_FRAME_TYPE_VTAIL:
/*
Tested with: Lynxmotion Hunter Vtail 400
- inverted rear outward blowing motors (at a 40 degree angle)
- should also work with non-inverted rear outward blowing motors
- no roll in rear motors
- no yaw in front motors
- should fly like some mix between a tricopter and X Quadcopter
Roll control comes only from the front motors, Yaw control only from the rear motors.
Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm.
Note: if we want the front motors to help with yaw,
motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle
motors 3's yaw factor should be changed to -sin(radians(40))
*/
add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
break;
case MOTOR_FRAME_TYPE_ATAIL:
/*
The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
- The Yaw factors are reversed, because the rear motors are facing different directions
With V-Shaped VTails, the props make a V-Shape when spinning, but with
A-Shaped VTails, the props make an A-Shape when spinning.
- Rear thrust on a V-Shaped V-Tail Quad is outward
- Rear thrust on an A-Shaped V-Tail Quad is inward
Still functions the same as the V-Shaped VTail mixing below:
- Yaw control is entirely in the rear motors
- Roll is is entirely in the front motors
*/
add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
break;
case MOTOR_FRAME_TYPE_PLUSREV:
// plus with reversed motor directions
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
break;
default:
// quad frame class does not support this frame type
success = false;
break;
}
break; // quad
case MOTOR_FRAME_HEXA:
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_X:
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
break;
case MOTOR_FRAME_TYPE_H:
// H is same as X except middle motors are closer to center
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0f, 0.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
add_motor_raw(AP_MOTORS_MOT_4, -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor_raw(AP_MOTORS_MOT_5, -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
break;
default:
// hexa frame class does not support this frame type
success = false;
break;
}
break;
case MOTOR_FRAME_OCTA:
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_X:
add_motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_3, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_4, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_V:
add_motor_raw(AP_MOTORS_MOT_1, 0.83f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_2, -0.67f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_3, 0.67f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, -0.50f, -1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.00f, 1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor_raw(AP_MOTORS_MOT_6, -0.83f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_7, -1.00f, 1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_8, 0.50f, -1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
break;
case MOTOR_FRAME_TYPE_H:
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, -1.0f, 0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_4, -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_7, 1.0f, 0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_8, -1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_I:
add_motor_raw(AP_MOTORS_MOT_1, 0.333f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, -0.333f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_4, 0.333f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -0.333f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor_raw(AP_MOTORS_MOT_6, -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_7, -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_8, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
default:
// octa frame class does not support this frame type
success = false;
break;
} // octa frame type
break;
case MOTOR_FRAME_OCTAQUAD:
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
break;
case MOTOR_FRAME_TYPE_X:
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
break;
case MOTOR_FRAME_TYPE_V:
add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
add_motor(AP_MOTORS_MOT_2, -45, -0.7981f, 7);
add_motor(AP_MOTORS_MOT_3, -135, 1.0000f, 5);
add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 3);
add_motor(AP_MOTORS_MOT_5, -45, 0.7981f, 8);
add_motor(AP_MOTORS_MOT_6, 45, -0.7981f, 2);
add_motor(AP_MOTORS_MOT_7, 135, 1.0000f, 4);
add_motor(AP_MOTORS_MOT_8, -135, -1.0000f, 6);
break;
case MOTOR_FRAME_TYPE_H:
// H frame set-up - same as X but motors spin in opposite directions
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8);
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
break;
default:
// octaquad frame class does not support this frame type
success = false;
break;
}
break;
case MOTOR_FRAME_DODECAHEXA: {
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); // forward-top
add_motor(AP_MOTORS_MOT_2, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); // forward-bottom
add_motor(AP_MOTORS_MOT_3, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); // forward-right-top
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); // forward-right-bottom
add_motor(AP_MOTORS_MOT_5, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); // back-right-top
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); // back-right-bottom
add_motor(AP_MOTORS_MOT_7, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); // back-top
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); // back-bottom
add_motor(AP_MOTORS_MOT_9, -120, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9); // back-left-top
add_motor(AP_MOTORS_MOT_10, -120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10); // back-left-bottom
add_motor(AP_MOTORS_MOT_11, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11); // forward-left-top
add_motor(AP_MOTORS_MOT_12, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12); // forward-left-bottom
break;
case MOTOR_FRAME_TYPE_X:
add_motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); // forward-right-top
add_motor(AP_MOTORS_MOT_2, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); // forward-right-bottom
add_motor(AP_MOTORS_MOT_3, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); // right-top
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); // right-bottom
add_motor(AP_MOTORS_MOT_5, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); // back-right-top
add_motor(AP_MOTORS_MOT_6, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); // back-right-bottom
add_motor(AP_MOTORS_MOT_7, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); // back-left-top
add_motor(AP_MOTORS_MOT_8, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); // back-left-bottom
add_motor(AP_MOTORS_MOT_9, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9); // left-top
add_motor(AP_MOTORS_MOT_10, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10); // left-bottom
add_motor(AP_MOTORS_MOT_11, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11); // forward-left-top
add_motor(AP_MOTORS_MOT_12, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12); // forward-left-bottom
break;
default:
// dodeca-hexa frame class does not support this frame type
success = false;
break;
}}
break;
case MOTOR_FRAME_Y6:
switch (frame_type) {
case MOTOR_FRAME_TYPE_Y6B:
// Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_3, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
break;
case MOTOR_FRAME_TYPE_Y6F:
// Y6 motor layout for FireFlyY6
add_motor_raw(AP_MOTORS_MOT_1, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
break;
default:
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_6, 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
break;
}
break;
default:
// matrix doesn't support the configured class
success = false;
break;
} // switch frame_class
// normalise factors to magnitude 0.5
normalise_rpy_factors();
_flags.initialised_ok = success;
}
void AP_Motors::add_motor_num(int8_t motor_num)函数如下。我们可以看到里面调用了SRV_Channels::get_motor_function（）函数和SRV_Channels::set_aux_channel_default（）函数，这两个函数来初始化输出通道。
/*
add a motor, setting up default output function as needed
*/
void AP_Motors::add_motor_num(int8_t motor_num)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
uint8_t chan;
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num);
SRV_Channels::set_aux_channel_default(function, motor_num);
if (!SRV_Channels::find_channel(function, chan)) {
gcs().send_text(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
}
}
}
木士易
发布了35 篇原创文章 · 获赞 11 · 访问量 4万+
私信
关注
标签：ardupilot,YAW,MOTORS,MOT,AP,add,motor,源代码,PWM 来源： https://blog.csdn.net/yangxueyangxue/article/details/104574511