• PX4实战之旅（二）：通过自定义mavlink消息和QGC通信

    文章目录

    前言

    PX4 1.13.2
    QGC 4.2.4
    pix2.4.8飞控

    一、PX4生成自定义mavlink消息文件

    PX4 1.13.0以及更新版本的固件生成自定义mavlink消息文件的方法老版本固件不同，老版本固件生成自定义MAVLINk消息可以参考：
    https://mbot1.blog.csdn.net/article/details/108802979
    PX4 1.13.0以前版本的固件生成自定义mavlink消息文件的方法可以参考本文。
    修改下图路径中的common.xml文件
    在这里插入图片描述
    在文件的下图位置加上

    
  Test Mavlink.
   
   
   


    • 1
    • 2
    • 3
    • 4
    • 5
    • 6

    在这里插入图片描述
    注意id不能与其他的消息相同，我这里取的是12921。
    然后编译
    先

    make clean

    • 1

    再

    make cuav_x7pro_default

    • 1

    编译完成后会在下图的路径中生成mavlink_msg_test_mavlink.h
    在这里插入图片描述

    二、PX4创建自定义进程和uorb消息

    假设PX4源码的路径为~/PX4-Autopilot

    2.1创建uorb消息

    定义两个消息

    gedit ~/PX4-Autopilot/msg/test_mavlink_rx.msg

    • 1
    gedit ~/PX4-Autopilot/msg/test_mavlink_tx.msg

    • 1

    都填入下面的内容

    uint64 timestamp # time since system start (microseconds)

uint8 test1
int16 test2
float32 test3

    • 1
    • 2
    • 3
    • 4
    • 5

    修改CMakeLists.txt

    gedit ~/PX4-Autopilot/msg/CMakeLists.txt

    • 1

    加入两行

    test_mavlink_tx.msg
test_mavlink_rx.msg

    • 1
    • 2

    如下图
    在这里插入图片描述
    然后编译固件生成消息头文件

    2.2创建自定义进程

    在PX4-Autopilot/src/modules下创建test_mavlink文件夹，
    在这里插入图片描述
    在该文件夹中创建CMakeLists.txt，Kconfig，test_mavlink.cpp，test_mavlink.h四个文件
    编辑CMakeLists，填入以下内容后保存退出

    px4_add_module(
	MODULE modules__test_mavlink
	MAIN test_mavlink
	COMPILE_FLAGS
		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
		#-O0             # uncomment when debugging
	SRCS
		test_mavlink.cpp
	DEPENDS
		px4_work_queue
	)

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11

    编辑Kconfig文件

    menuconfig MODULES_TEST_MAVLINK
	bool "test_mavlink"
	default n
	---help---
		Enable support for test_mavlink

    • 1
    • 2
    • 3
    • 4
    • 5

    编辑test_mavlink.cpp文件
    填写内容如下：

    #include "test_mavlink.h"

#include 
#include 
#include 

#include 
#include 


int test_mavlink::print_status()
{
    PX4_INFO("Running");
    // TODO: print additional runtime information about the state of the module

    return 0;
}

int test_mavlink::custom_command(int argc, char *argv[])
{
    /*
    if (!is_running()) {
        print_usage("not running");
        return 1;
    }

    // additional custom commands can be handled like this:
    if (!strcmp(argv[0], "do-something")) {
        get_instance()->do_something();
        return 0;
    }
     */

    return print_usage("unknown command");
}


int test_mavlink::task_spawn(int argc, char *argv[])
{
    _task_id = px4_task_spawn_cmd("module",
                                  SCHED_DEFAULT,
                                  SCHED_PRIORITY_DEFAULT,
                                  1024,
                                  (px4_main_t)&run_trampoline,
                                  (char *const *)argv);

    if (_task_id < 0)
    {
        _task_id = -1;
        return -errno;
    }

    return 0;
}

