Skip to content

Commit

Permalink
Merge pull request #555 from Dronecode/fix-command-lockup
Browse files Browse the repository at this point in the history 
Try to prevent command lockup
  • Loading branch information
@JonasVautherin
JonasVautherin authored Sep 22, 2018
2 parents 8a35f37 + 397acae commit c022f1c
Show file tree
Hide file tree
Showing 2 changed files with 34 additions and 25 deletions.
57 changes: 33 additions & 24 deletions core/mavlink_commands.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,6 @@ namespace dronecode_sdk {
// The limitation is made because:
// - We are not sure what exactly will happen if the commands are sent in parallel
// and what kind of edge cases we might run into.
// - The timeout handler only supports the (void *)this cookie and therefore only
// really supports one timeout per object. We could use (void *)this of the work
// item but it also seems a bit dodgy.
// - The queue used does not support going through and checking each and every
// item yet.

Expand Down Expand Up @@ -139,7 +136,8 @@ void MAVLinkCommands::receive_command_ack(mavlink_message_t message)

if (work->mavlink_command != command_ack.command) {
// If the command does not match with our current command, ignore it.
LogWarn() << "Command ack not matching our current command: " << work->mavlink_command;
LogWarn() << "Command ack " << int(command_ack.command)
<< " not matching our current command: " << work->mavlink_command;
_work_queue.return_front();
return;
}
Expand All @@ -149,55 +147,51 @@ void MAVLinkCommands::receive_command_ack(mavlink_message_t message)
case MAV_RESULT_ACCEPTED:
_state = State::NONE;
_state_mutex.unlock();
call_callback(work->callback, Result::SUCCESS, 1.0f);
_work_queue.pop_front();
_parent.unregister_timeout_handler(_timeout_cookie);
_work_queue.pop_front();
call_callback(work->callback, Result::SUCCESS, 1.0f);
break;

case MAV_RESULT_DENIED:
LogWarn() << "command denied (" << work->mavlink_command << ").";
_state = State::NONE;
_state_mutex.unlock();
call_callback(work->callback, Result::COMMAND_DENIED, NAN);
_work_queue.pop_front();
_parent.unregister_timeout_handler(_timeout_cookie);
_work_queue.pop_front();
call_callback(work->callback, Result::COMMAND_DENIED, NAN);
break;

case MAV_RESULT_UNSUPPORTED:
LogWarn() << "command unsupported (" << work->mavlink_command << ").";
_state = State::NONE;
_state_mutex.unlock();
call_callback(work->callback, Result::COMMAND_DENIED, NAN);
_work_queue.pop_front();
_parent.unregister_timeout_handler(_timeout_cookie);
_work_queue.pop_front();
call_callback(work->callback, Result::COMMAND_DENIED, NAN);
break;

case MAV_RESULT_TEMPORARILY_REJECTED:
LogWarn() << "command temporarily rejected (" << work->mavlink_command << ").";
_state = State::NONE;
_state_mutex.unlock();
call_callback(work->callback, Result::COMMAND_DENIED, NAN);
_work_queue.pop_front();
_parent.unregister_timeout_handler(_timeout_cookie);
_work_queue.pop_front();
call_callback(work->callback, Result::COMMAND_DENIED, NAN);
break;

case MAV_RESULT_FAILED:
_state = State::NONE;
_state_mutex.unlock();
call_callback(work->callback, Result::COMMAND_DENIED, NAN);
_work_queue.pop_front();
_parent.unregister_timeout_handler(_timeout_cookie);
_work_queue.pop_front();
call_callback(work->callback, Result::COMMAND_DENIED, NAN);
break;

case MAV_RESULT_IN_PROGRESS:
if (static_cast<int>(command_ack.progress) != 255) {
LogInfo() << "progress: " << static_cast<int>(command_ack.progress) << " % ("
<< work->mavlink_command << ").";
}
// FIXME: We can only call callbacks with promises once, so let's not do it
// on IN_PROGRESS.
// call_callback(work->callback, Result::IN_PROGRESS, command_ack.progress /
// 100.0f);
_state = State::IN_PROGRESS;
_state_mutex.unlock();
// If we get a progress update, we can raise the timeout
Expand All @@ -210,6 +204,10 @@ void MAVLinkCommands::receive_command_ack(mavlink_message_t message)
work->retries_to_do * work->timeout_s,
&_timeout_cookie);
_work_queue.return_front();
// FIXME: We can only call callbacks with promises once, so let's not do it
// on IN_PROGRESS.
// call_callback(work->callback, Result::IN_PROGRESS, command_ack.progress /
// 100.0f);
break;

default:
Expand All @@ -233,7 +231,15 @@ void MAVLinkCommands::receive_timeout()
auto work = _work_queue.borrow_front();

if (!work) {
// Nevermind, there is nothing to do.
// FIXME:
// There is nothing to do.
// However, we probably shouldn't be in State::WAITING if nothing is in the queue.
// Otherwise, we might wait forever because we don't register a new timeout.
// What's odd is that from my code review, it should not be possible to ever end up
// in State::WAITING with an empty queue, especially since the state is protected by
// a mutex.
_state = State::NONE;
LogErr() << "Received timeout without item in queue.";
return;
}

Expand All @@ -243,9 +249,9 @@ void MAVLinkCommands::receive_timeout()
<< work->mavlink_command << ").";
if (!_parent.send_message(work->mavlink_message)) {
LogErr() << "connection send error in retransmit (" << work->mavlink_command << ").";
call_callback(work->callback, Result::CONNECTION_ERROR, NAN);
_state = State::NONE;
_work_queue.pop_front();
call_callback(work->callback, Result::CONNECTION_ERROR, NAN);

} else {
--work->retries_to_do;
Expand All @@ -259,11 +265,14 @@ void MAVLinkCommands::receive_timeout()
// We have tried retransmitting, giving up now.
LogErr() << "Retrying failed (" << work->mavlink_command << ")";

if (_state == State::WAITING) {
call_callback(work->callback, Result::TIMEOUT, NAN);
}
const bool had_been_waiting = (_state == State::WAITING);

_state = State::NONE;
_work_queue.pop_front();

if (had_been_waiting) {
call_callback(work->callback, Result::TIMEOUT, NAN);
}
}
}

Expand All @@ -283,9 +292,9 @@ void MAVLinkCommands::do_work()
// LogDebug() << "sending it the first time (" << work->mavlink_command << ")";
if (!_parent.send_message(work->mavlink_message)) {
LogErr() << "connection send error (" << work->mavlink_command << ")";
call_callback(work->callback, Result::CONNECTION_ERROR, NAN);
_work_queue.pop_front();
_state = State::NONE;
call_callback(work->callback, Result::CONNECTION_ERROR, NAN);
} else {
_state = State::WAITING;
_parent.register_timeout_handler(std::bind(&MAVLinkCommands::receive_timeout, this),
Expand Down
2 changes: 1 addition & 1 deletion version
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
0.2.7
0.2.8

0 comments on commit c022f1c

Please sign in to comment.