4899817366

Build Type

github

Committed by GitHub

Commit Message

Merge c0bb90862 into e4e9c71dd

Pull Request Pull Request #1772: Refactor MAVLinkParameters into client and server classes

Run Details

2192 of 2192 new or added lines in 34 files covered. (100.0%)

7708 of 24812 relevant lines covered (31.07%)

19.96 hits per line

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

44.07

/src/mavsdk/core/mavlink_command_sender.cpp

#include "mavlink_command_sender.h"
#include "system_impl.h"
#include "unused.h"
#include <cmath>
#include <future>
#include <memory>

namespace mavsdk {

MavlinkCommandSender::MavlinkCommandSender(SystemImpl& system_impl) : _system_impl(system_impl)
{
    if (const char* env_p = std::getenv("MAVSDK_COMMAND_DEBUGGING")) {
        if (std::string(env_p) == "1") {
            LogDebug() << "Command debugging is on.";
            _command_debugging = true;
        }
    }

    _system_impl.register_mavlink_message_handler(
        MAVLINK_MSG_ID_COMMAND_ACK,
        [this](const mavlink_message_t& message) { receive_command_ack(message); },
        this);
}

MavlinkCommandSender::~MavlinkCommandSender()
{
    _system_impl.unregister_all_mavlink_message_handlers(this);
}

MavlinkCommandSender::Result
MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandInt& command)
{
    // We wrap the async call with a promise and future.
    auto prom = std::make_shared<std::promise<Result>>();
    auto res = prom->get_future();

    queue_command_async(command, [prom](Result result, float progress) {
        UNUSED(progress);
        // We can only fulfill the promise once in C++11.
        // Therefore, we have to ignore the IN_PROGRESS state and wait
        // for the final result.
        if (result != Result::InProgress) {
            prom->set_value(result);
        }
    });

    // Block now to wait for result.
    return res.get();
}

MavlinkCommandSender::Result
MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandLong& command)
{
    // We wrap the async call with a promise and future.
    auto prom = std::make_shared<std::promise<Result>>();
    auto res = prom->get_future();

    queue_command_async(command, [prom](Result result, float progress) {
        UNUSED(progress);
        // We can only fulfill the promise once in C++11.
        // Therefore, we have to ignore the IN_PROGRESS state and wait
        // for the final result.
        if (result != Result::InProgress) {
            prom->set_value(result);
        }
    });

    return res.get();
}

void MavlinkCommandSender::queue_command_async(
    const CommandInt& command, const CommandResultCallback& callback)
{
    if (_command_debugging) {
        LogDebug() << "COMMAND_INT " << (int)(command.command) << " to send to "
                   << (int)(command.target_system_id) << ", " << (int)(command.target_component_id);
    }

    CommandIdentification identification = identification_from_command(command);

    for (const auto& work : _work_queue) {
        if (work->identification == identification && callback == nullptr) {
            if (_command_debugging) {
                LogDebug() << "Dropping command " << static_cast<int>(identification.command)
                           << " that is already being sent";
            }
            return;
        }
    }

    auto new_work = std::make_shared<Work>();
    new_work->timeout_s = _system_impl.timeout_s();
    new_work->command = command;
    new_work->identification = identification;
    new_work->callback = callback;
    _work_queue.push_back(new_work);
}

void MavlinkCommandSender::queue_command_async(
    const CommandLong& command, const CommandResultCallback& callback)
{
    if (_command_debugging) {
        LogDebug() << "COMMAND_LONG " << (int)(command.command) << " to send to "
                   << (int)(command.target_system_id) << ", " << (int)(command.target_component_id);
    }

    CommandIdentification identification = identification_from_command(command);

    for (const auto& work : _work_queue) {
        if (work->identification == identification && callback == nullptr) {
            if (_command_debugging) {
                LogDebug() << "Dropping command " << static_cast<int>(identification.command)
                           << " that is already being sent";
            }
            return;
        }
    }

    auto new_work = std::make_shared<Work>();
    new_work->timeout_s = _system_impl.timeout_s();
    new_work->command = command;
    new_work->identification = identification;
    new_work->callback = callback;
    new_work->time_started = _system_impl.get_time().steady_time();
    _work_queue.push_back(new_work);
}