test_mavlink *test_mavlink::instantiate(int argc, char *argv[])
{
    int example_param = 0;
    bool example_flag = false;
    bool error_flag = false;

    int myoptind = 1;
    int ch;
    const char *myoptarg = nullptr;

    // parse CLI arguments
    while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF)
    {
        switch (ch)
        {
            case 'p':
                example_param = (int)strtol(myoptarg, nullptr, 10);
                break;

            case 'f':
                example_flag = true;
                break;

            case '?':
                error_flag = true;
                break;

            default:
                PX4_WARN("unrecognized flag");
                error_flag = true;
                break;
        }
    }

    if (error_flag)
    {
        return nullptr;
    }

    test_mavlink *instance = new test_mavlink(example_param, example_flag);

    if (instance == nullptr)
    {
        PX4_ERR("alloc failed");
    }

    return instance;
}

test_mavlink::test_mavlink(int example_param, bool example_flag)
    : ModuleParams(nullptr)
{
}

void test_mavlink::run()
{

    // initialize parameters
    parameters_update(true);

    while (!should_exit())
    {
//        PX4_INFO("run\n");
        if(test_mavlink_rx_sub.updated())
        {
            PX4_INFO("updated\n");
            test_mavlink_rx_sub.copy(&_test_mavlink_rx);
//            PX4_INFO("_test_mavlink_rx.test1=%c\n",_test_mavlink_rx.test1);
//            PX4_INFO("_test_mavlink_rx.test2=%d\n",_test_mavlink_rx.test2);
//            PX4_INFO("_test_mavlink_rx.test3=%f\n",(double)_test_mavlink_rx.test3);
            _test_mavlink_tx.test1=_test_mavlink_rx.test1+1;
            _test_mavlink_tx.test2=_test_mavlink_rx.test2+1;
            _test_mavlink_tx.test3=_test_mavlink_rx.test3+1;
            test_mavlink_tx_pub.publish(_test_mavlink_tx);
        }
        PX4_INFO("_test_mavlink_rx.test1=%d\n",_test_mavlink_rx.test1);
        PX4_INFO("_test_mavlink_rx.test2=%d\n",_test_mavlink_rx.test2);
        PX4_INFO("_test_mavlink_rx.test3=%f\n",(double)_test_mavlink_rx.test3);
        px4_usleep(50000);


        parameters_update();
    }
}

void test_mavlink::parameters_update(bool force)
{
    // check for parameter updates
    if (_parameter_update_sub.updated() || force)
    {
        // clear update
        parameter_update_s update;
        _parameter_update_sub.copy(&update);

        // update parameters from storage
        updateParams();
    }
}

int test_mavlink::print_usage(const char *reason)
{
    if (reason)
    {
        PX4_WARN("%s\n", reason);
    }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
Section that describes the provided module functionality.

This is a template for a module running as a task in the background with start/stop/status functionality.

### Implementation
Section describing the high-level implementation of this module.

### Examples
CLI usage example:
$ module start -f -p 42

)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("module", "template");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
	PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

