PX4源码分析以及思路随笔1

PX4源码分析，以及思路随笔。

目录：
1.0 环境安装
1.1 roll pitch yaw
2.1 loop()
3.1 fastloop()
3.1 .1 read_AHRS()
3.1.1.1 ins.update()
3.1.2 rate_controller_run()
(嵌套了rate_bf_to_motor_roll)
3.1.3 motors_output()
3.1.3.1 update_throttle_filter()
1.0环境安装
1.首先安装px4_toolchain_installer_v14_win.exe，并配置好java环境（安装jdk，32位）。

2.安装GitHub
网站：
若提示失败，在IE浏览器中打开网页，http变为https，不断尝试。

3.克隆程序（需要翻墙），可能多次失败。

4.从C:\px4\toolchain\msys\1.0内的eclipse批处理文件打开eclipse。

5.按照从第二张图开始。

注：第二张图位置为ardupilot的位置。

返回目录
1.1 ROLL YAW PITCH
在航空中，pitch, yaw, roll如图2所示。

pitch是围绕X轴旋转，也叫做俯仰角，如图3所示。

yaw是围绕Y轴旋转，也叫偏航角，如图4所示。

roll是围绕Z轴旋转，也叫翻滚角，如图5所示。

返回目录
2.1 loop函数：
1.Setup各种初始化，先忽略。

2.初始定义
第一个是函数名，第二个单位为赫兹为过多少时间调用一次，第三个单位为微秒，即为最大花费时间。

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(rc_loop, 100, 130), /* 控制角 */
SCHED_TASK(throttle_loop, 50, 75), /*油门控制*/
SCHED_TASK(update_GPS, 50, 200), /* GPS更新*/
3.从Loop()开始。

(1)等待数据
ins.wait_for_sample();
我认为"ins"是 "Inertial Sensor"，也就是指惯性传感器。

void AP_InertialSensor::wait_for_sample(void)
uint32_t timer = micros();
（2）计算最大循环时间。

perf_info_check_loop_time(timer - fast_loopTimer); 检查循环时间
被PI Loops使用？？
G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f; fast_loopTimer = timer; 记录当前循环时间
mainLoop_count++; 主要循环的故障检测
(3)快速循环，主要的循环，重要的一步！！！！！！！！！！！！因此首选需主要研究此函数！
fast_loop();
（4）任务调度
scheduler.tick(); 告诉调度程序，一个流程已经走完？？
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
任务周期
scheduler.run(time_available);任务调度
返回目录
3.1 fastloop()函数
Fastloop，最关键的循环(400hz)，下面对每个函数进行工作的分析。

1.更新传感器并更新姿态的函数
read_AHRS();
2.进行角速度PID运算的函数
attitude_control.rate_controller_run();
3.直升机框架判断（HELI_FRAME）
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif//HELI_FRAME
4.输出电机PWM值的函数
motors_output();
5.惯性导航
// Inertial Nav
read_inertia();
6.扩展卡尔曼滤波器
// check if ekf has reset target heading
check_ekf_yaw_reset();
7.更新控制模式,并计算本级控制输出下级控制输入... // run the attitude controllers
update_flight_mode();
8.扩展卡尔曼滤波器
// update home from EKF if necessary update_home_from_EKF();
9.检查是否落地or追回
// check if we've landed or crashed update_land_and_crash_detectors();
10.摄像机
#if MOUNT == ENABLED
// camera mount's fast update
camera_mount.update_fast();
#endif
11.记录传感器健康状态？？
// log sensor health
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health();
返回目录
3.1.1 Read_AHRS()函数1.read_AHRS
void Copter::read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
//Mavlink协议支持，全传感器仿真？？模拟？（full sensor simulation.）
gcs_check_input();
#endif
ahrs.update();//重要！
}
2.ahrs.update()
其中主要函数为ahrs.update()，函数为：
//DCM：Direction Cosine Matrix，方向余弦矩阵
AP_AHRS_DCM::update(void)
{
float delta_t;
if (_last_startup_ms == 0) {
_last_startup_ms = AP_HAL::millis();
}
(1)更新加速度计和陀螺仪
// tell the IMU to grab some data
_ins.update();
// ask the IMU how much time this sensor reading represents
delta_t = _ins.get_delta_time();
// if the update call took more than 0.2 seconds then discard it, // otherwise we may move too far. This happens when arming motors // in ArduCopter
if (delta_t > 0.2f) {
memset(&_ra_sum[0], 0, sizeof(_ra_sum));
_ra_deltat = 0;
return;
}
(2)使用陀螺仪更新四元素矩阵（做余弦矩阵的归一化）
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
(3)标准化
// Normalize the DCM matrix
normalize();
(4)执行漂移修正
使用加速度计和罗盘与该次计算的四元素矩阵做差，修正出下一次陀螺仪的输出
// Perform drift correction
drift_correction(delta_t);
(5)检查错误值
// paranoid check for bad values in the DCM matrix
check_matrix();
(6)计算俯仰角，偏航角，翻转角
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
(7)更新三角函数的值，包括俯仰角的余弦和翻滚角的余弦
// update trig values including _cos_roll, cos_pitch
update_trig();
}
返回目录
3.1.1.1 _ins.update()
更新加速度计和陀螺仪
// tell the IMU to grab some data
_ins.update();
函数定义：void AP_InertialSensor::update(void)：
在函数中：_backends[i]->update(); 为主要操作步骤。

