diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
index 289954cfa7..125775a5bf 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
@@ -20,14 +20,13 @@
 
 #include "AP_InertialSensor_ADIS1647x.h"
 
-#define BACKEND_SAMPLE_RATE 2000
-
 /*
   device registers
  */
 #define REG_PROD_ID  0x72
 #define  PROD_ID_16470     0x4056
 #define  PROD_ID_16477     0x405d
+#define  PROD_ID_16507     0x407b
 
 #define REG_GLOB_CMD 0x68
 #define  GLOB_CMD_SW_RESET 0x80
@@ -36,11 +35,28 @@
 
 #define REG_DATA_CNTR 0x22
 
+#define REG_MSC_CTRL 0x60
+# define REG_MSC_CTRL_BURST32  0x200
+# define REG_MSC_CTRL_BURSTSEL 0x100
+# define REG_MSC_CTRL_GCOMP    0x080
+# define REG_MSC_CTRL_PCOMP    0x040
+# define REG_MSC_CTRL_SENSBW   0x010
+# define REG_MSC_CTRL_DRPOL    0x001
+
+#define REG_DEC_RATE 0x64
+# define REG_DEC_RATE_2000Hz 0
+# define REG_DEC_RATE_1000Hz 1
+# define REG_DEC_RATE_666Hz 2
+# define REG_DEC_RATE_500Hz 3
+# define REG_DEC_RATE_400Hz 4
+
+#define REG_FILT_CTRL 0x5c
+
 /*
   timings
  */
 #define T_STALL_US   20U
-#define T_RESET_MS   250U
+#define T_RESET_MS   500U
 
 #define TIMING_DEBUG 0
 #if TIMING_DEBUG
@@ -89,8 +105,8 @@ AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu,
 
 void AP_InertialSensor_ADIS1647x::start()
 {
-    if (!_imu.register_accel(accel_instance, BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)) ||
-        !_imu.register_gyro(gyro_instance, BACKEND_SAMPLE_RATE,   dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X))) {
+    if (!_imu.register_accel(accel_instance, expected_sample_rate_hz, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)) ||
+        !_imu.register_gyro(gyro_instance, expected_sample_rate_hz,   dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X))) {
         return;
     }
 
@@ -113,21 +129,25 @@ void AP_InertialSensor_ADIS1647x::start()
 /*
   check product ID
  */
-bool AP_InertialSensor_ADIS1647x::check_product_id(void)
+bool AP_InertialSensor_ADIS1647x::check_product_id(uint16_t &prod_id)
 {
-    uint16_t prod_id = read_reg16(REG_PROD_ID);
+    prod_id = read_reg16(REG_PROD_ID);
     switch (prod_id) {
     case PROD_ID_16470:
         // can do up to 40G
+        opmode = OpMode::Basic;
         accel_scale = 1.25 * GRAVITY_MSS * 0.001;
         _clip_limit = 39.5f * GRAVITY_MSS;
         gyro_scale = radians(0.1);
+        expected_sample_rate_hz = 2000;
         return true;
 
-    case PROD_ID_16477:
+    case PROD_ID_16477: {
         // can do up to 40G
+        opmode = OpMode::Basic;
         accel_scale = 1.25 * GRAVITY_MSS * 0.001;
         _clip_limit = 39.5f * GRAVITY_MSS;
+        expected_sample_rate_hz = 2000;
         // RANG_MDL register used for gyro range
         uint16_t rang_mdl = read_reg16(REG_RANG_MDL);
         switch ((rang_mdl >> 2) & 3) {
@@ -145,6 +165,39 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(void)
         }
         return true;
     }
+        
+    case PROD_ID_16507: {
+        opmode = OpMode::Delta32;
+        expected_sample_rate_hz = 1200;
+        accel_scale = 392.0 / 2097152000.0;
+        dvel_scale = 400.0 / 0x7FFFFFFF;
+        _clip_limit = 39.5f * GRAVITY_MSS;
+        // RANG_MDL register used for gyro range
+        uint16_t rang_mdl = read_reg16(REG_RANG_MDL);
+        switch ((rang_mdl >> 2) & 3) {
+        case 0:
+            gyro_scale = radians(125) / 0x4E200000;
+            dangle_scale = radians(360.0 / 0x7FFFFFFF);
+            break;
+        case 1:
+            gyro_scale = radians(500) / 0x4E200000;
+            dangle_scale = radians(720.0 / 0x7FFFFFFF);
+            break;
+        case 3:
+            gyro_scale = radians(2000) / 0x4E200000;
+            dangle_scale = radians(2160.0 / 0x7FFFFFFF);
+            break;
+        default:
+            return false;
+        }
+        if (opmode == OpMode::Basic) {
+            accel_scale *= 0x10000;
+            gyro_scale *= 0x10000;
+        }
+        return true;
+    }
+
+    }
     return false;
 }
 
