10834742033

Committed 12 Sep 2024 04:17PM UTC coverage: 37.983% (-0.06%) from 38.046%

Build # 10834742033

Build Type

Pull #2400

github

Committed by

web-flow

Commit Message

Merge 846d1fca9 into 2de3994cb

Pull Request Pull Request #2400: core: Fix reading int8/16 params from ArduPilot

Run Details

9 of 14 new or added lines in 1 file covered. (64.29%)

33 existing lines in 3 files now uncovered.

11431 of 30095 relevant lines covered (37.98%)

265.15 hits per line

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

39.1

/src/mavsdk/core/mavlink_request_message.cpp


#include "log.h"
#include "mavlink_request_message.h"
#include "system_impl.h"
#include <cstdint>
#include <string>

namespace mavsdk {

MavlinkRequestMessage::MavlinkRequestMessage(
    SystemImpl& system_impl,
    MavlinkCommandSender& command_sender,
    MavlinkMessageHandler& message_handler,
    TimeoutHandler& timeout_handler) :
    _system_impl(system_impl),
    _command_sender(command_sender),
    _message_handler(message_handler),
    _timeout_handler(timeout_handler)
{}

void MavlinkRequestMessage::request(
    uint32_t message_id,
    uint8_t target_component,
    MavlinkRequestMessageCallback callback,
    uint32_t param2)
{
    std::unique_lock<std::mutex> lock(_mutex);

    // Cleanup previous requests.
    for (const auto id : _deferred_message_cleanup) {
        _message_handler.unregister_one(id, this);
    }
    _deferred_message_cleanup.clear();

    // Respond with 'Busy' if already in progress.
    for (auto& item : _work_items) {
        if (item.message_id == message_id && item.param2 == param2) {
            lock.unlock();
            if (callback) {
                callback(MavlinkCommandSender::Result::Busy, {});
            }
            return;
        }
    }

    // Otherwise, schedule it.
    _work_items.emplace_back(WorkItem{message_id, target_component, callback, param2});

    // Register for message
    _message_handler.register_one(
        message_id,
        [this](const mavlink_message_t& message) { handle_any_message(message); },
        this);

    // And send off command
    send_request(_work_items.back());
}

void MavlinkRequestMessage::send_request(WorkItem& item)
{
    // Every second time, starting with first retry, we try the old request commands.
    // If no old commands exist, we of course just send the new one again.
    if (item.retries % 2 != 0) {
        if (!try_sending_request_using_old_command(item)) {
            send_request_using_new_command(item);
        }
    } else {
        send_request_using_new_command(item);
    }
}

void MavlinkRequestMessage::send_request_using_new_command(WorkItem& item)
{
    if (item.retries > 0) {
        LogWarn() << "Request message " << item.message_id
                  << " again using REQUEST_MESSAGE (retries: " << item.retries << ")";
    } else {
        LogDebug() << "Request message " << item.message_id << " using REQUEST_MESSAGE";
    }

    MavlinkCommandSender::CommandLong command_request_message{};
    command_request_message.command = MAV_CMD_REQUEST_MESSAGE;
    command_request_message.target_system_id = _system_impl.get_system_id();
    command_request_message.target_component_id = item.target_component;
    command_request_message.params.maybe_param1 = {static_cast<float>(item.message_id)};
    _command_sender.queue_command_async(
        command_request_message,
        [this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {
            if (result != MavlinkCommandSender::Result::InProgress) {
                handle_command_result(message_id, result);
            }
        });
}

bool MavlinkRequestMessage::try_sending_request_using_old_command(WorkItem& item)
{
    MavlinkCommandSender::CommandLong command_request_message{};
    command_request_message.target_system_id = _system_impl.get_system_id();
    command_request_message.target_component_id = item.target_component;

    std::string command_name;

    switch (item.message_id) {
        case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
            command_request_message.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
            command_request_message.params.maybe_param1 = {1.f};
            command_name = "REQUEST_AUTOPILOT_CAPABILITIES";
            break;

        case MAVLINK_MSG_ID_CAMERA_INFORMATION:
            command_request_message.command = MAV_CMD_REQUEST_CAMERA_INFORMATION;
            command_request_message.params.maybe_param1 = {1.0f};
            command_name = "REQUEST_CAMERA_INFORMATION";
            break;

        case MAVLINK_MSG_ID_CAMERA_SETTINGS:
            command_request_message.command = MAV_CMD_REQUEST_CAMERA_SETTINGS;
            command_request_message.params.maybe_param1 = {1.0f};
            command_name = "REQUEST_CAMERA_SETTINGS";

            break;

        case MAVLINK_MSG_ID_STORAGE_INFORMATION:
            command_request_message.command = MAV_CMD_REQUEST_STORAGE_INFORMATION;
            command_request_message.params.maybe_param1 = {
                static_cast<float>(item.param2)}; // storage ID
            command_request_message.params.maybe_param2 = {1.0f};
            command_name = "REQUEST_STORAGE_INFORMATION";
            break;

        case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
            command_request_message.command = MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS;
            command_request_message.params.maybe_param1 = {1.0f};
            command_name = "REQUEST_CAMERA_CAPTURE_STATUS";
            break;

        case MAVLINK_MSG_ID_FLIGHT_INFORMATION:
            command_request_message.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION;
            command_request_message.params.maybe_param1 = {1.0f};
            command_name = "REQUEST_FLIGHT_INFORMATION";
            break;

        case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:
            command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_STATUS;
            command_request_message.params.maybe_param1 = {
                static_cast<float>(item.param2)}; // stream ID
            command_name = "REQUEST_VIDEO_STREAM_STATUS";
            break;

        case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:
            command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION;
            command_request_message.params.maybe_param1 = {
                static_cast<float>(item.param2)}; // stream ID
            command_name = "REQUEST_VIDEO_STREAM_INFORMATION";
            break;

        case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
            command_request_message.command = MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE;
            command_request_message.params.maybe_param1 = {
                static_cast<float>(item.param2)}; // sequence number
            command_name = "REQUEST_CAMERA_IMAGE_CAPTURE";
            break;

        default:
            // No alternative command for this message.
            return false;
    }

    LogWarn() << "Request message " << item.message_id << " again using " << command_name
              << " (retries: " << item.retries << ")";

    _command_sender.queue_command_async(
        command_request_message,
        [this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {
            if (result != MavlinkCommandSender::Result::InProgress) {
                handle_command_result(message_id, result);
            }
        });

    return true;
}

void MavlinkRequestMessage::handle_any_message(const mavlink_message_t& message)
{
    std::unique_lock<std::mutex> lock(_mutex);

    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
        // Check if we're waiting for this message.
        // TODO: check if params are correct.
        if (it->message_id != message.msgid && it->target_component == message.compid) {
            continue;
        }

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

        if (temp_callback) {
            temp_callback(MavlinkCommandSender::Result::Success, message);
        }
        return;
    }
}

void MavlinkRequestMessage::handle_command_result(
    uint32_t message_id, MavlinkCommandSender::Result result)
{
    std::unique_lock<std::mutex> lock(_mutex);

    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
        // Check if we're waiting for this result
        if (it->message_id != message_id) {
            continue;
        }

        switch (result) {
            case MavlinkCommandSender::Result::Success:
                // This is promising, let's hope the message will actually arrive.
                // We'll set a timeout in case we need to retry.
                it->timeout_cookie = _timeout_handler.add(
                    [this, message_id, target_component = it->target_component]() {
                        handle_timeout(message_id, target_component);
                    },
                    1.0);
                return;

            case MavlinkCommandSender::Result::NoSystem:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::ConnectionError:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Busy:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Denied:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Unsupported:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Timeout:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::TemporarilyRejected:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Failed:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Cancelled:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::UnknownError: {
                // It looks like this did not work, and we can report the error
                // No need to try again.
                auto temp_callback = it->callback;
                _message_handler.unregister_one(it->message_id, this);
                _work_items.erase(it);
                lock.unlock();
                if (temp_callback) {
                    temp_callback(result, {});
                }
                return;
            }

            case MavlinkCommandSender::Result::InProgress:
                // Should not happen, as it is already filtered out.
                return;
        }
    }
}

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

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

