Merge pull request #1380 from mavlink/pr-server-utility

Add ServerUtility plugin
mavlink · Mar 25, 2021 · 9126237 · 9126237
2 parents 5771357 + 4f498f1
commit 9126237
Show file tree

Hide file tree

Showing 23 changed files with 2,958 additions and 102 deletions.
diff --git a/proto b/proto
diff --git a/src/core/mavlink_statustext_handler.cpp b/src/core/mavlink_statustext_handler.cpp
@@ -3,33 +3,8 @@
 
 namespace mavsdk {
 
-std::optional<std::string>
-MavlinkStatustextHandler::process_severity(const mavlink_statustext_t& statustext)
-{
-    switch (statustext.severity) {
-        case MAV_SEVERITY_EMERGENCY:
-            return {"emergency"};
-        case MAV_SEVERITY_ALERT:
-            return {"alert"};
-        case MAV_SEVERITY_CRITICAL:
-            return {"critical"};
-        case MAV_SEVERITY_ERROR:
-            return {"error"};
-        case MAV_SEVERITY_WARNING:
-            return {"warning"};
-        case MAV_SEVERITY_NOTICE:
-            return {"notice"};
-        case MAV_SEVERITY_INFO:
-            return {"info"};
-        case MAV_SEVERITY_DEBUG:
-            return {"debug"};
-        default:
-            return std::nullopt;
-    }
-}
-
-std::optional<std::string>
-MavlinkStatustextHandler::process_text(const mavlink_statustext_t& statustext)
+std::optional<MavlinkStatustextHandler::Statustext>
+MavlinkStatustextHandler::process(const mavlink_statustext_t& statustext)
 {
     char text_with_null[sizeof(statustext.text) + 1]{};
     strncpy(text_with_null, statustext.text, sizeof(text_with_null) - 1);
@@ -39,6 +14,7 @@ MavlinkStatustextHandler::process_text(const mavlink_statustext_t& statustext)
             _temp_multi_str = "";
             _last_chunk_seq = 0;
             _last_id = statustext.id;
+            _last_severity = static_cast<MAV_SEVERITY>(statustext.severity);
         }
 
         // We can recover from missing chunks in-between but not if the first or last one is lost.
@@ -54,11 +30,35 @@ MavlinkStatustextHandler::process_text(const mavlink_statustext_t& statustext)
             // No zero termination yet, keep going.
             return std::nullopt;
         } else {
-            return {_temp_multi_str};
+            return Statustext{_temp_multi_str, _last_severity};
         }
     }
 
-    return {text_with_null};
+    return Statustext{text_with_null, _last_severity};
+}
+
+std::string MavlinkStatustextHandler::severity_str(MAV_SEVERITY severity)
+{
+    switch (severity) {
+        case MAV_SEVERITY_EMERGENCY:
+            return "emergency";
+        case MAV_SEVERITY_ALERT:
+            return "alert";
+        case MAV_SEVERITY_CRITICAL:
+            return "critical";
+        case MAV_SEVERITY_ERROR:
+            return "error";
+        case MAV_SEVERITY_WARNING:
+            return "warning";
+        case MAV_SEVERITY_NOTICE:
+            return "notice";
+        case MAV_SEVERITY_INFO:
+            return "info";
+        case MAV_SEVERITY_DEBUG:
+            return "debug";
+        default:
+            return "unknown severity";
+    }
 }
 
 } // namespace mavsdk
diff --git a/src/core/mavlink_statustext_handler.h b/src/core/mavlink_statustext_handler.h
@@ -11,13 +11,20 @@ class MavlinkStatustextHandler {
     MavlinkStatustextHandler() = default;
     ~MavlinkStatustextHandler() = default;
 
-    std::optional<std::string> process_severity(const mavlink_statustext_t& statustext);
-    std::optional<std::string> process_text(const mavlink_statustext_t& statustext);
+    struct Statustext {
+        std::string text;
+        MAV_SEVERITY severity;
+    };
+
+    std::optional<Statustext> process(const mavlink_statustext_t& statustext);
+
+    static std::string severity_str(MAV_SEVERITY severity);
 
 private:
     std::string _temp_multi_str{};
     uint16_t _last_id{0};
     uint8_t _last_chunk_seq{0};
+    MAV_SEVERITY _last_severity{MAV_SEVERITY_DEBUG};
 };
 
 } // namespace mavsdk