int test_mavlink_main(int argc, char *argv[])
{
	return test_mavlink::main(argc, argv);
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168
    • 169
    • 170
    • 171
    • 172
    • 173
    • 174
    • 175
    • 176
    • 177
    • 178
    • 179
    • 180
    • 181
    • 182
    • 183
    • 184
    • 185
    • 186
    • 187
    • 188
    • 189
    • 190

    编辑test_mavlink.h文件
    填写内容如下：

    #pragma once

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace time_literals;

extern "C" __EXPORT int test_mavlink_main(int argc, char *argv[]);


class test_mavlink : public ModuleBase, public ModuleParams
{
public:
    test_mavlink(int example_param, bool example_flag);

    virtual ~test_mavlink() = default;

    /** @see ModuleBase */
    static int task_spawn(int argc, char *argv[]);

    /** @see ModuleBase */
    static test_mavlink *instantiate(int argc, char *argv[]);

    /** @see ModuleBase */
    static int custom_command(int argc, char *argv[]);

    /** @see ModuleBase */
    static int print_usage(const char *reason = nullptr);

    /** @see ModuleBase::run() */
    void run() override;

    /** @see ModuleBase::print_status() */
    int print_status() override;

private:

    /**
     * Check for parameter changes and update them if needed.
     * @param parameter_update_sub uorb subscription to parameter_update
     * @param force for a parameter update
     */
    void parameters_update(bool force = false);


    DEFINE_PARAMETERS(
        (ParamInt) _param_sys_autostart,   /**< example parameter */
        (ParamInt) _param_sys_autoconfig  /**< another parameter */
    )
    test_mavlink_tx_s _test_mavlink_tx;
    test_mavlink_rx_s _test_mavlink_rx;
    // Subscriptions
    uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
    uORB::Subscription test_mavlink_rx_sub{ORB_ID(test_mavlink_rx)};
    uORB::Publication	test_mavlink_tx_pub{ORB_ID(test_mavlink_tx)};
};

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61

    编辑下图编译脚本
    在这里插入图片描述
    加入一行

    CONFIG_MODULES_TEST_MAVLINK=y

    • 1

    如下：
    在这里插入图片描述

    三、PX4接收QGC消息

    修改mavlink_receiver.h

    在这里插入图片描述
    添加自定义消息头文件：

    #include 

    • 1

    在这里插入图片描述
    添加自定义消息处理函数

    void handle_message_test_mavlink_rx(mavlink_message_t *msg);

    • 1

    在这里插入图片描述
    添加话题发布

    uORB::Publication _test_mavlink_rx_pub{ORB_ID(test_mavlink_rx)};

    • 1

    在这里插入图片描述

    修改mavlink_receiver.cpp

    在这里插入图片描述
    调用消息处理函数：

    case MAVLINK_MSG_ID_TEST_MAVLINK:
    handle_message_test_mavlink_rx(msg);
    break;

    • 1
    • 2
    • 3

    在这里插入图片描述
    实现消息处理函数，主要是解析mavlink消息并通过uorb发布：

    void
MavlinkReceiver::handle_message_test_mavlink_rx(mavlink_message_t *msg)
{
    mavlink_test_mavlink_t mavlink_test_msg;
    mavlink_msg_test_mavlink_decode(msg, &mavlink_test_msg);
    test_mavlink_rx_s __test_mavlink_rx;
    __test_mavlink_rx.test1=mavlink_test_msg.test1;
    __test_mavlink_rx.test2=mavlink_test_msg.test2;
    __test_mavlink_rx.test3=mavlink_test_msg.test3;
    _test_mavlink_rx_pub.publish(__test_mavlink_rx);
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11

    在这里插入图片描述

    四、PX4发送消息给QGC

    在src/modules/mavlink/streams下新建MAVLINK_TEST.hpp

    在这里插入图片描述
    在MAVLINK_TEST.hpp中填入下面内容，主要是订阅test_mavlink_tx消息然后将消息内容打包为MAVLINK数据帧并发送：

    #ifndef MAVLINK_TEST_HPP
#define MAVLINK_TEST_HPP

#include 
class MavlinkStreamMavlinktest : public MavlinkStream
{
public:
	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamMavlinktest(mavlink); }

	static constexpr const char *get_name_static() { return "TEST_MAVLINK"; }
	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TEST_MAVLINK; }

	const char *get_name() const override { return get_name_static(); }
	uint16_t get_id() override { return get_id_static(); }

	unsigned get_size() override
	{
		return test_mavlink_tx_sub.advertised() ? MAVLINK_MSG_ID_TEST_MAVLINK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
	}

private:
	explicit MavlinkStreamMavlinktest(Mavlink *mavlink) : MavlinkStream(mavlink) {}

	uORB::Subscription test_mavlink_tx_sub{ORB_ID(test_mavlink_tx)};

	bool send() override
	{
		test_mavlink_tx_s test_mavlink_tx;

		if (test_mavlink_tx_sub.update(&test_mavlink_tx)) {
			mavlink_test_mavlink_t msg{};
msg.test1=test_mavlink_tx.test1;
msg.test2=test_mavlink_tx.test2;
msg.test3=test_mavlink_tx.test3;			
			mavlink_msg_test_mavlink_send_struct(_mavlink->get_channel(), &msg);
			return true;
		}

		return false;
	}
};

#endif // MOUNT_ORIENTATION

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43

    修改mavlink_messages.cpp

    在这里插入图片描述
    添加hpp头文件

    #include "streams/MAVLINK_TEST.hpp"

    • 1

    在这里插入图片描述
    添加接口

    #if defined(MAVLINK_TEST_HPP)
    create_stream_list_item(),
