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

    
        
            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.
            Serial control device type.
            Bitmap of serial control flags.
            Timeout for reply data
            Baudrate of transfer. Zero means no change.
            how many bytes in this transfer
            serial data
        
    
    
        
            SERIAL_CONTROL device types
            
            First telemetry port
            
            
            Second telemetry port
            
            
            First GPS port
            
            
            Second GPS port
            
            
            system shell
            
        
    

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;icount();i++)
{
    Vehicle*  vehicle  = qobject_cast((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(ser.data), ser.count));
    }
    else
    {
        emit mavlinkSerialControl(ser.device, ser.flags, ser.timeout, ser.baudrate, QByteArray(reinterpret_cast(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) {  }
    }
}