core: added ping capability

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

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

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<uint64_t>(_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<uint64_t>(_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

src/core/ping.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "mavlink_include.h"
+#include <atomic>
+
+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<double>(_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<uint64_t> _last_ping_time_us{0};
+};
+
+} // namespace mavsdk

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

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;
 

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<Info::Result, Info::Version> version_result = info->get_version();
 
         EXPECT_EQ(version_result.first, Info::Result::Success);