16537824143

Committed 26 Jul 2025 08:11AM UTC coverage: 46.312% (-0.02%) from 46.33%

Build # 16537824143

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2625 from mavlink/pr-noisy-commands

core: make command sender less verbose

Run Details

1 of 15 new or added lines in 1 file covered. (6.67%)

2 existing lines in 1 file now uncovered.

16267 of 35125 relevant lines covered (46.31%)

358.67 hits per line

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

51.62

/src/mavsdk/core/mavlink_command_sender.cpp

#include "mavlink_command_sender.h"
#include "mavlink_address.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()
{
    if (_command_debugging) {
        LogDebug() << "CommandSender destroyed";
    }
    _system_impl.unregister_all_mavlink_message_handlers(this);

    LockedQueue<Work>::Guard work_queue_guard(_work_queue);
    for (const auto& work : _work_queue) {
        _system_impl.unregister_timeout_handler(work->timeout_cookie);
    }
}

MavlinkCommandSender::Result MavlinkCommandSender::send_command(
    const MavlinkCommandSender::CommandInt& command, unsigned retries)
{
    // 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);
            }
        },
        retries);

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

MavlinkCommandSender::Result MavlinkCommandSender::send_command(
    const MavlinkCommandSender::CommandLong& command, unsigned retries)
{
    // 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);
            }
        },
        retries);

    return res.get();
}

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

    CommandIdentification identification = identification_from_command(command);

    {
        LockedQueue<Work>::Guard work_queue_guard(_work_queue);
        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->retries_to_do = retries;
    _work_queue.push_back(new_work);
}

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

    CommandIdentification identification = identification_from_command(command);

    {
        LockedQueue<Work>::Guard work_queue_guard(_work_queue);
        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();
    new_work->retries_to_do = retries;
    _work_queue.push_back(new_work);
}

void MavlinkCommandSender::receive_command_ack(const 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) {
        const 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 << ").";
                    if (work->identification.command == 512) {
                        LogDebug() << "(message " << work->identification.maybe_param1 << ")";
                    }
                }
                _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.
                _system_impl.unregister_timeout_handler(work->timeout_cookie);
                work->timeout_cookie = _system_impl.register_timeout_handler(
                    [this, identification = work->identification] {
                        receive_timeout(identification);
                    },
                    3.0);

                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)
{
    if (_command_debugging) {
        LogDebug() << "Got timeout!";
    }
    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) {
        const 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.
            if (_command_debugging) {
                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;
                }
            }

            if (!send_mavlink_message(work->command)) {
                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;
                work->timeout_cookie = _system_impl.register_timeout_handler(
                    [this, identification = work->identification] {
                        receive_timeout(identification);
                    },
                    work->timeout_s);
            }
        } else {
            // We have tried retransmitting, giving up now.
            if (work->identification.command == 512) {
                uint8_t target_sysid;
                uint8_t target_compid;
                if (auto command_int = std::get_if<CommandInt>(&work->command)) {
                    target_sysid = command_int->target_system_id;
                    target_compid = command_int->target_component_id;
                } else if (auto command_long = std::get_if<CommandLong>(&work->command)) {
                    target_sysid = command_long->target_system_id;
                    target_compid = command_long->target_component_id;
                } else {
                    LogErr() << "No command, that's awkward";
                    continue;
                }
                if (_command_debugging) {
                    LogWarn() << "Retrying failed for REQUEST_MESSAGE command for message: "
                              << work->identification.maybe_param1 << ", to ("
                              << std::to_string(target_sysid) << "/"
                              << std::to_string(target_compid) << ")";
                }
            } else {
                if (_command_debugging) {
                    LogWarn() << "Retrying failed for command: " << 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();

        {
            if (!send_mavlink_message(work->command)) {
                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;

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

void MavlinkCommandSender::call_callback(
    const CommandResultCallback& callback, Result result, float progress) const
{
    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); });
}

bool MavlinkCommandSender::send_mavlink_message(const Command& command) const
{
    if (auto command_int = std::get_if<CommandInt>(&command)) {
        return _system_impl.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
            mavlink_message_t message;
            mavlink_msg_command_int_pack_chan(
                mavlink_address.system_id,
                mavlink_address.component_id,
                channel,
                &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));
            return message;
        });

    } else if (auto command_long = std::get_if<CommandLong>(&command)) {
        return _system_impl.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
            mavlink_message_t message;
            mavlink_msg_command_long_pack_chan(
                mavlink_address.system_id,
                mavlink_address.component_id,
                channel,
                &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;
        });
    } else {
        return false;
    }
}

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

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

} // namespace mavsdk

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

mavlink / MAVSDK / 16537824143

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