_backends[i]的定义为：(是一个指针数组)
AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
查找_backends所指向的数据，有两个函数值得注意：
*backend)
Void AP_InertialSensor::detect_backends(void)
考虑在detect_backends()中进行了调用：
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT ==
HAL_INS_VRBRAIN
_add_backend(AP_InertialSensor_PX4::detect(*this));
因此得出_backends指向的是类 AP_InertialSensor_PX4。

找到函数如下：bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
// uint8_t _num_accel_instances;
// uint8_t _num_gyro_instances;
for (uint8_t k=0; k<_num_accel_instances; k++) {
update_accel(_accel_instance[k]); //加速度计
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
update_gyro(_gyro_instance[k]); //陀螺仪
}
return true;
}
其中两个重要的函数分别为：
update_accel(_accel_instance[k])和update_gyro(_gyro_instance[k]).
首先看update_accel(_accel_instance[k])：
输入参数为_accel_instance[k]和_gyro_instance[k]，定义为：#define INS_MAX_INSTANCES 3
uint8_t _accel_instance[INS_MAX_INSTANCES];
uint8_t _gyro_instance[INS_MAX_INSTANCES];
随后看update_accel()和update_gyro()的定义：
void AP_InertialSensor_Backend::update_accel(uint8_t instance) {
hal.scheduler->suspend_timer_procs();//任务调度，推迟进程
//_imu._new_accel_data[instance]为布尔型变量，推测为观察是否可以更新 //其中_publish_accel()应该就是
if (_imu._new_accel_data[instance]) {
//两个参数，其中一个是instance，另一个参考为过滤后的加速度计数据？？
_publish_accel(instance, _imu._accel_filtered[instance]);
_imu._new_accel_data[instance] = false;
}
// possibly update filter frequency 设定频率？
if (_last_accel_filter_hz[instance] != _accel_filter_cutoff()) {
_imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_r ate(instance), _accel_filter_cutoff());
_last_accel_filter_hz[instance] = _accel_filter_cutoff();
}
//任务调度，推测为经过一个进程后重新设置
hal.scheduler->resume_timer_procs();
}
下面对于陀螺仪的控制基本没有区别：
void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
{
hal.scheduler->suspend_timer_procs();
if (_imu._new_gyro_data[instance]) {
_publish_gyro(instance, _imu._gyro_filtered[instance]);
_imu._new_gyro_data[instance] = false;
}
// possibly update filter frequency
if (_last_gyro_filter_hz[instance] != _gyro_filter_cutoff()) {
_imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rat e(instance), _gyro_filter_cutoff());
_last_gyro_filter_hz[instance] = _gyro_filter_cutoff();
}
hal.scheduler->resume_timer_procs();
}
然后需要看一下_publish_accel()这个函数:
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel)
{
_imu._accel[instance] = accel;
_imu._accel_healthy[instance] = true;
if (_imu._accel_raw_sample_rates[instance] <= 0) {
return;
}
// publish delta velocity
_imu._delta_velocity[instance] =
_imu._delta_velocity_acc[instance];
_imu._delta_velocity_dt[instance] =
_imu._delta_velocity_acc_dt[instance];
_imu._delta_velocity_valid[instance] = true;
if (_imu._accel_calibrator != NULL &&
_imu._accel_calibrator[instance].get_status() ==
ACCEL_CAL_COLLECTING_SAMPLE) {
Vector3f cal_sample = _imu._delta_velocity[instance];
//remove rotation
cal_sample.rotate_inverse(_imu._board_orientation);
// remove scale factors
const Vector3f &accel_scale = _imu._accel_scale[instance].get(); cal_sample.x /= accel_scale.x;
cal_sample.y /= accel_scale.y;
cal_sample.z /= accel_scale.z;
//remove offsets
cal_sample += _imu._accel_offset[instance].get() *
_imu._delta_velocity_dt[instance] ;
_imu._accel_calibrator[instance].new_sample(cal_sample,
_imu._delta_velocity_dt[instance]);
}
}
返回目录
3.1.2 attitude_control.rate_controller_run()
姿态控制函数（PID函数）
attitude_control.rate_controller_run();
定义为：
void AC_AttitudeControl::rate_controller_run()
{
_motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x));
_motors.set_pitch(rate_bf_to_motor_pitch(_ang_vel_target_rads.y));
_motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
}
注：其中，_ang_vel_target_rads.x
等三个输入参数的取得主要在void Copter::stabilize_run()内取得。

