12111769901

Committed 02 Dec 2024 02:51AM UTC coverage: 43.587% (+4.9%) from 38.69%

Build # 12111769901

Build Type

Pull #2386

github

Committed by

web-flow

Commit Message

Merge 522a301e0 into 68856f27a

Pull Request Pull Request #2386: camera: support multiple cameras within one instance

Run Details

1366 of 2037 new or added lines in 47 files covered. (67.06%)

47 existing lines in 7 files now uncovered.

13875 of 31833 relevant lines covered (43.59%)

290.84 hits per line

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

72.63

/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);
            }
        },
        1);
}

bool MavlinkRequestMessage::try_sending_request_using_old_command(WorkItem& item)
{
    LogInfo() << "Try old command";
    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);
            }
        },
        1);

    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::Busy:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Denied:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Unsupported:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::Timeout:
                // FALLTHROUGH
            case MavlinkCommandSender::Result::TemporarilyRejected:
                // It looks like we should try again
                if (++it->retries > RETRIES) {
                    // 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, {});
                    }
                } else {
                    send_request(*it);
                }
                return;

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

mavlink / MAVSDK / 12111769901

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