MavLink 地面站QGC与飞控PX4/APM外挂设备的通信实现

1 问题

去年写过 “PX4 <--> QGC透明串口转发” ，主要是把地面站通过mavlink和PX4飞控的实现代码撸了一遍，直接修改飞控代码实现控制。

问题在于一旦我们加入的代码有致命bug，造成飞控在空中宕机，炸机的风险实在太大，产生的后果，不管怎么有说不过去。

而且这种方法把飞控写死了，也不便于升级，不能兼容AMP飞控。

本文就是结合前期工作，提供一个不修改飞控，适用于PX4和APM的，地面站与机上设备的双向通信方案。主要是梳理思路，备忘用。

2 思路

之前文章在读码过程中发现MavLink有一个比较有意思的消息号MAVLINK_MSG_ID_SERIAL_CONTROL。

PX4只用了SERIAL_CONTROL_DEV_SHELL这个shell控制串口，暂且这么理解吧。

AMP倒是每个串口都实现了，但是只能地面站发，串口立即响应才能回送信息，相当于无连接的web服务器一样，不能从串口随机发送数据，要实现双向通信只能使用定时查询的方式浪费宝贵的控制带宽。

但是这个消息倒是可以为我们所用，至少不用重新定义消息，这一步不用改飞控。

PX4和AMP对于它自己没有使用消息的处理方式不一样。

PX4，过滤式，当不认识/不处理的消息出现时，先各个消息处理函数过一遍，没人要就看当前MavLink通道是否开启转发，如果转发就发送给其他的MavLink通道。

AMP,自己维护着一个路由表，每个MavLink设备都有自己的SYS_ID和COMP_ID，类似于IP和端口号，设备启动后发送心跳报告自己的两个ID和位置（如FD），一旦接到消息就在路由表中查找目的SYS_ID和COMP_ID的位置，一旦发现就转发。

需要在PX4上设置串口转发，挂载设备定时发送心跳就可以实现功能。

地面站使用MAVLINK_MSG_ID_SERIAL_CONTROL消息，串口号设置为SERIAL_CONTROL,肯定谁也不会处理，发送给指定的SYS_ID和COMP_ID设备就实现通信。

同样的设备向地面站代表的SYS_ID和COMP_ID发送SERIAL_CONTROL消息，串口号设置为SERIAL_CONTROL_DEV_ENUM_END的消息，地面站也可以收到。

这个思路不限制地面站不管是QGC还是MP都能实现，也不限飞行控制栈PX4和APM都通用，而且不用修改飞控代码，相对于修改代码提高了飞行的可靠性。

3 实现

3.0 mavlink 消息说明

common.xml

<mavlink>
    <messages>
        <message id="126" name="SERIAL_CONTROL">
            <description>Control a serial port. This can be used for raw access to an onboard serial peripheral such as a GPS or telemetry radio. It is designed to make it possible to update the devices firmware via MAVLink messages or change the devices settings. A message with zero bytes can be used to change just the baudrate.</description>
            <field type="uint8_t" name="device" enum="SERIAL_CONTROL_DEV">Serial control device type.</field>
            <field type="uint8_t" name="flags" enum="SERIAL_CONTROL_FLAG" display="bitmask">Bitmap of serial control flags.</field>
            <field type="uint16_t" name="timeout" units="ms">Timeout for reply data</field>
            <field type="uint32_t" name="baudrate" units="bits/s">Baudrate of transfer. Zero means no change.</field>
            <field type="uint8_t" name="count" units="bytes">how many bytes in this transfer</field>
            <field type="uint8_t[70]" name="data">serial data</field>
        </message>
    </messages>
    <enums>
        <enum name="SERIAL_CONTROL_DEV">
            <description>SERIAL_CONTROL device types</description>
            <entry value="0" name="SERIAL_CONTROL_DEV_TELEM1">
            <description>First telemetry port</description>
            </entry>
            <entry value="1" name="SERIAL_CONTROL_DEV_TELEM2">
            <description>Second telemetry port</description>
            </entry>
            <entry value="2" name="SERIAL_CONTROL_DEV_GPS1">
            <description>First GPS port</description>
            </entry>
            <entry value="3" name="SERIAL_CONTROL_DEV_GPS2">
            <description>Second GPS port</description>
            </entry>
            <entry value="10" name="SERIAL_CONTROL_DEV_SHELL">
            <description>system shell</description>
            </entry>
        </enum>
    </enums>