diff --git a/src/core/mavlink_statustext_handler_test.cpp b/src/core/mavlink_statustext_handler_test.cpp
@@ -15,7 +15,7 @@ using namespace mavsdk;
 
 TEST(MavlinkStatustextHandler, Severities)
 {
-    const std::vector<std::pair<uint8_t, std::string>> severities{
+    const std::vector<std::pair<MAV_SEVERITY, std::string>> severities{
         {MAV_SEVERITY_DEBUG, "debug"},
         {MAV_SEVERITY_INFO, "info"},
         {MAV_SEVERITY_NOTICE, "notice"},
@@ -26,24 +26,15 @@ TEST(MavlinkStatustextHandler, Severities)
         {MAV_SEVERITY_EMERGENCY, "emergency"}};
 
     for (const auto& severity : severities) {
-        mavlink_statustext_t statustext{};
-        statustext.severity = severity.first;
-
-        MavlinkStatustextHandler handler;
-        auto result = handler.process_severity(statustext);
-        ASSERT_TRUE(result);
-        EXPECT_EQ(severity.second, result.value());
+        auto result = MavlinkStatustextHandler::severity_str(severity.first);
+        EXPECT_EQ(severity.second, result);
     }
 }
 
 TEST(MavlinkStatustextHandler, WrongSeverity)
 {
-    mavlink_statustext_t statustext{};
-    statustext.severity = 255;
-
-    MavlinkStatustextHandler handler;
-    auto result = handler.process_severity(statustext);
-    EXPECT_FALSE(result);
+    auto result = MavlinkStatustextHandler::severity_str(static_cast<MAV_SEVERITY>(255));
+    EXPECT_EQ(result, "unknown severity");
 }
 
 TEST(MavlinkStatustextHandler, SingleStatustextWithNull)
@@ -53,9 +44,9 @@ TEST(MavlinkStatustextHandler, SingleStatustextWithNull)
     strncpy(statustext.text, str.c_str(), sizeof(statustext.text) - 1);
 
     MavlinkStatustextHandler handler;
-    auto result = handler.process_text(statustext);
+    auto result = handler.process(statustext);
     ASSERT_TRUE(result);
-    EXPECT_EQ(str, result.value());
+    EXPECT_EQ(str, result.value().text);
 }
 
 TEST(MavlinkStatustextHandler, SingleStatustextWithoutNull)
@@ -65,9 +56,9 @@ TEST(MavlinkStatustextHandler, SingleStatustextWithoutNull)
     strncpy(statustext.text, str.c_str(), sizeof(statustext.text));
 
     MavlinkStatustextHandler handler;
-    auto result = handler.process_text(statustext);
+    auto result = handler.process(statustext);
     ASSERT_TRUE(result);
-    EXPECT_EQ(str, result.value());
+    EXPECT_EQ(str, result.value().text);
 }
 
 TEST(MavlinkStatustextHandler, MultiStatustext)
@@ -97,11 +88,11 @@ TEST(MavlinkStatustextHandler, MultiStatustext)
         statustext.id = 42;
         statustext.chunk_seq = chunk_seq;
 
-        const auto result = handler.process_text(statustext);
+        const auto result = handler.process(statustext);
 
         if (is_last) {
             ASSERT_TRUE(result);
-            EXPECT_EQ(result.value(), str);
+            EXPECT_EQ(result.value().text, str);
         } else {
             EXPECT_FALSE(result);
         }
@@ -136,11 +127,11 @@ TEST(MavlinkStatustextHandler, MultiStatustextDivisibleByChunkLen)
         statustext.id = 42;
         statustext.chunk_seq = chunk_seq;
 
-        const auto result = handler.process_text(statustext);
+        const auto result = handler.process(statustext);
 
         if (is_last) {
             ASSERT_TRUE(result);
-            EXPECT_EQ(result.value(), str);
+            EXPECT_EQ(result.value().text, str);
         } else {
             EXPECT_FALSE(result);
         }