@@ -152,20 +205,52 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(void)
 bool AP_InertialSensor_ADIS1647x::init()
 {
     WITH_SEMAPHORE(dev->get_semaphore());
-    if (!check_product_id()) {
+
+
+    uint8_t tries = 10;
+    uint16_t prod_id = 0;
+    do {
+        // perform software reset
+        write_reg16(REG_GLOB_CMD, GLOB_CMD_SW_RESET);
+        hal.scheduler->delay(100);
+    } while (!check_product_id(prod_id) && --tries);
+    if (tries == 0) {
         return false;
     }
 
-    // perform software reset
-    write_reg16(REG_GLOB_CMD, GLOB_CMD_SW_RESET);
-    hal.scheduler->delay(T_RESET_MS);
-
-    // re-check after reset
-    if (!check_product_id()) {
-        return false;
+    // bring rate down
+    if (expected_sample_rate_hz < 450) {
+        if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_400Hz, true)) {
+            return false;
+        }
+    } else if (expected_sample_rate_hz < 600) {
+        if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_500Hz, true)) {
+            return false;
+        }
+    } else if (expected_sample_rate_hz < 700) {
+        if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_666Hz, true)) {
+            return false;
+        }
+    } else if (expected_sample_rate_hz < 1500) {
+        if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_1000Hz, true)) {
+            return false;
+        }
     }
 
-    // we leave all config registers at defaults
+    if (!write_reg16(REG_FILT_CTRL, 0, true)) {
+        return false;
+    }
+    
+    // choose burst type and compensation
+    uint16_t msc_ctrl = REG_MSC_CTRL_GCOMP | REG_MSC_CTRL_PCOMP | REG_MSC_CTRL_DRPOL;
+    if (opmode == OpMode::Delta32) {
+        msc_ctrl |= REG_MSC_CTRL_BURST32 | REG_MSC_CTRL_BURSTSEL;
+    } else if (opmode == OpMode::AG32) {
+        msc_ctrl |= REG_MSC_CTRL_BURST32;
+    }
+    if (!write_reg16(REG_MSC_CTRL, msc_ctrl, true)) {
+        return true;
+    }
 
 #if TIMING_DEBUG
     // useful for debugging scheduling of transfers
@@ -200,24 +285,32 @@ uint16_t AP_InertialSensor_ADIS1647x::read_reg16(uint8_t regnum) const
 /*
   write a 16 bit register value
  */