        if (++it->retries > 3) {
            // We have already retried, let's give up.
            auto temp_callback = it->callback;
            _message_handler.unregister_one(it->message_id, this);
            _work_items.erase(it);
            lock.unlock();
            if (temp_callback) {
                temp_callback(MavlinkCommandSender::Result::Timeout, {});
            }
            return;
        }

        send_request(*it);
    }
}

} // namespace mavsdk

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(	69✔
11	SystemImpl& system_impl,
12	MavlinkCommandSender& command_sender,
13	MavlinkMessageHandler& message_handler,
14	TimeoutHandler& timeout_handler) :	69✔
15	_system_impl(system_impl),
16	_command_sender(command_sender),
17	_message_handler(message_handler),
18	_timeout_handler(timeout_handler)	69✔
19	{}	69✔
20
21	void MavlinkRequestMessage::request(	35✔
22	uint32_t message_id,
23	uint8_t target_component,
24	MavlinkRequestMessageCallback callback,
25	uint32_t param2)
26	{
27	std::unique_lock<std::mutex> lock(_mutex);	35✔
28
29	// Cleanup previous requests.
30	for (const auto id : _deferred_message_cleanup) {	35✔
31	_message_handler.unregister_one(id, this);	×
32	}
33	_deferred_message_cleanup.clear();	35✔
34
35	// Respond with 'Busy' if already in progress.
36	for (auto& item : _work_items) {	36✔
37	if (item.message_id == message_id && item.param2 == param2) {	1✔
38	lock.unlock();	×
39	if (callback) {	×
40	callback(MavlinkCommandSender::Result::Busy, {});	×
41	}
42	return;	×
43	}
44	}
45
46	// Otherwise, schedule it.
47	_work_items.emplace_back(WorkItem{message_id, target_component, callback, param2});	35✔
48
49	// Register for message
50	_message_handler.register_one(	35✔
51	message_id,
52	[this](const mavlink_message_t& message) { handle_any_message(message); },	33✔
53	this);
54
55	// And send off command
56	send_request(_work_items.back());	35✔
57	}
58
59	void MavlinkRequestMessage::send_request(WorkItem& item)	35✔
60	{
61	// Every second time, starting with first retry, we try the old request commands.
62	// If no old commands exist, we of course just send the new one again.
63	if (item.retries % 2 != 0) {	35✔
UNCOV 64	if (!try_sending_request_using_old_command(item)) {	×
65	send_request_using_new_command(item);	×
66	}
67	} else {
68	send_request_using_new_command(item);	35✔
69	}
70	}	35✔
71
72	void MavlinkRequestMessage::send_request_using_new_command(WorkItem& item)	35✔
73	{
74	if (item.retries > 0) {	35✔
75	LogWarn() << "Request message " << item.message_id	×
76	<< " again using REQUEST_MESSAGE (retries: " << item.retries << ")";	×
77	} else {
78	LogDebug() << "Request message " << item.message_id << " using REQUEST_MESSAGE";	35✔
79	}
80
81	MavlinkCommandSender::CommandLong command_request_message{};	35✔
82	command_request_message.command = MAV_CMD_REQUEST_MESSAGE;	35✔
83	command_request_message.target_system_id = _system_impl.get_system_id();	35✔
84	command_request_message.target_component_id = item.target_component;	35✔
85	command_request_message.params.maybe_param1 = {static_cast<float>(item.message_id)};	35✔
86	_command_sender.queue_command_async(	70✔
87	command_request_message,
88	[this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {	35✔
89	if (result != MavlinkCommandSender::Result::InProgress) {	35✔
90	handle_command_result(message_id, result);	35✔
91	}
92	});	35✔
93	}	35✔
94
UNCOV 95	bool MavlinkRequestMessage::try_sending_request_using_old_command(WorkItem& item)	×
96	{
UNCOV 97	MavlinkCommandSender::CommandLong command_request_message{};	×
UNCOV 98	command_request_message.target_system_id = _system_impl.get_system_id();	×
UNCOV 99	command_request_message.target_component_id = item.target_component;	×
100
UNCOV 101	std::string command_name;	×
102
UNCOV 103	switch (item.message_id) {	×
UNCOV 104	case MAVLINK_MSG_ID_AUTOPILOT_VERSION:	×
UNCOV 105	command_request_message.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;	×
UNCOV 106	command_request_message.params.maybe_param1 = {1.f};	×
UNCOV 107	command_name = "REQUEST_AUTOPILOT_CAPABILITIES";	×
UNCOV 108	break;	×
109
110	case MAVLINK_MSG_ID_CAMERA_INFORMATION:	×
111	command_request_message.command = MAV_CMD_REQUEST_CAMERA_INFORMATION;	×
112	command_request_message.params.maybe_param1 = {1.0f};	×
113	command_name = "REQUEST_CAMERA_INFORMATION";	×
114	break;	×
115
116	case MAVLINK_MSG_ID_CAMERA_SETTINGS:	×
117	command_request_message.command = MAV_CMD_REQUEST_CAMERA_SETTINGS;	×
118	command_request_message.params.maybe_param1 = {1.0f};	×
119	command_name = "REQUEST_CAMERA_SETTINGS";	×
120
121	break;	×
122
123	case MAVLINK_MSG_ID_STORAGE_INFORMATION:	×
124	command_request_message.command = MAV_CMD_REQUEST_STORAGE_INFORMATION;	×
125	command_request_message.params.maybe_param1 = {	×
126	static_cast<float>(item.param2)}; // storage ID	×
127	command_request_message.params.maybe_param2 = {1.0f};	×
128	command_name = "REQUEST_STORAGE_INFORMATION";	×
129	break;	×
130
131	case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:	×
132	command_request_message.command = MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS;	×
133	command_request_message.params.maybe_param1 = {1.0f};	×
134	command_name = "REQUEST_CAMERA_CAPTURE_STATUS";	×
135	break;	×
136
137	case MAVLINK_MSG_ID_FLIGHT_INFORMATION:	×
138	command_request_message.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION;	×
139	command_request_message.params.maybe_param1 = {1.0f};	×
140	command_name = "REQUEST_FLIGHT_INFORMATION";	×
141	break;	×
142
143	case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:	×
144	command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_STATUS;	×
145	command_request_message.params.maybe_param1 = {	×
146	static_cast<float>(item.param2)}; // stream ID	×
147	command_name = "REQUEST_VIDEO_STREAM_STATUS";	×
148	break;	×
149
150	case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:	×
151	command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION;	×
152	command_request_message.params.maybe_param1 = {	×
153	static_cast<float>(item.param2)}; // stream ID	×
154	command_name = "REQUEST_VIDEO_STREAM_INFORMATION";	×
155	break;	×
156
157	case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:	×
158	command_request_message.command = MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE;	×
159	command_request_message.params.maybe_param1 = {	×
160	static_cast<float>(item.param2)}; // sequence number	×
161	command_name = "REQUEST_CAMERA_IMAGE_CAPTURE";	×
162	break;	×
163
164	default:	×
165	// No alternative command for this message.
166	return false;	×
167	}
168
UNCOV 169	LogWarn() << "Request message " << item.message_id << " again using " << command_name	×
UNCOV 170	<< " (retries: " << item.retries << ")";	×
171
UNCOV 172	_command_sender.queue_command_async(	×
173	command_request_message,
UNCOV 174	[this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {	×
UNCOV 175	if (result != MavlinkCommandSender::Result::InProgress) {	×
UNCOV 176	handle_command_result(message_id, result);	×
177	}
UNCOV 178	});	×
179
UNCOV 180	return true;	×
181	}
182
183	void MavlinkRequestMessage::handle_any_message(const mavlink_message_t& message)	33✔
184	{
185	std::unique_lock<std::mutex> lock(_mutex);	33✔
186
187	for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {	33✔
188	// Check if we're waiting for this message.
189	// TODO: check if params are correct.
190	if (it->message_id != message.msgid && it->target_component == message.compid) {	31✔
191	continue;	×
192	}
193
194	_timeout_handler.remove(it->timeout_cookie);	31✔
195	// We have at least the message which is what we care about most, so we
196	// tell the user about it and call it a day. If we receive the result
197	// of the command later, we ignore it, and we fake the result to be successful
198	// anyway.
199	auto temp_callback = it->callback;	62✔
200	// We can now get rid of the entry now.
201	_work_items.erase(it);	31✔
202	// We have to clean up later outside this callback, otherwise we lock ourselves out.
203	_deferred_message_cleanup.push_back(message.msgid);	31✔
204	lock.unlock();	31✔
205
206	if (temp_callback) {	31✔
207	temp_callback(MavlinkCommandSender::Result::Success, message);	1✔
208	}
209	return;	31✔
210	}
211	}
212
213	void MavlinkRequestMessage::handle_command_result(	35✔
214	uint32_t message_id, MavlinkCommandSender::Result result)
215	{
216	std::unique_lock<std::mutex> lock(_mutex);	35✔
217
218	for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {	36✔
219	// Check if we're waiting for this result
220	if (it->message_id != message_id) {	5✔
221	continue;	1✔
222	}
223
224	switch (result) {	4✔
225	case MavlinkCommandSender::Result::Success:	3✔
226	// This is promising, let's hope the message will actually arrive.
227	// We'll set a timeout in case we need to retry.
228	it->timeout_cookie = _timeout_handler.add(	6✔
229	[this, message_id, target_component = it->target_component]() {	3✔
UNCOV 230	handle_timeout(message_id, target_component);	×
UNCOV 231	},	×
232	1.0);
233	return;	3✔
234
235	case MavlinkCommandSender::Result::NoSystem:	1✔
236	// FALLTHROUGH
237	case MavlinkCommandSender::Result::ConnectionError:
238	// FALLTHROUGH
239	case MavlinkCommandSender::Result::Busy:
240	// FALLTHROUGH
241	case MavlinkCommandSender::Result::Denied:
242	// FALLTHROUGH
243	case MavlinkCommandSender::Result::Unsupported:
244	// FALLTHROUGH
245	case MavlinkCommandSender::Result::Timeout:
246	// FALLTHROUGH
247	case MavlinkCommandSender::Result::TemporarilyRejected:
248	// FALLTHROUGH
249	case MavlinkCommandSender::Result::Failed:
250	// FALLTHROUGH
251	case MavlinkCommandSender::Result::Cancelled:
252	// FALLTHROUGH
253	case MavlinkCommandSender::Result::UnknownError: {
254	// It looks like this did not work, and we can report the error
255	// No need to try again.
256	auto temp_callback = it->callback;	2✔
257	_message_handler.unregister_one(it->message_id, this);	1✔
258	_work_items.erase(it);	1✔
259	lock.unlock();	1✔
260	if (temp_callback) {	1✔
261	temp_callback(result, {});	×
262	}
263	return;	1✔
264	}
265
266	case MavlinkCommandSender::Result::InProgress:	×
267	// Should not happen, as it is already filtered out.
268	return;	×
269	}
270	}
271	}
272
UNCOV 273	void MavlinkRequestMessage::handle_timeout(uint32_t message_id, uint8_t target_component)	×
274	{
UNCOV 275	std::unique_lock<std::mutex> lock(_mutex);	×
276
UNCOV 277	for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {	×
278	// Check if we're waiting for this result
UNCOV 279	if (it->message_id != message_id \|\| it->target_component != target_component) {	×
280	continue;	×
281	}
282
UNCOV 283	if (++it->retries > 3) {	×
284	// We have already retried, let's give up.
285	auto temp_callback = it->callback;	×
286	_message_handler.unregister_one(it->message_id, this);	×
287	_work_items.erase(it);	×
288	lock.unlock();	×
289	if (temp_callback) {	×
290	temp_callback(MavlinkCommandSender::Result::Timeout, {});	×
291	}
292	return;	×
293	}
294
UNCOV 295	send_request(*it);	×
296	}
297	}
298
299	} // namespace mavsdk

mavlink / MAVSDK / 10834742033

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

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