@@ -185,11 +176,11 @@ TEST(MavlinkStatustextHandler, MultiStatustextMissingPart)
         statustext.chunk_seq = chunk_seq;
 
         if (chunk_seq != 3) {
-            const auto result = handler.process_text(statustext);
+            const auto result = handler.process(statustext);
 
             if (is_last) {
                 ASSERT_TRUE(result);
-                EXPECT_EQ(result.value(), str_missing);
+                EXPECT_EQ(result.value().text, str_missing);
             } else {
                 EXPECT_FALSE(result);
             }
@@ -226,11 +217,11 @@ TEST(MavlinkStatustextHandler, MultiStatustextConsecutive)
             statustext.id = 42;
             statustext.chunk_seq = chunk_seq;
 
-            const auto result = handler.process_text(statustext);
+            const auto result = handler.process(statustext);
 
             if (is_last) {
                 ASSERT_TRUE(result);
-                EXPECT_EQ(result.value(), str);
+                EXPECT_EQ(result.value().text, str);
             } else {
                 EXPECT_FALSE(result);
             }
@@ -256,11 +247,11 @@ TEST(MavlinkStatustextHandler, MultiStatustextConsecutive)
             statustext.id = 43;
             statustext.chunk_seq = chunk_seq;
 
-            const auto result = handler.process_text(statustext);
+            const auto result = handler.process(statustext);
 
             if (is_last) {
                 ASSERT_TRUE(result);
-                EXPECT_EQ(result.value(), str);
+                EXPECT_EQ(result.value().text, str);
             } else {
                 EXPECT_FALSE(result);
             }

diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp
@@ -161,6 +161,22 @@ void SystemImpl::remove_call_every(const void* cookie)
     _parent.call_every_handler.remove(cookie);
 }
 