void MavlinkCommandSender::receive_command_ack(mavlink_message_t message)
{
    mavlink_command_ack_t command_ack;
    mavlink_msg_command_ack_decode(&message, &command_ack);

    if ((command_ack.target_system &&
         command_ack.target_system != _system_impl.get_own_system_id()) ||
        (command_ack.target_component &&
         command_ack.target_component != _system_impl.get_own_component_id())) {
        if (_command_debugging) {
            LogDebug() << "Ignoring command ack for command "
                       << static_cast<int>(command_ack.command) << " from "
                       << static_cast<int>(message.sysid) << '/' << static_cast<int>(message.compid)
                       << " to " << static_cast<int>(command_ack.target_system) << '/'
                       << static_cast<int>(command_ack.target_component);
        }
        return;
    }

    LockedQueue<Work>::Guard work_queue_guard(_work_queue);

    for (auto it = _work_queue.begin(); it != _work_queue.end(); ++it) {
        auto work = *it;

        if (!work) {
            LogErr() << "No work available! (should not happen #1)";
            return;
        }

        if (work->identification.command != command_ack.command ||
            (work->identification.target_system_id != 0 &&
             work->identification.target_system_id != message.sysid) ||
            (work->identification.target_component_id != 0 &&
             work->identification.target_component_id != message.compid)) {
            if (_command_debugging) {
                LogDebug() << "Command ack for " << command_ack.command
                           << " (from: " << std::to_string(message.sysid) << "/"
                           << std::to_string(message.compid) << ")"
                           << " does not match command " << work->identification.command
                           << " (to: " << std::to_string(work->identification.target_system_id)
                           << "/" << std::to_string(work->identification.target_component_id) << ")"
                           << " after "
                           << _system_impl.get_time().elapsed_since_s(work->time_started) << " s";
            }
            continue;
        }

        if (_command_debugging) {
            LogDebug() << "Received command ack for " << command_ack.command << " with result "
                       << static_cast<int>(command_ack.result) << " after "
                       << _system_impl.get_time().elapsed_since_s(work->time_started) << " s";
        }

        CommandResultCallback temp_callback = work->callback;
        std::pair<Result, float> temp_result{Result::UnknownError, NAN};

        switch (command_ack.result) {
            case MAV_RESULT_ACCEPTED:
                _system_impl.unregister_timeout_handler(work->timeout_cookie);
                temp_result = {Result::Success, 1.0f};
                _work_queue.erase(it);
                break;

            case MAV_RESULT_DENIED:
                if (_command_debugging) {
                    LogDebug() << "command denied (" << work->identification.command << ").";
                }
                _system_impl.unregister_timeout_handler(work->timeout_cookie);
                temp_result = {Result::Denied, NAN};
                _work_queue.erase(it);
                break;

            case MAV_RESULT_UNSUPPORTED:
                if (_command_debugging) {
                    LogDebug() << "command unsupported (" << work->identification.command << ").";
                }
                _system_impl.unregister_timeout_handler(work->timeout_cookie);
                temp_result = {Result::Unsupported, NAN};
                _work_queue.erase(it);
                break;

            case MAV_RESULT_TEMPORARILY_REJECTED:
                if (_command_debugging) {
                    LogDebug() << "command temporarily rejected (" << work->identification.command
                               << ").";
                }
                _system_impl.unregister_timeout_handler(work->timeout_cookie);
                temp_result = {Result::TemporarilyRejected, NAN};
                _work_queue.erase(it);
                break;

            case MAV_RESULT_FAILED:
                if (_command_debugging) {
                    LogDebug() << "command failed (" << work->identification.command << ").";
                }
                _system_impl.unregister_timeout_handler(work->timeout_cookie);
                temp_result = {Result::Failed, NAN};
                _work_queue.erase(it);
                break;

            case MAV_RESULT_IN_PROGRESS:
                if (_command_debugging) {
                    if (static_cast<int>(command_ack.progress) != 255) {
                        LogDebug() << "progress: " << static_cast<int>(command_ack.progress)
                                   << " % (" << work->identification.command << ").";
                    }
                }
                // If we get a progress update, we can raise the timeout
                // to something higher because we know the initial command
                // has arrived. A possible timeout for this case is the initial
                // timeout * the possible retries because this should match the
                // case where there is no progress update, and we keep trying.
                _system_impl.unregister_timeout_handler(work->timeout_cookie);
                _system_impl.register_timeout_handler(
                    [this, identification = work->identification] {
                        receive_timeout(identification);
                    },
                    work->retries_to_do * work->timeout_s,
                    &work->timeout_cookie);

                temp_result = {
                    Result::InProgress, static_cast<float>(command_ack.progress) / 100.0f};
                break;

            case MAV_RESULT_CANCELLED:
                if (_command_debugging) {
                    LogDebug() << "command cancelled (" << work->identification.command << ").";
                }
                _system_impl.unregister_timeout_handler(work->timeout_cookie);
                temp_result = {Result::Cancelled, NAN};
                _work_queue.erase(it);
                break;

            default:
                LogWarn() << "Received unknown ack.";
                break;
        }

        if (temp_callback != nullptr) {
            call_callback(temp_callback, temp_result.first, temp_result.second);
        }

        return;
    }

    if (_command_debugging) {
        LogDebug() << "Received ack from " << static_cast<int>(message.sysid) << '/'
                   << static_cast<int>(message.compid)
                   << " for not-existing command: " << static_cast<int>(command_ack.command)
                   << "! Ignoring...";
    } else {
        LogWarn() << "Received ack for not-existing command: "
                  << static_cast<int>(command_ack.command) << "! Ignoring...";
    }
}

