MavLink 地面站QGC與飛控PX4/APM外掛設(shè)備的通信實(shí)現(xiàn)

1 問題

去年寫過 “PX4 <--> QGC透明串口轉(zhuǎn)發(fā)” ，主要是把地面站通過mavlink和PX4飛控的實(shí)現(xiàn)代碼擼了一遍，直接修改飛控代碼實(shí)現(xiàn)控制。

問題在于一旦我們加入的代碼有致命bug，造成飛控在空中宕機(jī)，炸機(jī)的風(fēng)險(xiǎn)實(shí)在太大，產(chǎn)生的后果，不管怎么有說不過去。

而且這種方法把飛控寫死了，也不便于升級，不能兼容AMP飛控。

本文就是結(jié)合前期工作，提供一個(gè)不修改飛控，適用于PX4和APM的，地面站與機(jī)上設(shè)備的雙向通信方案。主要是梳理思路，備忘用。

2 思路

之前文章在讀碼過程中發(fā)現(xiàn)MavLink有一個(gè)比較有意思的消息號MAVLINK_MSG_ID_SERIAL_CONTROL。

PX4只用了SERIAL_CONTROL_DEV_SHELL這個(gè)shell控制串口，暫且這么理解吧。

AMP倒是每個(gè)串口都實(shí)現(xiàn)了，但是只能地面站發(fā)，串口立即響應(yīng)才能回送信息，相當(dāng)于無連接的web服務(wù)器一樣，不能從串口隨機(jī)發(fā)送數(shù)據(jù)，要實(shí)現(xiàn)雙向通信只能使用定時(shí)查詢的方式浪費(fèi)寶貴的控制帶寬。

但是這個(gè)消息倒是可以為我們所用，至少不用重新定義消息，這一步不用改飛控。

PX4和AMP對于它自己沒有使用消息的處理方式不一樣。

PX4，過濾式，當(dāng)不認(rèn)識/不處理的消息出現(xiàn)時(shí)，先各個(gè)消息處理函數(shù)過一遍，沒人要就看當(dāng)前MavLink通道是否開啟轉(zhuǎn)發(fā)，如果轉(zhuǎn)發(fā)就發(fā)送給其他的MavLink通道。

AMP,自己維護(hù)著一個(gè)路由表，每個(gè)MavLink設(shè)備都有自己的SYS_ID和COMP_ID，類似于IP和端口號，設(shè)備啟動后發(fā)送心跳報(bào)告自己的兩個(gè)ID和位置（如FD），一旦接到消息就在路由表中查找目的SYS_ID和COMP_ID的位置，一旦發(fā)現(xiàn)就轉(zhuǎn)發(fā)。

需要在PX4上設(shè)置串口轉(zhuǎn)發(fā)，掛載設(shè)備定時(shí)發(fā)送心跳就可以實(shí)現(xiàn)功能。

地面站使用MAVLINK_MSG_ID_SERIAL_CONTROL消息，串口號設(shè)置為SERIAL_CONTROL,肯定誰也不會處理，發(fā)送給指定的SYS_ID和COMP_ID設(shè)備就實(shí)現(xiàn)通信。

同樣的設(shè)備向地面站代表的SYS_ID和COMP_ID發(fā)送SERIAL_CONTROL消息，串口號設(shè)置為SERIAL_CONTROL_DEV_ENUM_END的消息，地面站也可以收到。

這個(gè)思路不限制地面站不管是QGC還是MP都能實(shí)現(xiàn)，也不限飛行控制棧PX4和APM都通用，而且不用修改飛控代碼，相對于修改代碼提高了飛行的可靠性。

3 實(shí)現(xiàn)

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 飛控參數(shù)設(shè)置

假設(shè)設(shè)備通過串口掛在TELEM2上,速率為57600波特。

3.1.1 APM

  • SERIAL2_PROTOCOL = 2 ,TELEM2 設(shè)置為MAVLINK v2

  • SERIAL2_BAUD = 57, TELEM2 57600 波特

