Skip to content

Commit

Permalink
core: consolidate components and usage types
Browse files Browse the repository at this point in the history
This should make dealing with the configuration and components a bit
clearer.

Signed-off-by: Julian Oes <[email protected]>
  • Loading branch information
julianoes committed Oct 30, 2023
1 parent d380bb8 commit 7e4ac1f
Show file tree
Hide file tree
Showing 25 changed files with 201 additions and 248 deletions.
7 changes: 3 additions & 4 deletions src/integration_tests/camera_take_photo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,17 +12,16 @@ TEST(CameraTest, TakePhotoSingle)
{
Mavsdk mavsdk_groundstation;
mavsdk_groundstation.set_configuration(
Mavsdk::Configuration{Mavsdk::Configuration::UsageType::GroundStation});
Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation});

Mavsdk mavsdk_camera;
mavsdk_camera.set_configuration(
Mavsdk::Configuration{Mavsdk::Configuration::UsageType::Camera});
mavsdk_camera.set_configuration(Mavsdk::Configuration{Mavsdk::ComponentType::Camera});

ASSERT_EQ(mavsdk_groundstation.add_any_connection("udp://:17000"), ConnectionResult::Success);
ASSERT_EQ(mavsdk_camera.add_any_connection("udp://127.0.0.1:17000"), ConnectionResult::Success);

