c_uart_interface_example是mavlink团队提供的一个演示如何用c语言调用mavlink API对飞机做offboard控制的例子程序，这个程序写的挺漂亮的，但是，新的固件，比如：1.13.0d在进入offboard的方法上已经变化了，参见官网对这个问题的讨论。所以，现在的c_uart_interface_example并不能让飞机起飞，本文对这个程序做了修改，仿真环境下运行正常了。

    c_uart_interface_example这个程序可从mavlink主页下载：

     https://github.com/mavlink/c_uart_interface_example

在下载后的README.md中有详细的安装和运行说明。

程序的使用：

c_uart_interface_example的可执行程序是mavlink_control，它的使用方式如下：

mavlink_control [-d <devicename> -b <baudrate>] [-u <udp_ip> -p <udp_port>] [-a ]

常用的运行mavlink_control的两种形式：

1. 在本机运行mavlink_control, 仿真环境也在本机

mavlink_control -u 127.0.0.1 -a

其中：-u选项是指定PX4在127.0.0.1（本机地址）机器上运行；udp的port没有指定，缺省是14540；-a的意思是autotakeoff（自动起飞）。

2. 在offboard计算机中运行mavlink_control，将offboard计算机与安装有PX4固件的自驾仪Pixhawk做串口连接（注意，如果Pixhawk是在真实飞机上，需要卸下浆叶）

mavlink_control -d /dev/ttyUSB0 -b 57600 -a

其中：-d选项是指定串口名字，我的树梅派与Pixhawk连接采用的是USB转串口电缆，所以用的设备名为ttyUSB0；-b选项是指定这个串口的通讯波特率，这个波特率需要与地面站（比如：QGroundControl）为PX4的串口（通常为TELE2）设置的波特率一致，需要核实一下，否则连不通； -a的意思是autotakeoff（自动起飞）。

源代码组成

主要是三部分：

1。主程序，mavlink_control(.h和.cpp)

通过调用与PX4的接口，控制飞机进入offboard状态，并让飞机起飞、降落。

2。 PX4接口的实现，autopilot_interface(.h和.cpp)

用mavlink API实现从PX4接收以及向PX4发送mavlink消息。

3。 通讯接口的实现，generic.cpp、serial_port（.h和.cpp）、udp_port（.h和.cpp）

4。 mavlink代码，在mavlink目录中。

分析

    c_uart_interface_example在打开串口（port.start())后，就启动了2个线程，一个是读线程，一个是写线程，顾名思义，写线程是将PX4能接收的mavlink消息从串口送给pixhawk中的PX4,读线程等待pixhawk中的PX4从串口发送给外部计算机mavlink消息。

    读线程先启动，以10HZ的频率，也就是100ms从连接在PX4上的串口读取一条消息，每一条消息都是由port→read_message()函数完成的，每一次都是读1个字符：

int result = _read_port(cp)

   然后对这个字符解码：

msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);

   解码过程是一个字节一个字节处理的。msgRecevied标志表示是否一个完整的消息被收到了，如果是0则说明需要读下一个字节，如果是1表示一个完整的mavlink消息解码完成并组装好了。

   一个完整的message保存在这样一个的结构中（定义在mavlink/include/mavlink/v2.0/mavlink_types.h)：

MAVPACKED(

typedef struct __mavlink_message {

uint16_t checksum; ///< sent at end of packet

uint8_t magic; ///< protocol magic marker

uint8_t len; ///< Length of payload

uint8_t incompat_flags; ///< flags that must be understood

uint8_t compat_flags; ///< flags that can be ignored if not understood

uint8_t seq; ///< Sequence of packet

uint8_t sysid; ///< 表示飞行器或系统ID送来了消息

uint8_t compid; ///< 表示组件（飞行器上的）ID送来了消息

uint32_t msgid:24; ///< 消息内容（payload)的ID

uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; 编码后的消息内容。

uint8_t ck[2]; ///< incoming checksum bytes（用于校验）

uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];（用于签名算法）

}) mavlink_message_t;

读到一条完整的消息后，判断这个消息是什么。根据mavlink对消息的定义，每一条消息都有它的唯一标识msgid。初始时，msgid是MAVLINK_MSG_ID_HEARTBEAT，则需要对这个心跳信息再解码。

mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));

   解码后就得到了sysid，这个常量表示是谁在发送心跳消息。

   解码程序定义在文件：mavlink/include/mavlink/v2.0/common/mavlink_msg_heartbeat.h。

   这个.h文件一方面定义了heartbeat心跳信息的数据结构，另外还定义并实现了上述对heartbeat消息的解码过程。

   然后，做一个循环等待，这个循环确定如果接收到heartbeat消息，就必须再接收sys_status消息，就是说heartbeat后面应该紧随sys_status消息，必须成对出现。其他消息都是单独的。

   读线程进入一个循环，循环的每一次都从PX4读一条消息，PX4那一端相关server会按一定频率public各种mavlink消息，PX4端的mavlink（src/module/mavlink）程序会将这些消息发送给这个外部计算机。

   在读线程得到以下位置和姿态两条mavlink消息后：消息ID分别是：

针对消息MAVLINK_MSG_ID_LOCAL_POSITION_NED

对payload域再解码：mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));

针对消息MAVLINK_MSG_ID_ATTITUDE

对payload域在解码：mavlink_msg_attitude_decode(&message, &(current_messages.attitude));

   飞机的初始位置就有了。然后启动写线程。

   写线程先做初始化工作，就是把当前得到的位置消息发送给px4。当前的位置x，y，z就是启动读线程时得到的初始位置（比如：飞机在地面的某个位置），而它的速度分量是0。执行write_setpoint()。

   先把当前要写的数据current_setpoint.data放到sp中，这里要加锁，防止其它线程更改current_setpoint.data。

mavlink_set_position_target_local_ned_t sp;

{

std::lock_guard<std::mutex> lock(current_setpoint.mutex);

sp = current_setpoint.data;

}

   此时，current_setpoint.data是个全局变量，存有当前飞机的位置信息，而且vx,vy,vy都是0,把这个信息发给px4之前，需要通过current_setpoint.data变成message消息。

   在变成mavlink的message之前，先对位置点变量sp进行设置：

sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY &

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE;

sp.coordinate_frame = MAV_FRAME_LOCAL_NED;

   这个.type_mask是说后面在为mavlink消息编码时是针对VELOCITY和YAW_RATE这两组数据的,而.coordinate是说后面的位置设置都采用FRAME_LOCAL_NED坐标体系。

   然后把位置信息编码，生成mavlink消息：

mavlink_msg_set_position_target_local_ned_encode(system_id, companion_id, &message, &sp);

   其中，system_id标识一架px4所在的pixhawk飞机，已经在读线程从heartbeat消息中得到了，而companion_id标识一台外部计算机，是0，就一部。

   然后write_message(message);

   写线程进入一个循环，每250ms用setpoint（）对px4发送一次当前设定点（setpoint）。

   PX4规定，如果有一个外部计算机想让px4进入offboard状态，在进入offboard状态后，要每至少500ms（2hz）向px4发送一次当前“设定点”（setpoint）的消息，这样px4可以保持offboard状态，否则将退出offboard状态。

   换一种通俗一些的说法，你(比如：树梅派）要想从外部（offboard)控制我（PX4飞控），你就必须持续（至少500ms-2Hz）告诉我，下一步让我干什么（setpoint），否则，超过一定时间，我就失控了。这个例子程序是250ms发一次，够条件。

   set_point()函数总是用current_setpoint这个全局变量发送作为当前设定点数据的消息。初始时，current_setpoint.data中的x，y，z是飞机还没有动的位置，vx，vy，vz是0,后面就开始动起来了。

   这样，两个线程就启动起来了。

   在commands函数中：

   发送让px4进入offboard的命令，发送让px4 arm的命令。

///

set_position（), 设置新的位置

更新设定点

   写线程中的setpoint（）函数定期将新的设定点消息送给px4，飞机移动。

///

由于读线程的read_message()会定期从PX4拿到local_position_ned 消息，所以做一个每8秒一次的循环，就可以打印出当前飞机的位置信息。

///

set_velocity，设置向新的位置移动的速度

set_yaw，设置向新的位置移动的偏航角度

更新位置

同样，写线程的setpoint()函数将新的设定点发送给PX4,飞机移动。

///

set_velocity, 设置向下的位置移动速度（着陆），每秒下降1m。

设置消息为降落消息

sp.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_LAND;

更新位置

///

同样，由于读线程的read_message()会定期从PX4拿到local_position_ned 消息，做一个每4秒一次的循环，可以打印出当前飞机的位置信息。

///

发送disarm命令

飞机解锁。

停止两个线程：autopilot_interface.stop()

关闭uart接口：port->stop()

