diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 064e7b0277737e..ddfe64004219ac 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -435,9 +435,10 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) msg.orientation.w = orientation[3]; } 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;