ArduPilot第6章 电机模块解析
文章目录
- 前言
- 一、roll pitch yaw throtttle
- 二、rc_in()和rc_out()初始化
-
- 1.Copter::init_ardupilot()
- 2.init_rc_in()
- 3.init_rc_out()
- 三、电机初始化
-
- 1.MULTICOPTER_FRAME
- 2.AP_MotorsMulticopter
- 3.AP_MOTORS_MAX_NUM_MOTORS
- 4.AP_MotorsMatrix
- 5.scheduler_tasks
- 6.void Copter::one_hz_loop()
- 7. set_frame_class_and_type
- 8.AP_MotorsMatrix::init
- 9.setup_motors
- 10.AP_MotorsMatrix.cpp
- 11.normalise_rpy_factors()
- 二、电机pwm计算
-
- 1.fast_loop()
- 2.motors_output()
- 3.virtual void output_to_motors()
- 4.output_to_motors()
- 5.MOTOR_CLASS *&motors
- 6.define MOTOR_CLASS AP_MotorsMulticopter
- 7.output()
- 8.output_logic()
- 9.AP_MotorsMatrix : public AP_MotorsMulticopter
- 10.output_armed_stabilizing()
- 11.output_to_motors()
- 12.rc_write(float actuator)
- 13.output_to_pwm(float actuator)
- 14.output_to_pwm(float actuator)
- 总结
参考文献
ardupilot 中电机输出逻辑及电机转轴状态分析
https://blog.csdn.net/lixiaoweimashixiao/article/details/126745495
APM（Ardupilot）——电机输出流程图
https://blog.csdn.net/yinxian5224/article/details/102743618
APM_ArduCopter源码解析学习（一）——ArduCopter主程序
https://blog.csdn.net/zhangfengmei1987/article/details/110432871
APM_ArduCopter源码解析学习（二）——电机库学习
https://blog.csdn.net/zhangfengmei1987/article/details/110448094?spm=1001.2014.3001.5502
PX4用户指南-飞行-基本操作
https://www.ncnynl.com/archives/201810/2666.html
Ardupilot动力分配-混控部分分析
https://blog.csdn.net/qq_15390133/article/details/123822392
ardupilot 中电机输出逻辑及电机转轴状态分析
https://download.csdn.net/blog/column/7327436/126745495
ardupilot 函数output_armed_stabilizing
https://download.csdn.net/blog/column/7327436/109166189
[飞控]从零开始建模(三)-控制分配浅析
https://zhuanlan.zhihu.com/p/46839430
RC输入通道映射(RCMAP)
https://doc.cuav.net/tutorial/copter/advanced-configuration/rc-input-channel-mapping.html
Apm飞控学习笔记-AC_PosControl位置控制-Cxm
https://blog.csdn.net/chen_taifu/article/details/124610904
ardupilot开发 — 位置控制与导航制导篇
https://blog.csdn.net/weixin_43321489/article/details/132362043
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
rc_write(AP_MOTORS_MOT_1, pwm);
前言
本文介绍Ardupilot的电机模块相关知识。
一、roll pitch yaw throtttle
roll pitch yaw throtttle
二、rc_in()和rc_out()初始化
1.Copter::init_ardupilot()
在void Copter::init_ardupilot()函数中调用init_rc_in()和init_rc_out()函数
\ardupilot\ardupilot\ArduCopter\system.cpp
void Copter::init_ardupilot()
{
...
#endif
#if FRAME_CONFIG == HELI_FRAME
input_manager.set_loop_rate(scheduler.get_loop_rate_hz());
#endif
init_rc_in(); // sets up rc channels from radio
...
allocate_motors();
...
rc().init();
// sets up motors and output to escs
init_rc_out();
...
}
2.init_rc_in()
\ArduCopter\radio.cpp
void Copter::init_rc_in()
{
channel_roll = rc().channel(rcmap.roll()-1);
channel_pitch = rc().channel(rcmap.pitch()-1);
channel_throttle = rc().channel(rcmap.throttle()-1);
channel_yaw = rc().channel(rcmap.yaw()-1);
// set rc channel ranges
设置rc通道范围
channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
channel_throttle->set_range(1000);
// set default dead zones
default_dead_zones();
// initialise throttle_zero flag
ap.throttle_zero = true;
}
3.init_rc_out()
比例因子的给定在init_rc_out()函数中完成的初始化
\ArduCopter\radio.cpp
// init_rc_out -- initialise motors
void Copter::init_rc_out()
{
motors->set_loop_rate(scheduler.get_loop_rate_hz());
motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
// enable aux servos to cope with multiple output channels per motor
SRV_Channels::enable_aux_servos();
// update rate must be set after motors->init() to allow for motor mapping
motors->set_update_rate(g.rc_speed);
#if FRAME_CONFIG != HELI_FRAME
if (channel_throttle->configured_in_storage()) {
// throttle inputs setup, use those to set motor PWM min and max if not already configured
motors->convert_pwm_min_max_param(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
} else {
// throttle inputs default, force set motor PWM min and max to defaults so they will not be over-written by a future change in RC min / max
motors->convert_pwm_min_max_param(1000, 2000);
}
motors->update_throttle_range();
#else
// setup correct scaling for ESCs like the UAVCAN ESCs which
// take a proportion of speed.
hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif
// refresh auxiliary channel to function map
SRV_Channels::update_aux_servo_function();
#if FRAME_CONFIG != HELI_FRAME
/*
setup a default safety ignore mask, so that servo gimbals can be active while safety is on
*/
uint16_t safety_ignore_mask = (~copter.motors->get_motor_mask()) & 0x3FFF;
BoardConfig.set_default_safety_ignore_mask(safety_ignore_mask);
#endif
}
三、电机初始化
1.MULTICOPTER_FRAME
\ArduCopter\config.h
// FRAME_CONFIG
//
#ifndef FRAME_CONFIG
# define FRAME_CONFIG MULTICOPTER_FRAME
#endif
2.AP_MotorsMulticopter
在Copter.h中定义的Copter类中，指明了其所使用的电机类型
\ArduCopter\Copter.h
#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#else
#define MOTOR_CLASS AP_MotorsMulticopter
#endif
3.AP_MOTORS_MAX_NUM_MOTORS
\libraries\AP_Motors\AP_Motors_Class.h
#define AP_MOTORS_MAX_NUM_MOTORS 12
4.AP_MotorsMatrix
t\libraries\AP_Motors\AP_MotorsMatrix.h
class AP_MotorsMatrix : public AP_MotorsMulticopter {
}
ArduCopter的电机库配置位于libraries\AP_Motors路径下的AP_MotorsMulticopter.cpp/AP_MotorsMulticopter.h文件中。
5.scheduler_tasks
\ArduCopter\Copter.cpp
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
...
SCHED_TASK(one_hz_loop, 1, 100, 81),
...
}
6.void Copter::one_hz_loop()
\ArduCopter\Copter.cpp
// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
{
...
if (!motors->armed()) {
...
// check the user hasn't updated the frame class or type
motors->set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
...
}
...
}
7. set_frame_class_and_type
\libraries\AP_Motors\AP_MotorsMatrix.cpp
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
{
// exit immediately if armed or no change
if (armed() || (frame_class == _active_frame_class && _active_frame_type == frame_type)) {
return;
}
_active_frame_class = frame_class;
_active_frame_type = frame_type;
init(frame_class, frame_type);
}
8.AP_MotorsMatrix::init
配置电机初始化。
\libraries\AP_Motors\AP_MotorsMatrix.cpp
// init
void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// record requested frame class and type
_active_frame_class = frame_class;
_active_frame_type = frame_type;
if (frame_class == MOTOR_FRAME_SCRIPTING_MATRIX) {
// if Scripting frame class, do nothing scripting must call its own dedicated init function
return;
}
// setup the motors
setup_motors(frame_class, frame_type);
// enable fast channels or instant pwm
set_update_rate(_speed_hz);
}
9.setup_motors
这个函数最主要的内容就是配置电机，包括每个电机对于不同运动的影响程度（推力分配）。
(1)
函数刚开始，首先就是把最初的所有电机配置全部移除，方便后续进行更改。这里的AP_MOTORS_MAX_NUM_MOTORS为最大的电机数，于AP_Motors_Class.h中定义为12。
(2)
然后进入一个switch()函数中进行具体的配置内容，首先判断是属于哪一种frame_class的架构，ArduCopter这边给出了14种配置结构，定义于AP_Motors_Class.h中的枚举类型里，如下所示
(3)
以X型四旋翼进行说明：
add_motor()这个函数的作用就是配置每一个电机在某一运动功能上的影响因子，AP_MOTORS_MOT_1从右上角开始，逆时针进行编号增加。
\libraries\AP_Motors\AP_MotorsMatrix.cpp
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);
}
...
switch (frame_class) {
...
case MOTOR_FRAME_QUAD:
switch (frame_type) {
...
case MOTOR_FRAME_TYPE_X: {
_frame_type_string = "X";
static const AP_MotorsMatrix::MotorDef motors[] {
{ 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
{ -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
{ -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
{ 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
};
add_motors(motors, ARRAY_SIZE(motors));
break;
}
...
}
}
// normalise factors to magnitude 0.5
normalise_rpy_factors();
}
ArduCopter这边给出了14种配置结构，定义于AP_Motors_Class.h中的枚举类型里，如下所示
\libraries\AP_Motors\AP_Motors_Class.h
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,
MOTOR_FRAME_HELI_DUAL = 11,
MOTOR_FRAME_DODECAHEXA = 12,
MOTOR_FRAME_HELI_QUAD = 13,
MOTOR_FRAME_DECA = 14,
MOTOR_FRAME_SCRIPTING_MATRIX = 15,
MOTOR_FRAME_6DOF_SCRIPTING = 16,
MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX = 17,
};
...
}
根据不同的机身结构，frame_type定义了不同的机型类别。
\libraries\AP_Motors\AP_Motors_Class.h
class AP_Motors {
public:
...
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_PLUSREV = 6, // plus with reversed motor direction
MOTOR_FRAME_TYPE_Y6B = 10,
MOTOR_FRAME_TYPE_Y6F = 11, // for FireFlyY6
MOTOR_FRAME_TYPE_BF_X = 12, // X frame, betaflight ordering
MOTOR_FRAME_TYPE_DJI_X = 13, // X frame, DJI ordering
MOTOR_FRAME_TYPE_CW_X = 14, // X frame, clockwise ordering
MOTOR_FRAME_TYPE_I = 15, // (sideways H) octo only
MOTOR_FRAME_TYPE_NYT_PLUS = 16, // plus frame, no differential torque for yaw
MOTOR_FRAME_TYPE_NYT_X = 17, // X frame, no differential torque for yaw
MOTOR_FRAME_TYPE_BF_X_REV = 18, // X frame, betaflight ordering, reversed motors
MOTOR_FRAME_TYPE_Y4 = 19, //Y4 Quadrotor frame
};
...
}
AP_MotorsMatrix::MotorDef motors[]
\libraries\AP_Motors\AP_MotorsMatrix.h
class AP_MotorsMatrix : public AP_MotorsMulticopter {
public:
...
struct MotorDef {
float angle_degrees;
float yaw_factor;
uint8_t testing_order;
};
...
}
10.AP_MotorsMatrix.cpp
代码内容如下，这部分程序时直接调用了AP_MotorsMatrix.cpp中的add_motor_raw()函数，这个函数原本的作用是用来配置空中无人机的RPY方向上各个电机对各方向的影响因子，但是ROV在水下运动时多了沉浮、前后平移和左右平移的功能，因此在后面添加了3个相关的影响因子配置数组。
add_motor_raw()函数原型如下：
\libraries\AP_Motors\AP_MotorsMatrix.cpp
void AP_MotorsMatrix::add_motors(const struct MotorDef *motors, uint8_t num_motors)
{
for (uint8_t i=0; i<num_motors; i++) {
const auto &motor = motors[i];
add_motor(i, motor.angle_degrees, motor.yaw_factor, motor.testing_order);
}
}
// add_motor using just position and prop direction - assumes that for each motor, roll and pitch factors are equal
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);
}
// add_motor using position and prop direction. Roll and Pitch factors can differ (for asymmetrical frames)
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
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor)
{
if (initialised_ok()) {
// do not allow motors to be set if the current frame type has init correctly
return;
}
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
// enable motor
motor_enabled[motor_num] = true;
// set roll, pitch, yaw and throttle factors
_roll_factor[motor_num] = roll_fac;
_pitch_factor[motor_num] = pitch_fac;
_yaw_factor[motor_num] = yaw_fac;
_throttle_factor[motor_num] = throttle_factor;
// set order that motor appears in test
_test_order[motor_num] = testing_order;
// call parent class method
add_motor_num(motor_num);
}
}
11.normalise_rpy_factors()
该函数的作用是将影响因子约束在-0.5~0.5之间。
\libraries\AP_Motors\AP_MotorsMatrix.cpp
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
// normalizes throttle factors so max value is 1 and no value is less than 0
void AP_MotorsMatrix::normalise_rpy_factors()
{
float roll_fac = 0.0f;
float pitch_fac = 0.0f;
float yaw_fac = 0.0f;
float throttle_fac = 0.0f;
// find maximum roll, pitch and yaw factors
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
roll_fac = MAX(roll_fac,fabsf(_roll_factor[i]));
pitch_fac = MAX(pitch_fac,fabsf(_pitch_factor[i]));
yaw_fac = MAX(yaw_fac,fabsf(_yaw_factor[i]));
throttle_fac = MAX(throttle_fac,MAX(0.0f,_throttle_factor[i]));
}
}
// scale factors back to -0.5 to +0.5 for each axis
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (!is_zero(roll_fac)) {
_roll_factor[i] = 0.5f * _roll_factor[i] / roll_fac;
}
if (!is_zero(pitch_fac)) {
_pitch_factor[i] = 0.5f * _pitch_factor[i] / pitch_fac;
}
if (!is_zero(yaw_fac)) {
_yaw_factor[i] = 0.5f * _yaw_factor[i] / yaw_fac;
}
if (!is_zero(throttle_fac)) {
_throttle_factor[i] = MAX(0.0f,_throttle_factor[i] / throttle_fac);
}
}
}
}
二、电机pwm计算
1.fast_loop()
在fast_loop()中调用motors_output()
\ArduCopter\Copter.cpp
// Main loop - 400hz
void Copter::fast_loop()
{
...
// send outputs to the motors library immediately
motors_output();
...
}
2.motors_output()
void Copter::motors_output()
在motors_output()函数中
函数中完成的有一些解锁检查
调用flightmode->output_to_motors()
\ArduCopter\motors.cpp
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
...
// output any servo channels
SRV_Channels::calc_pwm();
// cork now, so that all channel outputs happen at once
SRV_Channels::cork();
...
if (ap.motor_test) {
// check if we are performing the motor test
motor_test_output();
} else {
// send output signals to motors
flightmode->output_to_motors();
}
// push all channels
SRV_Channels::push();
}
3.virtual void output_to_motors()
class Mode
在class Mode中找到virtual void output_to_motors()
\ArduCopter\mode.h
class Mode {
public:
virtual void output_to_motors();
...
}
4.output_to_motors()
void Mode::output_to_motors()
\ArduCopter\mode.cpp
// send output to the motors, can be overridden by subclasses
void Mode::output_to_motors()
{
motors->output();
}
5.MOTOR_CLASS *&motors
MOTOR_CLASS *&motors;
\ArduCopter\mode.h
MOTOR_CLASS *&motors;
6.define MOTOR_CLASS AP_MotorsMulticopter
#define MOTOR_CLASS AP_MotorsMulticopter
\ArduCopter\Copter.h
#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#else
#define MOTOR_CLASS AP_MotorsMulticopter
#endif
7.output()
void AP_MotorsMulticopter::output()
这个函数中完成由rpy_thrust到 PWM的输出转换过程
在AP_MotorsMulticopter::output()函数中，调用output_to_motors()
\libraries\AP_Motors\AP_MotorsMulticopter.cpp
// output - sends commands to the motors
void AP_MotorsMulticopter::output()
{
更新油门滤波// update throttle filter
update_throttle_filter();
计算电池电压滤波和最大升力// calc filtered battery voltage and lift_max
update_lift_max_from_batt_voltage();
运行电机轴状态逻辑// run spool logic
output_logic();
计算推力// calculate thrust
output_armed_stabilizing();
推力补偿// apply any thrust compensation for the frame
thrust_compensation();
转换rpy_thrust值到pwm// convert rpy_thrust values to pwm
output_to_motors();
输出所有的助推器油门值// output any booster throttle
output_boost_throttle();
输出原始roll pitch yaw推力// output raw roll/pitch/yaw/thrust
output_rpyt();
};
8.output_logic()
重点需要关注的一些变量值：
\libraries\AP_Motors\AP_MotorsMulticopter.h
class AP_MotorsMulticopter : public AP_Motors {
...
protected:
// parameters
AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range
AP_Float _thrust_curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation
AP_Float _slew_up_time; // throttle increase slew limitting
AP_Float _slew_dn_time; // throttle decrease slew limitting
pwm在零和最小值转换的esc时间
AP_Float _safe_time; // Time for the esc when transitioning between zero pwm to minimum
AP_Float _spin_min; // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _spin_max; // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _spin_arm; // throttle out ratio which produces the armed spin rate. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _batt_voltage_max; // maximum voltage used to scale lift
AP_Float _batt_voltage_min; // minimum voltage used to scale lift
AP_Float _batt_current_max; // current over which maximum throttle is limited
AP_Float _batt_current_time_constant; // Time constant used to limit the maximum current
AP_Int8 _batt_idx; // battery index used for compensation
AP_Int16 _pwm_min; // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
AP_Int16 _pwm_max; // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
AP_Float _throttle_hover; // estimated throttle required to hover throttle in the range 0 ~ 1
AP_Int8 _throttle_hover_learn; // enable/disabled hover thrust learning
0：当没有解锁时，pwm可以使能 1：当没有解锁时，pwm不使能
AP_Int8 _disarm_disable_pwm; // disable PWM output while disarmed
...
pwm从零到最小值转换时的esc定时器
float _disarm_safe_timer; // Timer for the esc when transitioning between zero pwm to minimum
,,,
将电机从零向上推至最小油门的时间
// time to spool motors to min throttle
AP_Float _spool_up_time;
...
油门百分比0-1(从零变化到最小油门值)
// spool variables
float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min
...
最大允许的油门推力0-1(最小油门到最大油门之间)
float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
\libraries\AP_Motors\AP_MotorsMulticopter.cpp
// parameters for the motor class
const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
...
// @Param: SPIN_MIN
// @DisplayName: Motor Spin minimum
// @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range. Should be higher than MOT_SPIN_ARM.
// @Values: 0.0:Low, 0.15:Default, 0.3:High
// @User: Advanced
AP_GROUPINFO("SPIN_MIN", 18, AP_MotorsMulticopter, _spin_min, AP_MOTORS_SPIN_MIN_DEFAULT),
// @Param: SPIN_ARM
// @DisplayName: Motor Spin armed
// @Description: Point at which the motors start to spin expressed as a number from 0 to 1 in the entire output range. Should be lower than MOT_SPIN_MIN.
// @Values: 0.0:Low, 0.1:Default, 0.2:High
// @User: Advanced
AP_GROUPINFO("SPIN_ARM", 19, AP_MotorsMulticopter, _spin_arm, AP_MOTORS_SPIN_ARM_DEFAULT),
...
0：当没有解锁时，pwm可以使能 1：当没有解锁时，pwm不使能
// @Param: SAFE_DISARM
// @DisplayName: Motor PWM output disabled when disarmed
// @Description: Disables motor PWM output when disarmed
// @Values: 0:PWM enabled while disarmed, 1:PWM disabled while disarmed
// @User: Advanced
AP_GROUPINFO("SAFE_DISARM", 23, AP_MotorsMulticopter, _disarm_disable_pwm, 0),
...
以秒为单位，将电机从零向上推至最小油门的时间
// @Param: SPOOL_TIME
// @DisplayName: Spool up time
// @Description: Time in seconds to spool up the motors from zero to min throttle.
// @Range: 0 2
// @Units: s
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("SPOOL_TIME", 36, AP_MotorsMulticopter, _spool_up_time, AP_MOTORS_SPOOL_UP_TIME_DEFAULT),
...
禁用和启用电机pwm输出所需要的时间
// @Param: SAFE_TIME
// @DisplayName: Time taken to disable and enable the motor PWM output when disarmed and armed.
// @Description: Time taken to disable and enable the motor PWM output when disarmed and armed.
// @Range: 0 5
// @Units: s
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("SAFE_TIME", 42, AP_MotorsMulticopter, _safe_time, AP_MOTORS_SAFE_TIME_DEFAULT),
}
\libraries\AP_Motors\AP_Motors.h
class AP_Motors {
public:
...
目标电机转轴模式
// desired spool states
enum class DesiredSpoolState : uint8_t {
所有i电机应当停转
SHUT_DOWN = 0, // all motors should move to stop
所有电机应当怠速
GROUND_IDLE = 1, // all motors should move to ground idle
油门应当不再受启动程序的限制
THROTTLE_UNLIMITED = 2, // motors should move to being a state where throttle is unconstrained (e.g. by start up procedure)
};
...
当前电机转轴模式
// spool states
enum class SpoolState : uint8_t {
所有电机停转
SHUT_DOWN = 0, // all motors stop
所有电机处以地面怠速
GROUND_IDLE = 1, // all motors at ground idle
当在自稳模式下增加最大油门值
SPOOLING_UP = 2, // increasing maximum throttle while stabilizing
油门不再受启动程序的限制
THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure
当在自稳模式下减小最大油门值
SPOOLING_DOWN = 4, // decreasing maximum throttle while stabilizing
};
...
// structure for holding motor limit flags
struct AP_Motors_limit {
达到横滚限制
uint8_t roll : 1; // we have reached roll or pitch limit
达到俯仰限制
uint8_t 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;
...
protected:
// internal variables
调用uotput()函数的频率，通常为400hz
uint16_t _loop_rate; // rate in Hz at which output() function is called (normally 400hz)
uint16_t _speed_hz; // speed in hz to send updates to motors
float _roll_in; // desired roll control from attitude controllers, -1 ~ +1
float _roll_in_ff; // desired roll feed forward control from attitude controllers, -1 ~ +1
float _pitch_in; // desired pitch control from attitude controller, -1 ~ +1
float _pitch_in_ff; // desired pitch feed forward control from attitude controller, -1 ~ +1
float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1
float _yaw_in_ff; // desired yaw feed forward control from attitude controller, -1 ~ +1
float _throttle_in; // last throttle input from set_throttle caller
float _throttle_out; // throttle after mixing is complete
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
目标电机转轴状态
DesiredSpoolState _spool_desired; // desired spool state
当前电机转轴状态
SpoolState _spool_state; // current spool mode
}
这个代码中主要就是实现合理的控制电机转轴的状态及异常处理情况
\libraries\AP_Motors\AP_MotorsMulticopter.cpp
// run spool logic
void AP_MotorsMulticopter::output_logic()
{
判断是否解锁，解锁执行if
这里面有两个全局参数进行设置_disarm_disable_pwm和_safe_time
if (armed()) {
if (_disarm_disable_pwm && (_disarm_safe_timer < _safe_time)) {
_disarm_safe_timer += 1.0f/_loop_rate;
} else {
_disarm_safe_timer = _safe_time;//默认时1s
}
} else {
_disarm_safe_timer = 0.0f;//默认是0s
}
如果没有解锁或者没有连锁，强制设定目标和当前转轴模式为停转状态
// force desired and current spool mode if disarmed or not interlocked
if (!armed() || !get_interlock()) {
设置目标电机转轴状态停转
_spool_desired = DesiredSpoolState::SHUT_DOWN;
设置当前电机转轴也设置停转
_spool_state = SpoolState::SHUT_DOWN;
}
//以秒为单位，将电机从零向上推至最小油门的时间
if (_spool_up_time < 0.05) {
//防止浮点异常// prevent float exception
_spool_up_time.set(0.05);
}
判断当前电机转轴状态
switch (_spool_state) {
如果当前电机转轴状态为停转状态
case SpoolState::SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
//电机应固定。
//伺服设置为其微调值或处于测试状态。
设置标记位限制 // set limits flags
//横滚限制
limit.roll = true;
//俯仰限制
limit.pitch = true;
//偏航限制
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
确保电机以正确的方向旋转-
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::SHUT_DOWN && _disarm_safe_timer >= _safe_time.get()) {
//设置电机怠速状态
_spool_state = SpoolState::GROUND_IDLE;
break;
}
设置和增加斜坡变量
油门百分比（0-1）：从零变化到最小油门值
// set and increment ramp variables
_spin_up_ratio = 0.0f;
最大允许的油门推力从（0.0---1.0）在最小油门（throttle_min）和最大油门（throttle_max）之间
_throttle_thrust_max = 0.0f;
初始化电机失败变量// initialise motor failure variables
_thrust_boost = false;
_thrust_boost_ratio = 0.0f;
break;
}
如果当前状态为怠速状态
case SpoolState::GROUND_IDLE:
{
// Motors should be stationary or at ground idle.
// Servos should be moving to correct the current attitude.
电机应静止或地面怠速。
伺服系统应移动以校正当前姿态。
设置限制标记---- set limits flags
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
设置和增加斜坡变量--- set and increment ramp variables
调用output（）函数的频率（通常为400hz）
float spool_step = 1.0f / (_spool_up_time * _loop_rate); //0.05*400=20，spool_step=0.05
根据目标电机转轴状态进行调整
switch (_spool_desired)
{
//如果目标电机转轴是关机停转状态
case DesiredSpoolState::SHUT_DOWN:
油门百分比从零变化到最小油门值
依次递减0.05
_spin_up_ratio -= spool_step;
约束 _spin_up_ratio值和更新模式----- constrain ramp value and update mode
if (_spin_up_ratio <= 0.0f)
{
//_spin_up_ratio设置0
_spin_up_ratio = 0.0f;
//设定当前电机转轴为关机停转状态
_spool_state = SpoolState::SHUT_DOWN;
}
break;
//如果目标电机转轴是油门不受限制状态
case DesiredSpoolState::THROTTLE_UNLIMITED:
//每次递增0.05
_spin_up_ratio += spool_step;
//约束_spin_up_ratio和更新模式---- constrain ramp value and update mode
if (_spin_up_ratio >= 1.0f)
{
//超过1就设置1
_spin_up_ratio = 1.0f;
//设定当前电机转轴为上升状态
_spool_state = SpoolState::SPOOLING_UP;
}
break;
//如果目标电机转轴是怠速状态
case DesiredSpoolState::GROUND_IDLE:
//解锁旋转上升速率
float spin_up_armed_ratio = 0.0f;
//判断最小值是否大于0
if (_spin_min > 0.0f)
{
spin_up_armed_ratio = _spin_arm / _spin_min; //一般时0.1/0.15=0.66
}
//上升速率
_spin_up_ratio += constrain_float(spin_up_armed_ratio - _spin_up_ratio, -spool_step, spool_step);
break;
}
//设置油门最大输出为0;最大允许的油门推力从（0.0---1.0）在最小油门（throttle_min）和最大油门（throttle_max）之间
_throttle_thrust_max = 0.0f;
//初始化电机失败变量----- initialise motor failure variables
_thrust_boost = false;
_thrust_boost_ratio = 0.0f;
break;
}
如果当前状态上升状态
case SpoolState::SPOOLING_UP:
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
//最大油门应从最小值移动到最大值。
//伺服系统应表现出正常的飞行行为。
//初始化限制标记位不受限制----- initialize limits flags
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
//确保电机以正确的方向旋转--- make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED)
{
//设定当前电机转轴为关机停转状态
_spool_state = SpoolState::SPOOLING_DOWN;
break;
}
//油门百分比，从零到最小油门值---- set and increment ramp variables
_spin_up_ratio = 1.0f;
//最大允许的油门推力从（0.0---1.0）在最小油门（throttle_min）和最大油门（throttle_max）之间
//_throttle_thrust_max每次递增0.05
_throttle_thrust_max += 1.0f / (_spool_up_time * _loop_rate);
//约束_throttle_thrust_max和更新模式----- constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle()))
{
//限制_throttle_thrust_max最大值 0 ~ 1之间
_throttle_thrust_max = get_current_limit_max_throttle();
//设定当前电机转轴不受限制状态
_spool_state = SpoolState::THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f)
{
_throttle_thrust_max = 0.0f; //设置为0
}
//初始化电机一些状态---- initialise motor failure variables
_thrust_boost = false;
//_thrust_boost_ratio 每次递减0.05
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0 / (_spool_up_time * _loop_rate));
break;
如果当前状态为油门不限制状态
case SpoolState::THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
///油门应表现出正常的飞行行为。
//伺服系统应表现出正常的飞行行为。
//初始化限制标记---- initialize limits flags
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
//确保电机以正确的方向旋转---- make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED)
{
_spool_state = SpoolState::SPOOLING_DOWN;
break;
}
//设置和增加斜坡变量---- set and increment ramp variables
_spin_up_ratio = 1.0f;
//获取最大油门推力的值
_throttle_thrust_max = get_current_limit_max_throttle();
//判断是否故障
if (_thrust_boost && !_thrust_balanced)
{
_thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio + 1.0f / (_spool_up_time * _loop_rate));
} else
{
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate));
}
break;
如果当前状态下降状态
case SpoolState::SPOOLING_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
//最大油门应从最大值移动到最小值。
//伺服系统应表现出正常的飞行行为。
//初始化为不受限制 initialize limits flags
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
//确保电机以正确的方向旋转---- make sure the motors are spooling in the correct direction
if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED)
{
_spool_state = SpoolState::SPOOLING_UP;
break;
}
设置和增加斜坡变量---- set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max -= 1.0f / (_spool_up_time * _loop_rate);
约束值和更新模式- constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f)
{
_throttle_thrust_max = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle())
{
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max))
{
设置当前状态为怠速状态
_spool_state = SpoolState::GROUND_IDLE;
}
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate));
break;
关于throttle_thrust_max这个值的具体含义：最大允许的油门推力从（0.0—1.0）在最小油门（throttle_min）和最大油门（throttle_max）之间。
电机转轴处于关机及怠速时：
_throttle_thrust_max=0；
电机转轴处于上升加速状态
//设置和增加临时变量---- set and increment ramp variables
_spin_up_ratio = 1.0f;
//最大允许的油门推力从（0.0---1.0）在最小油门（throttle_min）和最大油门（throttle_max）之间
//每次递增0.05
_throttle_thrust_max += 1.0f / (_spool_up_time * _loop_rate);
//约束渐变值和更新模式----- constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle()))
{
//传递位最大值
_throttle_thrust_max = get_current_limit_max_throttle();
//不受限制
_spool_state = SpoolState::THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f)
{
_throttle_thrust_max = 0.0f; //设置为0
}
电机转轴处于下降减速状态
//设置和增加斜坡变量---- set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max -= 1.0f / (_spool_up_time * _loop_rate);
//约束渐变值和更新模式- constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f)
{
_throttle_thrust_max = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle())
{
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max))
{
//设置怠速
_spool_state = SpoolState::GROUND_IDLE;
}
电机转轴处于不受限制状态
//获取最大油门推力的值
_throttle_thrust_max = get_current_limit_max_throttle();
需要重点分析这个函数的实现机理：_throttle_thrust_max = get_current_limit_max_throttle();
float AP_MotorsMulticopter::get_current_limit_max_throttle()
{
//电池信息
AP_BattMonitor &battery = AP::battery();
float _batt_current;
//不补偿就是设置1
if (_batt_current_max <= 0 || //如果禁用电流限制，则返回最大值---- return maximum if current limiting is disabled
!_flags.armed || //取消油门限制（如果已上锁）---- remove throttle limit if disarmed
!battery.current_amps(_batt_current, _batt_idx)) //没有可用的电流监控---- no current monitoring is available
{
_throttle_limit = 1.0f;
return 1.0f;
}
float _batt_resistance = battery.get_resistance(_batt_idx);
if (is_zero(_batt_resistance))
{
_throttle_limit = 1.0f;
return 1.0f;
}
//计算最大电流，以防止电压降低于_batt_voltage_min--- calculate the maximum current to prevent voltage sag below _batt_voltage_min
float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx) - _batt_voltage_min) / _batt_resistance);
float batt_current_ratio = _batt_current / batt_current_max;
float loop_interval = 1.0f / _loop_rate;
_throttle_limit += (loop_interval / (loop_interval + _batt_current_time_constant)) * (1.0f - batt_current_ratio);
//在悬停和全油门之间，油门限制降至20%----- throttle limit drops to 20% between hover and full throttle
_throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
//限制最大油门--- limit max throttle
return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit);
}
因此这个_throttle_thrust_max的范围就是【0-1】
9.AP_MotorsMatrix : public AP_MotorsMulticopter
class AP_MotorsMatrix : public AP_MotorsMulticopter
\libraries\AP_Motors\AP_MotorsMatrix.h
class AP_MotorsMatrix : public AP_MotorsMulticopter
10.output_armed_stabilizing()
void AP_MotorsMatrix::output_armed_stabilizing()
\libraries\AP_Motors\AP_MotorsMatrix.cpp
void AP_MotorsMatrix::output_armed_stabilizing()
{
//通用计数变量
uint8_t i;
//横滚推力输入值 范围是【-1 ,+1】
float roll_thrust;
//俯仰推力输入值 范围是【-1 ,+1】
float pitch_thrust;
//偏航推力输入值 范围是【-1 ,+1】
float yaw_thrust;
//油门推力输入值 范围是【0 ,+1】
float throttle_thrust;
//油门推力最大平均值0.0 - 1.0
float throttle_avg_max;
//油门提供最大的横滚，俯仰，偏航范围不包含爬升力
float throttle_thrust_best_rpy;
//用于缩放横滚、俯仰和偏航，以符合电机限制
float rpy_scale = 1.0f;
//最低电机值
float rpy_low = 0.0f;
//最高电机值
float rpy_high = 0.0f;
//我们能适应的偏航量
float yaw_allowed = 1.0f;
//我们能适应当前通道的偏航量
float unused_range;
//飞行员所需油门和油门推力之间的差值=pilot's desired throttle -throttle_thrust_best_rpy
float thr_adj;
//申请电压和气压补偿
const float compensation_gain = get_compensation_gain();
//计算横滚推力=_roll_in*系数
roll_thrust = _roll_in * compensation_gain;
//计算俯仰推力=_pitch_in*系数
pitch_thrust = _pitch_in * compensation_gain;
//计算偏航推力=_yaw_in*系数
yaw_thrust = _yaw_in * compensation_gain;
//获取油门推力值
throttle_thrust = get_throttle() * compensation_gain;
//获取油门最大推力值
throttle_avg_max = _throttle_avg_max * compensation_gain;
// sanity check throttle is above zero and below current limited throttle
//正常检查油门值是高于零，低于当前油门限制值
if (throttle_thrust <= 0.0f)
{
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
//限制最大油门值
if (throttle_thrust >= _throttle_thrust_max)
{
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
//油门最大平均值限制在一定范围
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);
// calculate throttle that gives most possible room for yaw which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
// 2. the higher of:
// a) the pilot's throttle input
// b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
//计算为偏航提供最大可能空间的油门值，取下列值中的较小值：
// 1. 0.5f-（rpy_low+rpy_high）/2.0-这将提供最高电机上方和最低电机下方的最大可能裕度
// 2.较高值：
// a）飞行员油门输入
// b）_throttle_rpy_mix:飞行员输入油门和悬停油门之间
// 情况#2确保我们绝不会将油门增加到悬停油门以上，除非飞行员已经命令这样做。
// 情况2b允许我们把油门提高到飞行员指令的高度，但不会使直升机上升。
// 如果这意味着减少发动机的油门，我们将选择#1（偏航控制的最佳油门）（即，我们倾向于减少油门*，因为*它提供更好的偏航控制）
// 只有当油门很低时，我们才会选择#2（飞行员和悬停油门的组合）。我们赞成减少油门，而不是更好的偏航控制，因为飞行员已经下令了
//计算出我们能适应油门范围的偏航量
//这始终等于或小于飞行员或速率控制器请求的偏航
throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
// calculate roll and pitch for each motor
// calculate the amount of yaw input that each motor can accept
//为每个电机计算横滚和俯仰值
//计算每个电机可以接受的偏航输入量
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) //最多支持12轴电机
{
if (motor_enabled[i]) //电机是否使能
{
//计算推力=横滚推力+俯仰推力
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
//判断偏航因素是否为0
if (!is_zero(_yaw_factor[i]))
{
//偏航因素影响是否大于0
if (yaw_thrust * _yaw_factor[i] > 0.0f)
{
//我们能适应当前通道的偏航量
unused_range = fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]);
if (yaw_allowed > unused_range)
{
yaw_allowed = unused_range;
}
} else
{
//我们能适应当前通道的偏航量
unused_range = fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]);
if (yaw_allowed > unused_range)
{
yaw_allowed = unused_range;
}
}
}
}
}
// todo: make _yaw_headroom 0 to 1
//确保偏航的范围是0-1
yaw_allowed = MAX(yaw_allowed, (float)_yaw_headroom/1000.0f);
if (fabsf(yaw_thrust) > yaw_allowed)
{
//限制在一个范围内
yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
limit.yaw = true;
}
// 将偏航添加到每个电机的中间编号
rpy_low = 0.0f;
rpy_high = 0.0f;
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++)
{
//判断电机是否使能
if (motor_enabled[i])
{
//计算推力
_thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
// 记录最低横滚+俯仰+偏航指令
if (_thrust_rpyt_out[i] < rpy_low)
{
rpy_low = _thrust_rpyt_out[i];
}
// 记录最高横滚+俯仰+偏航指令
if (_thrust_rpyt_out[i] > rpy_high)
{
rpy_high = _thrust_rpyt_out[i];
}
}
}
//检查是否合适------ check everything fits
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_avg_max);
if (is_zero(rpy_low) && is_zero(rpy_high))
{
rpy_scale = 1.0f;
} else if (is_zero(rpy_low))
{
rpy_scale = constrain_float((1.0f-throttle_thrust_best_rpy)/rpy_high, 0.0f, 1.0f);
} else if (is_zero(rpy_high))
{
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
} else
{
rpy_scale = constrain_float(MIN(-throttle_thrust_best_rpy/rpy_low, (1.0f-throttle_thrust_best_rpy)/rpy_high), 0.0f, 1.0f);
}
// calculate how close the motors can come to the desired throttle
//飞行员所需油门和油门推力之间的差值=pilot's desired throttle -throttle_thrust_best_rpy
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f)
{
// Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f)
{
limit.throttle_upper = true;
}
thr_adj = 0.0f;
} else
{
if (thr_adj < -(throttle_thrust_best_rpy+rpy_low))
{
// Throttle can't be reduced to desired value
//油门无法降低到所需值
thr_adj = -(throttle_thrust_best_rpy+rpy_low);
} else if (thr_adj > 1.0f - (throttle_thrust_best_rpy+rpy_high))
{
// Throttle can't be increased to desired value
//油门不能增加到期望值
thr_adj = 1.0f - (throttle_thrust_best_rpy+rpy_high);
limit.throttle_upper = true;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
//为每个电机添加缩放的横滚、俯仰、约束偏航和油门
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++)
{
if (motor_enabled[i])
{
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + rpy_scale*_thrust_rpyt_out[i];
}
}
//将所有输出限制为0.0f到1.0f
//测试代码应该在这些行被注释掉的情况下运行，因为它们不应该做任何事情
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++)
{
if (motor_enabled[i])
{
_thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i], 0.0f, 1.0f);
}
}
}
11.output_to_motors()
\libraries\AP_Motors\AP_MotorsMulticopter.cpp
随后在void AP_MotorsMatrix::output_to_motors()函数中, 将上述计算出来的_thrust_rpyt_out转换赋值给_actuator[i]
这个函数最开始的 _actuator数组就是用来储存每一个电机最后的pwm值，switch函数判断轴状态，这里只需要知道SHUT_DOWN和GROUND_IDLE状态下，推进器不工作即可。而在SPOOLING_UP、THROTTLE_UNLIMITED和SPOOLING_DOWN状态下，对每一个电机调用set_actuator_with_slew函数计算最后的pwm值并且保存到 _actuator数组中。最后通过rc_write()函数输出到每一个通道。
void AP_MotorsMatrix::output_to_motors()
{
变量定义
int8_t i;
当前实际的电机转轴状态
switch (_spool_state)
{
/当前实际的所有电机停转
case SpoolState::SHUT_DOWN:
{
不输出数据----- no output
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) //最多12轴
{
if (motor_enabled[i])
{
_actuator[i] = 0.0f;
}
}
break;
}
当前实际的状态为地面怠速
case SpoolState::GROUND_IDLE:
当解锁时但是没有飞行的时候，发送到电机输出---- sends output to motors when armed but not flying
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++)
{
if (motor_enabled[i])
{
设置怠速值
set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle());
}
}
break;
当前实际的状态为上升 油门不受限制 下降状态
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN:
根据推力需求设置电机输出---- set motor output based on thrust requests
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++)
{
if (motor_enabled[i])
{
set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
}
}
break;
}
最终写入到电机的PWM值
将输出转换为PWM并发送到每个电机----- convert output to PWM and send to each motor
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++)
{
if (motor_enabled[i])
{
rc_write(i, output_to_pwm(_actuator[i]));
}
}
}
class AP_MotorsMulticopter : public AP_Motors {
...
protected:
...
油门增加幅度限制
AP_Float _slew_up_time; // throttle increase slew limitting
油门减小幅度限制
AP_Float _slew_dn_time; // throttle decrease slew limitting
...
}
// parameters for the motor class
const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
...
// @Param: SLEW_UP_TIME
// @DisplayName: Output slew time for increasing throttle
// @Description: Time in seconds to slew output from zero to full. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
// @Range: 0 .5
// @Units: s
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("SLEW_UP_TIME", 40, AP_MotorsMulticopter, _slew_up_time, AP_MOTORS_SLEW_TIME_DEFAULT),
// @Param: SLEW_DN_TIME
// @DisplayName: Output slew time for decreasing throttle
// @Description: Time in seconds to slew output from full to zero. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
// @Range: 0 .5
// @Units: s
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("SLEW_DN_TIME", 41, AP_MotorsMulticopter, _slew_dn_time, AP_MOTORS_SLEW_TIME_DEFAULT),
...
}
void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float input)
{
//如果MOT_SLEW_UP_TIME为0（默认值），则不会对增加输出应用转换限制。
//如果MOT_SLEW_DN_TIME为0（默认值），则不会对降低输出应用转换限制。
//MOT_SLEW_UP_TIME和MOT_ MOT_SLEW_DN_TIME被限制为0.0~0.5，以保持正常。
//如果转轴模式关闭，则不应用回转限制以允许立即解除电机。
//未应用转换时间的输出限制----- Output limits with no slew time applied
float output_slew_limit_up = 1.0f;
float output_slew_limit_dn = 0.0f;
//如果设置了MOT_SLOW_UP_TIME，则计算允许的最高新输出值，约束为0.0~1.0
if (is_positive(_slew_up_time))
{
float output_delta_up_max = 1.0f / (constrain_float(_slew_up_time, 0.0f, 0.5f) * _loop_rate);
output_slew_limit_up = constrain_float(actuator_output + output_delta_up_max, 0.0f, 1.0f);
}
//If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0
//如果设置了MOT_SLOW_DN_TIME，则计算允许的最低新输出值，约束为0.0~1.0
if (is_positive(_slew_dn_time))
{
float output_delta_dn_max = 1.0f / (constrain_float(_slew_dn_time, 0.0f, 0.5f) * _loop_rate);
output_slew_limit_dn = constrain_float(actuator_output - output_delta_dn_max, 0.0f, 1.0f);
}
//将输出变化限制在上述限制范围内--- Constrain change in output to within the above limits
actuator_output = constrain_float(input, output_slew_limit_dn, output_slew_limit_up);
}
float AP_MotorsMulticopter::actuator_spin_up_to_ground_idle() const
{
return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
}
因此可以看出最终怠速的电机输出值就是在【0，_spin_min】
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in)
{
//输入值
thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
return _spin_min + (_spin_max - _spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in);
}
float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) const
{
float throttle_ratio = thrust;
// apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f); //一般设置_thrust_curve_expo=0.65
if (fabsf(thrust_curve_expo) < 0.001)
{
// zero expo means linear, avoid floating point exception for small values
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);
}
12.rc_write(float actuator)
\libraries\AP_Motors\AP_Motors_Class.cpp
/*
write to an output channel
*/
void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
{
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan);
if ((1U<<chan) & _motor_pwm_range_mask) {
// note that PWM_MIN/MAX has been forced to 1000/2000
SRV_Channels::set_output_scaled(function, pwm - 1000);
} else {
SRV_Channels::set_output_pwm(function, pwm);
}
}
13.output_to_pwm(float actuator)
\libraries\SRV_Channel\SRV_Channel_aux.cpp
/// map a function to a servo channel and output it
void SRV_Channel::output_ch(void)
{
if (!(SRV_Channels::disabled_mask & (1U<<ch_num))) {
hal.rcout->write(ch_num, output_pwm);
}
}
14.output_to_pwm(float actuator)
\libraries\AP_Motors\AP_MotorsMulticopter.cpp
int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
{
float pwm_output;
if (_spool_state == SpoolState::SHUT_DOWN)
{
//在关机模式下，使用PWM 0或最小PWM----- in shutdown mode, use PWM 0 or minimum PWM
if (_disarm_disable_pwm && !armed())
{
pwm_output = 0;
} else
{
pwm_output = get_pwm_output_min();
}
} else
{
//在所有其他线轴模式下，转换为所需PWM------ in all other spool modes, covert to desired PWM
pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * actuator;
}
return pwm_output;
}
总结
本文主要介绍Ardupilot的电机模块相关知识。