AP_InertialSensor: pre-fetch instances for use in filter setup

we use the instance numbers during filter configuration to check if
fast sampling is enabled. We need to ensure these instance numbers
have been setup before the filtering functions get called

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -760,6 +760,30 @@ bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rat
     return true;
 }
 
+/*
+  get the accel instance number we will get from register_accel()
+ */
+bool AP_InertialSensor::get_accel_instance(uint8_t &instance) const
+{
+    if (_accel_count == INS_MAX_INSTANCES) {
+        return false;
+    }
+    instance = _accel_count;
+    return true;
+}
+
+/*
+  get the gyro instance number we will get from register_accel()
+ */
+bool AP_InertialSensor::get_gyro_instance(uint8_t &instance) const
+{
+    if (_gyro_count == INS_MAX_INSTANCES) {
+        return false;
+    }
+    instance = _gyro_count;
+    return true;
+}
+
 /*
   register a new accel instance
  */

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -74,6 +74,10 @@ public:
     ///
     void init(uint16_t sample_rate_hz);
 
+    // get accel/gyro instance numbers that a backend will get when they register
+    bool get_accel_instance(uint8_t &instance) const;
+    bool get_gyro_instance(uint8_t &instance) const;
+
     /// Register a new gyro/accel driver, allocating an instance
     /// number
     bool register_gyro(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id);

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -223,6 +223,11 @@ bool AP_InertialSensor_Invensense::_has_auxiliary_bus()
 
 void AP_InertialSensor_Invensense::start()
 {
+    // pre-fetch instance numbers for checking fast sampling settings
+    if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) {
+        return;
+    }
+
     WITH_SEMAPHORE(_dev->get_semaphore());
 
     // initially run the bus at low speed

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp

@@ -166,6 +166,11 @@ bool AP_InertialSensor_Invensensev2::_has_auxiliary_bus()
 
 void AP_InertialSensor_Invensensev2::start()
 {
+    // pre-fetch instance numbers for checking fast sampling settings
+    if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) {
+        return;
+    }
+
     WITH_SEMAPHORE(_dev->get_semaphore());
 
     // initially run the bus at low speed

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

@@ -241,6 +241,10 @@ void AP_InertialSensor_Invensensev3::fifo_reset()
 
 void AP_InertialSensor_Invensensev3::start()
 {
+    // pre-fetch instance numbers for checking fast sampling settings
+    if (!_imu.get_gyro_instance(gyro_instance) || !_imu.get_accel_instance(accel_instance)) {
+        return;
+    }
     WITH_SEMAPHORE(dev->get_semaphore());
 
     // initially run the bus at low speed