#include <AP_HAL/AP_HAL_Boards.h>
#include "AP_DDS_config.h"
#if AP_DDS_ENABLED
#include <uxr/client/util/ping.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_HAL/AP_HAL.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_RTC/AP_RTC.h>
#include <AP_Math/AP_Math.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_AHRS/AP_AHRS.h>
#if AP_DDS_ARM_SERVER_ENABLED
#include <AP_Arming/AP_Arming.h>
# endif // AP_DDS_ARM_SERVER_ENABLED
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Common/AP_FWVersion.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h>
#if AP_DDS_ARM_SERVER_ENABLED
#include "ardupilot_msgs/srv/ArmMotors.h"
#endif // AP_DDS_ARM_SERVER_ENABLED
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
#include "ardupilot_msgs/srv/ModeSwitch.h"
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
#if AP_DDS_ARM_CHECK_SERVER_ENABLED
#include "std_srvs/srv/Trigger.h"
#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#include "ardupilot_msgs/srv/Takeoff.h"
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#if AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_ExternalControl.h"
#endif // AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_Frames.h"
#include "AP_DDS_Client.h"
#include "AP_DDS_Topic_Table.h"
#include "AP_DDS_Service_Table.h"
#include "AP_DDS_External_Odom.h"
#define STRCPY(D,S) strncpy(D, S, ARRAY_SIZE(D))
// Enable DDS at runtime by default
static constexpr uint8_t ENABLED_BY_DEFAULT = 1;
#if AP_DDS_TIME_PUB_ENABLED
static constexpr uint16_t DELAY_TIME_TOPIC_MS = AP_DDS_DELAY_TIME_TOPIC_MS;
#endif // AP_DDS_TIME_PUB_ENABLED
#if AP_DDS_BATTERY_STATE_PUB_ENABLED
static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = AP_DDS_DELAY_BATTERY_STATE_TOPIC_MS;
#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
#if AP_DDS_IMU_PUB_ENABLED
static constexpr uint16_t DELAY_IMU_TOPIC_MS = AP_DDS_DELAY_IMU_TOPIC_MS;
#endif // AP_DDS_IMU_PUB_ENABLED
#if AP_DDS_LOCAL_POSE_PUB_ENABLED
static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = AP_DDS_DELAY_LOCAL_POSE_TOPIC_MS;
#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
#if AP_DDS_LOCAL_VEL_PUB_ENABLED
static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = AP_DDS_DELAY_LOCAL_VELOCITY_TOPIC_MS;
#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
#if AP_DDS_AIRSPEED_PUB_ENABLED
static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_MS;
#endif // AP_DDS_AIRSPEED_PUB_ENABLED
#if AP_DDS_GEOPOSE_PUB_ENABLED
static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = AP_DDS_DELAY_GEO_POSE_TOPIC_MS;
#endif // AP_DDS_GEOPOSE_PUB_ENABLED
#if AP_DDS_GOAL_PUB_ENABLED & AP_SCRIPTING_ENABLED
static constexpr uint16_t DELAY_GOAL_TOPIC_MS = AP_DDS_DELAY_GOAL_TOPIC_MS ;
#endif // AP_DDS_GOAL_PUB_ENABLED & AP_SCRIPTING_ENABLED
#if AP_DDS_CLOCK_PUB_ENABLED
static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS;
#endif // AP_DDS_CLOCK_PUB_ENABLED
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = AP_DDS_DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS;
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
static constexpr uint16_t DELAY_PING_MS = 500;
#if AP_DDS_STATUS_PUB_ENABLED
static constexpr uint16_t DELAY_STATUS_TOPIC_MS = AP_DDS_DELAY_STATUS_TOPIC_MS;
#endif // AP_DDS_STATUS_PUB_ENABLED
// Define the subscriber data members, which are static class scope.
// If these are created on the stack in the subscriber,
// the AP_DDS_Client::on_topic frame size is exceeded.
#if AP_DDS_JOY_SUB_ENABLED
sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {};
#endif // AP_DDS_JOY_SUB_ENABLED
#if AP_DDS_DYNAMIC_TF_SUB_ENABLED
tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {};
#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
#if AP_DDS_VEL_CTRL_ENABLED
geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {};
#endif // AP_DDS_VEL_CTRL_ENABLED
#if AP_DDS_GLOBAL_POS_CTRL_ENABLED
ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {};
#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
// Define the parameter server data members, which are static class scope.
// If these are created on the stack, then the AP_DDS_Client::on_request
// frame size is exceeded.
#if AP_DDS_PARAMETER_SERVER_ENABLED
rcl_interfaces_srv_SetParameters_Request AP_DDS_Client::set_parameter_request {};
rcl_interfaces_srv_SetParameters_Response AP_DDS_Client::set_parameter_response {};
rcl_interfaces_srv_GetParameters_Request AP_DDS_Client::get_parameters_request {};
rcl_interfaces_srv_GetParameters_Response AP_DDS_Client::get_parameters_response {};
rcl_interfaces_msg_Parameter AP_DDS_Client::param {};
#endif
const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
// @Param: _ENABLE
// @DisplayName: DDS enable
// @Description: Enable DDS subsystem
// @Values: 0:Disabled,1:Enabled
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_DDS_Client, enabled, ENABLED_BY_DEFAULT, AP_PARAM_FLAG_ENABLE),
#if AP_DDS_UDP_ENABLED
// @Param: _UDP_PORT
// @DisplayName: DDS UDP port
// @Description: UDP port number for DDS
// @Range: 1 65535
// @RebootRequired: True
// @User: Standard
AP_GROUPINFO("_UDP_PORT", 2, AP_DDS_Client, udp.port, 2019),
// @Group: _IP
// @Path: ../AP_Networking/AP_Networking_address.cpp
AP_SUBGROUPINFO(udp.ip, "_IP", 3, AP_DDS_Client, AP_Networking_IPV4),
#endif
// @Param: _DOMAIN_ID
// @DisplayName: DDS DOMAIN ID
// @Description: Set the ROS_DOMAIN_ID
// @Range: 0 232
// @RebootRequired: True
// @User: Standard
AP_GROUPINFO("_DOMAIN_ID", 4, AP_DDS_Client, domain_id, 0),
// @Param: _TIMEOUT_MS
// @DisplayName: DDS ping timeout
// @Description: The time in milliseconds the DDS client will wait for a response from the XRCE agent before reattempting.
// @Units: ms
// @Range: 1 10000
// @RebootRequired: True
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_TIMEOUT_MS", 5, AP_DDS_Client, ping_timeout_ms, 1000),
// @Param: _MAX_RETRY
// @DisplayName: DDS ping max attempts
// @Description: The maximum number of times the DDS client will attempt to ping the XRCE agent before exiting.
// @Range: 1 100
// @RebootRequired: True
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_MAX_RETRY", 6, AP_DDS_Client, ping_max_retry, 10),
AP_GROUPEND
};
#if AP_DDS_STATIC_TF_PUB_ENABLED | AP_DDS_LOCAL_POSE_PUB_ENABLED | AP_DDS_GEOPOSE_PUB_ENABLED | AP_DDS_IMU_PUB_ENABLED
static void initialize(geometry_msgs_msg_Quaternion& q)
{
q.x = 0.0;
q.y = 0.0;
q.z = 0.0;
q.w = 1.0;
}
#endif // AP_DDS_STATIC_TF_PUB_ENABLED | AP_DDS_LOCAL_POSE_PUB_ENABLED | AP_DDS_GEOPOSE_PUB_ENABLED | AP_DDS_IMU_PUB_ENABLED
AP_DDS_Client::~AP_DDS_Client()
{
// close transport
if (is_using_serial) {
uxr_close_custom_transport(&serial.transport);
} else {
#if AP_DDS_UDP_ENABLED
uxr_close_custom_transport(&udp.transport);
#endif
}
}
#if AP_DDS_TIME_PUB_ENABLED
void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg)
{
uint64_t utc_usec;
if (!AP::rtc().get_utc_usec(utc_usec)) {
utc_usec = AP_HAL::micros64();
}
msg.sec = utc_usec / 1000000ULL;
msg.nanosec = (utc_usec % 1000000ULL) * 1000UL;
}
#endif // AP_DDS_TIME_PUB_ENABLED
#if AP_DDS_NAVSATFIX_PUB_ENABLED
bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance)
{
// Add a lambda that takes in navsatfix msg and populates the cov
// Make it constexpr if possible
// https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/
// constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; };
auto &gps = AP::gps();
WITH_SEMAPHORE(gps.get_semaphore());
if (!gps.is_healthy(instance)) {
msg.status.status = -1; // STATUS_NO_FIX
msg.status.service = 0; // No services supported
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
return false;
}
// No update is needed
const auto last_fix_time_ms = gps.last_fix_time_ms(instance);
if (last_nav_sat_fix_time_ms == last_fix_time_ms) {
return false;
} else {
last_nav_sat_fix_time_ms = last_fix_time_ms;
}
update_topic(msg.header.stamp);
static_assert(GPS_MAX_RECEIVERS <= 9, "GPS_MAX_RECEIVERS is greater than 9");
hal.util->snprintf(msg.header.frame_id, 2, "%u", instance);
msg.status.service = 0; // SERVICE_GPS
msg.status.status = -1; // STATUS_NO_FIX
//! @todo What about glonass, compass, galileo?
//! This will be properly designed and implemented to spec in #23277
msg.status.service = 1; // SERVICE_GPS
const auto status = gps.status(instance);
switch (status) {
case AP_GPS::NO_GPS:
case AP_GPS::NO_FIX:
msg.status.status = -1; // STATUS_NO_FIX
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
return true;
case AP_GPS::GPS_OK_FIX_2D:
case AP_GPS::GPS_OK_FIX_3D:
msg.status.status = 0; // STATUS_FIX
break;
case AP_GPS::GPS_OK_FIX_3D_DGPS:
msg.status.status = 1; // STATUS_SBAS_FIX
break;
case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT:
case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED:
msg.status.status = 2; // STATUS_SBAS_FIX
break;
default:
//! @todo Can we not just use an enum class and not worry about this condition?
break;
}
const auto loc = gps.location(instance);
msg.latitude = loc.lat * 1E-7;
msg.longitude = loc.lng * 1E-7;
int32_t alt_cm;
if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
// With absolute frame, this condition is unlikely
msg.status.status = -1; // STATUS_NO_FIX
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
return true;
}
msg.altitude = alt_cm * 0.01;
// ROS allows double precision, ArduPilot exposes float precision today
Matrix3f cov;
msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov);
msg.position_covariance[0] = cov[0][0];
msg.position_covariance[1] = cov[0][1];
msg.position_covariance[2] = cov[0][2];
msg.position_covariance[3] = cov[1][0];
msg.position_covariance[4] = cov[1][1];
msg.position_covariance[5] = cov[1][2];
msg.position_covariance[6] = cov[2][0];
msg.position_covariance[7] = cov[2][1];
msg.position_covariance[8] = cov[2][2];
return true;
}
#endif // AP_DDS_NAVSATFIX_PUB_ENABLED
#if AP_DDS_STATIC_TF_PUB_ENABLED
void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
{
msg.transforms_size = 0;
auto &gps = AP::gps();
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
const auto gps_type = gps.get_type(i);
if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
continue;
}
update_topic(msg.transforms[i].header.stamp);
char gps_frame_id[16];
//! @todo should GPS frame ID's be 0 or 1 indexed in ROS?
hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i);
STRCPY(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
STRCPY(msg.transforms[i].child_frame_id, gps_frame_id);
// The body-frame offsets
// X - Forward
// Y - Right
// Z - Down
// https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation
const auto offset = gps.get_antenna_offset(i);
// In ROS REP 103, it follows this convention
// X - Forward
// Y - Left
// Z - Up
// https://www.ros.org/reps/rep-0103.html#axis-orientation
msg.transforms[i].transform.translation.x = offset[0];
msg.transforms[i].transform.translation.y = -1 * offset[1];
msg.transforms[i].transform.translation.z = -1 * offset[2];
// Ensure rotation is initialized.
initialize(msg.transforms[i].transform.rotation);
msg.transforms_size++;
}
}
#endif // AP_DDS_STATIC_TF_PUB_ENABLED
#if AP_DDS_BATTERY_STATE_PUB_ENABLED
void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance)
{
if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
return;
}
static_assert(AP_BATT_MONITOR_MAX_INSTANCES <= 99, "AP_BATT_MONITOR_MAX_INSTANCES is greater than 99");
update_topic(msg.header.stamp);
hal.util->snprintf(msg.header.frame_id, 2, "%u", instance);
auto &battery = AP::battery();
if (!battery.healthy(instance)) {
msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD
msg.present = false;
return;
}
msg.present = true;
msg.voltage = battery.voltage(instance);
float temperature;
msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN;
float current;
msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN;
const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001;
msg.design_capacity = design_capacity;
uint8_t percentage;
if (battery.capacity_remaining_pct(percentage, instance)) {
msg.percentage = percentage * 0.01;
msg.charge = (percentage * design_capacity) * 0.01;
} else {
msg.percentage = NAN;
msg.charge = NAN;
}
msg.capacity = NAN;
if (battery.current_amps(current, instance)) {
if (percentage == 100) {
msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL
} else if (is_negative(current)) {
msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING
} else if (is_positive(current)) {
msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING
} else {
msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING
}
} else {
msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN
}
msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD
msg.power_supply_technology = 0; //POWER_SUPPLY_TECHNOLOGY_UNKNOWN
if (battery.has_cell_voltages(instance)) {
const auto &cells = battery.get_cell_voltages(instance);
const uint8_t ncells_max = MIN(ARRAY_SIZE(msg.cell_voltage), ARRAY_SIZE(cells.cells));
for (uint8_t i=0; i< ncells_max; i++) {
msg.cell_voltage[i] = cells.cells[i] * 0.001;
}
}
}
#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
#if AP_DDS_LOCAL_POSE_PUB_ENABLED
void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
{
update_topic(msg.header.stamp);
STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
auto &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
// ROS REP 103 uses the ENU convention:
// X - East
// Y - North
// Z - Up
// https://www.ros.org/reps/rep-0103.html#axis-orientation
// AP_AHRS uses the NED convention
// X - North
// Y - East
// Z - Down
// As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
// as well as invert Z
Vector3f position;
if (ahrs.get_relative_position_NED_home(position)) {
msg.pose.position.x = position[1];
msg.pose.position.y = position[0];
msg.pose.position.z = -position[2];
}
// In ROS REP 103, axis orientation uses the following convention:
// X - Forward
// Y - Left
// Z - Up
// https://www.ros.org/reps/rep-0103.html#axis-orientation
// As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
// as well as invert Z (NED to ENU conversion) as well as a 90 degree rotation in the Z axis
// for x to point forward
Quaternion orientation;
if (ahrs.get_quaternion(orientation)) {
Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation
Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation
orientation = aux * transformation;
msg.pose.orientation.w = orientation[0];
msg.pose.orientation.x = orientation[1];
msg.pose.orientation.y = orientation[2];
msg.pose.orientation.z = orientation[3];
} else {
initialize(msg.pose.orientation);
}
}
#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
#if AP_DDS_LOCAL_VEL_PUB_ENABLED
void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
{
update_topic(msg.header.stamp);
STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
auto &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
// ROS REP 103 uses the ENU convention:
// X - East
// Y - North
// Z - Up
// https://www.ros.org/reps/rep-0103.html#axis-orientation
// AP_AHRS uses the NED convention
// X - North
// Y - East
// Z - Down
// As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
// as well as invert Z
Vector3f velocity;
if (ahrs.get_velocity_NED(velocity)) {
msg.twist.linear.x = velocity[1];
msg.twist.linear.y = velocity[0];
msg.twist.linear.z = -velocity[2];
}
// In ROS REP 103, axis orientation uses the following convention:
// X - Forward
// Y - Left
// Z - Up
// https://www.ros.org/reps/rep-0103.html#axis-orientation
// The gyro data is received from AP_AHRS in body-frame
// X - Forward
// Y - Right
// Z - Down
// As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z
Vector3f angular_velocity = ahrs.get_gyro();
msg.twist.angular.x = angular_velocity[0];
msg.twist.angular.y = -angular_velocity[1];
msg.twist.angular.z = -angular_velocity[2];
}
#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
#if AP_DDS_AIRSPEED_PUB_ENABLED
bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
{
update_topic(msg.header.stamp);
STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
auto &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
// In ROS REP 103, axis orientation uses the following convention:
// X - Forward
// Y - Left
// Z - Up
// https://www.ros.org/reps/rep-0103.html#axis-orientation
// The true airspeed data is received from AP_AHRS in body-frame
// X - Forward
// Y - Right
// Z - Down
// As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z
Vector3f true_airspeed_vec_bf;
bool is_airspeed_available {false};
if (ahrs.airspeed_vector_true(true_airspeed_vec_bf)) {
msg.vector.x = true_airspeed_vec_bf[0];
msg.vector.y = -true_airspeed_vec_bf[1];
msg.vector.z = -true_airspeed_vec_bf[2];
is_airspeed_available = true;
}
return is_airspeed_available;
}
#endif // AP_DDS_AIRSPEED_PUB_ENABLED
#if AP_DDS_GEOPOSE_PUB_ENABLED
void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
{
update_topic(msg.header.stamp);
STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
auto &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Location loc;
if (ahrs.get_location(loc)) {
msg.pose.position.latitude = loc.lat * 1E-7;
msg.pose.position.longitude = loc.lng * 1E-7;
// TODO this is assumed to be absolute frame in WGS-84 as per the GeoPose message definition in ROS.
// Use loc.get_alt_frame() to convert if necessary.
msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m
}
// In ROS REP 103, axis orientation uses the following convention:
// X - Forward
// Y - Left
// Z - Up
// https://www.ros.org/reps/rep-0103.html#axis-orientation
// As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
// as well as invert Z (NED to ENU conversion) as well as a 90 degree rotation in the Z axis
// for x to point forward
Quaternion orientation;
if (ahrs.get_quaternion(orientation)) {
Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation
Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation
orientation = aux * transformation;
msg.pose.orientation.w = orientation[0];
msg.pose.orientation.x = orientation[1];
msg.pose.orientation.y = orientation[2];
msg.pose.orientation.z = orientation[3];
} else {
initialize(msg.pose.orientation);
}
}
#endif // AP_DDS_GEOPOSE_PUB_ENABLED
#if AP_DDS_GOAL_PUB_ENABLED & AP_SCRIPTING_ENABLED
bool AP_DDS_Client::update_topic_goal(geographic_msgs_msg_GeoPointStamped& msg)
{
const auto &vehicle = AP::vehicle();
update_topic(msg.header.stamp);
Location target_loc;
// Exit if no target is available.
if (!vehicle->get_target_location(target_loc)) {
return false;
}
target_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
msg.position.latitude = target_loc.lat * 1e-7;
msg.position.longitude = target_loc.lng * 1e-7;
msg.position.altitude = target_loc.alt * 1e-2;
// Check whether the goal has changed or if the topic has never been published.
const double tolerance_lat_lon = 1e-8; // One order of magnitude smaller than the target's resolution.
const double distance_alt = 1e-3;
if (abs(msg.position.latitude - prev_goal_msg.position.latitude) > tolerance_lat_lon ||
abs(msg.position.longitude - prev_goal_msg.position.longitude) > tolerance_lat_lon ||
abs(msg.position.altitude - prev_goal_msg.position.altitude) > distance_alt ||
prev_goal_msg.header.stamp.sec == 0 ) {
update_topic(prev_goal_msg.header.stamp);
prev_goal_msg.position.latitude = msg.position.latitude;
prev_goal_msg.position.longitude = msg.position.longitude;
prev_goal_msg.position.altitude = msg.position.altitude;
return true;
} else {
return false;
}
}
#endif // AP_DDS_GOAL_PUB_ENABLED & AP_SCRIPTING_ENABLED
#if AP_DDS_IMU_PUB_ENABLED
void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
{
update_topic(msg.header.stamp);
STRCPY(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
auto &imu = AP::ins();
auto &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Quaternion orientation;
if (ahrs.get_quaternion(orientation)) {
msg.orientation.x = orientation[0];
msg.orientation.y = orientation[1];
msg.orientation.z = orientation[2];
msg.orientation.w = orientation[3];
} else {
initialize(msg.orientation);
}
msg.orientation_covariance[0] = -1;
uint8_t accel_index = ahrs.get_primary_accel_index();
uint8_t gyro_index = ahrs.get_primary_gyro_index();
const Vector3f accel_data = imu.get_accel(accel_index);
const Vector3f gyro_data = imu.get_gyro(gyro_index);
// Populate the message fields
msg.linear_acceleration.x = accel_data.x;
msg.linear_acceleration.y = accel_data.y;
msg.linear_acceleration.z = accel_data.z;
msg.angular_velocity.x = gyro_data.x;
msg.angular_velocity.y = gyro_data.y;
msg.angular_velocity.z = gyro_data.z;
msg.angular_velocity_covariance[0] = -1;
msg.linear_acceleration_covariance[0] = -1;
}
#endif // AP_DDS_IMU_PUB_ENABLED
#if AP_DDS_CLOCK_PUB_ENABLED
void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
{
update_topic(msg.clock);
}
#endif // AP_DDS_CLOCK_PUB_ENABLED
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg)
{
update_topic(msg.header.stamp);
STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
auto &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Location ekf_origin;
// LLA is WGS-84 geodetic coordinate.
// Altitude converted from cm to m.
if (ahrs.get_origin(ekf_origin)) {
msg.position.latitude = ekf_origin.lat * 1E-7;
msg.position.longitude = ekf_origin.lng * 1E-7;
msg.position.altitude = ekf_origin.alt * 0.01;
}
}
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg)
{
// Fill the new message.
const auto &vehicle = AP::vehicle();
const auto &battery = AP::battery();
msg.vehicle_type = static_cast<uint8_t>(AP::fwversion().vehicle_type);
msg.armed = hal.util->get_soft_armed();
msg.mode = vehicle->get_mode();
msg.flying = vehicle->get_likely_flying();
msg.external_control = true; // Always true for now. To be filled after PR#28429.
uint8_t fs_iter = 0;
msg.failsafe_size = 0;
if (rc().in_rc_failsafe()) {
msg.failsafe[fs_iter++] = FS_RADIO;
}
if (battery.has_failsafed()) {
msg.failsafe[fs_iter++] = FS_BATTERY;
}
// TODO: replace flag with function.
if (AP_Notify::flags.failsafe_gcs) {
msg.failsafe[fs_iter++] = FS_GCS;
}
// TODO: replace flag with function.
if (AP_Notify::flags.failsafe_ekf) {
msg.failsafe[fs_iter++] = FS_EKF;
}
msg.failsafe_size = fs_iter;
// Compare with the previous one.
bool is_message_changed {false};
is_message_changed |= (last_status_msg_.flying != msg.flying);
is_message_changed |= (last_status_msg_.armed != msg.armed);
is_message_changed |= (last_status_msg_.mode != msg.mode);
is_message_changed |= (last_status_msg_.vehicle_type != msg.vehicle_type);
is_message_changed |= (last_status_msg_.failsafe_size != msg.failsafe_size);
is_message_changed |= (last_status_msg_.external_control != msg.external_control);
if ( is_message_changed ) {
last_status_msg_.flying = msg.flying;
last_status_msg_.armed = msg.armed;
last_status_msg_.mode = msg.mode;
last_status_msg_.vehicle_type = msg.vehicle_type;
last_status_msg_.failsafe_size = msg.failsafe_size;
last_status_msg_.external_control = msg.external_control;
update_topic(msg.header.stamp);
return true;
} else {
return false;
}
}
#endif // AP_DDS_STATUS_PUB_ENABLED
/*
start the DDS thread
*/
bool AP_DDS_Client::start(void)
{
AP_Param::setup_object_defaults(this, var_info);
AP_Param::load_object_from_eeprom(this, var_info);
if (enabled == 0) {
return true;
}
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_DDS_Client::main_loop, void),
"DDS",
8192, AP_HAL::Scheduler::PRIORITY_IO, 1)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Thread create failed", msg_prefix);
return false;
}
return true;
}
// read function triggered at every subscription callback
void AP_DDS_Client::on_topic_trampoline(uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length,
void* args)
{
AP_DDS_Client *dds = (AP_DDS_Client *)args;
dds->on_topic(uxr_session, object_id, request_id, stream_id, ub, length);
}
void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length)
{
/*
TEMPLATE for reading to the subscribed topics
1) Store the read contents into the ucdr buffer
2) Deserialize the said contents into the topic instance
*/
(void) uxr_session;
(void) request_id;
(void) stream_id;
(void) length;
switch (object_id.id) {
#if AP_DDS_JOY_SUB_ENABLED
case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id: {
const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &rx_joy_topic);
if (success == false) {
break;
}
if (rx_joy_topic.axes_size >= 4) {
const uint32_t t_now = AP_HAL::millis();
for (uint8_t i = 0; i < MIN(8U, rx_joy_topic.axes_size); i++) {
// Ignore channel override if NaN.
if (std::isnan(rx_joy_topic.axes[i])) {
// Setting the RC override to 0U releases the channel back to the RC.
RC_Channels::set_override(i, 0U, t_now);
} else {
const uint16_t mapped_data = static_cast<uint16_t>(
linear_interpolate(rc().channel(i)->get_radio_min(),
rc().channel(i)->get_radio_max(),
rx_joy_topic.axes[i],
-1.0, 1.0));
RC_Channels::set_override(i, mapped_data, t_now);
}
}
}
break;
}
#endif // AP_DDS_JOY_SUB_ENABLED
#if AP_DDS_DYNAMIC_TF_SUB_ENABLED
case topics[to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB)].dr_id.id: {
const bool success = tf2_msgs_msg_TFMessage_deserialize_topic(ub, &rx_dynamic_transforms_topic);
if (success == false) {
break;
}
if (rx_dynamic_transforms_topic.transforms_size > 0) {
#if AP_DDS_VISUALODOM_ENABLED
AP_DDS_External_Odom::handle_external_odom(rx_dynamic_transforms_topic);
#endif // AP_DDS_VISUALODOM_ENABLED
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received tf2_msgs/TFMessage: TF is empty", msg_prefix);
}
break;
}
#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
#if AP_DDS_VEL_CTRL_ENABLED
case topics[to_underlying(TopicIndex::VELOCITY_CONTROL_SUB)].dr_id.id: {
const bool success = geometry_msgs_msg_TwistStamped_deserialize_topic(ub, &rx_velocity_control_topic);
if (success == false) {
break;
}
#if AP_EXTERNAL_CONTROL_ENABLED
if (!AP_DDS_External_Control::handle_velocity_control(rx_velocity_control_topic)) {
// TODO #23430 handle velocity control failure through rosout, throttled.
}
#endif // AP_EXTERNAL_CONTROL_ENABLED
break;
}
#endif // AP_DDS_VEL_CTRL_ENABLED
#if AP_DDS_GLOBAL_POS_CTRL_ENABLED
case topics[to_underlying(TopicIndex::GLOBAL_POSITION_SUB)].dr_id.id: {
const bool success = ardupilot_msgs_msg_GlobalPosition_deserialize_topic(ub, &rx_global_position_control_topic);
if (success == false) {
break;
}
#if AP_EXTERNAL_CONTROL_ENABLED
if (!AP_DDS_External_Control::handle_global_position_control(rx_global_position_control_topic)) {
// TODO #23430 handle global position control failure through rosout, throttled.
}
#endif // AP_EXTERNAL_CONTROL_ENABLED
break;
}
#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
}
}
/*
callback on request completion
*/
void AP_DDS_Client::on_request_trampoline(uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length, void* args)
{
AP_DDS_Client *dds = (AP_DDS_Client *)args;
dds->on_request(uxr_session, object_id, request_id, sample_id, ub, length);
}
void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length)
{
(void) request_id;
(void) length;
switch (object_id.id) {
#if AP_DDS_ARM_SERVER_ENABLED
case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: {
ardupilot_msgs_srv_ArmMotors_Request arm_motors_request;
ardupilot_msgs_srv_ArmMotors_Response arm_motors_response;
const bool deserialize_success = ardupilot_msgs_srv_ArmMotors_Request_deserialize_topic(ub, &arm_motors_request);
if (deserialize_success == false) {
break;
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for %sing received", msg_prefix, arm_motors_request.arm ? "arm" : "disarm");
#if AP_EXTERNAL_CONTROL_ENABLED
const bool do_checks = true;
arm_motors_response.result = arm_motors_request.arm ? AP_DDS_External_Control::arm(AP_Arming::Method::DDS, do_checks) : AP_DDS_External_Control::disarm(AP_Arming::Method::DDS, do_checks);
if (!arm_motors_response.result) {
// TODO #23430 handle arm failure through rosout, throttled.
}
#endif // AP_EXTERNAL_CONTROL_ENABLED
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id,
.type = UXR_REPLIER_ID
};
uint8_t reply_buffer[8] {};
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
const bool serialize_success = ardupilot_msgs_srv_ArmMotors_Response_serialize_topic(&reply_ub, &arm_motors_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Arming/Disarming : %s", msg_prefix, arm_motors_response.result ? "SUCCESS" : "FAIL");
break;
}
#endif // AP_DDS_ARM_SERVER_ENABLED
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: {
ardupilot_msgs_srv_ModeSwitch_Request mode_switch_request;
ardupilot_msgs_srv_ModeSwitch_Response mode_switch_response;
const bool deserialize_success = ardupilot_msgs_srv_ModeSwitch_Request_deserialize_topic(ub, &mode_switch_request);
if (deserialize_success == false) {
break;
}
mode_switch_response.status = AP::vehicle()->set_mode(mode_switch_request.mode, ModeReason::DDS_COMMAND);
mode_switch_response.curr_mode = AP::vehicle()->get_mode();
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id,
.type = UXR_REPLIER_ID
};
uint8_t reply_buffer[8] {};
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
const bool serialize_success = ardupilot_msgs_srv_ModeSwitch_Response_serialize_topic(&reply_ub, &mode_switch_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Mode Switch : %s", msg_prefix, mode_switch_response.status ? "SUCCESS" : "FAIL");
break;
}
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
case services[to_underlying(ServiceIndex::TAKEOFF)].rep_id: {
ardupilot_msgs_srv_Takeoff_Request takeoff_request;
ardupilot_msgs_srv_Takeoff_Response takeoff_response;
const bool deserialize_success = ardupilot_msgs_srv_Takeoff_Request_deserialize_topic(ub, &takeoff_request);
if (deserialize_success == false) {
break;
}
takeoff_response.status = AP::vehicle()->start_takeoff(takeoff_request.alt);
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::TAKEOFF)].rep_id,
.type = UXR_REPLIER_ID
};
uint8_t reply_buffer[8] {};
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
const bool serialize_success = ardupilot_msgs_srv_Takeoff_Response_serialize_topic(&reply_ub, &takeoff_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Takeoff : %s", msg_prefix, takeoff_response.status ? "SUCCESS" : "FAIL");
break;
}
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#if AP_DDS_ARM_CHECK_SERVER_ENABLED
case services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id: {
std_srvs_srv_Trigger_Request prearm_check_request;
std_srvs_srv_Trigger_Response prearm_check_response;
const bool deserialize_success = std_srvs_srv_Trigger_Request_deserialize_topic(ub, &prearm_check_request);
if (deserialize_success == false) {
break;
}
prearm_check_response.success = AP::arming().pre_arm_checks(false);
STRCPY(prearm_check_response.message, prearm_check_response.success ? "Vehicle is Armable" : "Vehicle is Not Armable");
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id,
.type = UXR_REPLIER_ID
};
uint8_t reply_buffer[sizeof(prearm_check_response.message) + 1] {};
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
const bool serialize_success = std_srvs_srv_Trigger_Response_serialize_topic(&reply_ub, &prearm_check_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
break;
}
#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: {
const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request);
if (deserialize_success == false) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Set Parameters Request : Failed to deserialize request.", msg_prefix);
break;
}
if (set_parameter_request.parameters_size > 8U) {
break;
}
// Set parameters and responses for each one requested
set_parameter_response.results_size = set_parameter_request.parameters_size;
for (size_t i = 0; i < set_parameter_request.parameters_size; i++) {
param = set_parameter_request.parameters[i];
enum ap_var_type var_type;
// set parameter
AP_Param *vp;
char param_key[AP_MAX_NAME_SIZE + 1];
strncpy(param_key, (char *)param.name, AP_MAX_NAME_SIZE);
param_key[AP_MAX_NAME_SIZE] = 0;
// Currently only integer and double value types can be set.
// The following parameter value types are not handled:
// bool, string, byte_array, bool_array, integer_array, double_array and string_array
bool param_isnan = true;
bool param_isinf = true;
float param_value = 0.0f;
switch (param.value.type) {
case PARAMETER_INTEGER: {
param_isnan = isnan(param.value.integer_value);
param_isinf = isinf(param.value.integer_value);
param_value = float(param.value.integer_value);
break;
}
case PARAMETER_DOUBLE: {
param_isnan = isnan(param.value.double_value);
param_isinf = isinf(param.value.double_value);
param_value = float(param.value.double_value);
break;
}
default: {
break;
}
}
// find existing param to get the old value
uint16_t parameter_flags = 0;
vp = AP_Param::find(param_key, &var_type, ¶meter_flags);
if (vp == nullptr || param_isnan || param_isinf) {
set_parameter_response.results[i].successful = false;
strncpy(set_parameter_response.results[i].reason, "Parameter not found", sizeof(set_parameter_response.results[i].reason));
continue;
}
// Add existing parameter checks used in GCS_Param.cpp
if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) {
// The user can set BRD_OPTIONS to enable set of internal
// parameters, for developer testing or unusual use cases
if (AP_BoardConfig::allow_set_internal_parameters()) {
parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY;
}
}
if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {
set_parameter_response.results[i].successful = false;
strncpy(set_parameter_response.results[i].reason, "Parameter is read only",sizeof(set_parameter_response.results[i].reason));
continue;
}
// Set and save the value if it changed
bool force_save = vp->set_and_save_by_name_ifchanged(param_key, param_value);
if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) {
AP_Param::invalidate_count();
}
set_parameter_response.results[i].successful = true;
strncpy(set_parameter_response.results[i].reason, "Parameter accepted", sizeof(set_parameter_response.results[i].reason));
}
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id,
.type = UXR_REPLIER_ID
};
const uint32_t reply_size = rcl_interfaces_srv_SetParameters_Response_size_of_topic(&set_parameter_response, 0U);
uint8_t reply_buffer[reply_size];
memset(reply_buffer, 0, reply_size * sizeof(uint8_t));
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, reply_size);
const bool serialize_success = rcl_interfaces_srv_SetParameters_Response_serialize_topic(&reply_ub, &set_parameter_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
bool successful_params = true;
for (size_t i = 0; i < set_parameter_response.results_size; i++) {
// Check that all the parameters were set successfully
successful_params &= set_parameter_response.results[i].successful;
}
GCS_SEND_TEXT(successful_params ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Set Parameters Request : %s", msg_prefix, successful_params ? "SUCCESSFUL" : "ONE OR MORE PARAMS FAILED" );
break;
}
case services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id: {
const bool deserialize_success = rcl_interfaces_srv_GetParameters_Request_deserialize_topic(ub, &get_parameters_request);
if (deserialize_success == false) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Get Parameters Request : Failed to deserialize request.", msg_prefix);
break;
}
if (get_parameters_request.names_size > 8U) {
break;
}
bool successful_read = true;
get_parameters_response.values_size = get_parameters_request.names_size;
for (size_t i = 0; i < get_parameters_request.names_size; i++) {
enum ap_var_type var_type;
AP_Param *vp;
char param_key[AP_MAX_NAME_SIZE + 1];
strncpy(param_key, (char *)get_parameters_request.names[i], AP_MAX_NAME_SIZE);
param_key[AP_MAX_NAME_SIZE] = 0;
vp = AP_Param::find(param_key, &var_type);
if (vp == nullptr) {
get_parameters_response.values[i].type = PARAMETER_NOT_SET;
successful_read &= false;
continue;
}
switch (var_type) {
case AP_PARAM_INT8: {
get_parameters_response.values[i].type = PARAMETER_INTEGER;
get_parameters_response.values[i].integer_value = ((AP_Int8 *)vp)->get();
successful_read &= true;
break;
}
case AP_PARAM_INT16: {
get_parameters_response.values[i].type = PARAMETER_INTEGER;
get_parameters_response.values[i].integer_value = ((AP_Int16 *)vp)->get();
successful_read &= true;
break;
}
case AP_PARAM_INT32: {
get_parameters_response.values[i].type = PARAMETER_INTEGER;
get_parameters_response.values[i].integer_value = ((AP_Int32 *)vp)->get();
successful_read &= true;
break;
}
case AP_PARAM_FLOAT: {
get_parameters_response.values[i].type = PARAMETER_DOUBLE;
get_parameters_response.values[i].double_value = vp->cast_to_float(var_type);
successful_read &= true;
break;
}
default: {
get_parameters_response.values[i].type = PARAMETER_NOT_SET;
successful_read &= false;
break;
}
}
}
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id,
.type = UXR_REPLIER_ID
};
const uint32_t reply_size = rcl_interfaces_srv_GetParameters_Response_size_of_topic(&get_parameters_response, 0U);
uint8_t reply_buffer[reply_size];
memset(reply_buffer, 0, reply_size * sizeof(uint8_t));
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, reply_size);
const bool serialize_success = rcl_interfaces_srv_GetParameters_Response_serialize_topic(&reply_ub, &get_parameters_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
GCS_SEND_TEXT(successful_read ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Get Parameters Request : %s", msg_prefix, successful_read ? "SUCCESSFUL" : "ONE OR MORE PARAM NOT FOUND");
break;
}
#endif // AP_DDS_PARAMETER_SERVER_ENABLED
}
}
/*
main loop for DDS thread
*/
void AP_DDS_Client::main_loop(void)
{
if (!init_transport()) {
return;
}
//! @todo check for request to stop task
while (true) {
if (comm == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s transport invalid, exiting", msg_prefix);
return;
}
// check ping
if (!uxr_ping_agent_attempts(comm, ping_timeout_ms, ping_max_retry)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s No ping response, exiting", msg_prefix);
return;
}
// create session
if (!init_session() || !create()) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Creation Requests failed", msg_prefix);
return;
}
connected = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Initialization passed", msg_prefix);
#if AP_DDS_STATIC_TF_PUB_ENABLED
populate_static_transforms(tx_static_transforms_topic);
write_static_transforms();
#endif // AP_DDS_STATIC_TF_PUB_ENABLED
uint64_t last_ping_ms{0};
uint8_t num_pings_missed{0};
bool had_ping_reply{false};
while (connected) {
hal.scheduler->delay(1);
// publish topics
update();
// check ping response
if (session.on_pong_flag == PONG_IN_SESSION_STATUS) {
had_ping_reply = true;
}
const auto cur_time_ms = AP_HAL::millis64();
if (cur_time_ms - last_ping_ms > DELAY_PING_MS) {
last_ping_ms = cur_time_ms;
if (had_ping_reply) {
num_pings_missed = 0;
} else {
++num_pings_missed;
}
const int ping_agent_timeout_ms{0};
const uint8_t ping_agent_attempts{1};
uxr_ping_agent_session(&session, ping_agent_timeout_ms, ping_agent_attempts);
had_ping_reply = false;
}
if (num_pings_missed > 2) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,
"%s No ping response, disconnecting", msg_prefix);
connected = false;
}
}
// delete session if connected
if (connected) {
uxr_delete_session(&session);
}
}
}
bool AP_DDS_Client::init_transport()
{
// serial init will fail if the SERIALn_PROTOCOL is not setup
bool initTransportStatus = ddsSerialInit();
is_using_serial = initTransportStatus;
#if AP_DDS_UDP_ENABLED
// fallback to UDP if available
if (!initTransportStatus) {
initTransportStatus = ddsUdpInit();
}
#endif
if (!initTransportStatus) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Transport initialization failed", msg_prefix);
return false;
}
return true;
}
bool AP_DDS_Client::init_session()
{
// init session
uxr_init_session(&session, comm, key);
// Register topic callbacks
uxr_set_topic_callback(&session, AP_DDS_Client::on_topic_trampoline, this);
// ROS-2 Service : Register service request callbacks
uxr_set_request_callback(&session, AP_DDS_Client::on_request_trampoline, this);
while (!uxr_create_session(&session)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Initialization waiting...", msg_prefix);
hal.scheduler->delay(1000);
}
// setup reliable stream buffers
input_reliable_stream = NEW_NOTHROW uint8_t[DDS_BUFFER_SIZE];
output_reliable_stream = NEW_NOTHROW uint8_t[DDS_BUFFER_SIZE];
if (input_reliable_stream == nullptr || output_reliable_stream == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Allocation failed", msg_prefix);
return false;
}
reliable_in = uxr_create_input_reliable_stream(&session, input_reliable_stream, DDS_BUFFER_SIZE, DDS_STREAM_HISTORY);
reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream, DDS_BUFFER_SIZE, DDS_STREAM_HISTORY);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Init complete", msg_prefix);
return true;
}
bool AP_DDS_Client::create()
{
WITH_SEMAPHORE(csem);
// Participant
const uxrObjectId participant_id = {
.id = 0x01,
.type = UXR_PARTICIPANT_ID
};
const char* participant_name = AP_DDS_PARTICIPANT_NAME;
const auto participant_req_id = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id,
static_cast<uint16_t>(domain_id), participant_name, UXR_REPLACE);
//Participant requests
constexpr uint8_t nRequestsParticipant = 1;
const uint16_t requestsParticipant[nRequestsParticipant] = {participant_req_id};
constexpr uint16_t maxTimeMsPerRequestMs = 500;
constexpr uint16_t requestTimeoutParticipantMs = (uint16_t) nRequestsParticipant * maxTimeMsPerRequestMs;
uint8_t statusParticipant[nRequestsParticipant];
if (!uxr_run_session_until_all_status(&session, requestTimeoutParticipantMs, requestsParticipant, statusParticipant, nRequestsParticipant)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Participant session request failure", msg_prefix);
// TODO add a failure log message sharing the status results
return false;
}
for (uint16_t i = 0 ; i < ARRAY_SIZE(topics); i++) {
// Topic
const uxrObjectId topic_id = {
.id = topics[i].topic_id,
.type = UXR_TOPIC_ID
};
const auto topic_req_id = uxr_buffer_create_topic_bin(&session, reliable_out, topic_id,
participant_id, topics[i].topic_name, topics[i].type_name, UXR_REPLACE);
// Status requests
constexpr uint8_t nRequests = 3;
uint16_t requests[nRequests];
constexpr uint16_t requestTimeoutMs = nRequests * maxTimeMsPerRequestMs;
uint8_t status[nRequests];
if (topics[i].topic_rw == Topic_rw::DataWriter) {
// Publisher
const uxrObjectId pub_id = {
.id = topics[i].pub_id,
.type = UXR_PUBLISHER_ID
};
const auto pub_req_id = uxr_buffer_create_publisher_bin(&session, reliable_out, pub_id,
participant_id, UXR_REPLACE);
// Data Writer
const auto dwriter_req_id = uxr_buffer_create_datawriter_bin(&session, reliable_out, topics[i].dw_id,
pub_id, topic_id, topics[i].qos, UXR_REPLACE);
// save the request statuses
requests[0] = topic_req_id;
requests[1] = pub_req_id;
requests[2] = dwriter_req_id;
if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Topic/Pub/Writer session request failure for index '%u'", msg_prefix, i);
for (uint8_t s = 0 ; s < nRequests; s++) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Status '%d' result '%u'", msg_prefix, s, status[s]);
}
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Topic/Pub/Writer session pass for index '%u'", msg_prefix, i);
}
} else if (topics[i].topic_rw == Topic_rw::DataReader) {
// Subscriber
const uxrObjectId sub_id = {
.id = topics[i].sub_id,
.type = UXR_SUBSCRIBER_ID
};
const auto sub_req_id = uxr_buffer_create_subscriber_bin(&session, reliable_out, sub_id,
participant_id, UXR_REPLACE);
// Data Reader
const auto dreader_req_id = uxr_buffer_create_datareader_bin(&session, reliable_out, topics[i].dr_id,
sub_id, topic_id, topics[i].qos, UXR_REPLACE);
// save the request statuses
requests[0] = topic_req_id;
requests[1] = sub_req_id;
requests[2] = dreader_req_id;
if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Topic/Sub/Reader session request failure for index '%u'", msg_prefix, i);
for (uint8_t s = 0 ; s < nRequests; s++) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Status '%d' result '%u'", msg_prefix, s, status[s]);
}
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Topic/Sub/Reader session pass for index '%u'", msg_prefix, i);
uxr_buffer_request_data(&session, reliable_out, topics[i].dr_id, reliable_in, &delivery_control);
}
}
}
// ROS-2 Service : else case for service requests
for (uint16_t i = 0; i < ARRAY_SIZE(services); i++) {
constexpr uint16_t requestTimeoutMs = maxTimeMsPerRequestMs;
if (services[i].service_rr == Service_rr::Replier) {
const uxrObjectId rep_id = {
.id = services[i].rep_id,
.type = UXR_REPLIER_ID
};
const auto replier_req_id = uxr_buffer_create_replier_bin(&session, reliable_out, rep_id,
participant_id, services[i].service_name, services[i].request_type, services[i].reply_type,
services[i].request_topic_name, services[i].reply_topic_name, services[i].qos, UXR_REPLACE);
uint16_t request = replier_req_id;
uint8_t status;
if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, &request, &status, 1)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Service/Replier session request failure for index '%u'", msg_prefix, i);
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Status result '%u'", msg_prefix, status);
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Service/Replier session pass for index '%u'", msg_prefix, i);
uxr_buffer_request_data(&session, reliable_out, rep_id, reliable_in, &delivery_control);
}
} else if (services[i].service_rr == Service_rr::Requester) {
// TODO : Add Similar Code for Requester Profile
}
}
return true;
}
void AP_DDS_Client::write_time_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = builtin_interfaces_msg_Time_size_of_topic(&time_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::TIME_PUB)].dw_id, &ub, topic_size);
const bool success = builtin_interfaces_msg_Time_serialize_topic(&ub, &time_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: XRCE_Client failed to serialize");
}
}
}
#if AP_DDS_NAVSATFIX_PUB_ENABLED
void AP_DDS_Client::write_nav_sat_fix_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = sensor_msgs_msg_NavSatFix_size_of_topic(&nav_sat_fix_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::NAV_SAT_FIX_PUB)].dw_id, &ub, topic_size);
const bool success = sensor_msgs_msg_NavSatFix_serialize_topic(&ub, &nav_sat_fix_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_NAVSATFIX_PUB_ENABLED
#if AP_DDS_STATIC_TF_PUB_ENABLED
void AP_DDS_Client::write_static_transforms()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = tf2_msgs_msg_TFMessage_size_of_topic(&tx_static_transforms_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB)].dw_id, &ub, topic_size);
const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &tx_static_transforms_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_STATIC_TF_PUB_ENABLED
#if AP_DDS_BATTERY_STATE_PUB_ENABLED
void AP_DDS_Client::write_battery_state_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = sensor_msgs_msg_BatteryState_size_of_topic(&battery_state_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::BATTERY_STATE_PUB)].dw_id, &ub, topic_size);
const bool success = sensor_msgs_msg_BatteryState_serialize_topic(&ub, &battery_state_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
#if AP_DDS_LOCAL_POSE_PUB_ENABLED
void AP_DDS_Client::write_local_pose_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = geometry_msgs_msg_PoseStamped_size_of_topic(&local_pose_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::LOCAL_POSE_PUB)].dw_id, &ub, topic_size);
const bool success = geometry_msgs_msg_PoseStamped_serialize_topic(&ub, &local_pose_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
#if AP_DDS_LOCAL_VEL_PUB_ENABLED
void AP_DDS_Client::write_tx_local_velocity_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = geometry_msgs_msg_TwistStamped_size_of_topic(&tx_local_velocity_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::LOCAL_VELOCITY_PUB)].dw_id, &ub, topic_size);
const bool success = geometry_msgs_msg_TwistStamped_serialize_topic(&ub, &tx_local_velocity_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
#if AP_DDS_AIRSPEED_PUB_ENABLED
void AP_DDS_Client::write_tx_local_airspeed_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = geometry_msgs_msg_Vector3Stamped_size_of_topic(&tx_local_airspeed_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB)].dw_id, &ub, topic_size);
const bool success = geometry_msgs_msg_Vector3Stamped_serialize_topic(&ub, &tx_local_airspeed_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_AIRSPEED_PUB_ENABLED
#if AP_DDS_IMU_PUB_ENABLED
void AP_DDS_Client::write_imu_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = sensor_msgs_msg_Imu_size_of_topic(&imu_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::IMU_PUB)].dw_id, &ub, topic_size);
const bool success = sensor_msgs_msg_Imu_serialize_topic(&ub, &imu_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_IMU_PUB_ENABLED
#if AP_DDS_GEOPOSE_PUB_ENABLED
void AP_DDS_Client::write_geo_pose_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = geographic_msgs_msg_GeoPoseStamped_size_of_topic(&geo_pose_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GEOPOSE_PUB)].dw_id, &ub, topic_size);
const bool success = geographic_msgs_msg_GeoPoseStamped_serialize_topic(&ub, &geo_pose_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_GEOPOSE_PUB_ENABLED
#if AP_DDS_CLOCK_PUB_ENABLED
void AP_DDS_Client::write_clock_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = rosgraph_msgs_msg_Clock_size_of_topic(&clock_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::CLOCK_PUB)].dw_id, &ub, topic_size);
const bool success = rosgraph_msgs_msg_Clock_serialize_topic(&ub, &clock_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_CLOCK_PUB_ENABLED
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
void AP_DDS_Client::write_gps_global_origin_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = geographic_msgs_msg_GeoPointStamped_size_of_topic(&gps_global_origin_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB)].dw_id, &ub, topic_size);
const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &gps_global_origin_topic);
if (!success) {
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
#if AP_DDS_GOAL_PUB_ENABLED & AP_SCRIPTING_ENABLED
void AP_DDS_Client::write_goal_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = geographic_msgs_msg_GeoPointStamped_size_of_topic(&goal_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GOAL_PUB)].dw_id, &ub, topic_size);
const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &goal_topic);
if (!success) {
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_GOAL_PUB_ENABLED & AP_SCRIPTING_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
void AP_DDS_Client::write_status_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = ardupilot_msgs_msg_Status_size_of_topic(&status_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATUS_PUB)].dw_id, &ub, topic_size);
const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
#endif // AP_DDS_STATUS_PUB_ENABLED
void AP_DDS_Client::update()
{
WITH_SEMAPHORE(csem);
const auto cur_time_ms = AP_HAL::millis64();
#if AP_DDS_TIME_PUB_ENABLED
if (cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) {
update_topic(time_topic);
last_time_time_ms = cur_time_ms;
write_time_topic();
}
#endif // AP_DDS_TIME_PUB_ENABLED
#if AP_DDS_NAVSATFIX_PUB_ENABLED
constexpr uint8_t gps_instance = 0;
if (update_topic(nav_sat_fix_topic, gps_instance)) {
write_nav_sat_fix_topic();
}
#endif // AP_DDS_NAVSATFIX_PUB_ENABLED
#if AP_DDS_BATTERY_STATE_PUB_ENABLED
if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) {
for (uint8_t battery_instance = 0; battery_instance < AP_BATT_MONITOR_MAX_INSTANCES; battery_instance++) {
update_topic(battery_state_topic, battery_instance);
if (battery_state_topic.present) {
write_battery_state_topic();
}
}
last_battery_state_time_ms = cur_time_ms;
}
#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
#if AP_DDS_LOCAL_POSE_PUB_ENABLED
if (cur_time_ms - last_local_pose_time_ms > DELAY_LOCAL_POSE_TOPIC_MS) {
update_topic(local_pose_topic);
last_local_pose_time_ms = cur_time_ms;
write_local_pose_topic();
}
#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
#if AP_DDS_LOCAL_VEL_PUB_ENABLED
if (cur_time_ms - last_local_velocity_time_ms > DELAY_LOCAL_VELOCITY_TOPIC_MS) {
update_topic(tx_local_velocity_topic);
last_local_velocity_time_ms = cur_time_ms;
write_tx_local_velocity_topic();
}
#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
#if AP_DDS_AIRSPEED_PUB_ENABLED
if (cur_time_ms - last_airspeed_time_ms > DELAY_AIRSPEED_TOPIC_MS) {
last_airspeed_time_ms = cur_time_ms;
if (update_topic(tx_local_airspeed_topic)) {
write_tx_local_airspeed_topic();
}
}
#endif // AP_DDS_AIRSPEED_PUB_ENABLED
#if AP_DDS_IMU_PUB_ENABLED
if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) {
update_topic(imu_topic);
last_imu_time_ms = cur_time_ms;
write_imu_topic();
}
#endif // AP_DDS_IMU_PUB_ENABLED
#if AP_DDS_GEOPOSE_PUB_ENABLED
if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) {
update_topic(geo_pose_topic);
last_geo_pose_time_ms = cur_time_ms;
write_geo_pose_topic();
}
#endif // AP_DDS_GEOPOSE_PUB_ENABLED
#if AP_DDS_CLOCK_PUB_ENABLED
if (cur_time_ms - last_clock_time_ms > DELAY_CLOCK_TOPIC_MS) {
update_topic(clock_topic);
last_clock_time_ms = cur_time_ms;
write_clock_topic();
}
#endif // AP_DDS_CLOCK_PUB_ENABLED
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
if (cur_time_ms - last_gps_global_origin_time_ms > DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS) {
update_topic(gps_global_origin_topic);
last_gps_global_origin_time_ms = cur_time_ms;
write_gps_global_origin_topic();
}
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
#if AP_DDS_GOAL_PUB_ENABLED & AP_SCRIPTING_ENABLED
if (cur_time_ms - last_goal_time_ms > DELAY_GOAL_TOPIC_MS) {
if (update_topic_goal(goal_topic)) {
write_goal_topic();
}
last_goal_time_ms = cur_time_ms;
}
#endif // AP_DDS_GOAL_PUB_ENABLED & AP_SCRIPTING_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) {
if (update_topic(status_topic)) {
write_status_topic();
}
last_status_check_time_ms = cur_time_ms;
}
#endif // AP_DDS_STATUS_PUB_ENABLED
status_ok = uxr_run_session_time(&session, 1);
}
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD != HAL_BOARD_ESP32
extern "C" {
int clock_gettime(clockid_t clockid, struct timespec *ts);
}
int clock_gettime(clockid_t clockid, struct timespec *ts)
{
//! @todo the value of clockid is ignored here.
//! A fallback mechanism is employed against the caller's choice of clock.
uint64_t utc_usec;
if (!AP::rtc().get_utc_usec(utc_usec)) {
utc_usec = AP_HAL::micros64();
}
ts->tv_sec = utc_usec / 1000000ULL;
ts->tv_nsec = (utc_usec % 1000000ULL) * 1000UL;
return 0;
}
#endif // CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD != HAL_BOARD_ESP32
#endif // AP_DDS_ENABLED