PIXHAWK飞控最新控制部分源码详解与分析(v 1.8.0)
px4控制源码中文注释及解读
- 作 者：chen honglin
- 时 间：2018.9.27
- 联系方式(QQ): 260437028
写在前面：
关于本篇文章
作者经过两天的啃代码
发现新版本与旧版本相差不小
而网络上并没有详细的中文注释
故作此文
此文仍在改动，可能存在不正确之处，敬请谅解
关于代码怎么运行起来的
mc_att_control_main() 函数中调用基类的mian()函数
如果是start命令，调用start_command_base()函数
如果正常：调用子类中的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参数进行调整
除了对积分环节进行限幅等等什么的常规操作
就是对微分的数据进行低通滤波
最终发布到混控器那里
关于源码的资料
在使用方法中提到了一个网址
https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
可以下载这本电子书，百度文库有中文版
还可以参考官方wiki
讲解开始：
引入相关头文件
#include "mc_att_control.hpp"
#include 定义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
setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
via `manual_control_setpoint` 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
https://www.research-collection.ethz.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();
return 0;
}
姿态控制类的构造函数
- 首先进行了微分器的滤波的频率设置
- 清除陀螺仪传感器接收的标识
- 定义飞机的模式
- 初始化当前姿态及期望姿态
- 进行计算时产生的中间数据清零
- 初始化传感器的修正比例
- 最后进行传感器的更新
//初始化了一些成员
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++) {
_sensor_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_prev.zero();//前一刻角速度清零
_rates_prev_filtered.zero();//前一刻角速度滤波清零
_rates_sp.zero();//角速度设定值清零
_rates_int.zero();//角速度误差积分清零
_thrust_sp = 0.0f;//期望推力清零
_att_control.zero();//姿态控制清零
/* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */
//初始化陀螺仪尺度误差修正
for (unsigned i = 0; i < 3; i++) {
// used scale factors to unity
_sensor_correction.gyro_scale_0[i] = 1.0f;
_sensor_correction.gyro_scale_1[i] = 1.0f;
_sensor_correction.gyro_scale_2[i] = 1.0f;
}
//进行参数更新
parameters_updated();
}
参数获取函数
void
MulticopterAttitudeControl::parameters_updated()
{
/* Store some of the parameters in a more convenient way & precompute often-used values */
//存储一些参数到高速处理的部分，预算经常使用的量
/* roll gains */
_attitude_p(0) = _roll_p.get();//获取横滚角角度的比例系数
_rate_p(0) = _roll_rate_p.get();//获取横滚角角速度的比例系数、积分系数
_rate_i(0) = _roll_rate_i.get();
_rate_int_lim(0) = _roll_rate_integ_lim.get();//获取横滚速度的极限值
_rate_d(0) = _roll_rate_d.get();//获取横滚角速度的微分系数
_rate_ff(0) = _roll_rate_ff.get();//期望速度的正反馈速率
/* pitch gains */
//获取俯仰角的信息
_attitude_p(1) = _pitch_p.get();
_rate_p(1) = _pitch_rate_p.get();
_rate_i(1) = _pitch_rate_i.get();
_rate_int_lim(1) = _pitch_rate_integ_lim.get();
_rate_d(1) = _pitch_rate_d.get();
_rate_ff(1) = _pitch_rate_ff.get();
/* yaw gains */
//获取偏航角的信息
_attitude_p(2) = _yaw_p.get();
_rate_p(2) = _yaw_rate_p.get();
_rate_i(2) = _yaw_rate_i.get();
_rate_int_lim(2) = _yaw_rate_integ_lim.get();
_rate_d(2) = _yaw_rate_d.get();
_rate_ff(2) = _yaw_rate_ff.get();
//fabsf float-abs-float
//获取低通滤波的截止频率和积分项的截至频率
if (fabsf(_lp_filters_d[0].get_cutoff_freq() - _d_term_cutoff_freq.get()) > 0.01f) {
_lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[0].reset(_rates_prev(0));
_lp_filters_d[1].reset(_rates_prev(1));
_lp_filters_d[2].reset(_rates_prev(2));
}
/* angular rate limits */
//角速度限制
_mc_rate_max(0) = math::radians(_roll_rate_max.get());
_mc_rate_max(1) = math::radians(_pitch_rate_max.get());
_mc_rate_max(2) = math::radians(_yaw_rate_max.get());
/* auto angular rate limits */
//auto模式角速度限制
_auto_rate_max(0) = math::radians(_roll_rate_max.get());
_auto_rate_max(1) = math::radians(_pitch_rate_max.get());
_auto_rate_max(2) = math::radians(_yaw_auto_max.get());
/* manual rate control acro mode rate limits and expo */
//manual模式角速度限制
_acro_rate_max(0) = math::radians(_acro_roll_max.get());
_acro_rate_max(1) = math::radians(_acro_pitch_max.get());
_acro_rate_max(2) = math::radians(_acro_yaw_max.get());
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
/* get transformation matrix from sensor/board to body frame */
_board_rotation = get_rot_matrix((enum Rotation)_board_rotation_param.get());
/* fine tune the rotation */
/**<微调旋转*/
/*调整板上旋转偏移*/
Dcmf board_rotation_offset(Eulerf(
M_DEG_TO_RAD_F * _board_offset_x.get(),
M_DEG_TO_RAD_F * _board_offset_y.get(),
M_DEG_TO_RAD_F * _board_offset_z.get()));
_board_rotation = board_rotation_offset * _board_rotation;
}
一些轮询函数，但是这些函数并不会阻塞
//参数更新轮询
//注意下面的轮询函数基本原理都是一样的
//检查一下有没有更新
//每一次循环都检测，但是并不会产生阻塞
void
MulticopterAttitudeControl::parameter_update_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_params_sub, &updated);
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);
updateParams();
parameters_updated();
}
}
//飞机控制状态轮询
void
MulticopterAttitudeControl::vehicle_control_mode_poll()
{
bool updated;
/* Check if vehicle control mode has changed */
orb_check(_v_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
}
}
//手动状态轮询
void
MulticopterAttitudeControl::vehicle_manual_poll()
{
bool updated;
/* get pilots inputs */
orb_check(_manual_control_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
}
}
//飞机姿态设定值轮询
void
MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}
//飞机角速度设定值轮询
void
MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_rates_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
}
}
//飞机状态轮询
void
MulticopterAttitudeControl::vehicle_status_poll()
{
/* check if there is new status information */
bool vehicle_status_updated;
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (_rates_sp_id == nullptr) {
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
}
}
//飞机马达限制轮询
void
MulticopterAttitudeControl::vehicle_motor_limits_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_motor_limits_sub, &updated);
if (updated) {
multirotor_motor_limits_s motor_limits = {};
orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits);
_saturation_status.value = motor_limits.saturation_status;
}
}
//电池状态轮询
void
MulticopterAttitudeControl::battery_status_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_battery_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status);
}
}
//飞机姿态轮询
void
MulticopterAttitudeControl::vehicle_attitude_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_v_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
}
}
//传感器汇总轮询
//注意，_sensor_correction存储了传感器大量信息
void
MulticopterAttitudeControl::sensor_correction_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_sensor_correction_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction);//_sensor_correction_sub这里存储了传感器的值
}
/* update the latest gyro selection */
if (_sensor_correction.selected_gyro_instance < _gyro_count) {
_selected_gyro = _sensor_correction.selected_gyro_instance;
}
}
//传感器偏差轮询
void
MulticopterAttitudeControl::sensor_bias_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_sensor_bias_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias);
}
}
//飞机地面指挥轮询
void
MulticopterAttitudeControl::vehicle_land_detected_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_vehicle_land_detected_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
}
}
姿态控制函数，产生期望的角速度
/**
* Attitude controller.
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector, '_thrust_sp'
*/
void
MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();//检测设置的姿态数据是否已经更新
_thrust_sp = _v_att_sp.thrust;//给到油门
/* prepare yaw weight from the ratio between roll/pitch and yaw gains */
//从横摇/纵摇比和横摆增益的比值中得到横摆重量
Vector3f attitude_gain = _attitude_p;//期望的p
//这段是什么意思呢
//首先申明一点，横滚角和俯仰角的里的产生方式是一样的
//所以呢，他们在控制上有着一致性
//那么 ，怎么办，就把他们的比例系数平均一下吧，在调参的时候要注意了
//之后的constrain函数进行限幅，0最小，1最大
//至于为什么要除以一个横滚角他们的系数呢，我也不太清楚
//据说好像是为了提升手感
const float roll_pitch_gain = (attitude_gain(0) + attitude_gain(1)) / 2.f;//横滚角的p
const float yaw_w = math::constrain(attitude_gain(2) / roll_pitch_gain, 0.f, 1.f);//变比例
attitude_gain(2) = roll_pitch_gain;
/* get estimated and desired vehicle attitude */
Quatf q(_v_att.q);//构建当前姿态的四元数
Quatf qd(_v_att_sp.q_d);//构建期望的四元数
/* ensure input quaternions are exactly normalized because acosf(1.00001) == NaN */
//四元数完全标准化
q.normalize();
qd.normalize();
/* calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch */
//先忽略飞机的偏航，优先调节 横滚和俯仰来对齐z轴
//因为二者力不同，需要进行解耦控制
Vector3f e_z = q.dcm_z();//变换到旋转矩阵，同时选定z轴
Vector3f e_z_d = qd.dcm_z();//变换到旋转矩阵，同时选定z轴
//Quatf来自于matrix函数库，这个库不包含在固件源码中
//经过辛苦的查找，发现是最短旋转
Quatf qd_red(e_z, e_z_d);//算出e_z到e_z_d的最短旋转
if (abs(qd_red(1)) > (1.f - 1e-5f) || abs(qd_red(2)) > (1.f - 1e-5f)) {
/* In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
* full attitude control anyways generates no yaw input and directly takes the combination of
* roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. */
qd_red = qd;
//这是一种特殊情况，很少出现。
//就是二者的z轴基本重合，此时直接当作旋转完成
} else {
/* transform rotation from current to desired thrust vector into a world frame reduced desired attitude */
//将“最短旋转”旋转到机体坐标系
//得到代表中间姿态的旋转矩阵o
//此时可以看成z轴旋转到相应方向后的矩阵
qd_red *= q;
}
/* mix full and reduced desired attitude */
Quatf q_mix = qd_red.inversed() * qd;//将表示中间姿态的旋转矩阵与期望进行叉乘，得到x,y轴的误差
q_mix *= math::signNoZero(q_mix(0));//q_mix乘上符号矩阵
/* catch numerical problems with the domain of acosf and asinf */
q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);//进行限幅
q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);//进行旋转z轴的限幅
//将代表中间姿态的旋转矩阵进行z轴旋转，这次旋转不是简单的旋转，而是引入了步长的，简单来说，就是进行解耦后的旋转
//也代表最终由期望旋转进行合理化，进行解耦后产生最终要使用期望旋转
//qd就是这个最终的旋转
qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3))));
/* quaternion attitude control law, qe is rotation from q to qd */
//根据四元数姿态控制律
//qe是从q到qd的循环
//算出由 “当前姿态” 到 “解耦后的期望旋转” 的旋转四元数
Quatf qe = q.inversed() * qd;
/* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
* also taking care of the antipodal unit quaternion ambiguity */
//计算最终的旋转角
Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();
/* calculate angular rates setpoint */
//计算角速度设定值
_rates_sp = eq.emult(attitude_gain);
/* Feed forward the yaw setpoint rate.
* yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,
* but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
* Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
* and multiply it by the yaw setpoint rate (yaw_sp_move_rate).
* This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
* such that it can be added to the rates setpoint.
*/
//进行前馈控制
_rates_sp += q.inversed().dcm_z() * _v_att_sp.yaw_sp_move_rate;
/* limit rates */
//进行总的速度的限制
for (int i = 0; i < 3; i++) {
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_auto_rate_max(i), _auto_rate_max(i));
} else {
_rates_sp(i) = math::constrain(_rates_sp(i), -_mc_rate_max(i), _mc_rate_max(i));
}
}
}
/*
* Throttle PID attenuation
* Function visualization available here https://www.desmos.com/calculator/gn4mfoddje
* Input: 'tpa_breakpoint', 'tpa_rate', '_thrust_sp'
* Output: 'pidAttenuationPerAxis' vector
*/
进行TPA油门衰减
TPA 表示油门PID衰减率（ThrottlePID Attenuation）
TPA 主要用于允许强力调节过的多旋翼在其油门超过TPA阈值时减小PID增益，以消除振荡
TPA在达到全油门的时候减小（抑制）PID参数值
参数TPA 表示在全油门时会产生半分之几的抑制
tpa_breakpoint 表示油门曲线上TPA被投入使用时的阈值。
当TPA=50（%），tpa_breakpoint=1500（假设油门范围1000-2000）时，会发生：
油门>=1500时，PID参数开始衰减；
3/4油门（1750）时，PID参数衰减25%；
全油门（2000）时，PID参数衰减50%；
TPA会导致在高油门时旋转角速度增加，
这样在因为PID参数和角速度的耦合使油门升高的情况下可以获得快速翻转横滚的能力。
只有MWREWRITE 和LUX两个PID控制器使用线性TPA实现，
在TPA投入使用时，它们不会影响旋转角速度
如何使用
如果在3/4油门时飞行器出现震荡，
设置tpa_breakpoint=1750或者更低（假设油门行程：1000-2000），
然后缓慢增加TPA的值，直到震荡消失。
通常你希望阈值tpa_breakpoint稍早于震荡出现的点，通过实验多试几次以减小或消除震荡
-------此段源于网络-------
Vector3f
MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate)
{
/* throttle pid attenuation factor */
float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa));
Vector3f pidAttenuationPerAxis;
pidAttenuationPerAxis(AXIS_INDEX_ROLL) = tpa;
pidAttenuationPerAxis(AXIS_INDEX_PITCH) = tpa;
pidAttenuationPerAxis(AXIS_INDEX_YAW) = 1.0;
return pidAttenuationPerAxis;
}
角速度控制函数，接收姿态控制器的得出的期望值
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_v_control_mode.flag_armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();//重置积分
}
// get the raw gyro data and correct for thermal errors
Vector3f rates;//定义速率
//陀螺仪的值减去纠正值*比例
//进行传感器热补偿
if (_selected_gyro == 0) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2];
} else if (_selected_gyro == 1) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2];
} else if (_selected_gyro == 2) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2];
} else {
rates(0) = _sensor_gyro.x;
rates(1) = _sensor_gyro.y;
rates(2) = _sensor_gyro.z;
}
// rotate corrected measurements from sensor to body frame
//由于可能陀螺仪安装和板子有偏差等原因，进行微调旋转
rates = _board_rotation * rates;
// correct for in-run bias errors
//减去运行时产生的偏移
rates(0) -= _sensor_bias.gyro_x_bias;
rates(1) -= _sensor_bias.gyro_y_bias;
rates(2) -= _sensor_bias.gyro_z_bias;
//经过tpa算出最终的pid参数
Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_tpa_breakpoint_p.get(), _tpa_rate_p.get()));
Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_tpa_breakpoint_i.get(), _tpa_rate_i.get()));
Vector3f rates_d_scaled = _rate_d.emult(pid_attenuations(_tpa_breakpoint_d.get(), _tpa_rate_d.get()));
/* angular rates error */
//计算速率误差
Vector3f rates_err = _rates_sp - rates;
/* apply low-pass filtering to the rates for D-term */
Vector3f rates_filtered(
//把要用于微分的速率进行低通滤波
_lp_filters_d[0].apply(rates(0)),
_lp_filters_d[1].apply(rates(1)),
_lp_filters_d[2].apply(rates(2)));
//进行pid运算
//其中积分运算是单独运算的
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int -
rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt +
_rate_ff.emult(_rates_sp);
//进行数据迭代
_rates_prev = rates;
_rates_prev_filtered = rates_filtered;
/* update integral only if we are not landed */
//积分运算只是产生与起飞之后
//之后的代码是对积分运算的一些操作
if (!_vehicle_land_detected.maybe_landed && !_vehicle_land_detected.landed) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
// Check for positive control saturation
//检验有没有正饱和
bool positive_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos);
// Check for negative control saturation
//检测有没有负饱和
bool negative_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg);
// prevent further positive control saturation
//防止正积分饱和，此时只进行负积分
if (positive_saturation) {
rates_err(i) = math::min(rates_err(i), 0.0f);
}
// prevent further negative control saturation
//防止负积分饱和，此时只进行正积分
if (negative_saturation) {
rates_err(i) = math::max(rates_err(i), 0.0f);
}
// Perform the integration using a first order method and do not propagate the result if out of range or invalid
float rate_i = _rates_int(i) + rates_i_scaled(i) * rates_err(i) * dt;
//累加后再检测有没有超过限制
if (PX4_ISFINITE(rate_i) && rate_i > -_rate_int_lim(i) && rate_i < _rate_int_lim(i)) {
_rates_int(i) = rate_i;
}
}
}
/* explicitly limit the integrator state */
//积分限幅
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
_rates_int(i) = math::constrain(_rates_int(i), -_rate_int_lim(i), _rate_int_lim(i));
}
}
最终运行的函数
//主要的运行函数
void
MulticopterAttitudeControl::run()
{
/*
* do subscriptions
*/
/*订阅一些传感器的消息*/
/*
int orb_subscribe(const struct orb_metadata *meta)
功能：订阅主题（topic）;
说明：即使订阅的主题没有被公告，但是也能订阅成功；但是在这种情况下，却得不到数据，直到主题被公告；
参数：
meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值；
返回值：
错误则返回ERROR;成功则返回一个可以读取数据、更新话题的句柄；如果待订阅的主题没有定义或声明则会返回-1，然后会将errno赋值为ENOENT;
eg:
int fd = orb_subscribe(ORB_ID(topicName));
*/
/*
ORB_ID获取id号
*/
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));//飞行器姿态
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//飞行器姿态期望值
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));//飞行器角度速率期望值
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));//飞行器控制模式
_params_sub = orb_subscribe(ORB_ID(parameter_update));//飞行器参数更新
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));//人工控制期望值
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));//飞行器所处状态
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));//马达限制（是不是电机锁？）
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));//电池状态
//orb_group_count():返回主题的实例数量
_gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);
//保证有数据
if (_gyro_count == 0) {
_gyro_count = 1;
}
/*
功能：订阅主题（topic）;
说明：通过实例的ID索引来确定是主题的哪个实例；
参数： meta:uORB元对象，可以认为是主题id，
一般是通过ORB_ID(主题名)来赋值; instance:主题实例ID;
实例ID=0与orb_subscribe()实现相同；
返回值： 错误则返回ERROR;成功则返回一个可以读取数据、更新话题的句柄；
如果待订阅的主题没有定义或声明则会返回-1，
然后会将errno赋值为ENOENT;eg: int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
*/
//一次采集传感器的句柄
for (unsigned s = 0; s < _gyro_count; s++) {
_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
//传感器热校正订阅，温度对于陀螺仪有影响
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
//传感器运行偏差校正订阅，传感器运行时会产生误差，要对他进行补偿
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
//地面指挥订阅
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
//唤醒源:传感器app选择的陀螺仪数据
/* wakeup source: gyro data from sensor selected by the sensor app */
px4_pollfd_struct_t poll_fds = {};
poll_fds.events = POLLIN;
const hrt_abstime task_start = hrt_absolute_time();
hrt_abstime last_run = task_start;
//时间累计
float dt_accumulator = 0.f;
//循环次数累计
int loop_counter = 0;
//套路都一样，在while里不断运行该线程
while (!should_exit()) {
//在这里将要进行轮询等待
poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
//最多等待时间100ms
//轮询等待开始
/* wait for up to 100ms for data */
int pret = px4_poll(&poll_fds, 1, 100);
//如果超时，重新开始while循环，进行下一次等待
/* timed out - periodic check for should_exit() */
if (pret == 0) {
continue;
}
//这是不被期望的，但我们无能为力，就是拿不到数据有什么办法
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_ERR("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
//这里暂时是一个空函数
perf_begin(_loop_perf);
//此代码段是为了计算dt
/* run controller on gyro changes */
//如果收到了数据
if (poll_fds.revents & POLLIN) {
const hrt_abstime now = hrt_absolute_time();//计时用的
float dt = (now - last_run) / 1e6f;//存储时间
last_run = now;//时间迭代
//进行限幅
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* copy gyro data */
//使用热校正和ekf偏置估计之前的陀螺数据
//为了保存通过orb_subscribe()函数订阅获得的有效数据
//_sensor_gyro存储了未被滤波并且没有热补偿的原始数据
orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);
/* check for updates in other topics */
//这里只是单纯的进行数据检查，并不会产生阻塞
parameter_update_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_motor_limits_poll();
battery_status_poll();
vehicle_attitude_poll();
sensor_correction_poll();
sensor_bias_poll();
vehicle_land_detected_poll();
//数据检查完毕
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
* or roll (yaw can rotate 360 in normal att control). If both are true don't
* even bother running the attitude controllers */
/**<
*检查我们是否处于rattitude模式
*在俯仰或横滚时，检查飞行员是否在阈值以上
*如果两者都为真，那就不用麻烦运行姿态控制器了
飞行模式判断是否是MAIN_STATE_RATTITUD模式，
该模式是一种新的飞行模式，
只控制角速度，不控制角度，
俗称半自稳模式（小舵量自稳大舵量手动），
主要用在setpoint中，航点飞行。
根据介绍，这个模式只有在pitch和roll都设置为Rattitude模式时才有意义，
如果yaw也设置了该模式，那么就会自动被手动模式替代了。
所以代码中只做了x、y阈值的检测。
*/
/*
关于模式：
姿态模式（Attitude）：控制角度角速度
半自动模式（Rattitude）：自己手动直接控制飞机xy轴的角速度
手动模式(manual) ：自己手动控制xyz轴的角速度
*/
//进行姿态阈值检测，防止发生失控
if (_v_control_mode.flag_control_rattitude_enabled) { //vehicle control mode
if (fabsf(_manual_control_sp.y) > _rattitude_thres.get() ||
fabsf(_manual_control_sp.x) > _rattitude_thres.get()) {
_v_control_mode.flag_control_attitude_enabled = false;
}
}
//如果开启了姿态控制模式,
if (_v_control_mode.flag_control_attitude_enabled)
{
//进行姿态控制
/*
初始就是control_attitude(dt)，
控制数据都是由它来获取的。
该函数内部做了很多的处理，
控制理论基本都是在这个里面体现的，
所以需要深入研究理解它才可以进一步的研究后续的算法。
它的内部会通过算法处理获得控制量（目标姿态Target）
*/
//首先是姿态控制(control_attitude)，
//然后是速度控制(control_attitude_rates)
//姿态控制输出速度的期望量
//姿态控制，输出角速度控制量
control_attitude(dt);
/* publish attitude rates setpoint */
//将输出值外环输出值给到内环控制器
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();//做个时间戳
//如果速度发布句柄不是空指针，将速度发送出去
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
}
else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
}
else
{ //没有开启姿态模式
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled)
{ //如果开启了手动控制模式
/* manual rates control - ACRO mode */
Vector3f man_rate_sp(
math::superexpo(_manual_control_sp.y, _acro_expo_rp.get(), _acro_superexpo_rp.get()),
math::superexpo(-_manual_control_sp.x, _acro_expo_rp.get(), _acro_superexpo_rp.get()),
math::superexpo(_manual_control_sp.r, _acro_expo_y.get(), _acro_superexpo_y.get()));
_rates_sp = man_rate_sp.emult(_acro_rate_max);
_thrust_sp = _manual_control_sp.z;//thrust是油门设定值
//设置角速度期望值
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
//发布角速度期望值
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
}
else
{//没有开启姿态控制，也没有开启手动控制
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
}
//角速度内环控制
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
/* publish actuator controls */
//进行数值检验后导入控制器（三个方向，油门）
_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.control[7] = _v_att_sp.landing_gear;//起落架
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _sensor_gyro.timestamp;
/* scale effort by battery status */
//按电池状态进行缩放
if (_bat_scale_en.get() && _battery_status.scale > 0.0f) {
for (int i = 0; i < 4; i++) {
_actuators.control[i] *= _battery_status.scale;
}
}
//如果没有开启制动器的断路器（如果没有锁定电机）
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) { //发布句柄检查
orb_publish(_actuators_id, _actuators_0_pub, &_actuators); //发布到制动器
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
//重要的控制部分到此已经结束了，已经把速度发布给了制动器
//此时进行一些无关紧要的事情
/* publish controller status */
//发布速度控制器状态
rate_ctrl_status_s rate_ctrl_status;
rate_ctrl_status.timestamp = hrt_absolute_time();
rate_ctrl_status.rollspeed = _rates_prev(0);
rate_ctrl_status.pitchspeed = _rates_prev(1);
rate_ctrl_status.yawspeed = _rates_prev(2);
rate_ctrl_status.rollspeed_integ = _rates_int(0);
rate_ctrl_status.pitchspeed_integ = _rates_int(1);
rate_ctrl_status.yawspeed_integ = _rates_int(2);
int instance;
orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT);
}
//如果控制结束
if (_v_control_mode.flag_control_termination_enabled) {
//如果机体状态不是垂直起降
if (!_vehicle_status.is_vtol) {
//将速度期望值置为零
_rates_sp.zero();
//将速度积分置零
_rates_int.zero();
//油门置零
_thrust_sp = 0.0f;
//姿态最终控制置零
_att_control.zero();
/* publish actuator controls */
//将发布的制动器状态置0；
//关闭电机
_actuators.control[0] = 0.0f;
_actuators.control[1] = 0.0f;
_actuators.control[2] = 0.0f;
_actuators.control[3] = 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _sensor_gyro.timestamp;
//如果没有开启制动器断路开关
if (!_actuators_0_circuit_breaker_enabled) {
//进行发布
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
}
}
/* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */
//如果没有装备，或者时间在3300000内
if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) {
dt_accumulator += dt;
++loop_counter;
if (dt_accumulator > 1.f) {
const float loop_update_rate = (float)loop_counter / dt_accumulator;
_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
dt_accumulator = 0;
loop_counter = 0;
//调整积分滤波频率
_lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
}
}
}
//这应该是最终的定时器，但是他被写成了一个空函数
perf_end(_loop_perf);
}
//到此为止，循环控制器已经结束
//控制器都结束了，就把订阅也给取消了吧
orb_unsubscribe(_v_att_sub);
orb_unsubscribe(_v_att_sp_sub);
orb_unsubscribe(_v_rates_sp_sub);
orb_unsubscribe(_v_control_mode_sub);
orb_unsubscribe(_params_sub);
orb_unsubscribe(_manual_control_sp_sub);
orb_unsubscribe(_vehicle_status_sub);
orb_unsubscribe(_motor_limits_sub);
orb_unsubscribe(_battery_status_sub);
for (unsigned s = 0; s < _gyro_count; s++) {
orb_unsubscribe(_sensor_gyro_sub[s]);
}
orb_unsubscribe(_sensor_correction_sub);
orb_unsubscribe(_sensor_bias_sub);
orb_unsubscribe(_vehicle_land_detected_sub);
}
在基类的开始函数进行的，创建任务函数
int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_ATTITUDE_CONTROL,//最高级-4
1700,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
return 0;
}
创建高度控制器指针类型的实例，在基类函数中定义
MulticopterAttitudeControl *MulticopterAttitudeControl::instantiate(int argc, char *argv[])
{
return new MulticopterAttitudeControl();
}
自定义的命令函数可以写在这里
int MulticopterAttitudeControl::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
调用基类中写的主函数，新版本特性之一：将模块的一般功能写入到基类
int mc_att_control_main(int argc, char *argv[])
{
return MulticopterAttitudeControl::main(argc, argv);
}