3.1.2 PX4

  • MAV_1_CONFIG = TELEM2 mavlink 1 設(shè)置為 TELEM2，mavlink 0 默認(rèn)為TELEM1 ，一旦配置好后端口就有狀態(tài)信心

  • SER_TEL2_BUAD = 57600 8N1 TELEM2 57600 波特

  • MAV_1_FORWARD = 1 mavlink 1 支持轉(zhuǎn)發(fā)，可獲取我們定義的消息

  • MAV_0_FORWARD = 1 mavlink 0 支持轉(zhuǎn)發(fā)

3.2 地面站

以QGC為例，MP沒有深入研究，區(qū)別因該不大...

3.2.1 發(fā)送消息

//設(shè)備的SYS ID 一般1為飛控
#define SYS_ID 2
//設(shè)備的COMP ID 在mavlink協(xié)議中找MAV_COMPONENT，最好不要重合
#define COMP_ID 8

//自己定義的結(jié)構(gòu)體和數(shù)據(jù)，怎么實(shí)現(xiàn)都可以
m_pkg_t pkg;

//將結(jié)構(gòu)體數(shù)據(jù)復(fù)制到字節(jié)數(shù)組中
uint8_t data[sizeof(m_pkg_t)];
memcpy(data,&pkg,sizeof (m_pkg_t));

auto vehicles = qgcApp()->toolbox()->multiVehicleManager()->vehicles();
//向每個(gè)飛機(jī)都發(fā)送這個(gè)指令
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,//這個(gè)串口號PX4和arduplilot都不處理
                HELMET_DATA_UP_FLAGE,
                0,
                0,
                sizeof(m_pkg_t),
                data);
    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
}

3.2.2 接收數(shù)據(jù)

  • 參考MAVLINK_MSG_SERIAL_CONTROL消息在src/Vehicle/Vehicle.h中申明信號的函數(shù)根。
signals:
//...
// MAVlink Serial Data
void mavlinkSerialControl(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);

//Raw Sersial Data 接收數(shù)據(jù)信號
void rawSerialDataReceived(QByteArray data);
//...
  • 在src/Vehicle/Vehicle.cc中過濾MAVLINK_MSG_ID_RTX_UAV2GCS消息，并發(fā)射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 掛載設(shè)備

使用一個(gè)簡單的 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 設(shè)置為10ms一次
void tim3_config(void)
{
// 略...
}

uint32_t time = 0;
//TIM3中斷處理方法
void TIM3_IRQHandler(void)
{
    TIM_ClearITPendingBit(TIM3,TIM_IT_Update); //清空中斷，以便下次再響應(yīng)
    time++;
    
    if(time % 100 ==0)
    {
        //一秒發(fā)一次心跳
        sendHeartbeat();
    }

}


//串口2 設(shè)置 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); //打開輔助功能,定時(shí)器3時(shí)鐘
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE );
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//端口復(fù)用
    
    //接飛控 TELEM2 接收mavlink
    USART2_Config();
    tim3_config();
    
    while(1){}
}

//發(fā)送心跳
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);

//接收到消息分發(fā)處理 
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;
    }
}

//這里就可以知道系統(tǒng)中還要哪些設(shè)備
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)
{
    //當(dāng)收到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);
    
    //解析還原數(shù)據(jù)
    m_pkg_t mpkg;
    memcpy(&mpkg,pkg.data(),sizeof(m_pkg_t));
}

//發(fā)送數(shù)據(jù)
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,//這個(gè)串口號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) {  }
    }
}
最后編輯于 ：
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
【社區(qū)內(nèi)容提示】社區(qū)部分內(nèi)容疑似由AI輔助生成，瀏覽時(shí)請結(jié)合常識與多方信息審慎甄別。
平臺聲明：文章內(nèi)容（如有圖片或視頻亦包括在內(nèi)）由作者上傳并發(fā)布，文章內(nèi)容僅代表作者本人觀點(diǎn)，簡書系信息發(fā)布平臺，僅提供信息存儲服務(wù)。

友情鏈接更多精彩內(nèi)容