PixHawk学习笔记 之 源码浅析——mc_pos_control.cpp——task_main
注意：基于“Firmware-1.6.0rc1”
献上固件源码分享链接：https://pan.baidu.com/s/1kUPocmF 密码：j55a
自己边学边写的，一定有错，欢迎批评指正！
先说几组名词：
fw mode：Fixed-wing mode（固定翼模式）
VTOL：Vertical Take-Off and Landing（垂直起降）
nullptr：C++中空指针类型的关键字
再整一个框架
- 前期准备工作-开始
- 前期准备工作-结束
- while大循环-开始
- 大循环-第一部分：准备工作，获取一些更新内容
- 大循环-第二部分：位置控制
第一个if
if（
flag_control_altitude_enabled ||
flag_control_position_enabled ||
flag_control_climb_rate_enabled ||
flag_control_velocity_enabled ||
flag_control_acceleration_enabled
高度控制使能 || 位置控制使能 || 爬升速率控制使能 || 速度控制使能 || 加速度控制使能
）
{ 运行控制函数，得到各数值 }
else
如果以上均失能，则表明控制器失能
{ 重置设定值 }；
第二个if
if（
flag_control_manual_enabled &&
flag_control_attitude_enabled
手动控制使能 && 姿态控制使能
）
else
如果以上均失能，则表明控制器失能
{ }；
第三个if
if(
!(flag_control_offboard_enabled &&
!(flag_control_position_enabled ||
flag_control_velocity_enabled ||
flag_control_acceleration_enabled))
！（外部控制使能 && ！（位置控制使能 || 速度控制使能 || 加速度控制使能））
)
{ }；- while大循环-结束
再看 task_main() 源码
void
MulticopterPositionControl::task_main()
{
/* ====================================== 前期准备工作-开始 ====================================== */
/* do subscriptions
* 参数订阅 */
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
/* 参数更新 */
parameters_update(true);
/* initialize values of critical structs until first regular update
* 在第一次定期更新之前初始化关键结构的值 */
_arming.armed = false;
/* get an initial update for all sensor and status data
* 获取所有传感器和状态变量的初始更新 */
poll_subscriptions();
/* We really need to know from the beginning if we're landed or in-air.
* 已经着陆还是在空中 */
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
/* 定义上锁状态标识 */
bool was_armed = false;
/* 初始化“前一时刻”绝对时间为0 */
hrt_abstime t_prev = 0;
/* Let's be safe and have the landing gear down by default
* 默认情况下，起落架放下 */
_att_sp.landing_gear = -1.0f;
/* wakeup source
* 唤醒源码 */
px4_pollfd_struct_t fds[1];
/*判断是否有当地位置更新*/
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
/* ====================================== 前期准备工作-结束 ====================================== */
/* ====================================== while大循环-开始 ====================================== */
while (!_task_should_exit) {
/* =========================== 大循环-第一部分：准备工作，获取一些更新内容 ========================== */
/* wait for up to 20ms for data
* 等待20ms以获取数据 */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20);
/* timed out - periodic check for _task_should_exit
* 等待时间结束，周期性检查任务是否需要退出 */
if (pret == 0) {
/* Go through the loop anyway to copy manual input at 50 Hz.
* 在任何情况下都通过循环以50 Hz获取手动输入。 */
}
/* this is undesirable but not much we can do
* 下面这种情况是我们不愿意看到的，但我们无能为力 */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
/* 更新订阅 */
poll_subscriptions();
/* 更新数据 */
parameters_update(false);
/* 获取绝对时间，时间点 */
hrt_abstime t = hrt_absolute_time();
/* 获取相对时间，时间段 */
float dt = t_prev != 0 ? (t - t_prev) / 1e6f : 0.0f;
/* 更新t_prev */
t_prev = t;
/* set dt for control blocks
* 为控制块设置dt */
setDt(dt);
/* =====判断解锁状态，重置位置和高度设置===== */
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming
* 解锁时复位设定值和积分项 */
_reset_pos_sp = true;
_reset_alt_sp = true;
_do_reset_alt_pos_flag = true;
_vel_sp_prev.zero();
_reset_int_z = true;
_reset_int_xy = true;
_reset_yaw_sp = true;
}
/* reset yaw and altitude setpoint for VTOL which are in fw mode
* 为固定翼模式下的垂直起降复位航向和高度设定值 */
if (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing) {
_reset_yaw_sp = true;
_reset_alt_sp = true;
}
/* Update previous arming state
* 更新前一时刻的解锁状态 */
was_armed = _control_mode.flag_armed;
/* 更新坐标系xyz方向基准值 */
update_ref();
/* 独立于当前模式更新加速度 */
update_velocity_derivative();
/* reset the horizontal and vertical position hold flags for non-manual modes
* or if position / altitude is not controlled
* 当为非手动模式或者位置/高度未被控制时，复位水平和竖直位置标志位 */
if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) {
_pos_hold_engaged = false;
}
if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) {
_alt_hold_engaged = false;
}
/* ================================== 大循环-第二部分：位置控制 =================================== */
/********************************************* 第一个if *********************************************/
/* 高度控制、位置控制、爬升速率控制、速度控制、加速度控制均可以使能进入以下程序 */
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {
/* 飞控的核心都在这个子程序里，该子程序有源码解读，如需查看，请至文后链接 */
do_control(dt);
/* fill local position, velocity and thrust setpoint
* 填写时间戳、当前位置、速度、航向和推力设定值 */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);
/* publish local position setpoint
* 发布当前位置设定值 */
if (_local_pos_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
} else {
/* position controller disabled, reset setpoints
* 如果位置控制器失能，则重置设定值 */
_reset_pos_sp = true;
_reset_alt_sp = true;
_do_reset_alt_pos_flag = true;
_mode_auto = false;
_reset_int_z = true;
_reset_int_xy = true;
/* store last velocity in case a mode switch to position control occurs
* 存储最后一个速度，以防切换到位置控制模式 */
_vel_sp_prev = _vel;
}
/********************************************* 第二个if *********************************************/
/* generate attitude setpoint from manual controls
* 从手动控制中产生姿态设定值 */
if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
/* 手动控制部分，该子程序有源码解读，如需查看，请至文后链接 */
generate_attitude_setpoint(dt);
} else {
/* 手动控制失能 */
_reset_yaw_sp = true;
_att_sp.yaw_sp_move_rate = 0.0f;
}
/* update previous velocity for velocity controller D part
* 为速度PID控制器的D部分更新“以前的速度”
* 关于PID相关知识会在另一篇博客里写，如需查看，请至文后链接 */
_vel_prev = _vel;
/********************************************* 第三个if *********************************************/
/* publish attitude setpoint
* 发布姿态设定值
* Do not publish if offboard is enabled but position/velocity/accel control is disabled,
* in this case the attitude setpoint is published by the mavlink app. Also do not publish
* if the vehicle is a VTOL and it's just doing a transition
* (the VTOL attitude control module will generateattitude setpoints for the transition).
* 如果offboard被启用，但是位置/速度/加速度控制被禁用，则不发布速度设定值，
* 如果飞行器是垂直起降的，并且它正在进行模式转换，同样不发布速度设定值，
* （垂直起降姿态控制模块将为过渡时期产生转换的姿态设定值）。 */
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled))) {
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}
/* reset altitude controller integral (hovering throttle)
* to manual throttle after manual throttle control
* 手动油门控制后，重置高度控制器的积分项（悬停油门）至手动控制 */
_reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
&& !_control_mode.flag_control_climb_rate_enabled;
}
/* ====================================== while大循环-结束 ====================================== */
mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped");
_control_task = -1;
}
最后来几个子程序
mc_pos_control.cpp 子程序之 do_control(dt)
mc_pos_control.cpp 子程序之 generate_attitude_setpoint(dt)
PixHawk学习笔记 之 PID浅析
有些注释的地方格式不统一，是因为编辑器有BUG，我也说不清楚，将就看吧。
写本博客一是为了记录自己的学习过程，方便以后查看，二是给刚开始学习PixHawk的同学以参考，如果能有所帮助，将不胜欣慰。
欢迎共同探讨，同时虚心接受各路大神批评指正！
在编写本文过程中参考了czyv587的博客，在此表示感谢！