diff --git a/Tools/AP_Periph/imu.cpp b/Tools/AP_Periph/imu.cpp
index c0c5aaf401..06e961e44b 100644
--- a/Tools/AP_Periph/imu.cpp
+++ b/Tools/AP_Periph/imu.cpp
@@ -11,8 +11,15 @@ extern const AP_HAL::HAL &hal;
 void AP_Periph_FW::can_imu_update(void)
 {
     while (true) {
-        // sleep for a bit to avoid flooding the CPU
-        hal.scheduler->delay_microseconds(100);
+        // we need to delay by a ms value as hal->schedule->delay_microseconds_boost
+        // used in wait_for_sample() takes uint16_t
+        const uint32_t delay_ms = 1000U / g.imu_sample_rate;
+        hal.scheduler->delay(delay_ms);
+
+        if (delay_ms == 0) {
+            // sleep for a bit to avoid flooding the CPU
+            hal.scheduler->delay_microseconds(100);
+        }
 
         imu.update();
 
@@ -38,6 +45,11 @@ void AP_Periph_FW::can_imu_update(void)
         pkt.accelerometer_latest[1] = tmp.y;
         pkt.accelerometer_latest[2] = tmp.z;
 
+        tmp = imu.get_gyro();
+        pkt.rate_gyro_latest[0] = tmp.x;
+        pkt.rate_gyro_latest[1] = tmp.y;
+        pkt.rate_gyro_latest[2] = tmp.z;
+        
         uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE];
         uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout());
         canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE,