10732491566

Committed 06 Sep 2024 04:34AM UTC coverage: 37.95% (-0.1%) from 38.066%

Build # 10732491566

Build Type

Pull #2393

github

Committed by

web-flow

Commit Message

Merge 96dc65845 into 6106bf78c

Pull Request Pull Request #2393: CI: a few fixes and adding newer compilers

Run Details

11416 of 30082 relevant lines covered (37.95%)

265.81 hits per line

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

47.65

/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)
{
    // 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 " << 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);

    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 " << 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);

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

mavlink / MAVSDK / 10732491566

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