-void AP_InertialSensor_ADIS1647x::write_reg16(uint8_t regnum, uint16_t value) const
+bool AP_InertialSensor_ADIS1647x::write_reg16(uint8_t regnum, uint16_t value, bool confirm) const
 {
-    uint8_t req[2];
-    req[0] = (regnum | 0x80);
-    req[1] = value & 0xFF;
-    dev->transfer(req, sizeof(req), nullptr, 0);
-    hal.scheduler->delay_microseconds(T_STALL_US);
+    const uint8_t retries = 16;
+    for (uint8_t i=0; i<retries; i++) {
+        uint8_t req[2];
+        req[0] = (regnum | 0x80);
+        req[1] = value & 0xFF;
+        dev->transfer(req, sizeof(req), nullptr, 0);
+        hal.scheduler->delay_microseconds(T_STALL_US);
 
-    req[0] = ((regnum+1) | 0x80);
-    req[1] = (value>>8) & 0xFF;
-    dev->transfer(req, sizeof(req), nullptr, 0);
-    hal.scheduler->delay_microseconds(T_STALL_US);
+        req[0] = ((regnum+1) | 0x80);
+        req[1] = (value>>8) & 0xFF;
+        dev->transfer(req, sizeof(req), nullptr, 0);
+        hal.scheduler->delay_microseconds(T_STALL_US);
+
+        if (!confirm || read_reg16(regnum) == value) {
+            return true;
+        }
+    }
+    return false;
 }
 
 /*
-  loop to read the sensor
+  read the sensor using 16 bit burst transfer of gyro/accel data
  */
