MAVLink入门教程09.03：通过UDP发送数据至QGC

系列索引：MAVLink入门教程索引

目的

本文介绍如何通过UDP直接发送数据给QGC的Mavlink框架中。

MavSDK

先从MAVSDK下载静态库。

只能使用VS2022编译运行

心跳包

MavSDK初始化

1
2
3
4
5
6
7
8
auto component_type = Mavsdk::ComponentType::Autopilot;//1
auto config = Mavsdk::Configuration(component_type);//2
auto mav = Mavsdk(config);//3
std::cout << "Mavsdk version: " << mav.version() << std::endl;
std::cout << "Sys ID: " << unsigned(config.get_system_id())  // use unsigned for uin8_t
    << ", Component ID: " << unsigned(config.get_component_id())
    << ", Always heartbeats: " << config.get_always_send_heartbeats()
    << std::endl;

Mavsdk定义了一个Mavsdk类，需要先将这个类实例化后再使用。

主要语句就是这个第3行。config是Configuration类的一个实例，对应第一行代码。Autopilot是mavsdk中已经定义好的枚举类型，也可以指定其他类型。

运行输出为

1
2
3
[05:02:30|Info ] MAVSDK version: v2.10.0 (D:\a\MAVSDK\MAVSDK\src\mavsdk\core\mavsdk_impl.cpp:26)
Mavsdk version: v2.10.0
Sys ID: 1, Component ID: 1, Always heartbeats: 1

UDP

1
2
3
4
// Use my own udp library
auto udp = UdpCom();
int r = udp.Open("127.0.0.1", 14540, "127.0.0.1", 14550);
std::cout << "UDP Open: " << r << std::endl;

打开本地的14540/14550端口，这个是QGC/Mavlink默认端口

心跳包

根据流程创建心跳包并填充数据