void MavlinkCommandSender::receive_timeout(const CommandIdentification& identification)
{
    bool found_command = false;
    CommandResultCallback temp_callback = nullptr;
    std::pair<Result, float> temp_result{Result::UnknownError, NAN};

    LockedQueue<Work>::Guard work_queue_guard(_work_queue);

    for (auto it = _work_queue.begin(); it != _work_queue.end(); ++it) {
        auto work = *it;

        if (!work) {
            LogErr() << "No work available! (should not happen #2)";
            return;
        }

        if (work->identification != identification) {
            continue;
        }

        found_command = true;

        if (work->retries_to_do > 0) {
            // We're not sure the command arrived, let's retransmit.
            LogWarn() << "sending again after "
                      << _system_impl.get_time().elapsed_since_s(work->time_started)
                      << " s, retries to do: " << work->retries_to_do << "  ("
                      << work->identification.command << ").";

            if (work->identification.command == MAV_CMD_REQUEST_MESSAGE) {
                LogWarn() << "Request was for msg ID: " << work->identification.maybe_param1;
            }

            mavlink_message_t message = create_mavlink_message(work->command);
            if (!_system_impl.send_message(message)) {
                LogErr() << "connection send error in retransmit (" << work->identification.command
                         << ").";
                temp_callback = work->callback;
                temp_result = {Result::ConnectionError, NAN};
                _work_queue.erase(it);
                break;
            } else {
                --work->retries_to_do;
                _system_impl.register_timeout_handler(
                    [this, identification = work->identification] {
                        receive_timeout(identification);
                    },
                    work->timeout_s,
                    &work->timeout_cookie);
            }
        } else {
            // We have tried retransmitting, giving up now.
            LogErr() << "Retrying failed (" << work->identification.command << ")";

            temp_callback = work->callback;
            temp_result = {Result::Timeout, NAN};
            _work_queue.erase(it);
            break;
        }
    }

    if (temp_callback != nullptr) {
        call_callback(temp_callback, temp_result.first, temp_result.second);
    }

    if (!found_command) {
        LogWarn() << "Timeout for not-existing command: "
                  << static_cast<int>(identification.command) << "! Ignoring...";
    }
}

void MavlinkCommandSender::do_work()
{
    LockedQueue<Work>::Guard work_queue_guard(_work_queue);

    for (const auto& work : _work_queue) {
        if (work->already_sent) {
            continue;
        }

        bool already_being_sent = false;
        for (const auto& other_work : _work_queue) {
            // Ignore itself:
            if (other_work == work) {
                continue;
            }

            // Check if command with same command ID is already being sent.
            if (other_work->already_sent &&
                other_work->identification.command == work->identification.command) {
                if (_command_debugging) {
                    LogDebug() << "Command " << static_cast<int>(work->identification.command)
                               << " is already being sent, waiting...";
                }
                already_being_sent = true;
                break;
            }
        }

        if (already_being_sent) {
            continue;
        }

        // LogDebug() << "sending it the first time (" << work->mavlink_command << ")";
        work->time_started = _system_impl.get_time().steady_time();

        {
            mavlink_message_t message = create_mavlink_message(work->command);
            if (!_system_impl.send_message(message)) {
                LogErr() << "connection send error (" << work->identification.command << ")";
                // In this case we try again after the timeout. Chances are slim it will work next
                // time though.
            } else {
                if (_command_debugging) {
                    LogDebug() << "Sent command " << static_cast<int>(work->identification.command);
                }
            }
        }

        work->already_sent = true;

        _system_impl.register_timeout_handler(
            [this, identification = work->identification] { receive_timeout(identification); },
            work->timeout_s,
            &work->timeout_cookie);
    }
}