推测Angular velocity target rads的含义为：要求(期望)的角度速度注： rate_bf_roll_pitch_yaw():
此函数采用“车身骨架坐标系”标示翻滚、俯仰和偏航速率(度/秒)。

例：此函数的滚动= -1000,间距= -1500,偏航= 500会导致飞机向左滚动10度/秒,向前倾斜15度/秒,对于车辆的z轴旋转5度/秒。

返回目录
3.1.2.1 _motors.set_roll()
1.首先分析_motors.set_roll()
定义：
void set_roll(float roll_in)
{ _roll_in = roll_in; };
主要就是将输入的参数定义为roll_in，并传值给_roll_in。

注：_roll_in在fastloop()内的下个函数motors_output()函数进行操作，这里我认为是个切入点！
set_roll()中嵌套的函数为：rate_bf_to_motor_roll() 定义为：
float AC_AttitudeControl::rate_bf_to_motor_roll(float
rate_target_rads)
{
//从陀螺仪获取get_gyro().x，也就是当前的飞行器的翻滚角。

float current_rate_rads = _ahrs.get_gyro().x;
//翻滚角的期望与当前数值的差值
float rate_error_rads = rate_target_rads - current_rate_rads;
// pass error to PID controller 将误差传递给PID控制器,其中有微分过滤
//和将目标数据传入结构体PID。

get_rate_roll_pid().set_input_filter_d(rate_error_rads);
get_rate_roll_pid().set_desired_rate(rate_target_rads);
//进行积分
float integrator = get_rate_roll_pid().get_integrator();
// Ensure that integrator can only be reduced if the output is saturated
//确保积分只在饱和时下降
if(!_motors.limit.roll_pitch|| ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) {
integrator = get_rate_roll_pid().get_i();
}
// Compute output in range -1 ~ +1进行PID计算，得到控制量output。

//其中get_d()和getp中有参数_kp和_kd，没有找到是如何进行计算的！
//这里是个大问题。

float output = get_rate_roll_pid().get_p() + integrator +
get_rate_roll_pid().get_d();
// Constrain output，限制output的值，如果是非法的则变为0，如果比0小则变
//为0,大于1则为1
return constrain_float(output, -1.0f, 1.0f);
}
剩余的
rate_bf_to_motor_pitch(_ang_vel_target_rads .y)和
rate_bf_to_motor_yaw(_ang_vel_target_rads.z)
与第一个没有差别
在这三个函数中，我们分别获得了三个参数，
_roll_in，_pitch_in和_yaw_in，取值范围在-1到1，应该会应用在fastloop()中的下个函数
motors_output()。

3.1.3 motors_output()
函数解释为使用
_roll_control_input、_pitch_control_input、_yaw_control_input
再进行换算，得出各个电机的PWM值。