+void SystemImpl::register_statustext_handler(
+    std::function<void(const MavlinkStatustextHandler::Statustext&)> callback, void* cookie)
+{
+    std::lock_guard<std::mutex> lock(_statustext_handler_callbacks_mutex);
+    _statustext_handler_callbacks.push_back(StatustextCallback{callback, cookie});
+}
+
+void SystemImpl::unregister_statustext_handler(void* cookie)
+{
+    std::lock_guard<std::mutex> lock(_statustext_handler_callbacks_mutex);
+    _statustext_handler_callbacks.erase(std::remove_if(
+        _statustext_handler_callbacks.begin(),
+        _statustext_handler_callbacks.end(),
+        [&](const auto& entry) { return entry.cookie == cookie; }));
+}
+
 void SystemImpl::process_heartbeat(const mavlink_message_t& message)
 {
     mavlink_heartbeat_t heartbeat;
@@ -228,11 +244,17 @@ void SystemImpl::process_statustext(const mavlink_message_t& message)
     mavlink_statustext_t statustext;
     mavlink_msg_statustext_decode(&message, &statustext);
 
-    const auto result_severity = _statustext_handler.process_severity(statustext);
-    const auto result_text = _statustext_handler.process_text(statustext);
+    const auto maybe_result = _statustext_handler.process(statustext);
 
-    if (result_severity && result_text) {
-        LogDebug() << "MAVLink: " << result_severity.value() << ": " << result_text.value();
+    if (maybe_result.has_value()) {
+        LogDebug() << "MAVLink: "
+                   << MavlinkStatustextHandler::severity_str(maybe_result.value().severity) << ": "
+                   << maybe_result.value().text;
+
+        std::lock_guard<std::mutex> lock(_statustext_handler_callbacks_mutex);
+        for (const auto& entry : _statustext_handler_callbacks) {
+            entry.callback(maybe_result.value());
+        }
     }
 }
 

diff --git a/src/core/system_impl.h b/src/core/system_impl.h
@@ -78,6 +78,10 @@ class SystemImpl : public Sender {
     void reset_call_every(const void* cookie);
     void remove_call_every(const void* cookie);
 
+    void register_statustext_handler(
+        std::function<void(const MavlinkStatustextHandler::Statustext&)>, void* cookie);
+    void unregister_statustext_handler(void* cookie);
+
     bool send_message(mavlink_message_t& message) override;
 
     static FlightMode to_flight_mode_from_custom_mode(uint32_t custom_mode);
@@ -300,6 +304,13 @@ class SystemImpl : public Sender {
 
     MavlinkStatustextHandler _statustext_handler{};
 
+    struct StatustextCallback {
+        std::function<void(const MavlinkStatustextHandler::Statustext&)> callback;
+        void* cookie;
+    };
+    std::mutex _statustext_handler_callbacks_mutex{};
+    std::vector<StatustextCallback> _statustext_handler_callbacks;
+
     uint64_t _uuid{0};
 
     int _uuid_retries = 0;

diff --git a/src/integration_tests/CMakeLists.txt b/src/integration_tests/CMakeLists.txt
@@ -60,6 +60,7 @@ add_executable(integration_tests_runner
     path_checker.h
     system_connection_async.cpp
     system_multi_components.cpp
+    statustext.cpp
     telemetry_async.cpp
     telemetry_health.cpp
     telemetry_modes.cpp
@@ -94,6 +95,7 @@ target_link_libraries(integration_tests_runner
     mavsdk_param
     mavsdk_ftp
     mavsdk_mavlink_passthrough
+    mavsdk_server_utility
     gtest
     gtest_main
     gmock

diff --git a/src/integration_tests/statustext.cpp b/src/integration_tests/statustext.cpp
@@ -0,0 +1,69 @@
+#include "integration_test_helper.h"
+#include "global_include.h"
+#include "mavsdk.h"
+#include "plugins/server_utility/server_utility.h"
+#include "plugins/telemetry/telemetry.h"
+#include <string>
+#include <chrono>
+#include <thread>
+#include <future>
+
+using namespace mavsdk;
+
+static const std::string lorem_ipsum =
+    "Lorem ipsum dolor sit amet, consectetuer adipiscing elit. Aenean commodo ligula eget dolor. Aenean massa. Cum sociis natoque penatibus et magnis dis parturient montes, nascetur ridiculus mus. Donec quam felis, ultricies nec, pellentesque eu, pretium.";
+static const auto type = ServerUtility::StatusTextType::Info;
+
+TEST(StatusTextTest, TestServer)
+{
+    Mavsdk mavsdk_gcs;
+    Mavsdk::Configuration config_gcs(Mavsdk::Configuration::UsageType::GroundStation);
+    mavsdk_gcs.set_configuration(config_gcs);
+    ASSERT_EQ(mavsdk_gcs.add_udp_connection(24550), ConnectionResult::Success);
+
+    Mavsdk mavsdk_onboard;
+    Mavsdk::Configuration config_onboard(Mavsdk::Configuration::UsageType::CompanionComputer);
+    mavsdk_onboard.set_configuration(config_onboard);
+    ASSERT_EQ(mavsdk_onboard.setup_udp_remote("127.0.0.1", 24550), ConnectionResult::Success);
+
+    // Let the two connect to each other.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    for (auto system : mavsdk_gcs.systems()) {
+        LogInfo() << "gcs system: " << int(system->get_system_id());
+    }
+
+    for (auto system : mavsdk_onboard.systems()) {
+        LogInfo() << "onboard system: " << int(system->get_system_id());
+    }
+
+    ASSERT_EQ(mavsdk_gcs.systems().size(), 1);
+    // FIXME: For some reason there are two onboard systems:
+    //        - one of the discovered ground station (245)
+    //        - and one of the created system (1)
+    ASSERT_GE(mavsdk_onboard.systems().size(), 1);
+
+    auto system_gcs = mavsdk_gcs.systems().at(0);
+    auto system_onboard = mavsdk_onboard.systems().at(0);
+
+    auto telemetry = Telemetry(system_gcs);
+    auto server_utility = ServerUtility(system_onboard);
+
+    auto prom = std::promise<void>{};
+    auto fut = prom.get_future();
+
+    telemetry.subscribe_status_text([&prom](Telemetry::StatusText status_text) {
+        LogInfo() << "Received status text";
+        EXPECT_EQ(status_text.text, lorem_ipsum);
+        EXPECT_EQ(static_cast<int>(status_text.type), static_cast<int>(type));
+        prom.set_value();
+    });
+
+    EXPECT_EQ(
+        server_utility.send_status_text(ServerUtility::StatusTextType::Info, lorem_ipsum),
+        ServerUtility::Result::Success);
+
+    auto ret = fut.wait_for(std::chrono::seconds(1));
+
+    EXPECT_EQ(ret, std::future_status::ready);
+}