</mavlink>

3.1 飞控参数设置

假设设备通过串口挂在TELEM2上,速率为57600波特。

3.1.1 APM

SERIAL2_PROTOCOL = 2 ,TELEM2 设置为MAVLINK v2
SERIAL2_BAUD = 57, TELEM2 57600 波特

3.1.2 PX4

MAV_1_CONFIG = TELEM2 mavlink 1 设置为 TELEM2，mavlink 0 默认为TELEM1 ，一旦配置好后端口就有状态信心
SER_TEL2_BUAD = 57600 8N1 TELEM2 57600 波特
MAV_1_FORWARD = 1 mavlink 1 支持转发，可获取我们定义的消息
MAV_0_FORWARD = 1 mavlink 0 支持转发

3.2 地面站

以QGC为例，MP没有深入研究，区别因该不大...

3.2.1 发送消息

//设备的SYS ID 一般1为飞控
#define SYS_ID 2
//设备的COMP ID 在mavlink协议中找MAV_COMPONENT，最好不要重合
#define COMP_ID 8

//自己定义的结构体和数据，怎么实现都可以
m_pkg_t pkg;

//将结构体数据复制到字节数组中
uint8_t data[sizeof(m_pkg_t)];
memcpy(data,&pkg,sizeof (m_pkg_t));

auto vehicles = qgcApp()->toolbox()->multiVehicleManager()->vehicles();
//向每个飞机都发送这个指令
for(int i=0;i<vehicles->count();i++)
{
    Vehicle*  vehicle  = qobject_cast<Vehicle*>((vehicles->get(i)));
    if(vehicle==nullptr || !vehicle->parameterManager()->parametersReady())
        continue;
    mavlink_message_t msg;
    mavlink_msg_serial_control_pack_chan(
                SYS_ID,
                COMP_ID,
                vehicle->priorityLink()->mavlinkChannel(),
                &msg,
                SERIAL_CONTROL_DEV_ENUM_END,//这个串口号PX4和arduplilot都不处理
                HELMET_DATA_UP_FLAGE,
                0,
                0,
                sizeof(m_pkg_t),
                data);
    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
}

3.2.2 接收数据

参考MAVLINK_MSG_SERIAL_CONTROL消息在src/Vehicle/Vehicle.h中申明信号的函数根。

signals:
//...
// MAVlink Serial Data
void mavlinkSerialControl(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);

//Raw Sersial Data 接收数据信号
void rawSerialDataReceived(QByteArray data);
//...

在src/Vehicle/Vehicle.cc中过滤MAVLINK_MSG_ID_RTX_UAV2GCS消息，并发射rawSerialDataReceived信号

//...
case MAVLINK_MSG_ID_SERIAL_CONTROL:
{
    mavlink_serial_control_t ser;
    mavlink_msg_serial_control_decode(&message, &ser);
    if(ser.device == SERIAL_CONTROL_DEV_ENUM_END)
    {
        emit rawSerialDataReceived(QByteArray(reinterpret_cast<const char*>(ser.data), ser.count));
    }
    else
    {
        emit mavlinkSerialControl(ser.device, ser.flags, ser.timeout, ser.baudrate, QByteArray(reinterpret_cast<const char*>(ser.data), ser.count));
    }
}

//...

在需要处理的位置连接信号完成消息接收处理


connect(_vehicle, &Vehicle::rawSerialDataReceived, this, &XXXX::_receiveData);

void XXXX::_receiveData(QByteArray data)
{
    if(data.count()==)sizeof(m_pkg_t)
    {
        m_pkg_t pkg;
        memcpy(&pkg,data.data(),sizeof(m_pkg_t));
        //其他处理...
    }
}

3.3 挂载设备

使用一个简单的 STM32F103C8T6 小板, 串口连接飞控的TELEM2

#include "stm32f10x.h"
#include "../lib/mavlink/v2.0/ardupilotmega/mavlink.h"
#include "../bsp/led_status.h"

#define M_SYS_ID 2
#define M_COMP_ID 8
#define M_CHAN 2

#define HELMET_DATA_UP_FLAGE 30

void sendHeartbeat(void );
void handleMessage(const mavlink_message_t msg);


//time3 设置为10ms一次
void tim3_config(void)
{
// 略...
}

uint32_t time = 0;
//TIM3中断处理方法
void TIM3_IRQHandler(void)
{
    TIM_ClearITPendingBit(TIM3,TIM_IT_Update); //清空中断，以便下次再响应
    time++;
    
    if(time % 100 ==0)
    {
        //一秒发一次心跳
        sendHeartbeat();
    }

}