-void AP_InertialSensor_ADIS1647x::read_sensor(void)
+void AP_InertialSensor_ADIS1647x::read_sensor16(void)
 {
     struct adis_data {
         uint8_t cmd[2];
@@ -234,7 +327,6 @@ void AP_InertialSensor_ADIS1647x::read_sensor(void)
         uint8_t  checksum;
     } data {};
 
-    uint64_t sample_start_us = AP_HAL::micros64();
     do {
         WITH_SEMAPHORE(dev->get_semaphore());
         data.cmd[0] = REG_GLOB_CMD;
@@ -281,10 +373,196 @@ void AP_InertialSensor_ADIS1647x::read_sensor(void)
     gyro *= gyro_scale;
 
     _rotate_and_correct_accel(accel_instance, accel);
-    _notify_new_accel_raw_sample(accel_instance, accel, sample_start_us);
+    _notify_new_accel_raw_sample(accel_instance, accel);
 
     _rotate_and_correct_gyro(gyro_instance, gyro);
-    _notify_new_gyro_raw_sample(gyro_instance, gyro, sample_start_us);
+    _notify_new_gyro_raw_sample(gyro_instance, gyro);
+
+    /*
+      publish average temperature at 20Hz
+     */
+    temp_sum += float(int16_t(be16toh(data.temp))*0.1);
+    temp_count++;
+
+    if (temp_count == 100) {
+        _publish_temperature(accel_instance, temp_sum/temp_count);
+        temp_sum = 0;
+        temp_count = 0;
+    }
+    DEBUG_SET_PIN(1, 0);
+}
+
+
+/*
+  read the sensor using 32 bit burst transfer of accel/gyro
+ */
+void AP_InertialSensor_ADIS1647x::read_sensor32(void)
+{
+    struct adis_data {
+        uint8_t cmd[2];
+        uint16_t diag_stat;
+        uint16_t  gx_low;
+        uint16_t  gx_high;
+        uint16_t  gy_low;
+        uint16_t  gy_high;
+        uint16_t  gz_low;
+        uint16_t  gz_high;
+        uint16_t  ax_low;
+        uint16_t  ax_high;
+        uint16_t  ay_low;
+        uint16_t  ay_high;
+        uint16_t  az_low;
+        uint16_t  az_high;
+        uint16_t  temp;
+        uint16_t counter;
+        uint8_t  pad;
+        uint8_t  checksum;
+    } data {};
+
+    do {
+        WITH_SEMAPHORE(dev->get_semaphore());
+        data.cmd[0] = REG_GLOB_CMD;
+        DEBUG_SET_PIN(2, 1);
+        if (!dev->transfer((const uint8_t *)&data, sizeof(data), (uint8_t *)&data, sizeof(data))) {
+            break;
+        }
+        DEBUG_SET_PIN(2, 0);
+    } while (be16toh(data.counter) == last_counter);
+
+    DEBUG_SET_PIN(1, 1);
+
+    /*
+      check the 8 bit checksum of the packet
+     */
+    uint8_t sum = 0;
+    const uint8_t *b = (const uint8_t *)&data.diag_stat;
+    for (uint8_t i=0; i<offsetof(adis_data, pad) - offsetof(adis_data, diag_stat); i++) {
+        sum += b[i];
+    }
+    if (sum != data.checksum) {
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        // corrupt data
+        return;
+    }
+
+    /*
+      check if we have lost a sample
+     */
+    uint16_t counter = be16toh(data.counter);
+    if (done_first_read && uint16_t(last_counter+1) != counter) {
+        DEBUG_TOGGLE_PIN(3);
+    }
+    done_first_read = true;
+    last_counter = counter;
+    
+    Vector3f accel{float(accel_scale*int32_t(be16toh(data.ax_low) | (be16toh(data.ax_high)<<16))),
+                  -float(accel_scale*int32_t(be16toh(data.ay_low) | (be16toh(data.ay_high)<<16))),
+                  -float(accel_scale*int32_t(be16toh(data.az_low) | (be16toh(data.az_high)<<16)))};
+    Vector3f gyro{float(gyro_scale*int32_t(be16toh(data.gx_low) | (be16toh(data.gx_high)<<16))),
+                    -float(gyro_scale*int32_t(be16toh(data.gy_low) | (be16toh(data.gy_high)<<16))),
+                    -float(gyro_scale*int32_t(be16toh(data.gz_low) | (be16toh(data.gz_high)<<16)))};
+
+    _rotate_and_correct_accel(accel_instance, accel);
+    _notify_new_accel_raw_sample(accel_instance, accel);
+
+    _rotate_and_correct_gyro(gyro_instance, gyro);
+    _notify_new_gyro_raw_sample(gyro_instance, gyro);
+
+    /*
+      publish average temperature at 20Hz
+     */
+    temp_sum += float(int16_t(be16toh(data.temp))*0.1);
+    temp_count++;
+
+    if (temp_count == 100) {
+        _publish_temperature(accel_instance, temp_sum/temp_count);
+        temp_sum = 0;
+        temp_count = 0;
+    }
+    DEBUG_SET_PIN(1, 0);
+}
+
+/*
+  read the sensor using 32 bit burst transfer of delta-angle/delta-velocity
+ */
+void AP_InertialSensor_ADIS1647x::read_sensor32_delta(void)
+{
+    struct adis_data {
+        uint8_t cmd[2];
+        uint16_t diag_stat;
+        uint16_t  dax_low;
+        uint16_t  dax_high;
+        uint16_t  day_low;
+        uint16_t  day_high;
+        uint16_t  daz_low;
+        uint16_t  daz_high;
+        uint16_t  dvx_low;
+        uint16_t  dvx_high;
+        uint16_t  dvy_low;
+        uint16_t  dvy_high;
+        uint16_t  dvz_low;
+        uint16_t  dvz_high;
+        uint16_t  temp;
+        uint16_t counter;
+        uint8_t  pad;
+        uint8_t  checksum;
+    } data {};
+
+    do {
+        WITH_SEMAPHORE(dev->get_semaphore());
+        data.cmd[0] = REG_GLOB_CMD;
+        DEBUG_SET_PIN(2, 1);
+        if (!dev->transfer((const uint8_t *)&data, sizeof(data), (uint8_t *)&data, sizeof(data))) {
+            break;
+        }
+        DEBUG_SET_PIN(2, 0);
+    } while (be16toh(data.counter) == last_counter);
+
+    DEBUG_SET_PIN(1, 1);
+
+    /*
+      check the 8 bit checksum of the packet
+     */
+    uint8_t sum = 0;
+    const uint8_t *b = (const uint8_t *)&data.diag_stat;
+    for (uint8_t i=0; i<offsetof(adis_data, pad) - offsetof(adis_data, diag_stat); i++) {
+        sum += b[i];
+    }
+    if (sum != data.checksum) {
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        // corrupt data
+        return;
+    }
+
+    /*
+      check if we have lost a sample
+     */
+    uint16_t counter = be16toh(data.counter);
+    if (done_first_read && uint16_t(last_counter+1) != counter) {
+        DEBUG_TOGGLE_PIN(3);
+    }
+    done_first_read = true;
+    last_counter = counter;
+    
+    Vector3f dvel{float(dvel_scale*int32_t(be16toh(data.dvx_low) | (be16toh(data.dvx_high)<<16))),
+                  -float(dvel_scale*int32_t(be16toh(data.dvy_low) | (be16toh(data.dvy_high)<<16))),
+                  -float(dvel_scale*int32_t(be16toh(data.dvz_low) | (be16toh(data.dvz_high)<<16)))};
+    Vector3f dangle{float(dangle_scale*int32_t(be16toh(data.dax_low) | (be16toh(data.dax_high)<<16))),
+                    -float(dangle_scale*int32_t(be16toh(data.day_low) | (be16toh(data.day_high)<<16))),
+                    -float(dangle_scale*int32_t(be16toh(data.daz_low) | (be16toh(data.daz_high)<<16)))};
+
+    // compensate for clock errors, see "DELTA ANGLES" in datasheet
+    dangle *= expected_sample_rate_hz / _gyro_raw_sample_rate(gyro_instance);
+    dvel *= expected_sample_rate_hz / _accel_raw_sample_rate(gyro_instance);
+
+    _notify_new_delta_velocity(accel_instance, dvel);
+    _notify_new_delta_angle(gyro_instance, dangle);
 
     /*
       publish average temperature at 20Hz
@@ -309,15 +587,21 @@ void AP_InertialSensor_ADIS1647x::loop(void)
         uint32_t tstart = AP_HAL::micros();
         // we deliberately set the period a bit fast to ensure we
         // don't lose a sample
-        const uint32_t period_us = 480;
+        const uint32_t period_us = (1000000UL / expected_sample_rate_hz) - 20U;
         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);
+            wait_ok = hal.gpio->wait_pin(drdy_pin, AP_HAL::GPIO::INTERRUPT_RISING, 2100);
             DEBUG_SET_PIN(0, 0);
         }
-        read_sensor();
+        if (opmode == OpMode::Delta32) {
+            read_sensor32_delta();
+        } else if (opmode == OpMode::AG32) {
+            read_sensor32();
+        } else {
+            read_sensor16();
+        }
         uint32_t dt = AP_HAL::micros() - tstart;
         if (dt < period_us) {
             uint32_t wait_us = period_us - dt;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
index 9fe38387aa..6c2a6d2851 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
@@ -46,18 +46,26 @@ private:
       initialise driver
      */
     bool init();
-    void read_sensor(void);
+    void read_sensor16(void);
+    void read_sensor32(void);
+    void read_sensor32_delta(void);
     void loop(void);
-    bool check_product_id();
+    bool check_product_id(uint16_t &id);
 
     // read a 16 bit register
     uint16_t read_reg16(uint8_t regnum) const;
 
     // write a 16 bit register
-    void write_reg16(uint8_t regnum, uint16_t value) const;
+    bool write_reg16(uint8_t regnum, uint16_t value, bool confirm=false) const;
     
     AP_HAL::OwnPtr<AP_HAL::Device> dev;
 
+    enum class OpMode : uint8_t {
+        Basic      =1,
+        AG32       =2,
+        Delta32    =3
+    } opmode;
+
     uint8_t accel_instance;
     uint8_t gyro_instance;
     enum Rotation rotation;
@@ -67,7 +75,10 @@ private:
     bool done_first_read;
     float temp_sum;
     uint8_t temp_count;
+    float expected_sample_rate_hz;
 
     float accel_scale;
     float gyro_scale;
+    double dangle_scale;
+    double dvel_scale;
 };
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index bccd0aa5d9..550cd37401 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -7,8 +7,8 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #if AP_MODULE_SUPPORTED
 #include <AP_Module/AP_Module.h>
-#include <stdio.h>
 #endif
+#include <stdio.h>
 
 #define SENSOR_RATE_DEBUG 0
 
@@ -307,6 +307,127 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
     }
 }
 
