Merge pull request #2487 from cramke/feature-configure-mav-type-on-se…

…rver-component Add Option to set the MAV_TYPE manually
mavlink · Jan 8, 2025 · 6f0b1e3 · 6f0b1e3
2 parents 22b3a6b + d0e9089
commit 6f0b1e3
Show file tree

Hide file tree

Showing 6 changed files with 111 additions and 28 deletions.
diff --git a/src/integration_tests/CMakeLists.txt b/src/integration_tests/CMakeLists.txt
@@ -10,6 +10,7 @@ add_executable(integration_tests_runner
     action_transition_multicopter_fixedwing.cpp
     action_goto.cpp
     action_hold.cpp
+    autopilot_server_configuration.cpp
     calibration.cpp
     connection.cpp
     follow_me.cpp

diff --git a/src/integration_tests/autopilot_server_configuration.cpp b/src/integration_tests/autopilot_server_configuration.cpp
@@ -0,0 +1,51 @@
+#include "log.h"
+#include "future"
+#include "chrono"
+#include "mavsdk.h"
+#include "plugins/mavlink_passthrough/mavlink_passthrough.h"
+#include <gtest/gtest.h>
+#include <thread>
+
+using namespace mavsdk;
+
+TEST(SystemTest, Configuration)
+{
+    Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}};
+
+    Mavsdk::Configuration config{ComponentType::Autopilot};
+    MAV_TYPE TEST_VARIABLE = MAV_TYPE_GROUND_ROVER;
+    config.set_mav_type(TEST_VARIABLE);
+    Mavsdk mavsdk_autopilot{config};
+
+    ASSERT_EQ(
+        mavsdk_groundstation.add_any_connection("udpin://0.0.0.0:17000"),
+        ConnectionResult::Success);
+    ASSERT_EQ(
+        mavsdk_autopilot.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);
+
+    auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);
+    ASSERT_TRUE(maybe_system);
+    auto system = maybe_system.value();
+
+    ASSERT_TRUE(system->has_autopilot());
+
+    auto mavlink_passthrough = MavlinkPassthrough{system};
+
+    std::promise<void> promise;
+    auto future = promise.get_future();
+    mavlink_passthrough.subscribe_message(
+        MAVLINK_MSG_ID_HEARTBEAT, [&promise, TEST_VARIABLE](const mavlink_message_t& message) {
+            mavlink_heartbeat_t heartbeat;
+            mavlink_msg_heartbeat_decode(&message, &heartbeat);
+            LogInfo() << "Received heartbeat from system " << static_cast<int>(message.sysid)
+                      << " with type " << static_cast<int>(heartbeat.type);
+            ASSERT_EQ(heartbeat.type, TEST_VARIABLE);
+            promise.set_value();
+        });
+
+    // Wait for the message or timeout
+    if (future.wait_for(std::chrono::seconds(5)) == std::future_status::timeout) {
+        LogErr() << "Timeout: No heartbeat message received within 10 seconds";
+        FAIL();
+    }
+}
diff --git a/src/mavsdk/core/include/mavsdk/mavsdk.h b/src/mavsdk/core/include/mavsdk/mavsdk.h
@@ -249,13 +249,26 @@ class Mavsdk {
          */
         void set_component_type(ComponentType component_type);
 
+        /**
+         * @brief Get the mav type (vehicle type) of this configuration
+         * @return `uint8_t` the mav type stored in this configuration
+         */
+        uint8_t get_mav_type() const;
+
+        /**
+         * @brief Set the mav type (vehicle type) of this configuration.
+         */
+        void set_mav_type(uint8_t mav_type);
+
     private:
         uint8_t _system_id;
         uint8_t _component_id;
         bool _always_send_heartbeats;
         ComponentType _component_type;
+        MAV_TYPE _mav_type;
 
         static ComponentType component_type_for_component_id(uint8_t component_id);
+        static MAV_TYPE mav_type_for_component_type(ComponentType component_type);
     };
 
     /**

diff --git a/src/mavsdk/core/mavsdk.cpp b/src/mavsdk/core/mavsdk.cpp
@@ -84,7 +84,8 @@ Mavsdk::Configuration::Configuration(
     _system_id(system_id),
     _component_id(component_id),
     _always_send_heartbeats(always_send_heartbeats),
-    _component_type(component_type_for_component_id(component_id))
+    _component_type(component_type_for_component_id(component_id)),
+    _mav_type(mav_type_for_component_type(component_type_for_component_id(component_id)))
 {}
 
 ComponentType Mavsdk::Configuration::component_type_for_component_id(uint8_t component_id)
@@ -107,11 +108,34 @@ ComponentType Mavsdk::Configuration::component_type_for_component_id(uint8_t com
     }
 }
 
+MAV_TYPE Mavsdk::Configuration::mav_type_for_component_type(ComponentType component_type)
+{
+    switch (component_type) {
+        case ComponentType::Autopilot:
+            return MAV_TYPE_GENERIC;
+        case ComponentType::GroundStation:
+            return MAV_TYPE_GCS;
+        case ComponentType::CompanionComputer:
+            return MAV_TYPE_ONBOARD_CONTROLLER;
+        case ComponentType::Camera:
+            return MAV_TYPE_CAMERA;
+        case ComponentType::Gimbal:
+            return MAV_TYPE_GIMBAL;
+        case ComponentType::RemoteId:
+            return MAV_TYPE_ODID;
+        case ComponentType::Custom:
+            return MAV_TYPE_GENERIC;
+        default:
+            return MAV_TYPE_GENERIC;
+    }
+}
+
 Mavsdk::Configuration::Configuration(ComponentType component_type) :
     _system_id(Mavsdk::DEFAULT_SYSTEM_ID_GCS),
     _component_id(Mavsdk::DEFAULT_COMPONENT_ID_GCS),
     _always_send_heartbeats(false),
-    _component_type(component_type)
+    _component_type(component_type),
+    _mav_type(mav_type_for_component_type(component_type))
 {
     switch (component_type) {
         case ComponentType::GroundStation:
@@ -190,6 +214,16 @@ void Mavsdk::Configuration::set_component_type(ComponentType component_type)
     _component_type = component_type;
 }
 
+uint8_t Mavsdk::Configuration::get_mav_type() const
+{
+    return _mav_type;
+}
+
+void Mavsdk::Configuration::set_mav_type(uint8_t mav_type)
+{
+    _mav_type = static_cast<MAV_TYPE>(mav_type);
+}
+
 void Mavsdk::intercept_incoming_messages_async(std::function<bool(mavlink_message_t&)> callback)
 {
     _impl->intercept_incoming_messages_async(callback);

diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp
@@ -712,32 +712,7 @@ Autopilot MavsdkImpl::autopilot() const
 // FIXME: this should be per component
 uint8_t MavsdkImpl::get_mav_type() const
 {
-    switch (_configuration.get_component_type()) {
-        case ComponentType::Autopilot:
-            return MAV_TYPE_GENERIC;
-
-        case ComponentType::GroundStation:
-            return MAV_TYPE_GCS;
-
-        case ComponentType::CompanionComputer:
-            return MAV_TYPE_ONBOARD_CONTROLLER;
-
-        case ComponentType::Camera:
-            return MAV_TYPE_CAMERA;
-
-        case ComponentType::Gimbal:
-            return MAV_TYPE_GIMBAL;
-
-        case ComponentType::RemoteId:
-            return MAV_TYPE_ODID;
-
-        case ComponentType::Custom:
-            return MAV_TYPE_GENERIC;
-
-        default:
-            LogErr() << "Unknown configuration";
-            return 0;
-    }
+    return _configuration.get_mav_type();
 }
 
 void MavsdkImpl::make_system_with_component(uint8_t system_id, uint8_t comp_id)

diff --git a/src/mavsdk/core/mavsdk_test.cpp b/src/mavsdk/core/mavsdk_test.cpp
@@ -8,3 +8,12 @@ TEST(Mavsdk, version)
     Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}};
     ASSERT_GT(mavsdk.version().size(), 5);
 }
+
+TEST(Mavsdk, Configuration)
+{
+    Mavsdk::Configuration configuration{ComponentType::Autopilot};
+
+    ASSERT_EQ(configuration.get_mav_type(), MAV_TYPE::MAV_TYPE_GENERIC); // Default
+    configuration.set_mav_type(MAV_TYPE::MAV_TYPE_FIXED_WING);
+    ASSERT_EQ(configuration.get_mav_type(), MAV_TYPE::MAV_TYPE_FIXED_WING);
+}