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。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