+/*
+  handle a delta-angle sample from the backend. This assumes FIFO
+  style sampling and the sample should not be rotated or corrected for
+  offsets.
+  This function should be used when the sensor driver can directly
+  provide delta-angle values from the sensor.
+ */
+void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const Vector3f &dangle)
+{
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
+    float dt;
+
+    _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
+                        _imu._gyro_raw_sample_rates[instance]);
+
+    uint64_t last_sample_us = _imu._gyro_last_sample_us[instance];
+
+    // don't accept below 40Hz
+    if (_imu._gyro_raw_sample_rates[instance] < 40) {
+        return;
+    }
+
+    dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
+    _imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
+    uint64_t sample_us = _imu._gyro_last_sample_us[instance];
+
+    Vector3f gyro = dangle / dt;
+
+    _rotate_and_correct_gyro(instance, gyro);
+
+#if AP_MODULE_SUPPORTED
+    // call gyro_sample hook if any
+    AP_Module::call_hook_gyro_sample(instance, dt, gyro);
+#endif
+
+    // push gyros if optical flow present
+    if (hal.opticalflow) {
+        hal.opticalflow->push_gyro(gyro.x, gyro.y, dt);
+    }
+    
+    // compute delta angle, including corrections
+    Vector3f delta_angle = gyro * dt;
+
+    // compute coning correction
+    // see page 26 of:
+    // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
+    // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
+    // see also examples/coning.py
+    Vector3f delta_coning = (_imu._delta_angle_acc[instance] +
+                             _imu._last_delta_angle[instance] * (1.0f / 6.0f));
+    delta_coning = delta_coning % delta_angle;
+    delta_coning *= 0.5f;
+
+    {
+        WITH_SEMAPHORE(_sem);
+        uint64_t now = AP_HAL::micros64();
+
+        if (now - last_sample_us > 100000U) {
+            // zero accumulator if sensor was unhealthy for 0.1s
+            _imu._delta_angle_acc[instance].zero();
+            _imu._delta_angle_acc_dt[instance] = 0;
+            dt = 0;
+            delta_angle.zero();
+        }
+
+        // integrate delta angle accumulator
+        // the angles and coning corrections are accumulated separately in the
+        // referenced paper, but in simulation little difference was found between
+        // integrating together and integrating separately (see examples/coning.py)
+        _imu._delta_angle_acc[instance] += delta_angle + delta_coning;
+        _imu._delta_angle_acc_dt[instance] += dt;
+
+        // save previous delta angle for coning correction
+        _imu._last_delta_angle[instance] = delta_angle;
+        _imu._last_raw_gyro[instance] = gyro;
+#if HAL_WITH_DSP
+        // capture gyro window for FFT analysis
+        if (_imu._gyro_window_size > 0) {
+            const Vector3f& scaled_gyro = gyro * _imu._gyro_raw_sampling_multiplier[instance];
+            _imu._gyro_window[instance][0].push(scaled_gyro.x);
+            _imu._gyro_window[instance][1].push(scaled_gyro.y);
+            _imu._gyro_window[instance][2].push(scaled_gyro.z);
+        }
+#endif
+        Vector3f gyro_filtered = gyro;
+
+        // apply the notch filter
+        if (_gyro_notch_enabled()) {
+            gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);
+        }
+
+        // apply the harmonic notch filter
+        if (gyro_harmonic_notch_enabled()) {
+            gyro_filtered = _imu._gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
+        }
+
+        // apply the low pass filter last to attentuate any notch induced noise
+        gyro_filtered = _imu._gyro_filter[instance].apply(gyro_filtered);
+
+        // if the filtering failed in any way then reset the filters and keep the old value
+        if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) {
+            _imu._gyro_filter[instance].reset();
+            _imu._gyro_notch_filter[instance].reset();
+            _imu._gyro_harmonic_notch_filter[instance].reset();
+        } else {
+            _imu._gyro_filtered[instance] = gyro_filtered;
+        }
+
+        _imu._new_gyro_data[instance] = true;
+    }
+
+    if (!_imu.batchsampler.doing_post_filter_logging()) {
+        log_gyro_raw(instance, sample_us, gyro);
+    }
+    else {
+        log_gyro_raw(instance, sample_us, _imu._gyro_filtered[instance]);
+    }
+}
+
 void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gyro)
 {
     AP_Logger *logger = AP_Logger::get_singleton();
@@ -430,6 +551,79 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     }
 }
 
