QGC開發 顯示雙GPS/RTK信息以及自定義頁面(ubuntu)

一、QGC開發 顯示雙GPS/RTK信息

1. 在sitl中進行仿真,虛擬出第二個GPS mavlink發送到地面站。

在這裏插入圖片描述

如下圖中,在mavlink_msg_gps2_raw.h中找到發送第二組gps/rtk數據函數mavlink_msg_gps2_raw_send()發送,由於第一組已經有在發送,故只加入第二組。
在這裏插入圖片描述
發送代碼如下:

    mavlink_msg_gps2_raw_send  (
        chan,
        last_fix_time_ms(0)*(uint64_t)1000,
        status(0),
        loc.lat,        // in 1E7 degrees
        loc.lng,        // in 1E7 degrees
        loc.alt * 10UL, // in mm
        get_hdop(0),
        get_vdop(0),
        ground_speed(0)*100,  // cm/s
        ground_course(0)*100, // 1/100 degrees,
        num_sats(0),
        0,                    // TODO: Elipsoid height in mm
        0);   
        

2. 在QGC工程中加入解析第二個gps/rtl那包。

主要改了三個文件以及添加一個文件:分別爲FirmwarePlugin.ccVehicle.ccVehicle.h + GPS2Indicator.qml
FirmwarePlugin.cc中如圖中加入:

_toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPS2Indicator.qml")));

在這裏插入圖片描述

  • Vehicle.h 中,首先加入一個class
class VehicleGPS2FactGroup : public FactGroup
{
    Q_OBJECT

public:
    VehicleGPS2FactGroup(QObject* parent = nullptr);

    Q_PROPERTY(Fact* lat                READ lat                CONSTANT)
    Q_PROPERTY(Fact* lon                READ lon                CONSTANT)
    Q_PROPERTY(Fact* hdop               READ hdop               CONSTANT)
    Q_PROPERTY(Fact* vdop               READ vdop               CONSTANT)
    Q_PROPERTY(Fact* courseOverGround   READ courseOverGround   CONSTANT)
    Q_PROPERTY(Fact* count              READ count              CONSTANT)
    Q_PROPERTY(Fact* lock               READ lock               CONSTANT)

    Fact* lat               (void) { return &_latFact; }
    Fact* lon               (void) { return &_lonFact; }
    Fact* hdop              (void) { return &_hdopFact; }
    Fact* vdop              (void) { return &_vdopFact; }
    Fact* courseOverGround  (void) { return &_courseOverGroundFact; }
    Fact* count             (void) { return &_countFact; }
    Fact* lock              (void) { return &_lockFact; }

    static const char* _latFactName;
    static const char* _lonFactName;
    static const char* _hdopFactName;
    static const char* _vdopFactName;
    static const char* _courseOverGroundFactName;
    static const char* _countFactName;
    static const char* _lockFactName;

private:
    Fact        _latFact;
    Fact        _lonFact;
    Fact        _hdopFact;
    Fact        _vdopFact;
    Fact        _courseOverGroundFact;
    Fact        _countFact;
    Fact        _lockFact;
};

  • 再加入一些,聲明:(由於改的部分比較零散,故用直接看git改的內容)
    在這裏插入圖片描述
  • Vehicle.cc中加入mavlink解析函數:
    在這裏插入圖片描述
  • 加入:
    在這裏插入圖片描述在這裏插入圖片描述

3. 最後新建一個qml文件

在這裏插入圖片描述
—>
在這裏插入圖片描述然後把下面的內容複製粘貼進去:

/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

import QtQuick          2.11
import QtQuick.Layouts  1.11

import QGroundControl                       1.0
import QGroundControl.Controls              1.0
import QGroundControl.MultiVehicleManager   1.0
import QGroundControl.ScreenTools           1.0
import QGroundControl.Palette               1.0

