PixHawk学习笔记之源码浅析——mc_pos_control.cpp——task

注意：基于“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的博客，在此表示感谢！

PixHawk学习笔记之源码浅析——mc_pos_control.cpp——task_main相关推荐

sheng的学习笔记-Vector源码分析
概述 Vector底层也是数组,跟ArrayList很像(先看下ArrayList,再看Vector会很轻松),ArrayList可参考下文,并且由于效率低,已经被淘汰了,大概瞅瞅得了 sheng的学 ...
yolov1-v5学习笔记及源码解读
目录深度学习网络分类评价指标原理 yolov1 yolov2 yolov3 yolov4 yolov5 源码解读(v3为例) 深度学习网络分类深度学习经典检测方法通常分为 two-stage ...
狂神说SpringCloud学习笔记(附带源码和笔记)
狂神说Spring Cloud Netflix笔记-01(服务注册与发现) 狂神说Spring Cloud Netflix笔记-02(Eureka集群的搭建 ) 狂神说Spring Cloud Net ...
android源码编译简书,android学习笔记之源码编译
编译环境 1.需要Ubuntu 64bit,建议Ubuntu14.04 64-bit 2.安装openJDK7 $ sudo apt-get update $ sudo apt-get install ...
dubbo学习笔记一源码编译
前面学习了netty和rocketmq,当然前面的文章还会继续更新,继续往下写 2016 没几天了,我打算写下dubbo 2017 继续深入源码,大家有啥问题都可以一起来讨论源码搭建下载源码同 ...
as工程放到源码编译_方舟编译器学习笔记2 源码编译
根据方舟官方文档编译了方舟编译器的源码,在这里简单谈谈其源码的编译过程: 1.操作系统环境: 64位版本的Ubuntu(官方推荐Ubuntu 16.04).我自己本身就有Ubuntu 16.04的虚拟 ...
Opencv学习笔记 - imread源码解读
一.打开图片流程分析 1.读取图片头,进行解码器的寻找 2.根据参数flags,确定图像通道和是否缩放 3.给解码器指定缩放参数和源 4.使用解码器读取图像的头,确保没有问题,失败则输出错误并返回 5 ...
mac vim python3_VIM学习笔记编译源码(Compile Code)-Python
Python作为一种解释型编程语言,需要解释器来编译并执行Python代码. 测试Python 对于Linux和Mac操作系统,均已预装Python.而在Windows下,可以使用安装包或者直接解压版 ...
循环神经网络(RNN, Recurrent Neural Networks)学习笔记：源码分析（一）
前面帖子给出了RNN的基础理论,里面也提到了神牛Mikolov,这个帖子就基于此牛开源出的一个语言建模工具箱(RNN Language Modeling Tookit)进行代码走读,会加速理解RNN算 ...

PixHawk学习笔记之源码浅析——mc_pos_control.cpp——task_main

先说几组名词：

再整一个框架

再看 task_main() 源码

最后来几个子程序

PixHawk学习笔记之源码浅析——mc_pos_control.cpp——task_main相关推荐

最新文章

热门文章

PixHawk学习笔记 之 源码浅析——mc_pos_control.cpp——task_main

先说几组名词：

再整一个框架

再看 task_main() 源码

最后来几个子程序

PixHawk学习笔记 之 源码浅析——mc_pos_control.cpp——task_main相关推荐

最新文章

热门文章

PixHawk学习笔记之源码浅析——mc_pos_control.cpp——task_main

PixHawk学习笔记之源码浅析——mc_pos_control.cpp——task_main相关推荐