首先查看定义：
void Copter::motors_output()
{
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
if (!ing_interlock){
// if not using interlock switch, set according to Emergency Stop status
// where Emergency Stop is forced false during arming if Emergency Stop switch
// is not used. Interlock enabled means motors run, so we must
// invert motor_emergency_stop status for motors to run.
motors.set_interlock(!ap.motor_emergency_stop);
}
motors.output();
}
}
明显其中调用motors.output()为关键一步。

首先查看定义：
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 运行飞行(盘旋)逻辑
//进行各种判定，比如提升发动机推力，降低发动机推力，盘旋等等。

output_logic();
// calculate thrust 计算推力
output_armed_stabilizing();
// convert rpy_thrust values to pwm 将rpy_thrust 转化为pwm信号 output_to_motors();
};
返回目录
3.1.3.1 update_throttle_filter()
还是先查看定义：
void AP_MotorsMulticopter::update_throttle_filter()
{
if (armed()) { //首先查看电机是否已经装备
//其中_loop_rate是update被调用的频率，通常为400hz
//_throttle_in 被看做油门的输入
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
// constrain filtered throttle 将其数值限制在0到1之间。

if (_throttle_filter.get() < 0.0f) {
_throttle_filter.reset(0.0f);
}
if (_throttle_filter.get() > 1.0f) {
_throttle_filter.reset(1.0f);
}
} else {
_throttle_filter.reset(0.0f);
}
}
其中apply函数定义：(apply函数还未吃透)
apply(T sample, float dt) {
return_filter.apply(sample, _cutoff_freq, dt);
}
T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) { if (cutoff_freq <= 0.0f || dt <= 0.0f) {
_output = sample;
return_output;
}
感觉函数就是将_throttle_in传给了但是函数外部并没有传值，不知道是什么意思。

get()和reset()意思很简单，字面理解即可。

（内部函数没有找到）
返回目录
3.1.3.2 update_battery_resistance
意义为更新电池的内阻。

先看定义：
void AP_MotorsMulticopter::update_battery_resistance()
{
// if disarmed reset resting voltage and current
// 如果未装备则复位电压和电流
if (!_flags.armed) {
_batt_voltage_resting = _batt_voltage;
_batt_current_resting = _batt_current;
_batt_timer = 0;
} else {
// update battery resistance when throttle is over hover throttle
// 更新电池内阻，在盘旋油门的状态时？？
if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) <
_batt_current)) {
if (get_throttle() >= _hover_out) {
// filter reaches 90% in 1/4 the test time
_batt_resistance +=
0.05f*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_c urrent_resting) ) - _batt_resistance);
_batt_timer += 1;
} else {
// initialize battery resistance to prevent change in resting voltage estimate
_batt_resistance =
((_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_r esting));
}
}
}
}
返回目录
3.1.3.3 update_lift_max_from_batt_voltage 定义：（这个函数由于感觉用处不大，暂时先不考虑）
void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
{
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
if((_batt_voltage_max <= 0) || (_batt_voltage_min >=
_batt_voltage_max) || (_batt_voltage < 0.25f*_batt_voltage_min)) {
_batt_voltage_filt.reset(1.0f);
_lift_max = 1.0f;
return;
}
_batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max *
0.6f);
// add current based voltage sag to battery voltage
float batt_voltage = _batt_voltage + _batt_current *
_batt_resistance;
batt_voltage = constrain_float(batt_voltage, _batt_voltage_min,
_batt_voltage_max);
// filter at 0.5 Hz
float bvf = _batt_voltage_filt.apply(batt_voltage/_batt_voltage_max, 1.0f/_loop_rate);
// calculate lift max
_lift_max= bvf*(1-_thrust_curve_expo) + _thrust_curve_expo*bvf*bvf; }
3.1.3.4 output_logic
先看函数定义：
这个函数很大，所以我们分成一个个部分来进行分析。

看了一部分，罗师姐说这是多旋翼的，暂时不需要看。

