• Home
  • Features
  • Pricing
  • Docs
  • Announcements
  • Sign In

mavlink / MAVSDK / 17083234422

19 Aug 2025 10:23PM UTC coverage: 47.262% (+0.004%) from 47.258%
17083234422

push

github

web-flow
Merge pull request #2645 from mavlink/pr-proto-comment

proto: update to get comment regardig GPS fix

16702 of 35339 relevant lines covered (47.26%)

443.76 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

61.5
/src/mavsdk/core/mavlink_request_message.cpp
1

2
#include "log.h"
3
#include "mavlink_request_message.h"
4
#include "system_impl.h"
5
#include <cstdint>
6
#include <string>
7

8
namespace mavsdk {
9

10
MavlinkRequestMessage::MavlinkRequestMessage(
119✔
11
    SystemImpl& system_impl,
12
    MavlinkCommandSender& command_sender,
13
    MavlinkMessageHandler& message_handler,
14
    TimeoutHandler& timeout_handler) :
119✔
15
    _system_impl(system_impl),
119✔
16
    _command_sender(command_sender),
119✔
17
    _message_handler(message_handler),
119✔
18
    _timeout_handler(timeout_handler)
119✔
19
{
20
    if (const char* env_p = std::getenv("MAVSDK_COMMAND_DEBUGGING")) {
119✔
21
        if (std::string(env_p) == "1") {
×
22
            LogDebug() << "Command debugging is on.";
×
23
            _debugging = true;
×
24
        }
25
    }
26
}
119✔
27

28
void MavlinkRequestMessage::request(
98✔
29
    uint32_t message_id,
30
    uint8_t target_component,
31
    MavlinkRequestMessageCallback callback,
32
    uint32_t param2)
33
{
34
    std::unique_lock<std::mutex> lock(_mutex);
98✔
35

36
    // Cleanup previous requests.
37
    for (const auto id : _deferred_message_cleanup) {
99✔
38
        _message_handler.unregister_one(id, this);
1✔
39
    }
40
    _deferred_message_cleanup.clear();
98✔
41

42
    // Respond with 'Busy' if already in progress.
43
    for (auto& item : _work_items) {
200✔
44
        if (item.message_id == message_id && item.param2 == param2 &&
102✔
45
            item.target_component == target_component) {
×
46
            lock.unlock();
×
47
            if (callback) {
×
48
                callback(MavlinkCommandSender::Result::Busy, {});
×
49
            }
50
            if (_debugging) {
×
51
                LogDebug() << "Message " << item.message_id << " already requested";
×
52
            }
53
            return;
×
54
        }
55
    }
56

57
    // Otherwise, schedule it.
58
    _work_items.emplace_back(WorkItem{message_id, target_component, callback, param2});
98✔
59

60
    // Register for message
61
    _message_handler.register_one(
98✔
62
        message_id,
63
        [this](const mavlink_message_t& message) { handle_any_message(message); },
85✔
64
        this);
65

66
    // And send off command
67
    send_request(_work_items.back());
98✔
68
}
97✔
69

70
void MavlinkRequestMessage::send_request(WorkItem& item)
180✔
71
{
72
    // Every second time, starting with first retry, we try the old request commands.
73
    // If no old commands exist, we of course just send the new one again.
74
    if (item.retries % 2 != 0) {
180✔
75
        if (!try_sending_request_using_old_command(item)) {
46✔
76
            send_request_using_new_command(item);
2✔
77
        }
78
    } else {
79
        send_request_using_new_command(item);
134✔
80
    }
81
}
179✔
82

83
void MavlinkRequestMessage::send_request_using_new_command(WorkItem& item)
136✔
84
{
85
    if (_debugging) {
136✔
86
        if (item.retries > 0) {
×
87
            LogDebug() << "Request message " << item.message_id
×
88
                       << " again using REQUEST_MESSAGE (retries: " << item.retries << ")";
×
89
        } else {
90
            LogDebug() << "Request message " << item.message_id << " using REQUEST_MESSAGE";
×
91
        }
92
    }
93

94
    MavlinkCommandSender::CommandLong command_request_message{};
136✔
95
    command_request_message.command = MAV_CMD_REQUEST_MESSAGE;
136✔
96
    command_request_message.target_system_id = _system_impl.get_system_id();
136✔
97
    command_request_message.target_component_id = item.target_component;
136✔
98
    command_request_message.params.maybe_param1 = {static_cast<float>(item.message_id)};
136✔
99
    _command_sender.queue_command_async(
136✔
100
        command_request_message,
101
        [this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {
136✔
102
            if (result != MavlinkCommandSender::Result::InProgress) {
131✔
103
                handle_command_result(message_id, result);
131✔
104
            }
105
        },
131✔
106
        1);
107
}
135✔
108

109
bool MavlinkRequestMessage::try_sending_request_using_old_command(WorkItem& item)
46✔
110
{
111
    MavlinkCommandSender::CommandLong command_request_message{};
46✔
112
    command_request_message.target_system_id = _system_impl.get_system_id();
46✔
113
    command_request_message.target_component_id = item.target_component;
46✔
114

115
    std::string command_name;
46✔
116

117
    switch (item.message_id) {
46✔
118
        case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
×
119
            command_request_message.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
×
120
            command_request_message.params.maybe_param1 = {1.f};
×
121
            command_name = "REQUEST_AUTOPILOT_CAPABILITIES";
×
122
            break;
×
123

124
        case MAVLINK_MSG_ID_CAMERA_INFORMATION:
×
125
            command_request_message.command = MAV_CMD_REQUEST_CAMERA_INFORMATION;
×
126
            command_request_message.params.maybe_param1 = {1.0f};
×
127
            command_name = "REQUEST_CAMERA_INFORMATION";
×
128
            break;
×
129

130
        case MAVLINK_MSG_ID_CAMERA_SETTINGS:
7✔
131
            command_request_message.command = MAV_CMD_REQUEST_CAMERA_SETTINGS;
7✔
132
            command_request_message.params.maybe_param1 = {1.0f};
7✔
133
            command_name = "REQUEST_CAMERA_SETTINGS";
7✔
134

135
            break;
7✔
136

137
        case MAVLINK_MSG_ID_STORAGE_INFORMATION:
13✔
138
            command_request_message.command = MAV_CMD_REQUEST_STORAGE_INFORMATION;
13✔
139
            command_request_message.params.maybe_param1 = {
26✔
140
                static_cast<float>(item.param2)}; // storage ID
13✔
141
            command_request_message.params.maybe_param2 = {1.0f};
13✔
142
            command_name = "REQUEST_STORAGE_INFORMATION";
13✔
143
            break;
13✔
144

145
        case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
×
146
            command_request_message.command = MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS;
×
147
            command_request_message.params.maybe_param1 = {1.0f};
×
148
            command_name = "REQUEST_CAMERA_CAPTURE_STATUS";
×
149
            break;
×
150

151
        case MAVLINK_MSG_ID_FLIGHT_INFORMATION:
×
152
            command_request_message.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION;
×
153
            command_request_message.params.maybe_param1 = {1.0f};
×
154
            command_name = "REQUEST_FLIGHT_INFORMATION";
×
155
            break;
×
156

157
        case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:
13✔
158
            command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_STATUS;
13✔
159
            command_request_message.params.maybe_param1 = {
26✔
160
                static_cast<float>(item.param2)}; // stream ID
13✔
161
            command_name = "REQUEST_VIDEO_STREAM_STATUS";
13✔
162
            break;
13✔
163

164
        case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:
11✔
165
            command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION;
11✔
166
            command_request_message.params.maybe_param1 = {
22✔
167
                static_cast<float>(item.param2)}; // stream ID
11✔
168
            command_name = "REQUEST_VIDEO_STREAM_INFORMATION";
11✔
169
            break;
11✔
170

171
        case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
×
172
            command_request_message.command = MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE;
×
173
            command_request_message.params.maybe_param1 = {
×
174
                static_cast<float>(item.param2)}; // sequence number
×
175
            command_name = "REQUEST_CAMERA_IMAGE_CAPTURE";
×
176
            break;
×
177

178
        default:
2✔
179
            // No alternative command for this message.
180
            return false;
2✔
181
    }
182

183
    if (_debugging) {
44✔
184
        LogDebug() << "Request message " << item.message_id << " again using " << command_name
×
185
                   << " (retries: " << item.retries << ")";
×
186
    }
187

188
    _command_sender.queue_command_async(
44✔
189
        command_request_message,
190
        [this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {
44✔
191
            if (result != MavlinkCommandSender::Result::InProgress) {
44✔
192
                handle_command_result(message_id, result);
44✔
193
            }
194
        },
44✔
195
        1);
196

197
    return true;
44✔
198
}
46✔
199

200
void MavlinkRequestMessage::handle_any_message(const mavlink_message_t& message)
85✔
201
{
202
    std::unique_lock<std::mutex> lock(_mutex);
85✔
203

204
    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
119✔
205
        // Check if we're waiting for this message.
206
        // TODO: check if params are correct.
207
        if (it->message_id != message.msgid && it->target_component == message.compid) {
107✔
208
            continue;
34✔
209
        }
210

211
        _timeout_handler.remove(it->timeout_cookie);
73✔
212
        // We have at least the message which is what we care about most, so we
213
        // tell the user about it and call it a day. If we receive the result
214
        // of the command later, we ignore it, and we fake the result to be successful
215
        // anyway.
216
        auto temp_callback = it->callback;
73✔
217
        // We can now get rid of the entry now.
218
        _work_items.erase(it);
73✔
219
        // We have to clean up later outside this callback, otherwise we lock ourselves out.
220
        _deferred_message_cleanup.push_back(message.msgid);
73✔
221
        lock.unlock();
73✔
222

223
        if (temp_callback) {
73✔
224
            temp_callback(MavlinkCommandSender::Result::Success, message);
1✔
225
        }
226
        return;
73✔
227
    }
73✔
228
}
85✔
229

230
void MavlinkRequestMessage::handle_command_result(
175✔
231
    uint32_t message_id, MavlinkCommandSender::Result result)
232
{
233
    std::unique_lock<std::mutex> lock(_mutex);
175✔
234

235
    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
299✔
236
        // Check if we're waiting for this result
237
        if (it->message_id != message_id) {
244✔
238
            continue;
124✔
239
        }
240

241
        switch (result) {
120✔
242
            case MavlinkCommandSender::Result::Success:
20✔
243
                // This is promising, let's hope the message will actually arrive.
244
                // We'll set a timeout in case we need to retry.
245
                it->timeout_cookie = _timeout_handler.add(
20✔
246
                    [this, message_id, target_component = it->target_component]() {
20✔
247
                        handle_timeout(message_id, target_component);
×
248
                    },
×
249
                    1.0);
250
                return;
120✔
251

252
            case MavlinkCommandSender::Result::Busy:
100✔
253
                // FALLTHROUGH
254
            case MavlinkCommandSender::Result::Denied:
255
                // FALLTHROUGH
256
            case MavlinkCommandSender::Result::Unsupported:
257
                // FALLTHROUGH
258
            case MavlinkCommandSender::Result::Timeout:
259
                // FALLTHROUGH
260
            case MavlinkCommandSender::Result::TemporarilyRejected:
261
                // It looks like we should try again
262
                if (++it->retries > RETRIES) {
100✔
263
                    // We have already retried, let's give up.
264
                    auto temp_callback = it->callback;
18✔
265
                    _message_handler.unregister_one(it->message_id, this);
18✔
266
                    _work_items.erase(it);
18✔
267
                    lock.unlock();
18✔
268
                    if (temp_callback) {
18✔
269
                        temp_callback(MavlinkCommandSender::Result::Timeout, {});
×
270
                    }
271
                } else {
18✔
272
                    send_request(*it);
82✔
273
                }
274
                return;
100✔
275

276
            case MavlinkCommandSender::Result::NoSystem:
×
277
                // FALLTHROUGH
278
            case MavlinkCommandSender::Result::ConnectionError:
279
                // FALLTHROUGH
280
            case MavlinkCommandSender::Result::Failed:
281
                // FALLTHROUGH
282
            case MavlinkCommandSender::Result::Cancelled:
283
                // FALLTHROUGH
284
            case MavlinkCommandSender::Result::UnknownError: {
285
                // It looks like this did not work, and we can report the error
286
                // No need to try again.
287
                auto temp_callback = it->callback;
×
288
                _message_handler.unregister_one(it->message_id, this);
×
289
                _work_items.erase(it);
×
290
                lock.unlock();
×
291
                if (temp_callback) {
×
292
                    temp_callback(result, {});
×
293
                }
294
                return;
×
295
            }
×
296

297
            case MavlinkCommandSender::Result::InProgress:
×
298
                // Should not happen, as it is already filtered out.
299
                return;
×
300
        }
301
    }
302
}
175✔
303

304
void MavlinkRequestMessage::handle_timeout(uint32_t message_id, uint8_t target_component)
×
305
{
306
    std::unique_lock<std::mutex> lock(_mutex);
×
307

308
    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
×
309
        // Check if we're waiting for this result
310
        if (it->message_id != message_id || it->target_component != target_component) {
×
311
            continue;
×
312
        }
313

314
        if (++it->retries > RETRIES) {
×
315
            // We have already retried, let's give up.
316
            auto temp_callback = it->callback;
×
317
            _message_handler.unregister_one(it->message_id, this);
×
318
            _work_items.erase(it);
×
319
            lock.unlock();
×
320
            if (temp_callback) {
×
321
                temp_callback(MavlinkCommandSender::Result::Timeout, {});
×
322
            }
323
            return;
×
324
        }
×
325

326
        send_request(*it);
×
327
    }
328
}
×
329

330
} // namespace mavsdk
STATUS · Troubleshooting · Open an Issue · Sales · Support · CAREERS · ENTERPRISE · START FREE · SCHEDULE DEMO
ANNOUNCEMENTS · TWITTER · TOS & SLA · Supported CI Services · What's a CI service? · Automated Testing

© 2026 Coveralls, Inc