diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index cdaea47489..fd7b6fbc8c 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -23,6 +23,7 @@ add_library(mavsdk mavlink_parameters.cpp mavlink_receiver.cpp mavlink_message_handler.cpp + ping.cpp plugin_impl_base.cpp serial_connection.cpp tcp_connection.cpp diff --git a/src/core/mavlink_commands.h b/src/core/mavlink_commands.h index fa44fe5313..d7b39ee601 100644 --- a/src/core/mavlink_commands.h +++ b/src/core/mavlink_commands.h @@ -122,6 +122,8 @@ class MAVLinkCommands { void call_callback(const CommandResultCallback& callback, Result result, float progress); + static constexpr double _timeout_factor = 3.0; + SystemImpl& _parent; LockedQueue _work_queue{}; diff --git a/src/core/ping.cpp b/src/core/ping.cpp new file mode 100644 index 0000000000..ea67736e13 --- /dev/null +++ b/src/core/ping.cpp @@ -0,0 +1,75 @@ +#include "log.h" +#include "ping.h" +#include "system_impl.h" + +namespace mavsdk { + +Ping::Ping(SystemImpl& system_impl) : _system_impl(system_impl) +{ + _system_impl.register_mavlink_message_handler( + MAVLINK_MSG_ID_PING, + [this](const mavlink_message_t& message) { Ping::process_ping(message); }, + this); +} + +Ping::~Ping() +{ + _system_impl.unregister_all_mavlink_message_handlers(this); +} + +void Ping::run_once() +{ + mavlink_message_t message; + + uint64_t now = static_cast(_system_impl.get_time().elapsed_s() * 1e6); + + mavlink_msg_ping_pack( + _system_impl.get_own_system_id(), + _system_impl.get_own_component_id(), + &message, + now, + _ping_sequence, + 0, + 0); // to all + + _system_impl.send_message(message); +} + +void Ping::process_ping(const mavlink_message_t& message) +{ + mavlink_ping_t ping; + mavlink_msg_ping_decode(&message, &ping); + + if (ping.target_system == 0 && ping.target_component == 0) { + // Response to ping request. + mavlink_message_t response_message; + mavlink_msg_ping_pack( + _system_impl.get_own_system_id(), + _system_impl.get_own_component_id(), + &response_message, + ping.time_usec, + ping.seq, + message.sysid, + message.compid); + + _system_impl.send_message(response_message); + + } else { + // Answer from ping request. + if (ping.seq != _ping_sequence) { + LogWarn() << "Ignoring unknown ping sequence"; + return; + } + + if (message.compid != MAV_COMP_ID_AUTOPILOT1) { + // We're currently only interested in the ping of the autopilot. + return; + } + + auto now_us = static_cast(_system_impl.get_time().elapsed_s() * 1e6); + _last_ping_time_us = now_us - ping.time_usec; + LogDebug() << "Ping: " << _last_ping_time_us << " us"; + } +} + +} // namespace mavsdk diff --git a/src/core/ping.h b/src/core/ping.h new file mode 100644 index 0000000000..7c444ddaaf --- /dev/null +++ b/src/core/ping.h @@ -0,0 +1,26 @@ +#pragma once + +#include "mavlink_include.h" +#include + +namespace mavsdk { + +class SystemImpl; + +class Ping { +public: + explicit Ping(SystemImpl& system_impl); + ~Ping(); + + void run_once(); + double last_ping_time_s() const { return static_cast(_last_ping_time_us) * 1e-6; } + +private: + void process_ping(const mavlink_message_t& message); + + SystemImpl& _system_impl; + uint32_t _ping_sequence{0}; + std::atomic _last_ping_time_us{0}; +}; + +} // namespace mavsdk diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp index d94dd32140..a82ebf79bf 100644 --- a/src/core/system_impl.cpp +++ b/src/core/system_impl.cpp @@ -23,6 +23,7 @@ SystemImpl::SystemImpl(MavsdkImpl& parent, uint8_t system_id, uint8_t comp_id, b _params(*this), _commands(*this), _timesync(*this), + _ping(*this), _call_every_handler(_time), _mission_transfer(*this, _message_handler, _parent.timeout_handler) { @@ -258,14 +259,15 @@ void SystemImpl::heartbeats_timed_out() void SystemImpl::system_thread() { - dl_time_t last_time{}; + dl_time_t last_heartbeat_time{}; + dl_time_t last_ping_time{}; while (!_should_exit) { - if (_time.elapsed_since_s(last_time) >= SystemImpl::_HEARTBEAT_SEND_INTERVAL_S) { + if (_time.elapsed_since_s(last_heartbeat_time) >= SystemImpl::_HEARTBEAT_SEND_INTERVAL_S) { if (_parent.is_connected()) { send_heartbeat(); } - last_time = _time.steady_time(); + last_heartbeat_time = _time.steady_time(); } _call_every_handler.run_once(); @@ -274,6 +276,13 @@ void SystemImpl::system_thread() _timesync.do_work(); _mission_transfer.do_work(); + if (_time.elapsed_since_s(last_ping_time) >= SystemImpl::_ping_interval_s) { + if (_parent.is_connected()) { + _ping.run_once(); + } + last_ping_time = _time.steady_time(); + } + if (_connected) { // Work fairly fast if we're connected. std::this_thread::sleep_for(std::chrono::milliseconds(10)); diff --git a/src/core/system_impl.h b/src/core/system_impl.h index 363c493a20..89664bad58 100644 --- a/src/core/system_impl.h +++ b/src/core/system_impl.h @@ -7,6 +7,7 @@ #include "mavlink_commands.h" #include "mavlink_message_handler.h" #include "mavlink_mission_transfer.h" +#include "ping.h" #include "timeout_handler.h" #include "call_every_handler.h" #include "safe_queue.h" @@ -212,6 +213,8 @@ class SystemImpl : public Sender { Time& get_time() { return _time; }; AutopilotTime& get_autopilot_time() { return _autopilot_time; }; + double get_ping_time_s() const { return _ping.last_ping_time_s(); } + void register_plugin(PluginImplBase* plugin_impl); void unregister_plugin(PluginImplBase* plugin_impl); @@ -306,11 +309,13 @@ class SystemImpl : public Sender { void* _autopilot_version_timed_out_cookie = nullptr; static constexpr double _HEARTBEAT_SEND_INTERVAL_S = 1.0; + static constexpr double _ping_interval_s = 3.0; MAVLinkParameters _params; MAVLinkCommands _commands; Timesync _timesync; + Ping _ping; CallEveryHandler _call_every_handler; diff --git a/src/integration_tests/info.cpp b/src/integration_tests/info.cpp index 91250dd0b4..168b29c8ce 100644 --- a/src/integration_tests/info.cpp +++ b/src/integration_tests/info.cpp @@ -22,7 +22,7 @@ TEST_F(SitlTest, Info) // FIXME: we need to wait some time until Info has determined the version. std::this_thread::sleep_for(std::chrono::seconds(1)); - for (unsigned i = 0; i < 3; ++i) { + for (unsigned i = 0; i < 30; ++i) { std::pair version_result = info->get_version(); EXPECT_EQ(version_result.first, Info::Result::Success);