//-------------------------------------------------------------------------
//-- GPS Indicator
Item {
    id:             _root
    width:          (gps2ValuesColumn.x + gps2ValuesColumn.width) * 1.1
    anchors.top:    parent.top
    anchors.bottom: parent.bottom

    Component {
        id: gps2Info

        Rectangle {
            width:  gps2Col.width   + ScreenTools.defaultFontPixelWidth  * 3 //寬度
            height: gps2Col.height  + ScreenTools.defaultFontPixelHeight * 2 //高度
            radius: ScreenTools.defaultFontPixelHeight * 0.5                //半徑
            color:  qgcPal.window                                           //顏色
            border.color:   qgcPal.text                                     //加邊框

            Column {
                id:                 gps2Col
                spacing:            ScreenTools.defaultFontPixelHeight * 0.5
                width:              Math.max(gps2Grid.width, gps2Label.width)
                anchors.margins:    ScreenTools.defaultFontPixelHeight
                anchors.centerIn:   parent                                   // anchors.centerIn:parent,是將子控件放在父控件的正中心

                QGCLabel {
                    id:             gps2Label
                    text:           (activeVehicle && activeVehicle.gps2.count.value >= 0) ? qsTr("GPS Status") : qsTr("GPS Data Unavailable")
                    font.family:    ScreenTools.demiboldFontFamily          // 字體
                    anchors.horizontalCenter: parent.horizontalCenter       // 水平居中
                }

                GridLayout {
                    id:                 gps2Grid
                    visible:            (activeVehicle && activeVehicle.gps2.count.value >= 0)
                    anchors.margins:    ScreenTools.defaultFontPixelHeight
                    columnSpacing:      ScreenTools.defaultFontPixelWidth
                    anchors.horizontalCenter: parent.horizontalCenter
                    columns: 2

                    QGCLabel { text: qsTr("GPS Count:") }
                    QGCLabel { text: activeVehicle ? activeVehicle.gps2.count.valueString : qsTr("N/A", "No data to display") }
                    QGCLabel { text: qsTr("GPS Lock:") }
                    QGCLabel { text: activeVehicle ? activeVehicle.gps2.lock.enumStringValue : qsTr("N/A", "No data to display") }
                    QGCLabel { text: qsTr("HDOP:") }
                    QGCLabel { text: activeVehicle ? activeVehicle.gps2.hdop.valueString : qsTr("--.--", "No data to display") }
                    QGCLabel { text: qsTr("VDOP:") }
                    QGCLabel { text: activeVehicle ? activeVehicle.gps2.vdop.valueString : qsTr("--.--", "No data to display") }
                    QGCLabel { text: qsTr("Course Over Ground:") }
                    QGCLabel { text: activeVehicle ? activeVehicle.gps2.courseOverGround.valueString : activeVehicle.gps2.courseOverGround.valueString }
                }
            }
        }
    }

    QGCColoredImage {
        id:                 gps2Icon
        width:              height
        anchors.top:        parent.top
        anchors.bottom:     parent.bottom
        source:             "/qmlimages/Gps.svg"
        fillMode:           Image.PreserveAspectFit
        sourceSize.height:  height
        opacity:            (activeVehicle && activeVehicle.gps2.count.value >= 0) ? 1 : 0.5
        color:              qgcPal.buttonText
    }

    Column {
        id:                     gps2ValuesColumn
        anchors.verticalCenter: parent.verticalCenter
        anchors.leftMargin:     ScreenTools.defaultFontPixelWidth / 2
        anchors.left:           gps2Icon.right

        QGCLabel {
            anchors.horizontalCenter:   hdopValue.horizontalCenter
            visible:                    activeVehicle && !isNaN(activeVehicle.gps2.hdop.value)
            color:                      qgcPal.buttonText
            text:                       activeVehicle ? activeVehicle.gps2.count.valueString : ""
        }

        QGCLabel {
            id:         hdopValue
            visible:    activeVehicle && !isNaN(activeVehicle.gps2.hdop.value)
            color:      qgcPal.buttonText
            text:       activeVehicle ? activeVehicle.gps2.hdop.value.toFixed(1) : ""
        }
    }

    MouseArea {
        anchors.fill:   parent
        onClicked: {
            mainWindow.showPopUp(_root, gps2Info)
        }
    }
}

然後debug即可:
在這裏插入圖片描述
出現了兩個GPS,由於在飛控中發下來的數據一模一樣,所以存在兩個一樣,爲了驗證沒問題,去飛控把下發的衛星數量改下。
把衛星改爲40顆

    mavlink_msg_gps2_raw_send  (
        chan,
        last_fix_time_ms(0)*(uint64_t)1000,
        status(0),
        loc.lat,        // in 1E7 degrees
        loc.lng,        // in 1E7 degrees
        loc.alt * 10UL, // in mm
        get_hdop(0),
        get_vdop(0),
        ground_speed(0)*100,  // cm/s
        ground_course(0)*100, // 1/100 degrees,
        40,
        0,                    // TODO: Elipsoid height in mm
        0);   

第二個衛星顯示是40,故接收以及顯示成功:
在這裏插入圖片描述
完成

二、自定義頁面

用到的文件爲:MainRootWindow.qmlMainToolBar.qml
效果如下圖,功能可後續擴展:
在這裏插入圖片描述

如下圖,新建myadd.qml

在這裏插入圖片描述


import QtQuick                  2.11
import QtQuick.Controls         2.4
import QtQuick.Layouts          1.11
import QtQuick.Dialogs          1.3
import QtQuick.Controls.Styles  1.4
import QtLocation               5.3
import QtPositioning            5.3

import QGroundControl                       1.0
import QGroundControl.Controls              1.0
import QGroundControl.ScreenTools           1.0
import QGroundControl.Palette               1.0
import QGroundControl.FlightMap             1.0
import QGroundControl.QGCMapEngineManager   1.0
import QGroundControl.FactSystem            1.0
import QGroundControl.FactControls          1.0

Rectangle{
    id:         myRect
    color:      "red"
    visible:     true
    x:          100
    y:          100
    width:      100
    height:     100
    Text {
        id: myTest
        text: qsTr("!!!!!")
        color: "blue"
        font.pointSize: 20
    }

MainRootWindow.qml中加入內容如下圖:

在這裏插入圖片描述

MainToolBar.qml中加入

在這裏插入圖片描述

            QGCToolBarButton {
                id:                 addmyButton
                anchors.top:        parent.top
                anchors.bottom:     parent.bottom
                icon.source:        "/qmlimages/Plan.svg"
                onClicked: {
                    checked = true
                    mainWindow.showmyaddview()
                }
            }

編譯後完成

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章