From f64dd815d4c9cf5f805c1c6caf22551759b762ff Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Sep 2017 16:16:20 +0200 Subject: [PATCH 01/15] core: Added new locked list This is a simple wrapper around std::list to make it thread safe. The wrapper only implements a small subset of the features of std::list. --- CMakeLists.txt | 1 + core/locked_list.h | 85 +++++++++++++++++++++++++++++++++ core/locked_list_test.cpp | 98 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 184 insertions(+) create mode 100644 core/locked_list.h create mode 100644 core/locked_list_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d1ef377148..2c206b0d69 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -152,6 +152,7 @@ if(NOT IOS AND NOT ANDROID) core/unittests_main.cpp core/http_loader_test.cpp core/timeout_handler_test.cpp + core/locked_list_test.cpp ${plugin_unittest_source_files} ${unit_tests_src} ) diff --git a/core/locked_list.h b/core/locked_list.h new file mode 100644 index 0000000000..5654119936 --- /dev/null +++ b/core/locked_list.h @@ -0,0 +1,85 @@ +#pragma once + +#include +#include + +namespace dronecore { + +template +class LockedList +{ +public: + LockedList() : + _list(), + _mutex() + {}; + + class iterator: public std::list::iterator + { + public: + iterator(typename std::list::iterator c, std::mutex &mutex) : + std::list::iterator(c), + _mutex(&mutex) + {} + + T &operator*() + { + std::lock_guard lock(*_mutex); + return std::list::iterator::operator *(); + } + private: + std::mutex *_mutex; + }; + + iterator begin() + { + std::lock_guard lock(_mutex); + return iterator(_list.begin(), _mutex); + } + + iterator end() + { + std::lock_guard lock(_mutex); + return iterator(_list.end(), _mutex); + } + + iterator erase(iterator it) + { + std::lock_guard lock(_mutex); + return iterator(_list.erase(it), _mutex); + } + + void push_back(T item) + { + std::lock_guard lock(_mutex); + + _list.push_back(item); + } + + T &front() + { + std::lock_guard lock(_mutex); + + return _list.front(); + } + + void pop_front() + { + std::lock_guard lock(_mutex); + + _list.pop_front(); + } + + size_t size() + { + std::lock_guard lock(_mutex); + + return _list.size(); + } + +private: + std::list _list; + std::mutex _mutex; +}; + +} // namespace dronecore diff --git a/core/locked_list_test.cpp b/core/locked_list_test.cpp new file mode 100644 index 0000000000..a222f2d6dc --- /dev/null +++ b/core/locked_list_test.cpp @@ -0,0 +1,98 @@ +#include "locked_list.h" +#include + +using namespace dronecore; + +static const int LEN = 10; + + +TEST(LockedList, CreateAndDelete) +{ + LockedList locked_list; + EXPECT_EQ(locked_list.size(), 0); + + locked_list.push_back(1); + EXPECT_EQ(locked_list.size(), 1); + + locked_list.push_back(2); + EXPECT_EQ(locked_list.size(), 2); + + locked_list.pop_front(); + EXPECT_EQ(locked_list.size(), 1); + + locked_list.pop_front(); + EXPECT_EQ(locked_list.size(), 0); +} + +TEST(LockedList, Iterate) +{ + LockedList locked_list; + EXPECT_EQ(locked_list.size(), 0); + + for (int i = 0; i < LEN; ++i) { + locked_list.push_back(i); + } + EXPECT_EQ(locked_list.size(), LEN); + + int count = 0; + for (auto it = locked_list.begin(); it != locked_list.end(); ++it) { + ++count; + } + EXPECT_EQ(count, LEN); +} + +TEST(LockedList, RemoveInBetween) +{ + LockedList locked_list; + EXPECT_EQ(locked_list.size(), 0); + + for (int i = 0; i < LEN; ++i) { + locked_list.push_back(i); + EXPECT_EQ(locked_list.size(), i + 1); + } + + int count = 0; + for (auto it = locked_list.begin(); it != locked_list.end(); /*++it*/) { + if (*it == 5) { + it = locked_list.erase(it); + } else { + ++it; + } + ++count; + } + + EXPECT_EQ(locked_list.size(), LEN - 1); + EXPECT_EQ(count, LEN); +} + +TEST(LockedList, RemoveInBetweenAndAddAgain) +{ + LockedList locked_list; + EXPECT_EQ(locked_list.size(), 0); + + for (int i = 0; i < LEN; ++i) { + locked_list.push_back(i); + EXPECT_EQ(locked_list.size(), i + 1); + } + + int count = 0; + for (auto it = locked_list.begin(); it != locked_list.end(); /*++it*/) { + if (*it == 5) { + it = locked_list.erase(it); + locked_list.push_back(42); + } else { + ++it; + } + ++count; + } + + EXPECT_EQ(locked_list.size(), LEN); + EXPECT_EQ(count, LEN + 1); + + int last_one = 0; + for (auto it = locked_list.begin(); it != locked_list.end(); ++it) { + EXPECT_NE(*it, 5); + last_one = *it; + } + EXPECT_EQ(last_one, 42); +} From 1b3e4de89ecda49eda484fefbbb9f580d3465e9b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Sep 2017 10:42:26 +0200 Subject: [PATCH 02/15] Added .ycm_extr_conf.py for vim YCM This adds some completion and errors/warnings for vim with the YouCompleteMe plugin. --- .ycm_extra_conf.py | 206 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 206 insertions(+) create mode 100644 .ycm_extra_conf.py diff --git a/.ycm_extra_conf.py b/.ycm_extra_conf.py new file mode 100644 index 0000000000..a95735ab46 --- /dev/null +++ b/.ycm_extra_conf.py @@ -0,0 +1,206 @@ +# This file is NOT licensed under the GPLv3, which is the license for the rest +# of YouCompleteMe. +# +# Here's the license text for this file: +# +# This is free and unencumbered software released into the public domain. +# +# Anyone is free to copy, modify, publish, use, compile, sell, or +# distribute this software, either in source code form or as a compiled +# binary, for any purpose, commercial or non-commercial, and by any +# means. +# +# In jurisdictions that recognize copyright laws, the author or authors +# of this software dedicate any and all copyright interest in the +# software to the public domain. We make this dedication for the benefit +# of the public at large and to the detriment of our heirs and +# successors. We intend this dedication to be an overt act of +# relinquishment in perpetuity of all present and future rights to this +# software under copyright law. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +# OTHER DEALINGS IN THE SOFTWARE. +# +# For more information, please refer to + +import os +import ycm_core + +# These are the compilation flags that will be used in case there's no +# compilation database set (by default, one is not set). +# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR. +flags = [ +'-Wall', +'-Wextra', +'-Werror', +'-fno-exceptions', +'-Wshadow', +'-Wno-strict-aliasing', +# You 100% do NOT need -DUSE_CLANG_COMPLETER in your flags; only the YCM +# source code needs it. +'-DUSE_CLANG_COMPLETER', +# THIS IS IMPORTANT! Without a "-std=" flag, clang won't know which +# language to use when compiling headers. So it will guess. Badly. So C++ +# headers will be compiled as C headers. You don't want that so ALWAYS specify +# a "-std=". +# For a C project, you would set this to something like 'c99' instead of +# 'c++11'. +'-std=c++11', +# ...and the same thing goes for the magic -x option which specifies the +# language that the files to be compiled are written in. This is mostly +# relevant for c++ headers. +# For a C project, you would set this to 'c' instead of 'c++'. +'-x', +'c++', +'-isystem', +# This path will only work on OS X, but extra paths that don't exist are not +# harmful +'/System/Library/Frameworks/Python.framework/Headers', +'-isystem', +'../llvm/include', +'-isystem', +'../llvm/tools/clang/include', +'-I', +'.', +'-I', +'core', +'-I', +'include', +'-I', +'libs/include', +'-I', +'build/default/include', +'-I', +'build/default/core', +'-I', +'plugins/action', +'-I', +'plugins/info', +'-I', +'plugins/logging', +'-I', +'plugins/mission', +'-I', +'plugins/offboard', +'-I', +'plugins/telemetry', +'-I', +'./ClangCompleter', +'-isystem', +'./tests/gmock/gtest', +'-isystem', +'./tests/gmock/gtest/include', +'-isystem', +'./tests/gmock', +'-isystem', +'./tests/gmock/include', +] + + +# Set this to the absolute path to the folder (NOT the file!) containing the +# compile_commands.json file to use that instead of 'flags'. See here for +# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html +# +# You can get CMake to generate this file for you by adding: +# set( CMAKE_EXPORT_COMPILE_COMMANDS 1 ) +# to your CMakeLists.txt file. +# +# Most projects will NOT need to set this to anything; you can just change the +# 'flags' list of compilation flags. Notice that YCM itself uses that approach. +compilation_database_folder = '' + +if os.path.exists( compilation_database_folder ): + database = ycm_core.CompilationDatabase( compilation_database_folder ) +else: + database = None + +SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ] + +def DirectoryOfThisScript(): + return os.path.dirname( os.path.abspath( __file__ ) ) + + +def MakeRelativePathsInFlagsAbsolute( flags, working_directory ): + if not working_directory: + return list( flags ) + new_flags = [] + make_next_absolute = False + path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ] + for flag in flags: + new_flag = flag + + if make_next_absolute: + make_next_absolute = False + if not flag.startswith( '/' ): + new_flag = os.path.join( working_directory, flag ) + + for path_flag in path_flags: + if flag == path_flag: + make_next_absolute = True + break + + if flag.startswith( path_flag ): + path = flag[ len( path_flag ): ] + new_flag = path_flag + os.path.join( working_directory, path ) + break + + if new_flag: + new_flags.append( new_flag ) + return new_flags + + +def IsHeaderFile( filename ): + extension = os.path.splitext( filename )[ 1 ] + return extension in [ '.h', '.hxx', '.hpp', '.hh' ] + + +def GetCompilationInfoForFile( filename ): + # The compilation_commands.json file generated by CMake does not have entries + # for header files. So we do our best by asking the db for flags for a + # corresponding source file, if any. If one exists, the flags for that file + # should be good enough. + if IsHeaderFile( filename ): + basename = os.path.splitext( filename )[ 0 ] + for extension in SOURCE_EXTENSIONS: + replacement_file = basename + extension + if os.path.exists( replacement_file ): + compilation_info = database.GetCompilationInfoForFile( + replacement_file ) + if compilation_info.compiler_flags_: + return compilation_info + return None + return database.GetCompilationInfoForFile( filename ) + + +def FlagsForFile( filename, **kwargs ): + if database: + # Bear in mind that compilation_info.compiler_flags_ does NOT return a + # python list, but a "list-like" StringVec object + compilation_info = GetCompilationInfoForFile( filename ) + if not compilation_info: + return None + + final_flags = MakeRelativePathsInFlagsAbsolute( + compilation_info.compiler_flags_, + compilation_info.compiler_working_dir_ ) + + # NOTE: This is just for YouCompleteMe; it's highly likely that your project + # does NOT need to remove the stdlib flag. DO NOT USE THIS IN YOUR + # ycm_extra_conf IF YOU'RE NOT 100% SURE YOU NEED IT. + try: + final_flags.remove( '-stdlib=libc++' ) + except ValueError: + pass + else: + relative_to = DirectoryOfThisScript() + final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to ) + + return { + 'flags': final_flags, + 'do_cache': True + } From ba5e9f46a095f0834a057453bfd4dbfee94295e6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Sep 2017 11:02:00 +0200 Subject: [PATCH 03/15] gitignore: fix it so that ag understands it ag (the-silver-searcher) did not ignore the directories specified in gitignore presumably because they were wrong. --- .gitignore | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index 01bb6dd174..c8e47f5e0c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ # build folders -**/build -**/install -**/logs +build/* +install/* +logs/* From 64fceb328e2ffef0bb625929861c4a4e257a12e8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Sep 2017 14:12:22 +0200 Subject: [PATCH 04/15] log: make LogInfo blue --- core/log.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/core/log.h b/core/log.h index 2b25f04684..d33ea62b57 100644 --- a/core/log.h +++ b/core/log.h @@ -12,6 +12,7 @@ #define ANSI_COLOR_RED "\x1b[31m" #define ANSI_COLOR_YELLOW "\x1b[33m" +#define ANSI_COLOR_BLUE "\x1b[34m" #define ANSI_COLOR_GRAY "\x1b[37m" #define ANSI_COLOR_RESET "\x1b[0m" @@ -101,6 +102,7 @@ class LogDetailed std::cout << "|Debug] "; break; case LogLevel::Info: + std::cout << ANSI_COLOR_BLUE; std::cout << "|Info ] "; break; case LogLevel::Warn: @@ -117,9 +119,9 @@ class LogDetailed std::cout << " (" << _caller_filename << ":" << _caller_filenumber << ")"; switch (_log_level) { - case LogLevel::Info: - break; case LogLevel::Debug: + break; + case LogLevel::Info: // FALLTHROUGH case LogLevel::Warn: // FALLTHROUGH From 61c84cff36e63d8098c15d5a055154330aac28bb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Sep 2017 16:58:00 +0200 Subject: [PATCH 05/15] core: add update for TimeoutHandler This allows to change the timeout time of an existing (running) timeout. --- core/device_impl.cpp | 5 +++++ core/device_impl.h | 2 ++ core/timeout_handler.cpp | 18 +++++++++++++----- core/timeout_handler.h | 3 ++- core/timeout_handler_test.cpp | 25 +++++++++++++++++++++++++ 5 files changed, 47 insertions(+), 6 deletions(-) diff --git a/core/device_impl.cpp b/core/device_impl.cpp index ca3daf6e68..e249ae9214 100644 --- a/core/device_impl.cpp +++ b/core/device_impl.cpp @@ -79,6 +79,11 @@ void DeviceImpl::refresh_timeout_handler(const void *cookie) _timeout_handler.refresh(cookie); } +void DeviceImpl::update_timeout_handler(double new_duration_s, const void *cookie) +{ + _timeout_handler.update(new_duration_s, cookie); +} + void DeviceImpl::unregister_timeout_handler(const void *cookie) { _timeout_handler.remove(cookie); diff --git a/core/device_impl.h b/core/device_impl.h index 43e3aeee52..8323b513fd 100644 --- a/core/device_impl.h +++ b/core/device_impl.h @@ -40,6 +40,8 @@ class DeviceImpl void refresh_timeout_handler(const void *cookie); + void update_timeout_handler(double new_duration_s, const void *cookie); + void unregister_timeout_handler(const void *cookie); bool send_message(const mavlink_message_t &message); diff --git a/core/timeout_handler.cpp b/core/timeout_handler.cpp index be565e5510..07cdb9d8d1 100644 --- a/core/timeout_handler.cpp +++ b/core/timeout_handler.cpp @@ -14,7 +14,7 @@ void TimeoutHandler::add(std::function callback, double duration_s, void { auto new_timeout = std::make_shared(); new_timeout->callback = callback; - new_timeout->time = steady_time_in_future(duration_s); + new_timeout->start_time = steady_time(); new_timeout->duration_s = duration_s; void *new_cookie = static_cast(new_timeout.get()); @@ -35,8 +35,17 @@ void TimeoutHandler::refresh(const void *cookie) auto it = _timeouts.find((void *)(cookie)); if (it != _timeouts.end()) { - dl_time_t future_time = steady_time_in_future(it->second->duration_s); - it->second->time = future_time; + it->second->start_time = steady_time(); + } +} + +void TimeoutHandler::update(double new_duration_s, const void *cookie) +{ + std::lock_guard lock(_timeouts_mutex); + + auto it = _timeouts.find((void *)(cookie)); + if (it != _timeouts.end()) { + it->second->duration_s = new_duration_s; } } @@ -54,12 +63,11 @@ void TimeoutHandler::run_once() { _timeouts_mutex.lock(); - dl_time_t now = steady_time(); for (auto it = _timeouts.begin(); it != _timeouts.end(); /* no ++it */) { // If time is passed, call timeout callback. - if (it->second->time < now) { + if (elapsed_since_s(it->second->start_time) > it->second->duration_s) { if (it->second->callback) { diff --git a/core/timeout_handler.h b/core/timeout_handler.h index 27038824d3..312a6a2aea 100644 --- a/core/timeout_handler.h +++ b/core/timeout_handler.h @@ -22,6 +22,7 @@ class TimeoutHandler void add(std::function callback, double duration_s, void **cookie); void refresh(const void *cookie); + void update(double new_duration_s, const void *cookie); void remove(const void *cookie); void run_once(); @@ -29,7 +30,7 @@ class TimeoutHandler private: struct Timeout { std::function callback; - dl_time_t time; + dl_time_t start_time; double duration_s; }; diff --git a/core/timeout_handler_test.cpp b/core/timeout_handler_test.cpp index c86b40192f..09d51b19f9 100644 --- a/core/timeout_handler_test.cpp +++ b/core/timeout_handler_test.cpp @@ -138,3 +138,28 @@ TEST(TimeoutHandler, TimeoutRemovedDuringCallback) th.run_once(); EXPECT_TRUE(timeout_happened); } + +TEST(TimeoutHandler, TimeoutUpdate) +{ + TimeoutHandler th; + + bool timeout_happened = false; + + void *cookie = nullptr; + th.add([&timeout_happened]() { + timeout_happened = true; + }, 0.5, &cookie); + + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + th.run_once(); + EXPECT_FALSE(timeout_happened); + + th.update(1.0, cookie); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + th.run_once(); + EXPECT_FALSE(timeout_happened); + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + th.run_once(); + EXPECT_TRUE(timeout_happened); +} From 6565a554468721331dca1fdec0a39c796875b3a5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Sep 2017 17:02:42 +0200 Subject: [PATCH 06/15] locked_list: commented that this is not being used --- core/locked_list.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/core/locked_list.h b/core/locked_list.h index 5654119936..796de8346f 100644 --- a/core/locked_list.h +++ b/core/locked_list.h @@ -5,6 +5,9 @@ namespace dronecore { +// Note: this wrapper around list was developed for `MavlinkCommands` but was not used +// eventually. Instead, `std::list` was used directly and protected with a `std::mutex`. + template class LockedList { From c909d1aa36a16e3283303f598476f014d428da39 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Sep 2017 17:03:16 +0200 Subject: [PATCH 07/15] mavlink_commands: enable commands in parallel This is a big refactor of the MavlinkCommands class in order to support multiple commands in parallel. It won't do multiple commands with the same command ID at the same time in order to prevent conflicts/races with acks that can't be assigned uniquely. --- core/mavlink_commands.cpp | 331 +++++++++++++++++++------------------- core/mavlink_commands.h | 26 ++- 2 files changed, 179 insertions(+), 178 deletions(-) diff --git a/core/mavlink_commands.cpp b/core/mavlink_commands.cpp index 08f4f7d9c8..33afd88039 100644 --- a/core/mavlink_commands.cpp +++ b/core/mavlink_commands.cpp @@ -2,20 +2,13 @@ #include "device_impl.h" #include #include +#include +#include namespace dronecore { -// TODO: Currently the mavlink command handling is made in a way to only -// process one command at any time. Therefore, the work state is global -// for this whole class. -// 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. + +// TODO: we need to handle duplicate commands. MavlinkCommands::MavlinkCommands(DeviceImpl *parent) : _parent(parent) @@ -29,6 +22,15 @@ MavlinkCommands::MavlinkCommands(DeviceImpl *parent) : MavlinkCommands::~MavlinkCommands() { _parent->unregister_all_mavlink_message_handlers((void *)this); + + std::lock_guard lock(_mutex); + + for (auto it = _work_list.begin(); it != _work_list.end(); /* ++it */) { + if ((*it)->timeout_cookie != nullptr) { + _parent->unregister_timeout_handler((*it)->timeout_cookie); + it = _work_list.erase(it); + } + } } MavlinkCommands::Result MavlinkCommands::send_command(uint16_t command, @@ -61,10 +63,13 @@ MavlinkCommands::Result MavlinkCommands::send_command(uint16_t command, PromiseResult promise_result = res.get(); - if (promise_result.result == Result::IN_PROGRESS) { - LogInfo() << "In progress: " << promise_result.progress; - continue; - } + // We should not get notified for progress because `std::future` does not support + // being called multiple times. + + //if (promise_result.result == Result::IN_PROGRESS) { + // LogInfo() << "In progress: " << promise_result.progress; + // continue; + //} return promise_result.result; } } @@ -79,195 +84,193 @@ void MavlinkCommands::queue_command_async(uint16_t command, // LogDebug() << "Command " << (int)command << " to send to " << (int)target_system_id << ", " // << (int)target_component_id; - Work new_work {}; + std::lock_guard lock(_mutex); + + auto new_work = std::make_shared(); mavlink_msg_command_long_pack(_parent->get_own_system_id(), _parent->get_own_component_id(), - &new_work.mavlink_message, + &new_work->mavlink_message, target_system_id, target_component_id, command, 0, params.v[0], params.v[1], params.v[2], params.v[3], params.v[4], params.v[5], params.v[6]); - new_work.callback = callback; - new_work.mavlink_command = command; - _work_queue.push_back(new_work); + new_work->callback = callback; + new_work->mavlink_command = command; + _work_list.push_back(new_work); } void MavlinkCommands::receive_command_ack(mavlink_message_t message) { - // If nothing is in the queue, we ignore the message all together. - if (_work_queue.size() == 0) { - return; - } - - Work &work = _work_queue.front(); - mavlink_command_ack_t command_ack; mavlink_msg_command_ack_decode(&message, &command_ack); - // LogDebug() << "We got an ack: " << command_ack.command; - 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; + _mutex.lock(); + + // If nothing is in the queue, we ignore the message all together. + if (_work_list.size() == 0) { + LogDebug() << "Ignoring command ack (" << command_ack.command << ")"; + _mutex.unlock(); return; } - std::lock_guard lock(_state_mutex); - switch (command_ack.result) { - case MAV_RESULT_ACCEPTED: - _state = State::DONE; - if (work.callback) { - work.callback(Result::SUCCESS, 1.0f); - } - break; - - case MAV_RESULT_DENIED: - LogWarn() << "command denied (" << work.mavlink_command << ")."; - // FALLTHRU - - case MAV_RESULT_UNSUPPORTED: - LogWarn() << "command unsupported (" << work.mavlink_command << ")."; - // FALLTHRU - - case MAV_RESULT_TEMPORARILY_REJECTED: - LogWarn() << "command temporarily rejected (" << work.mavlink_command << ")."; - // FALLTHRU - case MAV_RESULT_FAILED: - LogWarn() << "command failed (" << work.mavlink_command << ")."; - _state = State::FAILED; - if (work.callback) { - work.callback(Result::COMMAND_DENIED, NAN); + // Go through all commands that require a response: + for (auto it = _work_list.begin(); it != _work_list.end(); /* ++it */) { + + if ((*it)->mavlink_command == command_ack.command) { + // If the command does not match with our current command, ignore it. + LogDebug() << "Matched ack with command: " << (int)(*it)->mavlink_command; + + switch (command_ack.result) { + case MAV_RESULT_ACCEPTED: + _parent->unregister_timeout_handler((*it)->timeout_cookie); + if ((*it)->callback) { + auto callback_tmp = (*it)->callback; + _mutex.unlock(); + callback_tmp(Result::SUCCESS, 1.0f); + _mutex.lock(); + } + it = _work_list.erase(it); + continue; + + case MAV_RESULT_DENIED: + LogWarn() << "command denied (" << (*it)->mavlink_command << ")."; + // FALLTHRU + + case MAV_RESULT_UNSUPPORTED: + LogWarn() << "command unsupported (" << (*it)->mavlink_command << ")."; + // FALLTHRU + + case MAV_RESULT_TEMPORARILY_REJECTED: + LogWarn() << "command temporarily rejected (" << (*it)->mavlink_command << ")."; + // FALLTHRU + case MAV_RESULT_FAILED: + _parent->unregister_timeout_handler((*it)->timeout_cookie); + LogWarn() << "command failed (" << (*it)->mavlink_command << ")."; + if ((*it)->callback) { + auto callback_tmp = (*it)->callback; + _mutex.unlock(); + callback_tmp(Result::COMMAND_DENIED, NAN); + _mutex.lock(); + } + it = _work_list.erase(it); + continue; + + case MAV_RESULT_IN_PROGRESS: + if ((int)command_ack.progress != 255) { + LogInfo() << "progress: " << (int)command_ack.progress + << " % (" << (*it)->mavlink_command << ")."; + } + // FIXME: We can only call callbacks with promises once, so let's not do it + // on IN_PROGRESS for now. + //if (callback) { + // callback(Result::IN_PROGRESS, command_ack.progress / 100.0f); + //} + + // 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. + _parent->refresh_timeout_handler((*it)->timeout_cookie); + _parent->update_timeout_handler(DEFAULT_TIMEOUT_IN_PROGRESS_S, + (*it)->timeout_cookie); + break; } - break; - - case MAV_RESULT_IN_PROGRESS: - if ((int)command_ack.progress != 255) { - LogInfo() << "progress: " << (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. - //if (work.callback) { - // work.callback(Result::IN_PROGRESS, command_ack.progress / 100.0f); - //} - _state = State::IN_PROGRESS; - // 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. - _parent->unregister_timeout_handler(_timeout_cookie); - _parent->register_timeout_handler( - std::bind(&MavlinkCommands::receive_timeout, this), - work.retries_to_do * work.timeout_s, &_timeout_cookie); - break; + } + ++it; } + + _mutex.unlock(); } -void MavlinkCommands::receive_timeout() +void MavlinkCommands::do_work() { - // If nothing is in the queue, we ignore the timeout. - if (_work_queue.size() == 0) { + _mutex.lock(); + + // Check if there is work to do. + if (_work_list.size() == 0) { + // Nothing to do. + _mutex.unlock(); return; } - Work &work = _work_queue.front(); + // We support multiple commands at the same time but we only ever want to + // have one command_id active at any time because otherwise, we have no idea which + // ack belongs to what sent message. + std::vector active_command_ids {}; - std::lock_guard lock(_state_mutex); + for (auto it = _work_list.begin(); it != _work_list.end(); /* ++it */) { - if (_state == State::WAITING) { + // We ignore what has already been sent and is not timed out yet. + if ((*it)->num_sent != 0 && !(*it)->timed_out) { + // Mark it as active. + active_command_ids.push_back((*it)->mavlink_command); - if (work.retries_to_do > 0) { + ++it; + continue; + } - LogInfo() << "sending again, retries to do: " << work.retries_to_do - << " (" << work.mavlink_command << ")."; - // We're not sure the command arrived, let's retransmit. - if (!_parent->send_message(work.mavlink_message)) { - LogErr() << "connection send error in retransmit (" << work.mavlink_command << ")."; - if (work.callback) { - work.callback(Result::CONNECTION_ERROR, NAN); - } - _state = State::FAILED; - } else { - --work.retries_to_do; - _parent->register_timeout_handler( - std::bind(&MavlinkCommands::receive_timeout, this), - work.timeout_s, &_timeout_cookie); + // We remove what has already been resent a few times. + if ((*it)->timed_out && (*it)->num_sent >= RETRIES) { + if ((*it)->callback) { + auto callback_tmp = (*it)->callback; + _mutex.unlock(); + callback_tmp(Result::TIMEOUT, NAN); + _mutex.lock(); } + it = _work_list.erase(it); + continue; + } - } else { - // We have tried retransmitting, giving up now. - LogErr() << "Retrying failed (" << work.mavlink_command << ")"; + // Let's check if this command ID is already being sent. + auto found_command_id = + (find(active_command_ids.begin(), active_command_ids.end(), (*it)->mavlink_command) + != active_command_ids.end()); - if (work.callback) { - if (_state == State::WAITING) { - work.callback(Result::TIMEOUT, NAN); - } + if (found_command_id) { + LogDebug() << "We need to wait to send " << (int)(*it)->mavlink_command; + ++it; + continue; + } + + // Now we can actually send it. + LogDebug() << "Sending command " << (*it)->num_sent + << " time (" << (int)(*it)->mavlink_command << ")"; + + if (!_parent->send_message((*it)->mavlink_message)) { + LogErr() << "connection send error (" << (*it)->mavlink_command << ")"; + if ((*it)->callback) { + auto callback_tmp = (*it)->callback; + _mutex.unlock(); + callback_tmp(Result::CONNECTION_ERROR, NAN); + _mutex.lock(); } - _state = State::FAILED; + // We tried and failed badly, no incentive to retry at this point. + // Let's delete the entry. + it = _work_list.erase(it); + continue; } - } -} -void MavlinkCommands::do_work() -{ - std::lock_guard lock(_state_mutex); - - // Clean up first - switch (_state) { - case State::NONE: - // FALLTHROUGH - case State::WAITING: - // FALLTHROUGH - case State::IN_PROGRESS: - break; - case State::DONE: - // FALLTHROUGH - case State::FAILED: - _parent->unregister_timeout_handler(_timeout_cookie); - _work_queue.pop_front(); - _state = State::NONE; - break; - } + ++((*it)->num_sent); - // Check if there is work to do. - if (_work_queue.size() == 0) { - // Nothing to do. - return; - } + std::shared_ptr work_ptr = *it; + // Let's set a timer for retransmission if needed. + _parent->register_timeout_handler( + std::bind([work_ptr]() { + // Send it again later, or remove it later. + LogDebug() << "timeout, need to send it again, ore remove it later."; + work_ptr->timed_out = true; + }), + (*it)->timeout_s, &((*it)->timeout_cookie)); - // If so, let's get the latest. - Work &work = _work_queue.front(); - - // If the work state is none, we can start the next command. - switch (_state) { - case State::NONE: - // LogDebug() << "sending it the first time (" << work.mavlink_command << ")"; - if (!_parent->send_message(work.mavlink_message)) { - LogErr() << "connection send error (" << work.mavlink_command << ")"; - if (work.callback) { - work.callback(Result::CONNECTION_ERROR, NAN); - } - _state = State::FAILED; - break; - } else { - _state = State::WAITING; - _parent->register_timeout_handler( - std::bind(&MavlinkCommands::receive_timeout, this), - work.timeout_s, &_timeout_cookie); - } - break; - case State::WAITING: - case State::IN_PROGRESS: - // LogWarn() << "wait until we can deal with this"; - break; - case State::DONE: - // FALLTHROUGH - case State::FAILED: - break; + active_command_ids.push_back((*it)->mavlink_command); + + ++it; } + _mutex.unlock(); } diff --git a/core/mavlink_commands.h b/core/mavlink_commands.h index 028eda1809..f1a8102005 100644 --- a/core/mavlink_commands.h +++ b/core/mavlink_commands.h @@ -1,11 +1,12 @@ #pragma once #include "mavlink_include.h" -#include "locked_queue.h" #include #include #include #include +#include +#include namespace dronecore { @@ -53,30 +54,27 @@ class MavlinkCommands const MavlinkCommands &operator=(const MavlinkCommands &) = delete; private: - enum class State { - NONE, - WAITING, - IN_PROGRESS, - DONE, - FAILED - } _state = State::NONE; - std::mutex _state_mutex {}; + static constexpr double DEFAULT_TIMEOUT_NORMAL_S = 0.5; + static constexpr double DEFAULT_TIMEOUT_IN_PROGRESS_S = 3.0; + static constexpr int RETRIES = 3; struct Work { - int retries_to_do = 3; - double timeout_s = 0.5; + int num_sent = 0; + bool timed_out = true; + double timeout_s = DEFAULT_TIMEOUT_NORMAL_S; uint16_t mavlink_command = 0; mavlink_message_t mavlink_message {}; command_result_callback_t callback {}; + void *timeout_cookie = nullptr; }; + void receive_command_ack(mavlink_message_t message); - void receive_timeout(); DeviceImpl *_parent; - LockedQueue _work_queue {}; - void *_timeout_cookie = nullptr; + std::mutex _mutex {}; + std::list> _work_list {}; }; } // namespace dronecore From 752bee39f79e1b99848641e2e81d5a384534ff05 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Sep 2017 17:07:03 +0200 Subject: [PATCH 08/15] async_hover: improve debug outputs --- integration_tests/async_hover.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/integration_tests/async_hover.cpp b/integration_tests/async_hover.cpp index b47b865ff2..027f8a7a99 100644 --- a/integration_tests/async_hover.cpp +++ b/integration_tests/async_hover.cpp @@ -30,22 +30,26 @@ TEST_F(SitlTest, ActionAsyncHover) device.telemetry().in_air_async(std::bind(&receive_in_air, _1)); while (!_all_ok) { - std::cout << "Waiting to be ready..." << std::endl; + LogInfo() << "Waiting to be ready..."; std::this_thread::sleep_for(std::chrono::seconds(1)); } + LogInfo() << "Arming..."; device.action().arm_async(std::bind(&receive_result, _1)); std::this_thread::sleep_for(std::chrono::seconds(2)); + LogInfo() << "Setting takeoff altitude..."; device.action().set_takeoff_altitude(5.0f); + LogInfo() << "Taking off..."; device.action().takeoff_async(std::bind(&receive_result, _1)); std::this_thread::sleep_for(std::chrono::seconds(5)); + LogInfo() << "Landing..."; device.action().land_async(std::bind(&receive_result, _1)); while (_in_air) { - std::cout << "Waiting to be landed..." << std::endl; + LogInfo() << "Waiting to be landed..."; std::this_thread::sleep_for(std::chrono::seconds(1)); } From 4d9a39e4597074e9ff15f4607db204f77f82781e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Sep 2017 14:16:39 +0200 Subject: [PATCH 09/15] core: added commented out printf --- core/dronecore_impl.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/dronecore_impl.cpp b/core/dronecore_impl.cpp index 789bdfaea1..92cfa48543 100644 --- a/core/dronecore_impl.cpp +++ b/core/dronecore_impl.cpp @@ -62,6 +62,8 @@ void DroneCoreImpl::receive_message(const mavlink_message_t &message) return; } + // LogDebug() << "Received " << (int)message.msgid; + std::lock_guard lock(_devices_mutex); // Change system id of null device From ac024a1e11a3cb25414c39b9696d607e7c2f9c28 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Sep 2017 14:16:58 +0200 Subject: [PATCH 10/15] core: remove unneeded while (true) --- core/mavlink_commands.cpp | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/core/mavlink_commands.cpp b/core/mavlink_commands.cpp index 33afd88039..237cc97504 100644 --- a/core/mavlink_commands.cpp +++ b/core/mavlink_commands.cpp @@ -57,21 +57,19 @@ MavlinkCommands::Result MavlinkCommands::send_command(uint16_t command, ); std::future res = prom->get_future(); - while (true) { - // Block now to wait for result. - res.wait(); + // Block now to wait for result. + res.wait(); - PromiseResult promise_result = res.get(); + PromiseResult promise_result = res.get(); - // We should not get notified for progress because `std::future` does not support - // being called multiple times. + // We should not get notified for progress because `std::future` does not support + // being called multiple times. - //if (promise_result.result == Result::IN_PROGRESS) { - // LogInfo() << "In progress: " << promise_result.progress; - // continue; - //} - return promise_result.result; - } + //if (promise_result.result == Result::IN_PROGRESS) { + // LogInfo() << "In progress: " << promise_result.progress; + // continue; + //} + return promise_result.result; } From 438ad05528aa848e8aaf0c03067ac68e3fa8c5b9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Sep 2017 14:18:09 +0200 Subject: [PATCH 11/15] core: comment out a printf --- core/mavlink_commands.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/mavlink_commands.cpp b/core/mavlink_commands.cpp index 237cc97504..2799e9d5ce 100644 --- a/core/mavlink_commands.cpp +++ b/core/mavlink_commands.cpp @@ -229,7 +229,7 @@ void MavlinkCommands::do_work() != active_command_ids.end()); if (found_command_id) { - LogDebug() << "We need to wait to send " << (int)(*it)->mavlink_command; + // LogDebug() << "We need to wait to send " << (int)(*it)->mavlink_command; ++it; continue; } From 6e910a2eeb4dd7bde99dfc5dba13476fbd730d73 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Sep 2017 14:18:20 +0200 Subject: [PATCH 12/15] core: don't forget to reset timeout flag --- core/mavlink_commands.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/mavlink_commands.cpp b/core/mavlink_commands.cpp index 2799e9d5ce..28e955abfd 100644 --- a/core/mavlink_commands.cpp +++ b/core/mavlink_commands.cpp @@ -238,6 +238,8 @@ void MavlinkCommands::do_work() LogDebug() << "Sending command " << (*it)->num_sent << " time (" << (int)(*it)->mavlink_command << ")"; + // Reset timeout flag for next re-send. + (*it)->timed_out = false; if (!_parent->send_message((*it)->mavlink_message)) { LogErr() << "connection send error (" << (*it)->mavlink_command << ")"; if ((*it)->callback) { From 173ac1a952442a301f96045836391dd459600ac8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Sep 2017 12:19:15 +0200 Subject: [PATCH 13/15] locked_list: use better variable name --- core/locked_list.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/locked_list.h b/core/locked_list.h index 796de8346f..190e213db0 100644 --- a/core/locked_list.h +++ b/core/locked_list.h @@ -20,8 +20,8 @@ class LockedList class iterator: public std::list::iterator { public: - iterator(typename std::list::iterator c, std::mutex &mutex) : - std::list::iterator(c), + iterator(typename std::list::iterator iter, std::mutex &mutex) : + std::list::iterator(iter), _mutex(&mutex) {} From a746eabc9678d71d7036442caee487e15fc11e96 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Sep 2017 12:22:10 +0200 Subject: [PATCH 14/15] core: improve variable name --- core/mavlink_commands.cpp | 8 ++++---- core/mavlink_commands.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/core/mavlink_commands.cpp b/core/mavlink_commands.cpp index 28e955abfd..a1b461c2c3 100644 --- a/core/mavlink_commands.cpp +++ b/core/mavlink_commands.cpp @@ -203,7 +203,7 @@ void MavlinkCommands::do_work() for (auto it = _work_list.begin(); it != _work_list.end(); /* ++it */) { // We ignore what has already been sent and is not timed out yet. - if ((*it)->num_sent != 0 && !(*it)->timed_out) { + if ((*it)->num_command_sent != 0 && !(*it)->timed_out) { // Mark it as active. active_command_ids.push_back((*it)->mavlink_command); @@ -212,7 +212,7 @@ void MavlinkCommands::do_work() } // We remove what has already been resent a few times. - if ((*it)->timed_out && (*it)->num_sent >= RETRIES) { + if ((*it)->timed_out && (*it)->num_command_sent >= RETRIES) { if ((*it)->callback) { auto callback_tmp = (*it)->callback; _mutex.unlock(); @@ -235,7 +235,7 @@ void MavlinkCommands::do_work() } // Now we can actually send it. - LogDebug() << "Sending command " << (*it)->num_sent + LogDebug() << "Sending command " << (*it)->num_command_sent << " time (" << (int)(*it)->mavlink_command << ")"; // Reset timeout flag for next re-send. @@ -254,7 +254,7 @@ void MavlinkCommands::do_work() continue; } - ++((*it)->num_sent); + ++((*it)->num_command_sent); std::shared_ptr work_ptr = *it; // Let's set a timer for retransmission if needed. diff --git a/core/mavlink_commands.h b/core/mavlink_commands.h index f1a8102005..de132451c5 100644 --- a/core/mavlink_commands.h +++ b/core/mavlink_commands.h @@ -59,7 +59,7 @@ class MavlinkCommands static constexpr int RETRIES = 3; struct Work { - int num_sent = 0; + int num_command_sent = 0; bool timed_out = true; double timeout_s = DEFAULT_TIMEOUT_NORMAL_S; uint16_t mavlink_command = 0; From ea4cdb818212efdfe5ad2dc0b7390addb4ff55cd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Sep 2017 12:23:39 +0200 Subject: [PATCH 15/15] core: improve variable name --- core/timeout_handler.cpp | 4 ++-- core/timeout_handler.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/core/timeout_handler.cpp b/core/timeout_handler.cpp index 07cdb9d8d1..d36a7c221a 100644 --- a/core/timeout_handler.cpp +++ b/core/timeout_handler.cpp @@ -39,13 +39,13 @@ void TimeoutHandler::refresh(const void *cookie) } } -void TimeoutHandler::update(double new_duration_s, const void *cookie) +void TimeoutHandler::update(double updated_duration_s, const void *cookie) { std::lock_guard lock(_timeouts_mutex); auto it = _timeouts.find((void *)(cookie)); if (it != _timeouts.end()) { - it->second->duration_s = new_duration_s; + it->second->duration_s = updated_duration_s; } } diff --git a/core/timeout_handler.h b/core/timeout_handler.h index 312a6a2aea..d8ba478ec6 100644 --- a/core/timeout_handler.h +++ b/core/timeout_handler.h @@ -22,7 +22,7 @@ class TimeoutHandler void add(std::function callback, double duration_s, void **cookie); void refresh(const void *cookie); - void update(double new_duration_s, const void *cookie); + void update(double updated_duration_s, const void *cookie); void remove(const void *cookie); void run_once();