diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 309d76d42b..445b6c2614 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -17,6 +17,7 @@ #include #include #endif +#include #define SCHED_TASK(func, rate_hz, max_time_micros, prio) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros, prio) @@ -124,6 +125,12 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_SUBGROUPINFO(nmea, "NMEA_", 17, AP_Vehicle, AP_NMEA_Output), #endif +#if AP_DDS_ENABLED + // @Group: XRCE + // @Path: ../AP_DDS/AP_DDS_Client.cpp + AP_SUBGROUPPTR(dds_client, "XRCE_", 18, AP_Vehicle, AP_DDS_Client), +#endif + AP_GROUPEND }; @@ -301,6 +308,12 @@ void AP_Vehicle::setup() AP_Param::invalidate_count(); gcs().send_text(MAV_SEVERITY_INFO, "ArduPilot Ready"); + +#if AP_DDS_ENABLED + if (!init_dds_client()) { + gcs().send_text(MAV_SEVERITY_ERROR, "DDS Client: Failed to Initialize"); + } +#endif } void AP_Vehicle::loop() @@ -817,6 +830,16 @@ void AP_Vehicle::check_motor_noise() #endif } +#if AP_DDS_ENABLED +bool AP_Vehicle::init_dds_client() +{ + if (AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_DDS_XRCE, 0)) { + dds_client = new AP_DDS_Client(); + } + return dds_client != nullptr; +} +#endif // AP_DDS_ENABLED + AP_Vehicle *AP_Vehicle::_singleton = nullptr; AP_Vehicle *AP_Vehicle::get_singleton() diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index f0f8ac0a7f..c4d47f0f35 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -60,6 +60,8 @@ #include #include +class AP_DDS_Client; + class AP_Vehicle : public AP_HAL::HAL::Callbacks { public: @@ -393,6 +395,12 @@ protected: SITL::SIM sitl; #endif +#if AP_DDS_ENABLED + // Declare the dds client for communication with ROS2 and DDS(common for all vehicles) + AP_DDS_Client *dds_client; + bool init_dds_client() WARN_IF_UNUSED; +#endif + private: // delay() callback that processing MAVLink packets