diff --git a/src/integration_tests/CMakeLists.txt b/src/integration_tests/CMakeLists.txt index ebb5235f7..fd2cf8905 100644 --- 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 new file mode 100644 index 000000000..0287bf60c --- /dev/null +++ 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 +#include + +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 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(message.sysid) + << " with type " << static_cast(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 index 5f4e1d147..fc7c899c5 100644 --- 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 index a2673e0da..3dcdd555a 100644 --- 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); +} + void Mavsdk::intercept_incoming_messages_async(std::function callback) { _impl->intercept_incoming_messages_async(callback); diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index c9525c635..2483addee 100644 --- 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 index d5b5cf976..15ae11463 100644 --- 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); +} \ No newline at end of file