#endif // MAVLINK_TEST_HPP

    • 1
    • 2
    • 3

    在这里插入图片描述

    修改mavlink_main.cpp

    在这里插入图片描述
    配置MAVLINK发送速率，注意这里要加在switch (_mode)上面

    configure_stream_local("TEST_MAVLINK", unlimited_rate);

    • 1

    在这里插入图片描述
    注意双引号里面的是消息的名称，在前面的MAVLINK_TEST.hpp中定义：
    在这里插入图片描述

    五、在QGC中添加自定义mavlink消息

    QGC中的MAVLINK库和PX4中的不一样，使用的方言也不一样，因此两者的库不能直接复制粘贴用，在QGC中也需要进行定义

    安装MAVLINK生成器mavgenerate

    sudo apt-get install python3-pip

    • 1
    pip3 install --user future

    • 1
    sudo apt-get install python3-tk

    • 1
    git clone https://github.com/mavlink/mavlink.git --recursive

    • 1

    下载后进入到工程文件夹
    在这里插入图片描述
    进入这个文件夹路径下执行

    python3 -m mavgenerate

    • 1

    会出现下面这个窗口
    在这里插入图片描述

    修改XML文件并生成MAVLINK库文件

    QGC采用的是ardupilotmega方言
    修改QGC下图目录的ardupilotmega.xml
    在这里插入图片描述
    在最后加上

    
  Test Mavlink.
   
   
   


    • 1
    • 2
    • 3
    • 4
    • 5
    • 6

    在这里插入图片描述
    打开上一节安装的mavgenerate
    XML选择修改后的ardupilotmega.xml，Out选择库文件的输出目录，Language选择C，Protocol选择2.0。最后点Generate
    在这里插入图片描述
    然后会在指定的输出目录下生成消息库文件
    在这里插入图片描述
    将生成的这些文件全部复制并覆盖到qgroundcontrol/libs/mavlink/include/mavlink/v2.0目录下

    六、QGC消息处理

    设计界面

    参考src/AnalyzeView/MavlinkConsolePage.qml 创建src/AnalyzeView/MavlinkTestPage.qml 。
    在这里插入图片描述
    MavlinkTestPage.qml内容如下：

    import QtQuick          2.3
import QtQuick.Controls 1.2
import QtQuick.Dialogs  1.2
import QtQuick.Layouts      1.2

import QGroundControl               1.0
import QGroundControl.Palette       1.0
import QGroundControl.FactSystem    1.0
import QGroundControl.FactControls  1.0
import QGroundControl.Controls      1.0
import QGroundControl.ScreenTools   1.0
import QGroundControl.Controllers   1.0

AnalyzePage {
    id:              mavlinkConsolePage
    pageComponent:   pageComponent
    pageName:        qsTr("Mavlink Test")

    Mavlinktest {
       id: test_mavlink
    }

    TextEdit {
        id: textEdit
        x: 285
        y: 189
        width: 81
        height: 25
        color: "#f5f3f3"
        text: qsTr("TEST1")
        font.family: "Times New Roman"
        font.pixelSize: 12
    }

    Button {
        id: button
        x: 372
        y: 189
        text: qsTr("发送")
        onClicked: {
       test_mavlink._sendcom(textEdit.text,textEdit1.text,textEdit2.text)
        }
    }

    TextEdit {
        id: textEdit1
        x: 285
        y: 220
        width: 81
        height: 25
        color: "#f5f3f3"
        text: qsTr("TEST2")
        font.pixelSize: 12
    }

    TextEdit {
        id: textEdit2
        x: 285
        y: 251
        width: 81
        height: 25
        color: "#f5f3f3"
        text: qsTr("TEST3")
        font.pixelSize: 12
    }

    Label {
        id: label
        x: 285
        y: 317
        width: 30
        height: 19
        text: qsTr("接收")
    }

    TextEdit {
        id: textEdit3
        x: 286
        y: 354
        width: 80
        height: 19
        color: "#f5f3f3"
        text: test_mavlink.test1
        font.pixelSize: 12
    }

    TextEdit {
        id: textEdit4
        x: 286
        y: 391
        width: 80
        height: 19
        color: "#f5f3f3"
        text: test_mavlink.test2
        font.pixelSize: 12
    }

    TextEdit {
        id: textEdit5
        x: 286
        y: 424
        width: 80
        height: 19
        color: "#f5f3f3"
        text:test_mavlink.test3
        font.pixelSize: 12
    }



    // Component
} // AnalyzePage

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112

    在qgroundcontrol.qrc中将上面创建的QML资源添加进去，取别名叫MavlinkTestPage.qml。注意添加完后一定不要忘了把下面别名那个输入框填上别名，注意别名不要写错，不要忘了加上后缀.qml。否则如果后面QGCCorePlugin.cc引用的时候和别名不一样，会导致MavlinkTestPage页面里是空的，不是MavlinkTestPage.qml设计的页面
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    然后修改QGCCorePlugin.cc文件
    添加一行

    _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Mavlinktest"),     QUrl::fromUserInput("qrc:/qml/MavlinkTestPage.qml"),      QUrl::fromUserInput("qrc:/qmlimages/MavlinkConsoleIcon"))));

    • 1

    在这里插入图片描述

    消息处理

    参考src/AnalyzeView/MavlinkConsoleController.h
    创建src/AnalyzeView/Mavlinktest.h
    参考src/AnalyzeView/MavlinkConsoleController.cc
    创建src/AnalyzeView/Mavlinktest.cpp
    在这里插入图片描述
    Mavlinktest.cpp内容如下：

    #include "Mavlinktest.h"
