PIXHAWK飞控最新控制部分源码详解与分析（v1.8.0）px4控制源码中⽂注释及解读
作 者：chen honglin
时 间：2018.9.27
联系⽅式(QQ): 260437028
写在前⾯：
关于本篇⽂章
作者经过两天的啃代码
发现新版本与旧版本相差不⼩
⽽⽹络上并没有详细的中⽂注释
故作此⽂
此⽂仍在改动，可能存在不正确之处，敬请谅解
关于代码怎么运⾏起来的
mc_att_control_main() 函数中调⽤基类的mian()函数
如果是start命令，调⽤start_command_ba()函数
如果正常：调⽤⼦类中的task_spawn()函数
在task_spawn()函数中创建新的线程
在线程函数中调⽤初始化函数instantiate(argc, argv)来创建⼀个新的类;
创建成功之后进⼊到run()函数
关于旋翼部分控制的摘要
基于版本v1.8.0
该控制还是经典的串级pid
外环⾓度控制部分仅仅采⽤了⽐例控制
但是在控制具体过程中
采⽤了 偏航⾓ 与 俯仰横滚⾓ 解耦的控制⽅法
原因很简单 ⼆者的控制最终产⽣的⼒的类型不同
所以纠正的步长应该单独来看
具体怎么实现的呢：
第⼀步只是对齐 z 轴，就是只是调整 俯仰横滚⾓ 保证z轴对到其往上⾯
提取当前姿态的z轴与期望姿态的z轴的⽅向余弦矩阵DCM
由矩阵库matrix中的函数qd_red()产⽣对其z轴的最短旋转的四元数
将当前姿态通过这最短旋转 旋转到 对其z轴的那个矩阵姑且叫他 中间姿态矩阵
在由中间姿态矩阵经过计算xy轴的误差，得到能进⾏偏航⾓纠正的⼀个旋转矩阵
这时候并不是直接把中间状态矩阵直接旋转
⽽是将旋转矩阵乘上⼀个⽐例，再进⾏旋转
这样就实现了解耦
全部的⼯作，都只是为了求⼀个和“期望姿态”有⼀定差别的却⼜⼤致相同的“解耦期望姿态”所有的⼯作都是为了进⾏解耦
好的，解耦完毕，得到了“解耦期望姿态”
可以求出要进⾏下⼀步运算的、要旋转的 欧拉⾓了
之后⾓速度的控制并没有什么新意
进⾏tpa对pid参数进⾏调整
除了对积分环节进⾏限幅等等什么的常规操作
就是对微分的数据进⾏低通滤波
最终发布到混控器那⾥
关于源码的资料
讲解开始：
引⼊相关头⽂件
#include"mc_att_control.hpp"
#include<conversion/rotation.h>
#include<drivers/drv_hrt.h>
#include<lib/ecl/geo/geo.h>
#include<circuit_breaker/circuit_breaker.h>
#include<mathlib/math/Limits.hpp>
#include<mathlib/math/Functions.hpp>
定义tpa的最低速度限制
关于什么是tpa，请继续向下读
#define TPA_RATE_LOWER_LIMIT 0.05f
进⾏定义三个轴的名字
#define AXIS_INDEX_ROLL 0
#define AXIS_INDEX_PITCH 1
#define AXIS_INDEX_YAW 2
#define AXIS_COUNT 3
引⼊matrix库的命名空间
这⾥要注意⼀下，这个库的⽂件可能要单独下载
这个并不⼀定包含在固件中
在ubuntu上clone的应该不会有问题
下载压缩包的可能没有这个矩阵库
using namespace matrix;
⽤法函数：打印⼀些消息
打印⼀些关于指令的使⽤⽅法
int MulticopterAttitudeControl::print_usage(const char*reason)
{
if(reason){
PX4_WARN("%s\n", reason);
}
//在C++ 11中，增加了这样的语法。前⾯写R()代表⾮转义形式
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This implements the multicopter attitude and rate controller. It takes attitude
tpoints (`vehicle_attitude_tpoint`) or rate tpoints (in acro mode
via `manual_control_tpoint` topic) as inputs and outputs actuator control messages.
The controller has two loops: a P loop for angular error and a PID loop for angular rate error.
Publication documenting the implemented Quaternion Attitude Control:
Nonlinear Quadrocopter Attitude Control (2013)
by Dario Brescianini, Markus Hehn and Raffaello D'Andrea
Institute for Dynamic Systems and Control (IDSC), ETH Zurich
hz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf ### Implementation
To reduce control latency, the module directly polls on the gyro topic published by the IMU driver. )DESCR_STR");
PRINT_MODULE_USAGE_NAME("mc_att_control","controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return0;
}
姿态控制类的构造函数
1. ⾸先进⾏了微分器的滤波的频率设置
2. 清除陀螺仪传感器接收的标识
3. 定义飞机的模式
4. 初始化当前姿态及期望姿态
5. 进⾏计算时产⽣的中间数据清零
6. 初始化传感器的修正⽐例
7. 最后进⾏传感器的更新
//初始化了⼀些成员
MulticopterAttitudeControl::MulticopterAttitudeControl():
ModuleParams(nullptr),//调⽤基类ModuleParams的构造函数
_loop_perf(perf_alloc(PC_ELAPSED,"mc_att_control")),//循环执⾏计数器
//设定微分阶段低通滤波的更新频率
_lp_filters_d{/**< low-pass filters for D-term (roll, pitch & yaw) */
{initial_update_rate_hz,50.f},
{initial_update_rate_hz,50.f},
{initial_update_rate_hz,50.f}}// will be initialized correctly when params are loaded
{
//将陀螺仪的接受句柄恢复到默认
for(uint8_t i =0; i < MAX_GYRO_COUNT; i++){
_nsor_gyro_sub[i]=-1;//#define MAX_GYRO_COUNT 3
}
/
/定义为多旋翼模式
_vehicle_status.is_rotary_wing =true;
/* initialize quaternions in messages to be valid */
//将消息中的四元数初始化为有效
_v_att.q[0]=1.f;/*当前姿态 */
_v_att_sp.q_d[0]=1.f;/*期望的姿态*/
//进⾏清零操作
_();//前⼀刻⾓速度清零
_rates_();//前⼀刻⾓速度滤波清零
_();//⾓速度设定值清零
_();//⾓速度误差积分清零
_thrust_sp =0.0f;//期望推⼒清零
_();//姿态控制清零
/* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */ //初始化陀螺仪尺度误差修正
for(unsigned i =0; i <3; i++){
// ud scale factors to unity
__scale_0[i]=1.0f;
__scale_1[i]=1.0f;
__scale_2[i]=1.0f;
}
//进⾏参数更新
parameters_updated();
}
参数获取函数
void
MulticopterAttitudeControl::parameters_updated()
{
/* Store some of the parameters in a more convenient way & precompute often-ud values */
//存储⼀些参数到⾼速处理的部分，预算经常使⽤的量
/* roll gains */
_attitude_p(0)= _();//获取横滚⾓⾓度的⽐例系数
_rate_p(0)= _roll_();//获取横滚⾓⾓速度的⽐例系数、积分系数
_rate_i(0)= _roll_();
_rate_int_lim(0)= _roll_rate_();//获取横滚速度的极限值
_rate_d(0)= _roll_();//获取横滚⾓速度的微分系数
_rate_ff(0)= _roll_();//期望速度的正反馈速率
/* pitch gains */
//获取俯仰⾓的信息
_attitude_p(1)= _();
_rate_p(1)= _pitch_();
_rate_i(1)= _pitch_();
_rate_int_lim(1)= _pitch_rate_();
_rate_d(1)= _pitch_();
_rate_ff(1)= _pitch_();
/
* yaw gains */
//获取偏航⾓的信息
_attitude_p(2)= _();
_rate_p(2)= _yaw_();
_rate_i(2)= _yaw_();
_rate_int_lim(2)= _yaw_rate_();
_rate_d(2)= _yaw_();
_rate_ff(2)= _yaw_();
//fabsf float-abs-float
//获取低通滤波的截⽌频率和积分项的截⾄频率
if(fabsf(_lp_filters_d[0].get_cutoff_freq()- _d_term_())>0.01f){
_lp_filters_d[0].t_cutoff_frequency(_loop_update_rate_hz, _d_term_());
_lp_filters_d[1].t_cutoff_frequency(_loop_update_rate_hz, _d_term_());
_lp_filters_d[2].t_cutoff_frequency(_loop_update_rate_hz, _d_term_());
_lp_filters_d[0].ret(_rates_prev(0));
_lp_filters_d[1].ret(_rates_prev(1));
_lp_filters_d[2].ret(_rates_prev(2));
}
/* angular rate limits */
//⾓速度限制
_mc_rate_max(0)= math::radians(_roll_());
_mc_rate_max(1)= math::radians(_pitch_());
_mc_rate_max(2)= math::radians(_yaw_());
/* auto angular rate limits */
//auto模式⾓速度限制
_auto_rate_max(0)= math::radians(_roll_());
_auto_rate_max(1)= math::radians(_pitch_());
_auto_rate_max(2)= math::radians(_yaw_());
/* manual rate control acro mode rate limits and expo */
//manual模式⾓速度限制
_acro_rate_max(0)= math::radians(_acro_());
_acro_rate_max(1)= math::radians(_acro_());
_acro_rate_max(2)= math::radians(_acro_());
_actuators_0_circuit_breaker_enabled =circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
/* get transformation matrix from nsor/board to body frame */
_board_rotation =get_rot_matrix((enum Rotation)_board_());
/* fine tune the rotation */
/**<；微调旋转*/
/*调整板上旋转偏移*/
Dcmf board_rotation_offt(Eulerf(
M_DEG_TO_RAD_F * _board_(),
M_DEG_TO_RAD_F * _board_(),
M_DEG_TO_RAD_F * _board_()));
_board_rotation = board_rotation_offt * _board_rotation;
}
⼀些轮询函数，但是这些函数并不会阻塞
//参数更新轮询
//注意下⾯的轮询函数基本原理都是⼀样的
//检查⼀下有没有更新
//每⼀次循环都检测，但是并不会产⽣阻塞
void
MulticopterAttitudeControl::parameter_update_poll()
{
