在PX4中如何使用offboard模式以及对c_uart_interface_example程序的分析_px4 autotakeoff

    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。