#include "QGCApplication.h"
#include "UAS.h"
#include "MAVLinkInspectorController.h"
#include "MultiVehicleManager.h"
#include 
Mavlinktest::Mavlinktest()
    : QStringListModel(),
      _cursor_home_pos{-1},
      _cursor{0},
      _vehicle{nullptr}
{
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &Mavlinktest::_setActiveVehicle);
    _setActiveVehicle(manager->activeVehicle());
    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
    connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &Mavlinktest::_receiveMessage);
}

Mavlinktest::~Mavlinktest()
{
    if (_vehicle)
    {
        QByteArray msg;
        _sendSerialData(msg, true);
    }
}

void
Mavlinktest::sendCommand(QString command)
{
    _history.append(command);
    command.append("\n");
    _sendSerialData(qPrintable(command));
    _cursor_home_pos = -1;
    _cursor = rowCount();
}

QString
Mavlinktest::historyUp(const QString& current)
{
    return _history.up(current);
}

QString
Mavlinktest::historyDown(const QString& current)
{
    return _history.down(current);
}

void
Mavlinktest::_setActiveVehicle(Vehicle* vehicle)
{
    for (auto &con : _uas_connections)
    {
        disconnect(con);
    }
    _uas_connections.clear();

    _vehicle = vehicle;

    if (_vehicle)
    {
        _incoming_buffer.clear();
        // Reset the model
        setStringList(QStringList());
        _cursor = 0;
        _cursor_home_pos = -1;
        _uas_connections << connect(_vehicle, &Vehicle::mavlinkSerialControl, this, &Mavlinktest::_receiveData);
    }
}

void
Mavlinktest::_receiveData(uint8_t device, uint8_t, uint16_t, uint32_t, QByteArray data)
{
    if (device != SERIAL_CONTROL_DEV_SHELL)
    {
        return;
    }
   // auto idx = index(_cursor);
//setData(idx,  QString("%1 ttyS6 -> * [%2]").arg(QTime::currentTime().toString("HH:mm:ss.zzz")).arg(12));


    // Append incoming data and parse for ANSI codes
    _incoming_buffer.append(data);
    while(!_incoming_buffer.isEmpty())
    {
        bool newline = false;
        int idx = _incoming_buffer.indexOf('\n');
        if (idx == -1)
        {
            // Read the whole incoming buffer
            idx = _incoming_buffer.size();
        }
        else
        {
            newline = true;
        }

        QByteArray fragment = _incoming_buffer.mid(0, idx);
        if (_processANSItext(fragment))
        {
            writeLine(_cursor, fragment);
            if (newline)
            {
                _cursor++;
            }
            _incoming_buffer.remove(0, idx + (newline ? 1 : 0));
        }
        else
        {
            // ANSI processing failed, need more data
            return;
        }
    }
}