void MavlinkCommandSender::call_callback(
    const CommandResultCallback& callback, Result result, float progress)
{
    if (!callback) {
        return;
    }

    // It seems that we need to queue the callback on the thread pool otherwise
    // we lock ourselves out when we send a command in the callback receiving a command result.
    auto temp_callback = callback;
    _system_impl.call_user_callback(
        [temp_callback, result, progress]() { temp_callback(result, progress); });
}

mavlink_message_t MavlinkCommandSender::create_mavlink_message(const Command& command)
{
    mavlink_message_t message;

    if (auto command_int = std::get_if<CommandInt>(&command)) {
        mavlink_msg_command_int_pack(
            _system_impl.get_own_system_id(),
            _system_impl.get_own_component_id(),
            &message,
            command_int->target_system_id,
            command_int->target_component_id,
            command_int->frame,
            command_int->command,
            command_int->current,
            command_int->autocontinue,
            maybe_reserved(command_int->params.maybe_param1),
            maybe_reserved(command_int->params.maybe_param2),
            maybe_reserved(command_int->params.maybe_param3),
            maybe_reserved(command_int->params.maybe_param4),
            command_int->params.x,
            command_int->params.y,
            maybe_reserved(command_int->params.maybe_z));

    } else if (auto command_long = std::get_if<CommandLong>(&command)) {
        mavlink_msg_command_long_pack(
            _system_impl.get_own_system_id(),
            _system_impl.get_own_component_id(),
            &message,
            command_long->target_system_id,
            command_long->target_component_id,
            command_long->command,
            command_long->confirmation,
            maybe_reserved(command_long->params.maybe_param1),
            maybe_reserved(command_long->params.maybe_param2),
            maybe_reserved(command_long->params.maybe_param3),
            maybe_reserved(command_long->params.maybe_param4),
            maybe_reserved(command_long->params.maybe_param5),
            maybe_reserved(command_long->params.maybe_param6),
            maybe_reserved(command_long->params.maybe_param7));
    }
    return message;
}

float MavlinkCommandSender::maybe_reserved(const std::optional<float>& maybe_param) const
{
    if (maybe_param) {
        return maybe_param.value();

    } else {
        if (_system_impl.autopilot() == SystemImpl::Autopilot::ArduPilot) {
            return 0.0f;
        } else {
            return NAN;
        }
    }
}

} // namespace mavsdk

