fixed merge blunder

ArduPilot · Feb 20, 2024 · 875077b · 875077b
1 parent 4e37504
commit 875077b
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 7 deletions.
diff --git 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
@@ -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
@@ -130,10 +130,10 @@
     </topic>
     <qos>
       <reliability>
-        <kind>RELIABLE</kind>
+        <kind>BEST_EFFORT</kind>
       </reliability>
       <durability>
-        <kind>TRANSIENT_LOCAL</kind>
+        <kind>VOLATILE</kind>
       </durability>
     </qos>
   </data_writer>