void
Mavlinktest::_receiveMessage(LinkInterface*, mavlink_message_t message)
{

    if(message.msgid==MAVLINK_MSG_ID_TEST_MAVLINK)
    {
        qDebug()<<"receive MAVLINK_MSG_ID_TEST_MAVLINK";
        mavlink_test_mavlink_t mavlink_test;

        mavlink_msg_test_mavlink_decode(&message, &mavlink_test);

//id=pwm.base_mode;
//_name=QString::number(heartbeat.base_mode);
        _test1=QString::number(mavlink_test.test1);
        _test2=QString::number(mavlink_test.test2);
        _test3=QString::number(mavlink_test.test3);
        emit valueChanged();
        qDebug()<<_test1<<_test2<<_test3;
//        qDebug()<<"从模式"<vehicleLinkManager()->primaryLink();
    if (!weakLink.expired()) {
        SharedLinkInterfacePtr sharedLink = weakLink.lock();

        if (!sharedLink) {
            qCDebug(VehicleLog) << "_handlePing: primary link gone!";
            return;
        }
//            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
            auto priority_link =sharedLink;

            uint8_t send_test1=test1.toUInt();
            int16_t send_test2=test2.toShort();
            float send_test3=test3.toFloat();
            mavlink_message_t msg;
            mavlink_msg_test_mavlink_pack_chan(_vehicle->id(),
                                               1,
                                               priority_link->mavlinkChannel(),
                                               &msg,
                                               send_test1,
                                               send_test2,
                                               send_test3);
           _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
            qDebug()<<"send"<vehicleLinkManager()->primaryLink();
    if (!weakLink.expired()) {
        SharedLinkInterfacePtr sharedLink = weakLink.lock();

        if (!sharedLink) {
            qCDebug(VehicleLog) << "_handlePing: primary link gone!";
            return;
        }


        // Send maximum sized chunks until the complete buffer is transmitted
//        while(data.size())
//        {
//            QByteArray chunk{data.left(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN)};
//            uint8_t flags = SERIAL_CONTROL_FLAG_EXCLUSIVE |  SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
//            if (close)
//            {
//                flags = 0;
//            }
//            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
//            auto priority_link =sharedLink;
//            mavlink_message_t msg;



//            mavlink_msg_serial_control_pack_chan(
//                protocol->getSystemId(),
//                protocol->getComponentId(),
//                priority_link->mavlinkChannel(),
//                &msg,
//                SERIAL_CONTROL_DEV_SHELL,
//                flags,
//                0,
//                0,
//                chunk.size(),
//                reinterpret_cast(chunk.data()));
//            _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
//            data.remove(0, chunk.size());
//        }
    }




}

bool
Mavlinktest::_processANSItext(QByteArray &line)
{
    // Iterate over the incoming buffer to parse off known ANSI control codes
    for (int i = 0; i < line.size(); i++)
    {
        if (line.at(i) == '\x1B')
        {
            // For ANSI codes we expect at least 3 incoming chars
            if (i < line.size() - 2 && line.at(i+1) == '[')
            {
                // Parse ANSI code
                switch(line.at(i+2))
                {
                    default:
                        continue;
                    case 'H':
                        if (_cursor_home_pos == -1)
                        {
                            // Assign new home position if home is unset
                            _cursor_home_pos = _cursor;
                        }
                        else
                        {
                            // Rewind write cursor position to home
                            _cursor = _cursor_home_pos;
                        }
                        break;
                    case 'K':
                        // Erase the current line to the end
                        if (_cursor < rowCount())
                        {
                            setData(index(_cursor), "");
                        }
                        break;
                    case '2':
                        // Check for sufficient buffer size
                        if ( i >= line.size() - 3)
                        {
                            return false;
                        }

                        if (line.at(i+3) == 'J' && _cursor_home_pos != -1)
                        {
                            // Erase everything and rewind to home
                            bool blocked = blockSignals(true);
                            for (int j = _cursor_home_pos; j < rowCount(); j++)
                            {
                                setData(index(j), "");
                            }
                            blockSignals(blocked);
                            QVector roles;
                            roles.reserve(2);
                            roles.append(Qt::DisplayRole);
                            roles.append(Qt::EditRole);
                            emit dataChanged(index(_cursor), index(rowCount()), roles);
                        }
                        // Even if we didn't understand this ANSI code, remove the 4th char
                        line.remove(i+3,1);
                        break;
                }
                // Remove the parsed ANSI code and decrement the bufferpos
                line.remove(i, 3);
                i--;
            }
            else
            {
                // We can reasonably expect a control code was fragemented
                // Stop parsing here and wait for it to come in
                return false;
            }
        }
    }
    return true;
}

void
Mavlinktest::writeLine(int line, const QByteArray &text)
{
    auto rc = rowCount();
    if (line >= rc)
    {
        insertRows(rc, 1 + line - rc);
    }
    auto idx = index(line);
    setData(idx, data(idx, Qt::DisplayRole).toString() + text);
}

void Mavlinktest::CommandHistory::append(const QString& command)
{
    if (command.length() > 0)
    {

        // do not append duplicates
        if (_history.length() == 0 || _history.last() != command)
        {

            if (_history.length() >= maxHistoryLength)
            {
                _history.removeFirst();
            }
            _history.append(command);
        }
    }
    _index = _history.length();
}

QString Mavlinktest::CommandHistory::up(const QString& current)
{
    if (_index <= 0)
    {
        return current;
    }

    --_index;
    if (_index < _history.length())
    {
        return _history[_index];
    }
    return "";
}

QString Mavlinktest::CommandHistory::down(const QString& current)
{
    if (_index >= _history.length())
    {
        return current;
    }

    ++_index;
    if (_index < _history.length())
    {
        return _history[_index];
    }
    return "";
}






    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168
    • 169
    • 170
    • 171
    • 172
    • 173
    • 174
    • 175
    • 176
    • 177
    • 178
    • 179
    • 180
    • 181
    • 182
    • 183
    • 184
    • 185
    • 186
    • 187
    • 188
    • 189
    • 190
    • 191
    • 192
    • 193
    • 194
    • 195
    • 196
    • 197
    • 198
    • 199
    • 200
    • 201
    • 202
    • 203
    • 204
    • 205
    • 206
    • 207
    • 208
    • 209
    • 210
    • 211
    • 212
    • 213
    • 214
    • 215
    • 216
    • 217
    • 218
    • 219
    • 220
    • 221
    • 222
    • 223
    • 224
    • 225
    • 226
    • 227
    • 228
    • 229
    • 230
    • 231
    • 232
    • 233
    • 234
    • 235
    • 236
    • 237
    • 238
    • 239
    • 240
    • 241
    • 242
    • 243
    • 244
    • 245
    • 246
    • 247
    • 248
    • 249
    • 250
    • 251
    • 252
    • 253
    • 254
    • 255
    • 256
    • 257
    • 258
    • 259
    • 260
    • 261
    • 262
    • 263
    • 264
    • 265
    • 266
    • 267
    • 268
    • 269
    • 270
    • 271
    • 272
    • 273
    • 274
    • 275
    • 276
    • 277
    • 278
    • 279
    • 280
    • 281
    • 282
    • 283
    • 284
    • 285
    • 286
    • 287
    • 288
    • 289
    • 290
    • 291
    • 292
    • 293
    • 294
    • 295
    • 296
    • 297
    • 298
    • 299
    • 300
    • 301
    • 302
    • 303
    • 304
    • 305
    • 306
    • 307
    • 308
    • 309
    • 310
    • 311
    • 312
    • 313
    • 314
    • 315
    • 316
    • 317
    • 318
    • 319
    • 320
    • 321
    • 322
    • 323
    • 324
    • 325
    • 326
    • 327
    • 328
    • 329
    • 330
    • 331
    • 332
    • 333
    • 334
    • 335
    • 336
    • 337
    • 338
    • 339
    • 340
    • 341
    • 342
    • 343
    • 344
    • 345
    • 346
    • 347
    • 348
    • 349
    • 350
    • 351
    • 352
    • 353
    • 354
    • 355
    • 356
    • 357
    • 358
    • 359
    • 360
    • 361
    • 362
    • 363
    • 364
    • 365
    • 366
    • 367
    • 368
    • 369
    • 370
    • 371

    Mavlinktest.h内容如下：

    #pragma once

#include "MAVLinkProtocol.h"
#include "Vehicle.h"
#include "QmlObjectListModel.h"
#include "Fact.h"
#include "FactMetaData.h"
#include 
#include 
#include 
#include 

// Fordward decls
class Vehicle;

/// Controller for MavlinkConsole.qml.
class Mavlinktest : public QStringListModel
{
    Q_OBJECT

public:
    Mavlinktest();
    virtual ~Mavlinktest();

    Q_INVOKABLE void sendCommand(QString command);
    Q_INVOKABLE void _sendcom(QString test1,QString test2,QString test3);
    Q_INVOKABLE QString historyUp(const QString& current);
    Q_INVOKABLE QString historyDown(const QString& current);

    Q_PROPERTY(QString          test1       READ test1      NOTIFY valueChanged)
    QString         test1           ()
    {
        return _test1;
    }

    Q_PROPERTY(QString          test2       READ test2      NOTIFY valueChanged)
    QString         test2           ()
    {
        return _test2;
    }

    Q_PROPERTY(QString          test3       READ test3      NOTIFY valueChanged)
    QString         test3           ()
    {
        return _test3;
    }


private slots:
    void _setActiveVehicle  (Vehicle* vehicle);
    void _receiveData(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);
    void _receiveMessage            (LinkInterface* link, mavlink_message_t message);
signals:
    void            valueChanged        ();
    void selectedChanged                ();

private:
    bool _processANSItext(QByteArray &line);
    void _sendSerialData(QByteArray, bool close = false);
    void writeLine(int line, const QByteArray &text);

    class CommandHistory
    {
    public:
        void append(const QString& command);
        QString up(const QString& current);
        QString down(const QString& current);
    private:
        static constexpr int maxHistoryLength = 100;
        QList _history;
        int _index = 0;
    };
    QString     _test1,_test2,_test3;
    int           _cursor_home_pos;
    int           _cursor;
    QByteArray    _incoming_buffer;
    Vehicle*      _vehicle;
    QList _uas_connections;
    CommandHistory _history;

};

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81

    将.h和.cpp源文件加入工程（如果QT Creator 没有自动添加的话），修改qgroundcontrol.pro

    src/AnalyzeView/Mavlinktest.h\
src/AnalyzeView/Mavlinktest.cpp\

    • 1
    • 2

    分别添加到下图的位置：
    在这里插入图片描述
    在src/QGCApplication.cc注册QML

    先添加头文件

    #include "Mavlinktest.h"

    • 1

    在这里插入图片描述
    然后添加

    qmlRegisterType (kQGCControllers, 1, 0, "Mavlinktest");

    • 1

    在这里插入图片描述
    经过上面注册后就可以在ＱＭＬ中用双引号里的类名进行直接构造，通过id来访问属性和方法，例如：

    Mavlinktest {
    id: testsa
}

    • 1
    • 2
    • 3

    七、测试

    将编辑好的PX4固件编译烧入飞控，编译生成QGC地面站

    编译QGC后，界面如下，上面有三个输入框，输入需要发送的数据，点击发送即可通过自定义MAVLINK消息发送给飞控，下面有显示接收数据的地方，会将飞控飞控QGC的自定义MAVLIK消息解析后在下面显示。
    在这里插入图片描述

    将飞控通过USB连接地面站。先在MAVLINK Console里面输入

    test_mavlink start

    • 1

    在这里插入图片描述
    然后打开Mavlinktest那一栏，在“发送”按钮左边的三个输入框输入发送的数据，正常的话飞控会通过自定义MAVLINK消息接收到这些数据然后加1发送给地面站，并在地面站上显示。

    在这里插入图片描述

  • 相关阅读:
    传输层协议-TCP各个字段含义
    YOLOV8离谱报错
    20k招的工程师刚加入团队，4行代码写3个NPE异常，服了！
    排序算法——稳定性及其汇总
    第11章 字符串和字符串函数
    [附源码]Python计算机毕业设计SSM抗包虫病药物查询与推荐系统（程序+LW)
    详解C语言指针（二）
    计算机组成原理（六）
    Linux-rpm命令
    MYSQL的存储过程
  • 原文地址：https://blog.csdn.net/qq_38768959/article/details/125510104