void AP_MotorsMulticopter::output_logic()
{
(1)if的判断中arm为装备，interlock为查看电机是否可用，简单来说就是当点击不可用或者未装备的时候，进行shut down也就是关闭，但是这里有个很重要的变量
_multicopter_flags.spool_mode，这个我还没有在其他函数内找到，但明显这是一个重要的信号，判断电机的转动等等。

// force desired and current spool mode if disarmed or not interlocked if (!_flags.armed || !_flags.interlock) {
_spool_desired = DESIRED_SHUT_DOWN;
_multicopter_flags.spool_mode = SHUT_DOWN;
}
switch (_multicopter_flags.spool_mode) {
（2）shut down直译过来就是停止，很好理解。

case SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags 设置各项参数的限制，数值为1应该就就是到达了限制。

//uint8_t roll_pitch : 1;
//we have reached roll or pitch limit
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction //确定电机的转动方向正确，这里给出的判断是如果目标不是将电机停止，那么 //转换为旋转(spin)模式
if (_spool_desired != DESIRED_SHUT_DOWN) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
break;
}
// set and increment ramp variables 设置和增加斜坡变量？
_throttle_low_end_pct = 0.0f;
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
（注：需要看一下以上四个参数的定义：
float_throttle_low_end_pct;
// throttle percentage (0 ~ 1) between zero and throttle_min
float_throttle_thrust_max;
// the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
float_throttle_rpy_mix;
// mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float_throttle_rpy_mix_desired;
// desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
）
Break;
（3）盘旋模式？
case SPIN_WHEN_ARMED: {
// Motors should be stationary or at spin when armed.
// Servos should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// set and increment ramp variables
float spool_step =
1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
if (_spool_desired == DESIRED_SHUT_DOWN){
_throttle_low_end_pct -= spool_step;
// constrain ramp value and update mode
if (_throttle_low_end_pct <= 0.0f) {
_throttle_low_end_pct = 0.0f;
_multicopter_flags.spool_mode = SHUT_DOWN;
}
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_throttle_low_end_pct += spool_step;
// constrain ramp value and update mode
if (_throttle_low_end_pct >= 1.0f) {
_throttle_low_end_pct = 1.0f;
_multicopter_flags.spool_mode = SPOOL_UP;
}
} else { // _spool_desired == SPIN_WHEN_ARMED
float spin_when_armed_low_end_pct = 0.0f;
if (_min_throttle > 0) {
spin_when_armed_low_end_pct = (float)_spin_when_armed / _min_throttle;
}
_throttle_low_end_pct +=
constrain_float(spin_when_armed_low_end_pct-_throttle_low_end_pct,
-spool_step, spool_step);
}
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
break;
}
case SPOOL_UP:
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_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 != DESIRED_THROTTLE_UNLIMITED ){
_multicopter_flags.spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max +=
1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
// 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();
_multicopter_flags.spool_mode = THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f;
}
break;
case THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_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 != DESIRED_THROTTLE_UNLIMITED) {
_multicopter_flags.spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
update_throttle_rpy_mix();
break;
case SPOOL_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_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 == DESIRED_THROTTLE_UNLIMITED) {
_multicopter_flags.spool_mode = SPOOL_UP;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max -=
1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix -=
1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix_desired = _throttle_rpy_mix;
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f){
_throttle_thrust_max = 0.0f;
}
if (_throttle_rpy_mix <= 0.0f){
_throttle_rpy_mix = 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) &&
is_zero(_throttle_rpy_mix)) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
}
break;
}
}
3.1.3.5 output_armed_stabilizing
这个函数很大，首先要分析主要的函数:
void AP_MotorsMatrix::output_armed_stabilizing()
{
1.首先看下定义的许多变量：
uint8_t i; // general purpose counter通用计数器
float roll_thrust; // roll thrust input value, +/- 1.0
//Roll的推力输入值，同理可看pitch yaw以及throttle的输入值
float pitch_thrust; // pitch thrust input value, +/- 1.0 float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
//可以提供的最大的三个角度范围，不爬升？
float throttle_thrust_rpy_mix; // partial calculation of
throttle_thrust_best_rpy 上一个变量的部分计算？
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits 放大三个角度的比例，适合电机
//的限制？
float rpy_low = 0.0f; // lowest motor value
float rpy_high = 0.0f; // highest motor value
float yaw_allowed = 1.0f; // amount of yaw we can fit in //这个需要在后面看一下，我们可以使适合的yaw数值？
float unused_range; // amount of yaw we can fit in the current channel 多加了一个在此频道
float thr_adj; // the difference between the
pilot's desired throttle and throttle_thrust_best_rpy
//最大范围和飞行员要求的误差？
float throttle_thrust_hover =
get_hover_throttle_as_high_end_pct();
// throttle hover thrust value, 0.0 - 1.0
//hover有悬停或者盘旋的意思，还要自己看下。

2.这里角度三个输入的参数都是姿态控制里给出的控制量，乘以补偿后直接赋值给了推力。

油门的控制量则由
get_throttle()函数获得。

// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
再看一下get_compensation_gain()函数：{
float AP_MotorsMulticopter::get_compensation_gain() const
{
// avoid divide by zero 最大可飞距离为0时候，无补偿
if (_lift_max <= 0.0f) {
return 1.0f;
}
float ret = 1.0f / _lift_max;
//_lift_max是最大升力系数，由update_lift_max_from_batt_voltage()函数获得
#if AP_MOTORS_DENSITY_COMP == 1
// air density ratio is increasing in density / decreasing in altitude //空气密度根据飞行高度的而不同而变化，对其有影响。

if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) {
ret *= 1.0f / constrain_float(_air_density_ratio,0.5f,1.25f); }
#endif
return ret;
}
}
3.此处用来检查油门是否大于0且小于限定的最大值。