1
2
3
4
5
6
7
// Heartbeat message
mavlink_message_t msg_heartbeat;
uint8_t len_heartbeat;
uint8_t buf_heartbeat[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_heartbeat_pack(config.get_system_id(), config.get_component_id(), &msg_heartbeat,
    MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len_heartbeat = mavlink_msg_to_send_buffer(buf_heartbeat, &msg_heartbeat);

无论是串口还是udp，最后发送的都是字节码buf_heartbeat而不是msg_heartbeat结构体。

发送

以1Hz的频率发送

1
2
3
4
5
6
7
8
9
10
11
// Whenever a second has passed, we send a message.
while (true)
{
    current_time = time(NULL);
    if (current_time - last_time >= 1) {
        udp.Send((char*)(buf_heartbeat), len_heartbeat);
        std::cout << "Heartbeat sent." << std::endl;

        last_time = current_time;
    }
}

效果

编译运行程序。

打开QGC

QGC

先打开数据发送程序，后打开QGC，此时QGC是连接状态

点击左上角打开工具

工具

选择应用设置

应用设置

点击MAVLink Inspector标签，自动显示接收的数据

MAVLink Inspector

频率为1Hz，数据类型，数据值都有显示。

Vibration（震动）

根据心跳包的创建流程创建震动数据包

1
2
3
4
5
6
7
8
9
10
11
12
13
14
// Vibration message
mavlink_message_t msg_vibration;
uint8_t len_vibration;
uint8_t buf_vibration[MAVLINK_MAX_PACKET_LEN];
uint64_t time_usec=1000;
float vx = 25;
float vy = 50;
float vz = 100;
uint32_t c0=1;
uint32_t c1=2;
uint32_t c2=3;
mavlink_msg_vibration_pack(config.get_system_id(), config.get_component_id(), &msg_vibration, 
    time_usec, vx, vy, vz, c0, c1, c2);
len_vibration = mavlink_msg_to_send_buffer(buf_vibration, &msg_vibration);

通过UDP发送数据。

查看Analyze Tools -> Vibration界面

震动

数据开始发送后

接收数据

完整代码

库文件

此部分是VS工程设置

VS编译设置为C++17标准

C++17标准

头文件

1
$(SolutionDir)mavsdk\include;

库文件

1
2
$(SolutionDir)/mavsdk/lib;
ws2_32.lib;mavsdk.lib;

UDP

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
/*****************************************************************
 * @file   samll_udp.h
 * @brief  A very simple header-only library for udp communication in Windows.
 *
 * @author zswzy
 * @date   20230529
 *********************************************************************/

#ifndef SUDP_HPP
#define SUDP_HPP

#include <stdio.h>
#include <string>
#include <WS2tcpip.h>

#pragma comment (lib, "ws2_32.lib")

class UdpCom {
private:
    SOCKET			m_in;
    sockaddr_in		m_local;
    const char*		m_local_ip;
    int				m_local_port;
    int				m_local_length;
    sockaddr_in		m_dest;
    const char*		m_dest_ip;
    int				m_dest_port;
    int				m_dest_length;
    char			m_buffer[4096];
public:
    /**
     * @brief The UDP class constructor.
     *
     * @return
     */
    UdpCom()
    {
        m_in			= NULL;
        m_local			= sockaddr_in();
        m_local_length  = sizeof(m_local);
        m_local_ip		= "127.0.0.1";
        m_local_port	= 0;
        m_dest			= sockaddr_in();
        m_dest_length	= sizeof(m_dest);
        m_dest_ip		= "0.0.0.0";
        m_dest_port		= 0;
        memset(m_buffer, 0, sizeof(m_buffer));
        ZeroMemory(&m_dest, sizeof(m_dest));
    }

    /**
     * @brief destructor.
     *
     */
    ~UdpCom()
    {Close();}

    /**
     * @brief Open UDP.
     * @return 0: normal, 1: Winsock error, 2: bind error.
     */
    int Open()
    {
        int return_code = 0;
        // Startup Winsock 开启winsock
        WSADATA data;
        WORD version = MAKEWORD(2, 2);
        int wsOK = WSAStartup(version, &data);
        if (wsOK != 0) {
            printf("can't start Winsock. Error: %d.\n", wsOK);
            return_code = 1;
        }
        // Bind socket 绑定本地端口号
        m_in = socket(AF_INET, SOCK_DGRAM, 0);
        m_local.sin_family = AF_INET;
        m_local.sin_port = htons(m_local_port); // Convert from little to big endian
        inet_pton(AF_INET, m_local_ip, &m_local.sin_addr);
        // 检测是否成功绑定
        if (bind(m_in, (sockaddr*)&m_local, sizeof(m_local)) == SOCKET_ERROR)
        {
            printf("Can't bind socket! Error: %d.\n", WSAGetLastError());
            return_code = 2;
        }
        // 绑定远端
        m_dest.sin_family = AF_INET;
        inet_pton(AF_INET, m_dest_ip, &m_dest.sin_addr);
        m_dest.sin_port = htons(m_dest_port);
        printf("UDP is open.\n");
        return return_code;
    }

    /**
     * @brief Open UDP with parameters.
     *
     * @param local_ip		local IPv4 address.
     * @param local_port	local port.
     * @param dest_ip		remote IPv4 address.
     * @param dest_port		remote port.
     * @return
     */
    int Open(const char* local_ip, const int local_port, const char* dest_ip, const int dest_port)
    {
        SetLocal(local_ip, local_port);
        SetDest(dest_ip, dest_port);
        Open();
        return 0;
    }

    /**
     * @brief Receive message from remote.
     *
     * @return
     */
    int Receive()
    {
        ZeroMemory(m_buffer, 4096);
        // 接收消息，储存为变量buf，类型为字符串
        int byte_in = recvfrom(m_in, m_buffer, 4096, 0, (sockaddr*)&m_dest, &m_dest_length);

        if (byte_in == SOCKET_ERROR)
        {
            printf("Error receiving from client. Error code: %d", WSAGetLastError());
        }
        printf("%s \n",m_buffer);
        //recv_data = jsonxx::json::parse(m_buffer);
        return 0;
    }

    /**
     * @brief Send message to remote.
     *
     * @param message_send
     * @return 0: normal, 1: Error.
     */
    int Send(char* message_send, int len)
    {
        int return_code = 0;
        //printf("message_send: %s, sizeof: %d\n", message_send, strlen(message_send));
        int send_ok = sendto(m_in, message_send, len, 0, (sockaddr*)&m_dest, sizeof(m_dest));
        if (send_ok == SOCKET_ERROR)
        {
            printf("Send error.\n");
            return_code = 1;
        }
        else
        {
            // std::cout << "Send OK" << std::endl;
        }

        return return_code;
    }

    /**
     * @brief Close UDP. It is called in the destructor.
     *
     * @return
     */
    int Close()
    {
        // Close socket
        closesocket(m_in);
        // Shutdown winsock
        WSACleanup();
        return 0;
    }

    /**
     * @brief Set remote parameters: IP and port.
     *
     * @param dest_ip	remote IPv4 address.
     * @param dest_port remote port.
     * @return
     */
    int SetDest(const char* dest_ip, const int dest_port)
    {
        m_dest_ip = dest_ip;
        m_dest_port = dest_port;
        return 0;
    }

    /**
     * @brief Set local parameters: IP and port.
     *
     * @param local_ip		local IPv4 address.
     * @param local_port	local port
     * @return
     */
    int SetLocal(const char* local_ip, const int local_port)
    {
        m_local_ip = local_ip;
        m_local_port = local_port;
        return 0;
    }

    /**
     * @brief Get received message.
     *
     * @return
     */
    char* GetContent()
    {
        return m_buffer;
    }
};

#endif // SUDP_HPP

main

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
// System
#include <iostream>
#include <time.h>

// Project header
#include "sudp.hpp"

// MAVSDK
#include "mavsdk/mavsdk.h"

using namespace mavsdk;

int main()
{
    auto component_type = Mavsdk::ComponentType::Autopilot;
    auto config = Mavsdk::Configuration(component_type);
    auto mav = Mavsdk(config);
    std::cout << "Mavsdk version: " << mav.version() << std::endl;
    std::cout << "Sys ID: " << unsigned(config.get_system_id())  // use unsigned for uin8_t
        << ", Component ID: " << unsigned(config.get_component_id())
        << ", Always heartbeats: " << config.get_always_send_heartbeats()
        << std::endl;

    // Use my own udp library
    auto udp = UdpCom();
    int r = udp.Open("127.0.0.1", 14540, "127.0.0.1", 14550);
    std::cout << "UDP Open: " << r << std::endl;

    // Timing
    static time_t last_time = 0;
    time_t start_time = time(NULL);
    time_t current_time = time(NULL);

    // Heartbeat message
    mavlink_message_t msg_heartbeat;
    uint8_t len_heartbeat;
    uint8_t buf_heartbeat[MAVLINK_MAX_PACKET_LEN];
    mavlink_msg_heartbeat_pack(config.get_system_id(), config.get_component_id(), &msg_heartbeat,
        MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
    len_heartbeat = mavlink_msg_to_send_buffer(buf_heartbeat, &msg_heartbeat);

    // Vibration message
    mavlink_message_t msg_vibration;
    uint8_t len_vibration;
    uint8_t buf_vibration[MAVLINK_MAX_PACKET_LEN];
    uint64_t time_usec=1000;
    float vx = 25;
    float vy = 50;
    float vz = 100;
    uint32_t c0=1;
    uint32_t c1=2;
    uint32_t c2=3;
    mavlink_msg_vibration_pack(config.get_system_id(), config.get_component_id(), &msg_vibration, 
        time_usec, vx, vy, vz, c0, c1, c2);
    len_vibration = mavlink_msg_to_send_buffer(buf_vibration, &msg_vibration);

    // Whenever a second has passed, we send a message.
    while (true)
    {
        current_time = time(NULL);
        if (current_time - last_time >= 1) {
            udp.Send((char*)(buf_heartbeat), len_heartbeat);
            std::cout << "Heartbeat sent." << std::endl;

            udp.Send((char*)(buf_vibration), len_vibration);
            std::cout << "Vibration sent." << std::endl;

            last_time = current_time;
        }
    }

    return 0;
}
MAVLink
#QGC #UDP #MAVLink #QGroundControl #通信
MAVLink入门教程09.03：通过UDP发送数据至QGC
https://blog.jackeylea.com/mavlink/send-data-to-qgc-with-udp/
作者
JackeyLea
发布于
2024年6月15日
许可协议