1	#include "mavlink_command_sender.h"
2	#include "system_impl.h"
3	#include "unused.h"
4	#include <cmath>
5	#include <future>
6	#include <memory>
7
8	namespace mavsdk {
9
10	MavlinkCommandSender::MavlinkCommandSender(SystemImpl& system_impl) : _system_impl(system_impl)	20✔
11	{
12	if (const char* env_p = std::getenv("MAVSDK_COMMAND_DEBUGGING")) {	20✔
13	if (std::string(env_p) == "1") {	×
14	LogDebug() << "Command debugging is on.";	×
15	_command_debugging = true;	×
16	}
17	}
18
19	_system_impl.register_mavlink_message_handler(	20✔
20	MAVLINK_MSG_ID_COMMAND_ACK,
21	[this](const mavlink_message_t& message) { receive_command_ack(message); },	12✔
22	this);
23	}	20✔
24
25	MavlinkCommandSender::~MavlinkCommandSender()	20✔
26	{
27	_system_impl.unregister_all_mavlink_message_handlers(this);	20✔
28	}	20✔
29
30	MavlinkCommandSender::Result
31	MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandInt& command)	×
32	{
33	// We wrap the async call with a promise and future.
34	auto prom = std::make_shared<std::promise<Result>>();	×
35	auto res = prom->get_future();	×
36
37	queue_command_async(command, [prom](Result result, float progress) {	×
38	UNUSED(progress);	×
39	// We can only fulfill the promise once in C++11.
40	// Therefore, we have to ignore the IN_PROGRESS state and wait
41	// for the final result.
42	if (result != Result::InProgress) {	×
43	prom->set_value(result);	×
44	}
45	});	×
46
47	// Block now to wait for result.
48	return res.get();	×
49	}
50
51	MavlinkCommandSender::Result
52	MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandLong& command)	×
53	{
54	// We wrap the async call with a promise and future.
55	auto prom = std::make_shared<std::promise<Result>>();	×
56	auto res = prom->get_future();	×
57
58	queue_command_async(command, [prom](Result result, float progress) {	×
59	UNUSED(progress);	×
60	// We can only fulfill the promise once in C++11.
61	// Therefore, we have to ignore the IN_PROGRESS state and wait
62	// for the final result.
63	if (result != Result::InProgress) {	×
64	prom->set_value(result);	×
65	}
66	});	×
67
68	return res.get();	×
69	}
70
71	void MavlinkCommandSender::queue_command_async(	×
72	const CommandInt& command, const CommandResultCallback& callback)
73	{
74	if (_command_debugging) {	×
75	LogDebug() << "COMMAND_INT " << (int)(command.command) << " to send to "	×
76	<< (int)(command.target_system_id) << ", " << (int)(command.target_component_id);	×
77	}
78
79	CommandIdentification identification = identification_from_command(command);	×
80
81	for (const auto& work : _work_queue) {	×
82	if (work->identification == identification && callback == nullptr) {	×
83	if (_command_debugging) {	×
84	LogDebug() << "Dropping command " << static_cast<int>(identification.command)	×
85	<< " that is already being sent";	×
86	}
87	return;	×
88	}
89	}
90
91	auto new_work = std::make_shared<Work>();	×
92	new_work->timeout_s = _system_impl.timeout_s();	×
93	new_work->command = command;	×
94	new_work->identification = identification;	×
95	new_work->callback = callback;	×
96	_work_queue.push_back(new_work);	×
97	}
98
99	void MavlinkCommandSender::queue_command_async(	14✔
100	const CommandLong& command, const CommandResultCallback& callback)
101	{
102	if (_command_debugging) {	14✔
103	LogDebug() << "COMMAND_LONG " << (int)(command.command) << " to send to "	×
104	<< (int)(command.target_system_id) << ", " << (int)(command.target_component_id);	×
105	}
106
107	CommandIdentification identification = identification_from_command(command);	14✔
108
109	for (const auto& work : _work_queue) {	20✔
110	if (work->identification == identification && callback == nullptr) {	6✔
111	if (_command_debugging) {	×
112	LogDebug() << "Dropping command " << static_cast<int>(identification.command)	×
113	<< " that is already being sent";	×
114	}
115	return;	×
116	}
117	}
118
119	auto new_work = std::make_shared<Work>();	28✔
120	new_work->timeout_s = _system_impl.timeout_s();	14✔
121	new_work->command = command;	14✔
122	new_work->identification = identification;	14✔
123	new_work->callback = callback;	14✔
124	new_work->time_started = _system_impl.get_time().steady_time();	14✔
125	_work_queue.push_back(new_work);	14✔
126	}
127
128	void MavlinkCommandSender::receive_command_ack(mavlink_message_t message)	12✔
129	{
130	mavlink_command_ack_t command_ack;	12✔
131	mavlink_msg_command_ack_decode(&message, &command_ack);	12✔
132
133	if ((command_ack.target_system &&	12✔
134	command_ack.target_system != _system_impl.get_own_system_id()) \|\|	24✔
135	(command_ack.target_component &&	24✔
136	command_ack.target_component != _system_impl.get_own_component_id())) {	12✔
137	if (_command_debugging) {	×
138	LogDebug() << "Ignoring command ack for command "	×
139	<< static_cast<int>(command_ack.command) << " from "	×
140	<< static_cast<int>(message.sysid) << '/' << static_cast<int>(message.compid)	×
141	<< " to " << static_cast<int>(command_ack.target_system) << '/'	×
142	<< static_cast<int>(command_ack.target_component);	×
143	}
144	return;	×
145	}
146
147	LockedQueue<Work>::Guard work_queue_guard(_work_queue);	12✔
148
149	for (auto it = _work_queue.begin(); it != _work_queue.end(); ++it) {	16✔
150	auto work = *it;	16✔
151
152	if (!work) {	16✔
153	LogErr() << "No work available! (should not happen #1)";	×
154	return;	×
155	}
156
157	if (work->identification.command != command_ack.command \|\|	16✔
158	(work->identification.target_system_id != 0 &&	12✔
159	work->identification.target_system_id != message.sysid) \|\|	40✔
160	(work->identification.target_component_id != 0 &&	12✔
161	work->identification.target_component_id != message.compid)) {	12✔
162	if (_command_debugging) {	4✔
163	LogDebug() << "Command ack for " << command_ack.command	×
164	<< " (from: " << std::to_string(message.sysid) << "/"	×
165	<< std::to_string(message.compid) << ")"	×
166	<< " does not match command " << work->identification.command	×
167	<< " (to: " << std::to_string(work->identification.target_system_id)	×
168	<< "/" << std::to_string(work->identification.target_component_id) << ")"	×
169	<< " after "	×
170	<< _system_impl.get_time().elapsed_since_s(work->time_started) << " s";	×
171	}
172	continue;	4✔
173	}
174
175	if (_command_debugging) {	12✔
176	LogDebug() << "Received command ack for " << command_ack.command << " with result "	×
177	<< static_cast<int>(command_ack.result) << " after "	×
178	<< _system_impl.get_time().elapsed_since_s(work->time_started) << " s";	×
179	}
180
181	CommandResultCallback temp_callback = work->callback;	24✔
182	std::pair<Result, float> temp_result{Result::UnknownError, NAN};	12✔
183
184	switch (command_ack.result) {	12✔
185	case MAV_RESULT_ACCEPTED:	10✔
186	_system_impl.unregister_timeout_handler(work->timeout_cookie);	10✔
187	temp_result = {Result::Success, 1.0f};	10✔
188	_work_queue.erase(it);	10✔
189	break;	10✔
190
191	case MAV_RESULT_DENIED:	×
192	if (_command_debugging) {	×
193	LogDebug() << "command denied (" << work->identification.command << ").";	×
194	}
195	_system_impl.unregister_timeout_handler(work->timeout_cookie);	×
196	temp_result = {Result::Denied, NAN};	×
197	_work_queue.erase(it);	×
198	break;	×
199
200	case MAV_RESULT_UNSUPPORTED:	×
201	if (_command_debugging) {	×
202	LogDebug() << "command unsupported (" << work->identification.command << ").";	×
203	}
204	_system_impl.unregister_timeout_handler(work->timeout_cookie);	×
205	temp_result = {Result::Unsupported, NAN};	×
206	_work_queue.erase(it);	×
207	break;	×
208
209	case MAV_RESULT_TEMPORARILY_REJECTED:	2✔
210	if (_command_debugging) {	2✔
211	LogDebug() << "command temporarily rejected (" << work->identification.command	×
212	<< ").";	×
213	}
214	_system_impl.unregister_timeout_handler(work->timeout_cookie);	2✔
215	temp_result = {Result::TemporarilyRejected, NAN};	2✔
216	_work_queue.erase(it);	2✔
217	break;	2✔
218
219	case MAV_RESULT_FAILED:	×
220	if (_command_debugging) {	×
221	LogDebug() << "command failed (" << work->identification.command << ").";	×
222	}
223	_system_impl.unregister_timeout_handler(work->timeout_cookie);	×
224	temp_result = {Result::Failed, NAN};	×
225	_work_queue.erase(it);	×
226	break;	×
227
228	case MAV_RESULT_IN_PROGRESS:	×
229	if (_command_debugging) {	×
230	if (static_cast<int>(command_ack.progress) != 255) {	×
231	LogDebug() << "progress: " << static_cast<int>(command_ack.progress)	×
232	<< " % (" << work->identification.command << ").";	×
233	}
234	}
235	// If we get a progress update, we can raise the timeout
236	// to something higher because we know the initial command
237	// has arrived. A possible timeout for this case is the initial
238	// timeout * the possible retries because this should match the
239	// case where there is no progress update, and we keep trying.
240	_system_impl.unregister_timeout_handler(work->timeout_cookie);	×
241	_system_impl.register_timeout_handler(	×
242	[this, identification = work->identification] {	×
243	receive_timeout(identification);	×
244	},	×
245	work->retries_to_do * work->timeout_s,	×
246	&work->timeout_cookie);	×
247
248	temp_result = {	×
249	Result::InProgress, static_cast<float>(command_ack.progress) / 100.0f};	×
250	break;	×
251
252	case MAV_RESULT_CANCELLED:	×
253	if (_command_debugging) {	×
254	LogDebug() << "command cancelled (" << work->identification.command << ").";	×
255	}
256	_system_impl.unregister_timeout_handler(work->timeout_cookie);	×
257	temp_result = {Result::Cancelled, NAN};	×
258	_work_queue.erase(it);	×
259	break;	×
260
261	default:	×
262	LogWarn() << "Received unknown ack.";	×
263	break;	×
264	}
265
266	if (temp_callback != nullptr) {	12✔
267	call_callback(temp_callback, temp_result.first, temp_result.second);	4✔
268	}
269
270	return;	12✔
271	}
272
273	if (_command_debugging) {	×
274	LogDebug() << "Received ack from " << static_cast<int>(message.sysid) << '/'	×
275	<< static_cast<int>(message.compid)	×
276	<< " for not-existing command: " << static_cast<int>(command_ack.command)	×
277	<< "! Ignoring...";	×
278	} else {
279	LogWarn() << "Received ack for not-existing command: "	×
280	<< static_cast<int>(command_ack.command) << "! Ignoring...";	×
281	}
282	}
283
284	void MavlinkCommandSender::receive_timeout(const CommandIdentification& identification)	2✔
285	{
286	bool found_command = false;	2✔
287	CommandResultCallback temp_callback = nullptr;	2✔
288	std::pair<Result, float> temp_result{Result::UnknownError, NAN};	2✔
289
290	LockedQueue<Work>::Guard work_queue_guard(_work_queue);	2✔
291
292	for (auto it = _work_queue.begin(); it != _work_queue.end(); ++it) {	4✔
293	auto work = *it;	2✔
294
295	if (!work) {	2✔
296	LogErr() << "No work available! (should not happen #2)";	×
297	return;	×
298	}
299
300	if (work->identification != identification) {	2✔
301	continue;	×
302	}
303
304	found_command = true;	2✔
305
306	if (work->retries_to_do > 0) {	2✔
307	// We're not sure the command arrived, let's retransmit.
308	LogWarn() << "sending again after "	4✔
309	<< _system_impl.get_time().elapsed_since_s(work->time_started)	4✔
310	<< " s, retries to do: " << work->retries_to_do << " ("	2✔
311	<< work->identification.command << ").";	6✔
312
313	if (work->identification.command == MAV_CMD_REQUEST_MESSAGE) {	2✔
314	LogWarn() << "Request was for msg ID: " << work->identification.maybe_param1;	×
315	}
316
317	mavlink_message_t message = create_mavlink_message(work->command);	2✔
318	if (!_system_impl.send_message(message)) {	2✔
319	LogErr() << "connection send error in retransmit (" << work->identification.command	×
320	<< ").";	×
321	temp_callback = work->callback;	×
322	temp_result = {Result::ConnectionError, NAN};	×
323	_work_queue.erase(it);	×
324	break;	×
325	} else {
326	--work->retries_to_do;	2✔
327	_system_impl.register_timeout_handler(	4✔
328	[this, identification = work->identification] {	2✔
329	receive_timeout(identification);	×
330	},	×
331	work->timeout_s,	2✔
332	&work->timeout_cookie);	2✔
333	}
334	} else {
335	// We have tried retransmitting, giving up now.
336	LogErr() << "Retrying failed (" << work->identification.command << ")";	×
337
338	temp_callback = work->callback;	×
339	temp_result = {Result::Timeout, NAN};	×
340	_work_queue.erase(it);	×
341	break;	×
342	}
343	}
344
345	if (temp_callback != nullptr) {	2✔
346	call_callback(temp_callback, temp_result.first, temp_result.second);	×
347	}
348
349	if (!found_command) {	2✔
350	LogWarn() << "Timeout for not-existing command: "	×
351	<< static_cast<int>(identification.command) << "! Ignoring...";	×
352	}
353	}
354
355	void MavlinkCommandSender::do_work()	609✔
356	{
357	LockedQueue<Work>::Guard work_queue_guard(_work_queue);	1,218✔
358
359	for (const auto& work : _work_queue) {	648✔
360	if (work->already_sent) {	39✔
361	continue;	26✔
362	}
363
364	bool already_being_sent = false;	13✔
365	for (const auto& other_work : _work_queue) {	35✔
366	// Ignore itself:
367	if (other_work == work) {	22✔
368	continue;	13✔
369	}
370
371	// Check if command with same command ID is already being sent.
372	if (other_work->already_sent &&	15✔
373	other_work->identification.command == work->identification.command) {	6✔
374	if (_command_debugging) {	×
375	LogDebug() << "Command " << static_cast<int>(work->identification.command)	×
376	<< " is already being sent, waiting...";	×
377	}
378	already_being_sent = true;	×
379	break;	×
380	}
381	}
382
383	if (already_being_sent) {	13✔
384	continue;	×
385	}
386
387	// LogDebug() << "sending it the first time (" << work->mavlink_command << ")";
388	work->time_started = _system_impl.get_time().steady_time();	13✔
389
390	{
391	mavlink_message_t message = create_mavlink_message(work->command);	13✔
392	if (!_system_impl.send_message(message)) {	13✔
393	LogErr() << "connection send error (" << work->identification.command << ")";	×
394	// In this case we try again after the timeout. Chances are slim it will work next
395	// time though.
396	} else {
397	if (_command_debugging) {	13✔
398	LogDebug() << "Sent command " << static_cast<int>(work->identification.command);	×
399	}
400	}
401	}
402
403	work->already_sent = true;	13✔
404
405	_system_impl.register_timeout_handler(	26✔
406	[this, identification = work->identification] { receive_timeout(identification); },	15✔
407	work->timeout_s,	13✔
408	&work->timeout_cookie);	13✔
409	}
410	}	609✔
411
412	void MavlinkCommandSender::call_callback(	4✔
413	const CommandResultCallback& callback, Result result, float progress)
414	{
415	if (!callback) {	4✔
416	return;	×
417	}
418
419	// It seems that we need to queue the callback on the thread pool otherwise
420	// we lock ourselves out when we send a command in the callback receiving a command result.
421	auto temp_callback = callback;	8✔
422	_system_impl.call_user_callback(	8✔
423	[temp_callback, result, progress]() { temp_callback(result, progress); });
424	}
425
426	mavlink_message_t MavlinkCommandSender::create_mavlink_message(const Command& command)	15✔
427	{
428	mavlink_message_t message;
429
430	if (auto command_int = std::get_if<CommandInt>(&command)) {	15✔
431	mavlink_msg_command_int_pack(	×
432	_system_impl.get_own_system_id(),	×
433	_system_impl.get_own_component_id(),	×
434	&message,
435	command_int->target_system_id,	×
436	command_int->target_component_id,	×
437	command_int->frame,	×
438	command_int->command,	×
439	command_int->current,	×
440	command_int->autocontinue,	×
441	maybe_reserved(command_int->params.maybe_param1),	×
442	maybe_reserved(command_int->params.maybe_param2),	×
443	maybe_reserved(command_int->params.maybe_param3),	×
444	maybe_reserved(command_int->params.maybe_param4),	×
445	command_int->params.x,	×
446	command_int->params.y,	×
447	maybe_reserved(command_int->params.maybe_z));	×
448
449	} else if (auto command_long = std::get_if<CommandLong>(&command)) {	15✔
450	mavlink_msg_command_long_pack(	15✔
451	_system_impl.get_own_system_id(),	15✔
452	_system_impl.get_own_component_id(),	15✔
453	&message,
454	command_long->target_system_id,	15✔
455	command_long->target_component_id,	15✔
456	command_long->command,	15✔
457	command_long->confirmation,	15✔
458	maybe_reserved(command_long->params.maybe_param1),	15✔
459	maybe_reserved(command_long->params.maybe_param2),	15✔
460	maybe_reserved(command_long->params.maybe_param3),	15✔
461	maybe_reserved(command_long->params.maybe_param4),	15✔
462	maybe_reserved(command_long->params.maybe_param5),	15✔
463	maybe_reserved(command_long->params.maybe_param6),	15✔
464	maybe_reserved(command_long->params.maybe_param7));	15✔
465	}
466	return message;	15✔
467	}
468
469	float MavlinkCommandSender::maybe_reserved(const std::optional<float>& maybe_param) const	105✔
470	{
471	if (maybe_param) {	105✔
472	return maybe_param.value();	16✔
473
474	} else {
475	if (_system_impl.autopilot() == SystemImpl::Autopilot::ArduPilot) {	89✔
476	return 0.0f;	×
477	} else {
478	return NAN;	89✔
479	}
480	}
481	}
482
483	} // namespace mavsdk

mavlink / MAVSDK / 4899817366

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