final cleanup

libraries/AC_CustomControl/README.md

@@ -255,8 +255,4 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl &frontend, AP_AHRS_V
 
 12. Add reset functionality inside `reset` function of `AC_CustomControl_XYZ.cpp` file. It is user's responsibility to add proper controller resetting functionality. This highly depends on the controller and it should not be copy-pasted from another backend without testing it in SITL.
 
-13. You can access target attitude and target rate values using `atti_control->get_attitude_target_quat()` and `atti_control->rate_bf_targets()` functions, respectively. You can also access latest gyro measurement using `_ahrs->get_gyro_latest()` function. Take a look at other backends
-
-## Integral action Linear Quadratic Regulator controller
-
-1. This controller serves as an example on using this library for deploying your own attitude controllers. The LQR controller implemented here essentially works like an attitude PID controller, wherein the PID gains for each axis have been tuned by solving the Algebraic Riccati Equation (ARE) for the attitude state-space of a quadcopter. Details about quadcopter dynamics are documented here: https://ardupilot.org/copter/docs/systemid-mode-operation.html#identification-of-a-multicopter. For further details behind the mathematics and implementation of this attitude controller, refer to this publication: https://www.researchgate.net/publication/279448184_Quadrotor_Quaternion_Control and this article: https://www.mathworks.com/help/control/ref/ss.lqi.html 
+13. You can access target attitude and target rate values using `atti_control->get_attitude_target_quat()` and `atti_control->rate_bf_targets()` functions, respectively. You can also access latest gyro measurement using `_ahrs->get_gyro_latest()` function. Take a look at other backendsgit 

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;

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";

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>