// 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_thrust_rpy_mix，不知道是什么意思
throttle_thrust_rpy_mix = MAX(throttle_thrust,
throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hove r*_throttle_rpy_mix);
// calculate throttle that gives most possible room for yaw which is the lower of: 计算油门：给yaw角最有可能的空间，而且要比以下的低：
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
0.5-(最小+最大)/2 这将给与最大余裕（margin）比最大的高，比最小的低？？
// 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
这一点（此时？）_throttle_rpy_mix 在油门输入和盘旋油门之间？
// 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)
我们将会选择#1（#1是yaw角操作的最佳形式，问题是但是这里根本没有给出#1的解释啊。

），因为那意味着减少电机的油门（换句话说，我们喜欢减少油门，因为他提供了更好的yaw角的控制）
// 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
我们选择#2（一种混合了飞行员和盘旋油门的方式）只是因为当油门已经非常低了，我们
想要减少油门而不是更好的yaw叫控制，在飞行员的命令下
// calculate amount of yaw we can fit into the throttle range
将yaw角转换到我们可以适用的油门范围
// this is always equal to or less than the requested yaw from the pilot or rate controller
将总是小于或者等于飞行员或者速度控制器要求的yaw角度
赋值给throttle_thrust_best_rpy，要仔细看下几个参数的意义。

throttle_thrust_best_rpy = MIN(0.5f, throttle_thrust_rpy_mix);
// calculate roll and pitch for each motor
计算每个电机的pitch和roll
// calculate the amount of yaw input that each motor can accept
计算电机可以接受的yaw角度
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {//条件很显然是电机的数目if (motor_enabled[i]) {//明显是看电机是否可用
//_thrust_rpyt_out的定义为结合三种角度和油门输出，输出值为0到1（combined roll,
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] +
pitch_thrust * _pitch_factor[i];
if (!is_zero(_yaw_factor[i])){
if (yaw_thrust * _yaw_factor[i] > 0.0f) {
unused_range = fabsf((1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]))/_yaw_factor[i]);
if (yaw_allowed > unused_range) {
yaw_allowed = unused_range;
}
} else {
unused_range = fabsf((throttle_thrust_best_rpy +
_thrust_rpyt_out[i])/_yaw_factor[i]);
if (yaw_allowed > unused_range) {
yaw_allowed = unused_range;
}
}
}
}
}
// todo: make _yaw_headroom 0 to 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;
}
// add yaw to intermediate numbers for each motor
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];
// record lowest roll+pitch+yaw command
if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i];
}
// record highest roll+pitch+yaw command
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_thrust_rpy_mix);
if (is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
}
// calculate how close the motors can come to the desired throttle
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];
}
}
// constrain all outputs to 0.0f to 1.0f
// test code should be run with these lines commented out as they should not do anything 将所有的输出限制在0到1之间！！
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);
}
}
}。