core: added ping capability

src/core/CMakeLists.txt

@@ -25,6 +25,7 @@ add_library(mavsdk
     mavlink_receiver.cpp
     mavlink_statustext_handler.cpp
     mavlink_message_handler.cpp
+    ping.cpp
     plugin_impl_base.cpp
     serial_connection.cpp
     tcp_connection.cpp

src/core/ping.cpp

@@ -0,0 +1,74 @@
+#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;
+    }
+}
+
+} // 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

@@ -24,6 +24,7 @@ SystemImpl::SystemImpl(MavsdkImpl& parent, uint8_t system_id, uint8_t comp_id, b
     _send_commands(*this),
     _receive_commands(*this),
     _timesync(*this),
+    _ping(*this),
     _mission_transfer(*this, _message_handler, _parent.timeout_handler)
 {
     _target_address.system_id = system_id;
@@ -238,12 +239,21 @@ void SystemImpl::heartbeats_timed_out()
 
 void SystemImpl::system_thread()
 {
+    dl_time_t last_ping_time{};
+
     while (!_should_exit) {
         _params.do_work();
         _send_commands.do_work();
         _timesync.do_work();
         _mission_transfer.do_work();
 
+        if (_time.elapsed_since_s(last_ping_time) >= SystemImpl::_ping_interval_s) {
+            if (_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

@@ -8,6 +8,7 @@
 #include "mavlink_message_handler.h"
 #include "mavlink_mission_transfer.h"
 #include "mavlink_statustext_handler.h"
+#include "ping.h"
 #include "timeout_handler.h"
 #include "safe_queue.h"
 #include "timesync.h"
@@ -217,6 +218,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);
 
@@ -322,11 +325,14 @@ class SystemImpl : public Sender {
     std::atomic<bool> _autopilot_version_pending{false};
     void* _autopilot_version_timed_out_cookie = nullptr;
 
+    static constexpr double _ping_interval_s = 5.0;
+
     MAVLinkParameters _params;
     MavlinkCommandSender _send_commands;
     MavlinkCommandReceiver _receive_commands;
 
     Timesync _timesync;
+    Ping _ping;
 
     MAVLinkMissionTransfer _mission_transfer;