diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index db95be4419..b1fa54e8e3 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -394,35 +394,45 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
 
 }
 
-/*
-  detect the sensor
- */
-AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_spi(AP_InertialSensor &_imu)
+AP_InertialSensor_MPU6000::~AP_InertialSensor_MPU6000()
 {
-    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, new AP_MPU6000_BusDriver_SPI());
-    if (sensor == NULL) {
-        return NULL;
-    }
-    if (!sensor->_init_sensor()) {
-        delete sensor;
-        return NULL;
-    }
-
-    return sensor;
+    delete _bus;
 }
 
-AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_i2c(AP_InertialSensor &_imu,
+/* Detect the sensor on SPI bus. It must have a corresponding device on
+ * SPIDriver table */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_spi(AP_InertialSensor &imu)
+{
+    AP_MPU6000_BusDriver *bus = new AP_MPU6000_BusDriver_SPI();
+    if (!bus)
+        return nullptr;
+    return _detect(imu, bus);
+}
+
+/* Detect the sensor on the specified I2C bus and address */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_i2c(AP_InertialSensor &imu,
                                                                  AP_HAL::I2CDriver *i2c,
                                                                  uint8_t addr)
 {
-    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu,
-                                        new AP_MPU6000_BusDriver_I2C(i2c, addr));
+    AP_MPU6000_BusDriver *bus = new AP_MPU6000_BusDriver_I2C(i2c, addr);
+    if (!bus)
+        return nullptr;
+    return _detect(imu, bus);
+}
+
+/* Common detection method - it takes ownership of the bus, freeing it if it's
+ * not possible to return an AP_InertialSensor_Backend */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::_detect(AP_InertialSensor &_imu,
+                                                              AP_MPU6000_BusDriver *bus)
+{
+    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, bus);
     if (sensor == NULL) {
-        return NULL;
+        delete bus;
+        return nullptr;
     }
     if (!sensor->_init_sensor()) {
         delete sensor;
-        return NULL;
+        return nullptr;
     }
 
     return sensor;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index 6056eaf299..905e39c402 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -31,6 +31,7 @@
 class AP_MPU6000_BusDriver
 {
 public:
+    virtual ~AP_MPU6000_BusDriver() { };
     virtual void init(bool &fifo_mode, uint8_t &max_samples) = 0;
     virtual void read8(uint8_t reg, uint8_t *val) = 0;
     virtual void write8(uint8_t reg, uint8_t val) = 0;
@@ -45,6 +46,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
 {
 public:
     AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus);
+    ~AP_InertialSensor_MPU6000();
     static AP_InertialSensor_Backend *detect_i2c(AP_InertialSensor &_imu,
                                                  AP_HAL::I2CDriver *i2c,
                                                  uint8_t addr);
@@ -57,6 +59,9 @@ public:
     bool accel_sample_available(void) { return _sum_count >= _sample_count; }
 
 private:
+    static AP_InertialSensor_Backend *_detect(AP_InertialSensor &_imu,
+                                              AP_MPU6000_BusDriver *bus);
+
 #if MPU6000_DEBUG
     void _dump_registers(void);
 #endif