//串口2 设置 57600 ,连接大TELEM2上
void USART2_Config(void)
{   
/// 略 ...
}

//从串口中解析mav消息
void USART2_IRQHandler(void)
{
    mavlink_message_t msg;
    mavlink_status_t status;
    uint8_t utbyte;
    if(USART_GetFlagStatus(USART2,USART_IT_RXNE) != RESET)
    {
        utbyte = USART_ReceiveData(USART2);
        //USART_SendData(USART1,utbyte);
        if(mavlink_parse_char(M_CHAN, utbyte, &msg, &status)){
            handleMessage(msg);
        }
    }
}

int main(void)
{
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE); //打开辅助功能,定时器3时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE );
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//端口复用
    
    //接飞控 TELEM2 接收mavlink
    USART2_Config();
    tim3_config();
    
    while(1){}
}

//发送心跳
void sendHeartbeat()
{
    mavlink_message_t message;
    mavlink_msg_heartbeat_pack_chan(M_SYS_ID,   //SYS_ID
                                    M_COMP_ID,  //COMP_ID
                                    M_CHAN,     //CHANEL
                                    &message,
                                    MAV_TYPE_GIMBAL,         // MAV_TYPE
                                    MAV_AUTOPILOT_INVALID,   // MAV_AUTOPILOT
                                    MAV_MODE_MANUAL_ARMED,   // MAV_MODE
                                    0,                       // custom mode
                                    MAV_STATE_ACTIVE);       // MAV_STATE

    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int len = mavlink_msg_to_send_buffer(buffer, &message);

    for(int i = 0 ; i < len ; i++)
    {
            USART_SendData(USART2,buffer[i]);
            while (USART_GetFlagStatus(USART2,USART_FLAG_TXE)==RESET) {  }
    }

}
void handle_heartbeat(const mavlink_message_t msg);
void handle_serial_control(const mavlink_message_t msg);

//接收到消息分发处理 
void handleMessage(const mavlink_message_t msg)
{
    switch(msg.msgid)
    {
        case MAVLINK_MSG_ID_HEARTBEAT:handle_heartbeat(msg);break;
        case MAVLINK_MSG_ID_SERIAL_CONTROL:handle_serial_control(msg);break;
        default: break;
    }
}

//这里就可以知道系统中还要哪些设备
void handle_heartbeat(const mavlink_message_t msg)
{
    mavlink_heartbeat_t pkg;
    mavlink_msg_heartbeat_decode(&msg,&pkg);
    char dbgStr[50];
    memset(dbgStr,0,50);
    sprintf(dbgStr,"heatbeat:{sysid:%d,compid:%d} ",msg.sysid,msg.compid);
    printStringToUSART1(dbgStr);
}
uint8_t gcs_sysid=0;
uint8_t gcs_compid=0;
void handle_serial_control(const mavlink_message_t msg)
{
    //当收到MAVLINK_MSG_ID_SERIAL_CONTROL，就可以通过msg记下地面站的SYSID和COMP_ID了
    gcs_sysid=pkg.sysid;
    gcs_compid=pkg.compid;         ///< ID of the message sender component

    mavlink_serial_control_t pkg;
    mavlink_msg_serial_control_decode(&msg,&pkg);
    
    //解析还原数据
    m_pkg_t mpkg;
    memcpy(&mpkg,pkg.data(),sizeof(m_pkg_t));
}

//发送数据
void sendData(m_pkg_t *pkg)
{
    uint8_t data[sizeof(m_pkg_t)];
    memcpy(data,pkg,sizeof(m_pkg_t));

    mavlink_message_t message;
    mavlink_msg_serial_control_pack_chan(
                gcs_sysid,
                gcs_compid,
                vehicle->priorityLink()->mavlinkChannel(),
                &message,
                SERIAL_CONTROL_DEV_ENUM_END,//这个串口号PX4和arduplilot都不处理
                HELMET_DATA_UP_FLAGE,
                0,
                0,
                sizeof(m_pkg_t),
                data);

    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int len = mavlink_msg_to_send_buffer(buffer, &message);

    for(int i = 0 ; i < len ; i++)
    {
            USART_SendData(USART2,buffer[i]);
            while (USART_GetFlagStatus(USART2,USART_FLAG_TXE)==RESET) {  }
    }
}