diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
index 5f49226a44..a2646b87d6 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
@@ -44,8 +44,8 @@
 
 #define TIMING_DEBUG 0
 #if TIMING_DEBUG
-#define DEBUG_SET_PIN(n,v) hal.gpio->write(55-n, v)
-#define DEBUG_TOGGLE_PIN(n) hal.gpio->toggle(55-n)
+#define DEBUG_SET_PIN(n,v) hal.gpio->write(52+n, v)
+#define DEBUG_TOGGLE_PIN(n) hal.gpio->toggle(52+n)
 #else
 #define DEBUG_SET_PIN(n,v)
 #define DEBUG_TOGGLE_PIN(n)
@@ -54,23 +54,26 @@
 extern const AP_HAL::HAL& hal;
 
 AP_InertialSensor_ADIS1647x::AP_InertialSensor_ADIS1647x(AP_InertialSensor &imu,
-                                                   AP_HAL::OwnPtr<AP_HAL::Device> _dev,
-                                                   enum Rotation _rotation)
+                                                         AP_HAL::OwnPtr<AP_HAL::Device> _dev,
+                                                         enum Rotation _rotation,
+                                                         uint8_t drdy_gpio)
     : AP_InertialSensor_Backend(imu)
     , dev(std::move(_dev))
     , rotation(_rotation)
+    , drdy_pin(drdy_gpio)
 {
 }
 
 AP_InertialSensor_Backend *
 AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu,
-                                AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                enum Rotation rotation)
+                                   AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                   enum Rotation rotation,
+                                   uint8_t drdy_gpio)
 {
     if (!dev) {
         return nullptr;
     }
-    auto sensor = new AP_InertialSensor_ADIS1647x(imu, std::move(dev), rotation);
+    auto sensor = new AP_InertialSensor_ADIS1647x(imu, std::move(dev), rotation, drdy_gpio);
 
     if (!sensor) {
         return nullptr;
@@ -305,10 +308,22 @@ void AP_InertialSensor_ADIS1647x::loop(void)
         // we deliberately set the period a bit fast to ensure we
         // don't lose a sample
         const uint32_t period_us = 480;
+        bool wait_ok = false;
+        if (drdy_pin != 0) {
+            // when we have a DRDY pin then wait for it to go high
+            DEBUG_SET_PIN(0, 1);
+            wait_ok = hal.gpio->wait_pin(drdy_pin, AP_HAL::GPIO::INTERRUPT_RISING, 1000);
+            DEBUG_SET_PIN(0, 0);
+        }
         read_sensor();
         uint32_t dt = AP_HAL::micros() - tstart;
         if (dt < period_us) {
-            hal.scheduler->delay_microseconds(period_us - dt);
+            uint32_t wait_us = period_us - dt;
+            if (!wait_ok || wait_us > period_us/2) {
+                DEBUG_SET_PIN(3, 1);
+                hal.scheduler->delay_microseconds(wait_us);
+                DEBUG_SET_PIN(3, 0);
+            }
         }
     }
 }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
index 1ac6b86bcd..9fe38387aa 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
@@ -27,7 +27,8 @@ class AP_InertialSensor_ADIS1647x : public AP_InertialSensor_Backend {
 public:
     static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
                                             AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                            enum Rotation rotation);
+                                            enum Rotation rotation,
+                                            uint8_t drdy_gpio);
 
     /**
      * Configure the sensors and start reading routine.
@@ -38,7 +39,8 @@ public:
 private:
     AP_InertialSensor_ADIS1647x(AP_InertialSensor &imu,
                                 AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                enum Rotation rotation);
+                                enum Rotation rotation,
+                                uint8_t drdy_gpio);
 
     /*
       initialise driver
@@ -59,6 +61,7 @@ private:
     uint8_t accel_instance;
     uint8_t gyro_instance;
     enum Rotation rotation;
+    uint8_t drdy_pin;
 
     uint16_t last_counter;
     bool done_first_read;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index f8edf4b1dd..77f6cdd0ed 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -64,8 +64,8 @@ void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &s
         count++;
         if (now - start_us > 1000000UL) {
             float observed_rate_hz = count * 1.0e6f / (now - start_us);
-#if SENSOR_RATE_DEBUG
-            printf("RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz);
+#if 0
+            printf("IMU RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz);
 #endif
             float filter_constant = 0.98f;
             float upper_limit = 1.05f;