8351061687

Committed 19 Mar 2024 10:26PM UTC coverage: 35.777% (+0.02%) from 35.755%

Build # 8351061687

Build Type

push

github

Committed by

web-flow

Commit Message

Ftp server : use a thread to send ftp burst (#2250)

* Ftp server : use a thread to send ftp burst

* system tests : increase FtpDownloadBurstStopAndTryAgain file length

FTP server is now faster, we need to increase file length because it was
sometimes already entirely received when the callback was called.

* Revert "system tests : increase FtpDownloadBurstStopAndTryAgain file length"

This reverts commit c233cdd2e.

* Test commit debug

* System tests: fix FtpDownloadBurstStopAndTryAgain

Only use intercept callbacks to simulation connection problem to avoid
buffering.

* windows

* remove debug

Run Details

32 of 35 new or added lines in 2 files covered. (91.43%)

7 existing lines in 2 files now uncovered.

10103 of 28239 relevant lines covered (35.78%)

120.7 hits per line

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

45.18

/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()
{
    _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 << ").";
                    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. 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;
            }

            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;
                _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();

        {
            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;

        _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); });
}

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

mavlink / MAVSDK / 8351061687

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