关于进入，保持和退出offboard飞行模式

   进入offboard飞行模式并保持这个模式的方法

   注意：以下方法是2021年3月以前的PX4固件版本，这之后，比如我的固件是1.13.0d，已经不支持MAV_CMD_NAV_GUIDED_ENABLE这个命令。这意味着下载的c_uart_interface_example例子程序不能进入offboard模式，也就是这个程序不能让飞机移动。新的做法是用mavlink送命令MAV_CMD_DO_MODE_SET。

   以下我先列出老的方法，然后列出新的方法：

老的方法，基于MAV_CMD_NAV_GUIDED_ENABLE，已被弃用。

   首先要组装一个mavlink消息，发给PX4，让PX4进入offboard飞行模式，其中：设置浮点参数flag>0.5f为启动offboard飞行模式。

toggle_offboard_control( bool flag )
{
        // Prepare command for off-board mode

mavlink_command_long_t com = { 0 };
        com.target_system    = system_id;
        com.target_component = autopilot_id;
        com.command          = MAV_CMD_NAV_GUIDED_ENABLE;
        com.confirmation     = true;
        com.param1           = (float) flag; // flag >0.5 => 启动, <0.5 => 停止

// 编码成mavlink消息
        mavlink_message_t message;
        mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);

// 向PX4发送消息
        int len = port->write_message(message);

// Done!

return len；

}

这是一条PX4的命令，先编码，然后发给PX4。退出offboard飞行模式，需要发送上述命令，其中：flag < 0.5f。进入和退出offboard飞行模式的命令只需要发送一次。

新的方法，基于命令MAV_CMD_DO_SET_MODE，注意，我没有在新的方法中对参数flag进行处理，停止发送setpoint就自然退出offboard模式了。

toggle_offboard_control( bool flag )
{
        // Prepare command for off-board mode
        mavlink_command_long_t com = { 0 };
        com.target_system    = system_id;
        com.target_component = autopilot_id;
        com.command          = MAV_CMD_DO_SET_MODE;
        com.confirmation     = true;
        // base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,

// 参见src/modules/commander/Commander.cpp对这个常量的定义
        com.param1 = 1;
        // custom_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD,

// 惨见src/modules/commander/px4_custom_mode.h对这个常量的定义
        com.param2 = 6;

// custom_sub_mode
        com.param3 = 0;

// 编码成mavlink消息
        mavlink_message_t message;
        mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);

// 向PX4发送消息
        int len = port->write_message(message);

// Done!
        return len;
}

为了让PX4保持offboard飞行模式，需要以不小于2HZ的频率（每500毫秒）向PX4发送当前飞机“设定点”（setpoint）的mavlink消息，这个动作本例由写线程按4Hz的频率（每250毫秒）完成。

在PX4中，“设定点”（setpoint）分成三类，他们是

  • SET_POSITION_TARGET_LOCAL_NED
  • SET_POSITION_GLOBAL_INT
  • SET_ATTITUDE_TARGET

本例采用的是第一类，即：SET_POSITION_TARGET_LOCAL_NED。

这一类又可采用三种方法发送设定点。

Position setpoint (only x, y, z)，发送飞机当前的位置

Velocity setpoint (only vx, yy, vz)，发送飞机当前各方向的速度

Acceleration setpoint (only afx, afy, afz)，发送飞机当前各方向的加速度，程序有，但本例没有用到

本例用到了前两种，即发送当前位置以及发送当前速度。当PX4有了当前的位置又有了当前的速度之后，这个速度设定点将作为一个正反馈累加到位置控制器的输出上，其结果又被用作速度控制器的输入。

当PX4有了当前的位置，速度以及加速度设定点之后，速度和加速度将作为一个正反馈累加到位置控制器的输出上，其结果又被用作速度控制器的输入。加速度设定点将被累加到速度控制器的输出上，其结果用来计算推力矢量。

在发送了上述位置或速度或加速度设定点之后，可以发送偏航设定点，让飞机机头转向(set_yaw函数)或者偏航速率（set_yaw_rate函数，函数有，但本例没有用到）。就是说在set_yaw函数执行之前，必须先执行set_position函数和/或set_velocity函数。

如果PX4在500ms之内没能接到当前位置的消息，则会退出offboard飞行模式。PX4系统中已经定义了若干种当退出offboard飞行模式后该干什么参数设置，可参考官网说明：Offboard Mode | PX4 User Guide。