auto camera_server =
CameraServer{mavsdk_camera.server_component_by_type(Mavsdk::ServerComponentType::Camera)};
CameraServer{mavsdk_camera.server_component_by_type(Mavsdk::ComponentType::Camera)};
camera_server.subscribe_take_photo([&camera_server](int32_t index) {
LogInfo() << "Let's take photo " << index;

Expand Down
2 changes: 1 addition & 1 deletion src/integration_tests/connection.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ connection_test(const std::string& client_system_address, const std::string& ser
{
// Start instance as a UDP server pretending to be an autopilot
Mavsdk mavsdk_server;
Mavsdk::Configuration config_autopilot(Mavsdk::Configuration::UsageType::Autopilot);
Mavsdk::Configuration config_autopilot(Mavsdk::ComponentType::Autopilot);
mavsdk_server.set_configuration(config_autopilot);
ConnectionResult ret_server = mavsdk_server.add_any_connection(server_system_address);
ASSERT_EQ(ret_server, ConnectionResult::Success);
Expand Down
4 changes: 2 additions & 2 deletions src/integration_tests/statustext.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,12 @@ static const auto type = ServerUtility::StatusTextType::Info;
TEST(StatusTextTest, TestServer)
{
Mavsdk mavsdk_gcs;
Mavsdk::Configuration config_gcs(Mavsdk::Configuration::UsageType::GroundStation);
Mavsdk::Configuration config_gcs(Mavsdk::ComponentType::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::Configuration config_onboard(Mavsdk::ComponentType::CompanionComputer);
mavsdk_onboard.set_configuration(config_onboard);
ASSERT_EQ(mavsdk_onboard.setup_udp_remote("127.0.0.1", 24550), ConnectionResult::Success);

Expand Down
57 changes: 23 additions & 34 deletions src/mavsdk/core/include/mavsdk/mavsdk.h
Original file line number Diff line number Diff line change
Expand Up @@ -223,23 +223,23 @@ class Mavsdk {
*/
std::optional<std::shared_ptr<System>> first_autopilot(double timeout_s) const;

/**
* @brief ComponentType of configurations, used for automatic ID setting
*/
enum class ComponentType {
Autopilot, /**< @brief SDK is used as an autopilot. */
GroundStation, /**< @brief SDK is used as a ground station. */
CompanionComputer, /**< @brief SDK is used as a companion computer on board the MAV. */
Camera, /** < @brief SDK is used as a camera. */
Custom /**< @brief the SDK is used in a custom configuration, no automatic ID will be
provided */
};

/**
* @brief Possible configurations.
*/
class Configuration {
public:
/**
* @brief UsageTypes of configurations, used for automatic ID setting
*/
enum class UsageType {
Autopilot, /**< @brief SDK is used as an autopilot. */
GroundStation, /**< @brief SDK is used as a ground station. */
CompanionComputer, /**< @brief SDK is used as a companion computer on board the MAV. */
Camera, /** < @brief SDK is used as a camera. */
Custom /**< @brief the SDK is used in a custom configuration, no automatic ID will be
provided */
};

/**
* @brief Create new Configuration via manually configured
* system and component ID.
Expand All @@ -250,11 +250,11 @@ class Mavsdk {
explicit Configuration(
uint8_t system_id, uint8_t component_id, bool always_send_heartbeats);
/**
* @brief Create new Configuration using a usage type.
* @brief Create new Configuration using a component type.
* In this mode, the system and component ID will be automatically chosen.
* @param usage_type the usage type, used for automatically choosing ids.
* @param component_type the component type, used for automatically choosing ids.
*/
explicit Configuration(UsageType usage_type);
explicit Configuration(ComponentType component_type);

Configuration() = delete;
~Configuration() = default;
Expand Down Expand Up @@ -292,21 +292,21 @@ class Mavsdk {
*/
void set_always_send_heartbeats(bool always_send_heartbeats);

/** @brief Usage type of this configuration, used for automatic ID set */
UsageType get_usage_type() const;
/** @brief Component type of this configuration, used for automatic ID set */
ComponentType get_component_type() const;

/**
* @brief Set the usage type of this configuration.
* @brief Set the component type of this configuration.
*/
void set_usage_type(UsageType usage_type);
void set_component_type(ComponentType component_type);

private:
uint8_t _system_id;
uint8_t _component_id;
bool _always_send_heartbeats;
UsageType _usage_type;
ComponentType _component_type;

static Mavsdk::Configuration::UsageType usage_type_for_component(uint8_t component_id);
static Mavsdk::ComponentType component_type_for_component_id(uint8_t component_id);
};

/**
Expand Down Expand Up @@ -367,30 +367,19 @@ class Mavsdk {
*/
void unsubscribe_on_new_system(NewSystemHandle handle);

/**
* @brief High level type of a server component.
*/
enum class ServerComponentType {
Autopilot, /**< @brief The component identifies as an autopilot. */
GroundStation, /**< @brief The component identifies as a ground station. */
CompanionComputer, /**< @brief The component identifies as a companion computer on board the
system. */
Camera, /** < @brief The component identifies as a camera. */
};

/**
* @brief Get server component by a high level type.
*
* This represents a server component of the MAVSDK instance.
*
* @param server_component_type The high level type of the component.
* @param component_type The high level type of the component.
* @param instance The instance of the component if there are multiple, starting at 0.
*
* @return A valid shared pointer to a server component if it was successful, an empty pointer
* otherwise.
*/
std::shared_ptr<ServerComponent>
server_component_by_type(ServerComponentType server_component_type, unsigned instance = 0);
server_component_by_type(ComponentType component_type, unsigned instance = 0);

/**
* @brief Get server component by the low MAVLink component ID.
Expand Down
2 changes: 1 addition & 1 deletion src/mavsdk/core/lazy_server_plugin.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ template<typename ServerPlugin> class LazyServerPlugin {
std::lock_guard<std::mutex> lock(_mutex);
if (_server_plugin == nullptr) {
_server_plugin = std::make_unique<ServerPlugin>(
_mavsdk.server_component_by_type(Mavsdk::ServerComponentType::CompanionComputer));
_mavsdk.server_component_by_type(Mavsdk::ComponentType::CompanionComputer));
}
return _server_plugin.get();
}
Expand Down
39 changes: 19 additions & 20 deletions src/mavsdk/core/mavsdk.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ void Mavsdk::unsubscribe_on_new_system(NewSystemHandle handle)
}

std::shared_ptr<ServerComponent>
Mavsdk::server_component_by_type(ServerComponentType server_component_type, unsigned instance)
Mavsdk::server_component_by_type(ComponentType server_component_type, unsigned instance)
{
return _impl->server_component_by_type(server_component_type, instance);
}
Expand All @@ -111,50 +111,49 @@ Mavsdk::Configuration::Configuration(
_system_id(system_id),
_component_id(component_id),
_always_send_heartbeats(always_send_heartbeats),
_usage_type(usage_type_for_component(component_id))
_component_type(component_type_for_component_id(component_id))
{}

Mavsdk::Configuration::UsageType
Mavsdk::Configuration::usage_type_for_component(uint8_t component_id)
Mavsdk::ComponentType Mavsdk::Configuration::component_type_for_component_id(uint8_t component_id)
{
switch (component_id) {
case MavsdkImpl::DEFAULT_COMPONENT_ID_GCS:
return UsageType::GroundStation;
return ComponentType::GroundStation;
case MavsdkImpl::DEFAULT_COMPONENT_ID_CC:
return UsageType::CompanionComputer;
return ComponentType::CompanionComputer;
case MavsdkImpl::DEFAULT_COMPONENT_ID_AUTOPILOT:
return UsageType::Autopilot;
return ComponentType::Autopilot;
case MavsdkImpl::DEFAULT_COMPONENT_ID_CAMERA:
return UsageType::Camera;
return ComponentType::Camera;
default:
return UsageType::Custom;
return ComponentType::Custom;
}
}

Mavsdk::Configuration::Configuration(UsageType usage_type) :
Mavsdk::Configuration::Configuration(ComponentType component_type) :
_system_id(MavsdkImpl::DEFAULT_SYSTEM_ID_GCS),
_component_id(MavsdkImpl::DEFAULT_COMPONENT_ID_GCS),
_always_send_heartbeats(false),
_usage_type(usage_type)
_component_type(component_type)
{
switch (usage_type) {
case Mavsdk::Configuration::UsageType::GroundStation:
switch (component_type) {
case Mavsdk::ComponentType::GroundStation:
_system_id = MavsdkImpl::DEFAULT_SYSTEM_ID_GCS;
_component_id = MavsdkImpl::DEFAULT_COMPONENT_ID_GCS;
_always_send_heartbeats = false;
break;
case Mavsdk::Configuration::UsageType::CompanionComputer:
case Mavsdk::ComponentType::CompanionComputer:
// TODO implement auto-detection of system ID - maybe from heartbeats?
_system_id = MavsdkImpl::DEFAULT_SYSTEM_ID_CC;
_component_id = MavsdkImpl::DEFAULT_COMPONENT_ID_CC;
_always_send_heartbeats = true;
break;
case Mavsdk::Configuration::UsageType::Autopilot:
case Mavsdk::ComponentType::Autopilot:
_system_id = MavsdkImpl::DEFAULT_SYSTEM_ID_AUTOPILOT;
_component_id = MavsdkImpl::DEFAULT_COMPONENT_ID_AUTOPILOT;
_always_send_heartbeats = true;
break;
case Mavsdk::Configuration::UsageType::Camera:
case Mavsdk::ComponentType::Camera:
_system_id = MavsdkImpl::DEFAULT_SYSTEM_ID_CAMERA;
_component_id = MavsdkImpl::DEFAULT_COMPONENT_ID_CAMERA;
_always_send_heartbeats = true;
Expand Down Expand Up @@ -194,14 +193,14 @@ void Mavsdk::Configuration::set_always_send_heartbeats(bool always_send_heartbea
_always_send_heartbeats = always_send_heartbeats;
}

Mavsdk::Configuration::UsageType Mavsdk::Configuration::get_usage_type() const
Mavsdk::ComponentType Mavsdk::Configuration::get_component_type() const
{
return _usage_type;
return _component_type;
}

void Mavsdk::Configuration::set_usage_type(Mavsdk::Configuration::UsageType usage_type)
void Mavsdk::Configuration::set_component_type(Mavsdk::ComponentType component_type)
{
_usage_type = usage_type;
_component_type = component_type;
}

void Mavsdk::intercept_incoming_messages_async(std::function<bool(mavlink_message_t&)> callback)
Expand Down
26 changes: 13 additions & 13 deletions src/mavsdk/core/mavsdk_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -164,27 +164,27 @@ std::optional<std::shared_ptr<System>> MavsdkImpl::first_autopilot(double timeou
}
}

std::shared_ptr<ServerComponent> MavsdkImpl::server_component_by_type(
Mavsdk::ServerComponentType server_component_type, unsigned instance)
std::shared_ptr<ServerComponent>
MavsdkImpl::server_component_by_type(Mavsdk::ComponentType server_component_type, unsigned instance)
{
switch (server_component_type) {
case Mavsdk::ServerComponentType::Autopilot:
case Mavsdk::ComponentType::Autopilot:
if (instance == 0) {
return server_component_by_id(MAV_COMP_ID_AUTOPILOT1);
} else {
LogErr() << "Only autopilot instance 0 is valid";
return {};
}

case Mavsdk::ServerComponentType::GroundStation:
case Mavsdk::ComponentType::GroundStation:
if (instance == 0) {
return server_component_by_id(MAV_COMP_ID_MISSIONPLANNER);
} else {
LogErr() << "Only one ground station supported at this time";
return {};
}

case Mavsdk::ServerComponentType::CompanionComputer:
case Mavsdk::ComponentType::CompanionComputer:
if (instance == 0) {
return server_component_by_id(MAV_COMP_ID_ONBOARD_COMPUTER);
} else if (instance == 1) {
Expand All @@ -198,7 +198,7 @@ std::shared_ptr<ServerComponent> MavsdkImpl::server_component_by_type(
return {};
}

case Mavsdk::ServerComponentType::Camera:
case Mavsdk::ComponentType::Camera:
if (instance == 0) {
return server_component_by_id(MAV_COMP_ID_CAMERA);
} else if (instance == 1) {
Expand Down Expand Up @@ -342,7 +342,7 @@ void MavsdkImpl::receive_message(mavlink_message_t& message, Connection* connect
// mavlink instances which leads to existing implementations (including
// examples and integration tests) to connect to QGroundControl by accident
// instead of PX4 because the check `has_autopilot()` is not used.
if (_configuration.get_usage_type() == Mavsdk::Configuration::UsageType::GroundStation &&
if (_configuration.get_component_type() == Mavsdk::ComponentType::GroundStation &&
message.sysid == 255 && message.compid == MAV_COMP_ID_MISSIONPLANNER) {
if (_message_logging_on) {
LogDebug() << "Ignoring messages from QGC as we are also a ground station";
Expand Down Expand Up @@ -677,20 +677,20 @@ Autopilot MavsdkImpl::autopilot() const
// FIXME: this should be per component
uint8_t MavsdkImpl::get_mav_type() const
{
switch (_configuration.get_usage_type()) {
case Mavsdk::Configuration::UsageType::Autopilot:
switch (_configuration.get_component_type()) {
case Mavsdk::ComponentType::Autopilot:
return MAV_TYPE_GENERIC;

case Mavsdk::Configuration::UsageType::GroundStation:
case Mavsdk::ComponentType::GroundStation:
return MAV_TYPE_GCS;

case Mavsdk::Configuration::UsageType::CompanionComputer:
case Mavsdk::ComponentType::CompanionComputer:
return MAV_TYPE_ONBOARD_CONTROLLER;

case Mavsdk::Configuration::UsageType::Camera:
case Mavsdk::ComponentType::Camera:
return MAV_TYPE_CAMERA;

case Mavsdk::Configuration::UsageType::Custom:
case Mavsdk::ComponentType::Custom:
return MAV_TYPE_GENERIC;

default:
Expand Down
6 changes: 3 additions & 3 deletions src/mavsdk/core/mavsdk_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -100,8 +100,8 @@ class MavsdkImpl {
void intercept_incoming_messages_async(std::function<bool(mavlink_message_t&)> callback);
void intercept_outgoing_messages_async(std::function<bool(mavlink_message_t&)> callback);

std::shared_ptr<ServerComponent> server_component_by_type(
Mavsdk::ServerComponentType server_component_type, unsigned instance = 0);
std::shared_ptr<ServerComponent>
server_component_by_type(Mavsdk::ComponentType server_component_type, unsigned instance = 0);
std::shared_ptr<ServerComponent> server_component_by_id(uint8_t component_id);

Time time{};
Expand Down Expand Up @@ -149,7 +149,7 @@ class MavsdkImpl {

CallbackList<> _new_system_callbacks{};

Mavsdk::Configuration _configuration{Mavsdk::Configuration::UsageType::GroundStation};
Mavsdk::Configuration _configuration{Mavsdk::ComponentType::GroundStation};

struct UserCallback {
UserCallback() = default;
Expand Down
9 changes: 4 additions & 5 deletions src/system_tests/action_arm_disarm.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,18 +11,17 @@ TEST(SystemTest, ActionArmDisarm)
{
Mavsdk mavsdk_groundstation;
mavsdk_groundstation.set_configuration(
Mavsdk::Configuration{Mavsdk::Configuration::UsageType::GroundStation});
Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation});

Mavsdk mavsdk_autopilot;
mavsdk_autopilot.set_configuration(
Mavsdk::Configuration{Mavsdk::Configuration::UsageType::Autopilot});
mavsdk_autopilot.set_configuration(Mavsdk::Configuration{Mavsdk::ComponentType::Autopilot});

ASSERT_EQ(mavsdk_groundstation.add_any_connection("udp://:17000"), ConnectionResult::Success);
ASSERT_EQ(
mavsdk_autopilot.add_any_connection("udp://127.0.0.1:17000"), ConnectionResult::Success);

auto action_server = ActionServer{
mavsdk_autopilot.server_component_by_type(Mavsdk::ServerComponentType::Autopilot)};
auto action_server =
ActionServer{mavsdk_autopilot.server_component_by_type(Mavsdk::ComponentType::Autopilot)};

auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);
ASSERT_TRUE(maybe_system);
Expand Down
Loading

0 comments on commit 7e4ac1f

Please sign in to comment.