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)系作者
  • 人面猴
    序言：七十年代末铸磅，一起剝皮案震驚了整個(gè)濱河市，隨后出現(xiàn)的幾起案子杭朱，更是在濱河造成了極大的恐慌阅仔，老刑警劉巖，帶你破解...
    沈念sama閱讀 218,122評論 6贊 505
  • 死咒
    序言：濱河連續(xù)發(fā)生了三起死亡事件痕檬，死亡現(xiàn)場離奇詭異霎槐，居然都是意外死亡，警方通過查閱死者的電腦和手機(jī)梦谜，發(fā)現(xiàn)死者居然都...
    沈念sama閱讀 93,070評論 3贊 395
  • 救了他兩次的神仙讓他今天三更去死
    文/潘曉璐 我一進(jìn)店門丘跌，熙熙樓的掌柜王于貴愁眉苦臉地迎上來，“玉大人唁桩，你說我怎么就攤上這事闭树。” “怎么了荒澡？”我有些...
    開封第一講書人閱讀 164,491評論 0贊 354
  • 道士緝兇錄：失蹤的賣姜人
    文/不壞的土叔 我叫張陵报辱，是天一觀的道長。 經(jīng)常有香客問我单山，道長碍现，這世上最難降的妖魔是什么？ 我笑而不...
    開封第一講書人閱讀 58,636評論 1贊 293
  • ?港島之戀（遺憾婚禮）
    正文 為了忘掉前任米奸，我火速辦了婚禮昼接，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘悴晰。我一直安慰自己慢睡，他們只是感情好，可當(dāng)我...
    茶點(diǎn)故事閱讀 67,676評論 6贊 392
  • 惡毒庶女頂嫁案：這布局不是一般人想出來的
    文/花漫 我一把揭開白布铡溪。 她就那樣靜靜地躺著漂辐，像睡著了一般。 火紅的嫁衣襯著肌膚如雪棕硫。 梳的紋絲不亂的頭發(fā)上髓涯，一...
    開封第一講書人閱讀 51,541評論 1贊 305
  • 城市分裂傳說
    那天，我揣著相機(jī)與錄音饲帅，去河邊找鬼复凳。 笑死瘤泪，一個(gè)胖子當(dāng)著我的面吹牛，可吹牛的內(nèi)容都是我干的育八。 我是一名探鬼主播对途，決...
    沈念sama閱讀 40,292評論 3贊 418
  • 雙鴛鴦連環(huán)套：你想象不到人心有多黑
    文/蒼蘭香墨 我猛地睜開眼，長吁一口氣：“原來是場噩夢啊……” “哼髓棋！你這毒婦竟也來了实檀？” 一聲冷哼從身側(cè)響起，我...
    開封第一講書人閱讀 39,211評論 0贊 276
  • 萬榮殺人案實(shí)錄
    序言：老撾萬榮一對情侶失蹤按声，失蹤者是張志新（化名）和其女友劉穎膳犹，沒想到半個(gè)月后，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體签则，經(jīng)...
    沈念sama閱讀 45,655評論 1贊 314
  • ?護(hù)林員之死
    正文 獨(dú)居荒郊野嶺守林人離奇死亡须床，尸身上長有42處帶血的膿包…… 初始之章·張勛 以下內(nèi)容為張勛視角 年9月15日...
    茶點(diǎn)故事閱讀 37,846評論 3贊 336
  • ?白月光啟示錄
    正文 我和宋清朗相戀三年，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了渐裂。 大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片豺旬。...
    茶點(diǎn)故事閱讀 39,965評論 1贊 348
  • 活死人
    序言：一個(gè)原本活蹦亂跳的男人離奇死亡，死狀恐怖柒凉，靈堂內(nèi)的尸體忽然破棺而出族阅，到底是詐尸還是另有隱情，我是刑警寧澤膝捞，帶...
    沈念sama閱讀 35,684評論 5贊 347
  • ?日本核電站爆炸內(nèi)幕
    正文 年R本政府宣布坦刀，位于F島的核電站，受9級特大地震影響蔬咬，放射性物質(zhì)發(fā)生泄漏鲤遥。R本人自食惡果不足惜，卻給世界環(huán)境...
    茶點(diǎn)故事閱讀 41,295評論 3贊 329
  • 男人毒藥：我在死后第九天來索命
    文/蒙蒙 一林艘、第九天 我趴在偏房一處隱蔽的房頂上張望渴频。 院中可真熱鬧，春花似錦北启、人聲如沸。這莊子的主人今日做“春日...
    開封第一講書人閱讀 31,894評論 0贊 22
  • 一樁弒父案咕村，背后竟有這般陰謀
    文/蒼蘭香墨 我抬頭看了看天上的太陽。三九已至蚊俺，卻和暖如春懈涛，著一層夾襖步出監(jiān)牢的瞬間，已是汗流浹背泳猬。 一陣腳步聲響...
    開封第一講書人閱讀 33,012評論 1贊 269
  • 情欲美人皮
    我被黑心中介騙來泰國打工批钠， 沒想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留宇植，地道東北人。 一個(gè)月前我還...
    沈念sama閱讀 48,126評論 3贊 370
  • 代替公主和親
    正文 我出身青樓埋心，卻偏偏與公主長得像指郁，于是被迫代替她去往敵國和親。 傳聞我的和親對象是個(gè)殘疾皇子拷呆，可洞房花燭夜當(dāng)晚...
    茶點(diǎn)故事閱讀 44,914評論 2贊 355