在PX4中如何使用offboard模式以及对c_uart_interface_example程序的分析相关推荐

  1. PX4 offboard模式能接收的mavros指令

    以下内容针对px4 v1.11.3(2021.01) px4 offboard模式下可以接收上位机发送来的setpoint值,可以利用ROS包mavros来发送这些setpoint(期望值). ps: ...

  2. PX4代码学习系列博客(6)——offboard模式位置控制代码分析（之前转载过，这是第二次转载了）

    我刚刚发现这篇文章去年八月份的时候转载过一次了 https://blog.csdn.net/sinat_16643223/article/details/107874349 转载自:https://b ...

  3. PX4位置控制offboard模式说明

    offboard模式的开发及应用 一.px4固件的模式 px4固件支持10几种飞行模式,从代码结构上分析,分为基本模式.自定义模式和自定义子模式. 1.基本模式 基本模式又分为,位置控制模式.自稳模式 ...

  4. PX4+Offboard模式+代码控制无人机起飞（Gazebo）

    参考PX4自动驾驶用户指南 https://docs.px4.io/main/zh/ros/mavros_offboard_cpp.html 我的另一篇博客写了 键盘控制PX4无人机飞行 PX4无人机 ...

  5. GAAS 无人机自动驾驶学习（01-使用机载电脑，通过OFFBOARD模式进行控制飞行）

    原文网址:https://gaas.gitbook.io/guide/wu-ren-ji-zi-dong-jia-shi-xi-lie-offboard-kong-zhi-yi-ji-gazebo-f ...

  6. 关于PX4中的高度若干问题

    飞行的高度是如何测量的?地面的高度和海平面的高度差别很大,飞控又是如何有效判别进行降落的?这是我脑子里的疑问.搜索的一圈发现很少有人讨论这方面的问题,于是本次我就直接看一下源代码,一起分析一下PX4中 ...

  7. 安装ROS、gazebo、PX4基础细节及offboard控制

    新手参考教程安装ROS.gazebo.PX4基础细节及offboard控制 1.安装ROS 参考教程 2.安装PX4 参考教程 注: 1.在编译px4_Firmware前会经过安装步骤,安装需要去gi ...

  8. PX4中vtol_att_control 源码解析

    px4中vtol姿态控制源码分析 /src/modules/vtol_att_control/文件夹中包含vtol_att_control_main.vtol_type.standard/tailsi ...

  9. PX4中的mavlink

    简介 px4与地面站的通信协议是mavlink,对于其消息格式的介绍看这里和这里. 需要注意几点: 不光是px4与qgroundcontrol通信通过mavlink,有一些sensor也支持mavli ...

最新文章

  1. cv2.matchTemplate()函数的应用,匹配图片后画出矩形
  2. 钢材种类有哪些？怎么分类
  3. python定时器 是线程吗_python线程定时器Timer（32）
  4. python容器数据类型_python collections 容器数据类型
  5. 如何在 ASP.NET MVC 中集成 AngularJS
  6. 串口通讯编程一日通2（Overlapped IO模型）
  7. java 内置web服务器_Webstorm 2016内置web服务器配置
  8. 魅族Flyme5系统内置原生铃声免费下载
  9. Python Tricks（十七）—— enumerate 的实现
  10. BJ-100型矩形波导传输TE10模，测得波导中相邻两个电场波节点之间的距离为19.88mm
  11. Effective C++ 之 Item 5：了解C++默默编写并调用哪些函数
  12. Java基本语法格式
  13. MySQL 索引原理 图文讲解
  14. 云计算未来的5个发展趋势分析
  15. elementUI的表格标题换行
  16. membase数据库_Meta分析之文献检索步骤
  17. LPMS1000-全自动压力校验系统
  18. Photoshop 2021 mac(ps 2021)新增功能
  19. 干货分享 | 如何在业余时间学习数据分析？
  20. 华师大 OJ 3040

热门文章

  1. Python小程序：你看？这千年难遇的雪景——简直“美到犯规”【满屏雪花飞舞】
  2. 2019 全球 AI 安防市场十大事件丨年终盘点
  3. 电影购票系统项目实战
  4. python新年贺卡_python验指导书.docx
  5. “剥皮”法求区域中心 (转)
  6. 预付费智能电表，做到一户一表、远程自动抄表、电费预充值、电表实时计量扣费、欠费自动跳闸。-安科瑞黄安南
  7. 关于 网页图片优化和体积压缩
  8. 网页中在线玩拳皇KOF
  9. 从浅到深使用pm2来彻底掌握微服务运维精髓
  10. layui使用tips_使用layer.tips实现鼠标悬浮时触发事件提示消息实现