diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index c36c8238e8a228..98b4c572159d66 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -420,13 +420,13 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) { update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); + 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]; @@ -436,14 +436,16 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) } msg.orientation_covariance[0] = -1; - Vector3f accel_data = imu.get_accel(); - Vector3f gyro_data = imu.get_gyro(); + 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; diff --git a/libraries/AP_DDS/AP_DDS_Frames.h b/libraries/AP_DDS/AP_DDS_Frames.h index 0bdaf354ec116f..3b6898f62d8ddd 100644 --- a/libraries/AP_DDS/AP_DDS_Frames.h +++ b/libraries/AP_DDS/AP_DDS_Frames.h @@ -3,5 +3,6 @@ static constexpr char WGS_84_FRAME_ID[] = "WGS-84"; // https://www.ros.org/reps/rep-0105.html#base-link static constexpr char BASE_LINK_FRAME_ID[] = "base_link"; +static constexpr char BASE_LINK_NED_FRAME_ID[] = "base_link_ned"; // https://www.ros.org/reps/rep-0105.html#map static constexpr char MAP_FRAME[] = "map"; diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 53680ca73a30c9..8c7a560ee87494 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -130,10 +130,10 @@ - RELIABLE + BEST_EFFORT - TRANSIENT_LOCAL + VOLATILE