+/*
+  handle a delta-velocity sample from the backend. This assumes FIFO style sampling and
+  the sample should not be rotated or corrected for offsets
+
+  This function should be used when the sensor driver can directly
+  provide delta-velocity values from the sensor.
+ */
+void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, const Vector3f &dvel)
+{
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
+    float dt;
+
+    _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
+                        _imu._accel_raw_sample_rates[instance]);
+
+    uint64_t last_sample_us = _imu._accel_last_sample_us[instance];
+
+    // don't accept below 40Hz
+    if (_imu._accel_raw_sample_rates[instance] < 40) {
+        return;
+    }
+
+    dt = 1.0f / _imu._accel_raw_sample_rates[instance];
+    _imu._accel_last_sample_us[instance] = AP_HAL::micros64();
+    uint64_t sample_us = _imu._accel_last_sample_us[instance];
+
+    Vector3f accel = dvel / dt;
+
+    _rotate_and_correct_accel(instance, accel);
+
+#if AP_MODULE_SUPPORTED
+    // call accel_sample hook if any
+    AP_Module::call_hook_accel_sample(instance, dt, accel, false);
+#endif    
+    
+    _imu.calc_vibration_and_clipping(instance, accel, dt);
+
+    {
+        WITH_SEMAPHORE(_sem);
+
+        uint64_t now = AP_HAL::micros64();
+
+        if (now - last_sample_us > 100000U) {
+            // zero accumulator if sensor was unhealthy for 0.1s
+            _imu._delta_velocity_acc[instance].zero();
+            _imu._delta_velocity_acc_dt[instance] = 0;
+            dt = 0;
+        }
+        
+        // delta velocity including corrections
+        _imu._delta_velocity_acc[instance] += accel * dt;
+        _imu._delta_velocity_acc_dt[instance] += dt;
+
+        _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
+        if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) {
+            _imu._accel_filter[instance].reset();
+        }
+
+        _imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]);
+
+        _imu._new_accel_data[instance] = true;
+    }
+
+    if (!_imu.batchsampler.doing_post_filter_logging()) {
+        log_accel_raw(instance, sample_us, accel);
+    } else {
+        log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
+    }
+}
+
+
 void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel)
 {
     if (!_imu.batchsampler.doing_sensor_rate_logging()) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index fda3da647d..173c420241 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -149,6 +149,9 @@ protected:
     // sensors, and should be set to zero for FIFO based sensors
     void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0);
 
+    // alternative interface using delta-angles. Rotation and correction is handled inside this function
+    void _notify_new_delta_angle(uint8_t instance, const Vector3f &dangle);
+    
     // rotate accel vector, scale, offset and publish
     void _publish_accel(uint8_t instance, const Vector3f &accel);
 
@@ -160,6 +163,9 @@ protected:
     // sensors, and should be set to zero for FIFO based sensors
     void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false);
 
+    // alternative interface using delta-velocities. Rotation and correction is handled inside this function
+    void _notify_new_delta_velocity(uint8_t instance, const Vector3f &dvelocity);
+    
     // set the amount of oversamping a accel is doing
     void _set_accel_oversampling(uint8_t instance, uint8_t n);