Support high-resolution sampling on ICM4xxxx IMUs

libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat

@@ -98,3 +98,4 @@ BOARD_VALIDATE $CHECK_IMU0_PRESENT $CHECK_IMU1_PRESENT $CHECK_IMU2_PRESENT $CHEC
 
 # build ABIN for flash-from-bootloader support:
 env BUILD_ABIN True
+define HAL_INS_HIGHRES_SAMPLE 6

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 @@ class AP_InertialSensor : AP_AccelCal_Client
     ///
     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_Backend.cpp

@@ -662,23 +662,32 @@ void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, con
 }
 
 
-void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel)
+void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &_accel)
 {
 #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
     if (!_imu.batchsampler.doing_sensor_rate_logging()) {
         return;
     }
 
+    // get batch sampling in correct orientation
+    Vector3f accel = _accel;
+    accel.rotate(_imu._accel_orientation[instance]);
+
     _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, AP_HAL::micros64(), accel);
 #endif
 }
 
-void AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro)
+void AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &_gyro)
 {
 #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
     if (!_imu.batchsampler.doing_sensor_rate_logging()) {
         return;
     }
+
+    // get batch sampling in correct orientation
+    Vector3f gyro = _gyro;
+    gyro.rotate(_imu._gyro_orientation[instance]);
+
     _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, AP_HAL::micros64(), gyro);
 #endif
 }

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -29,6 +29,10 @@
 
 #include "AP_InertialSensor.h"
 
+#ifndef HAL_INS_HIGHRES_SAMPLE
+#define HAL_INS_HIGHRES_SAMPLE 0
+#endif
+
 class AuxiliaryBus;
 class AP_Logger;
 
@@ -284,10 +288,15 @@ class AP_InertialSensor_Backend
     }
 
     // should fast sampling be enabled on this IMU?
-    bool enable_fast_sampling(uint8_t instance) {
+    bool enable_fast_sampling(uint8_t instance) const {
         return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
     }
 
+    // should highres sampling be enabled on this IMU?
+    bool enable_highres_sampling(uint8_t instance) const {
+        return (HAL_INS_HIGHRES_SAMPLE & (1U<<instance)) != 0;
+    }
+
     // if fast sampling is enabled, the rate to use in kHz
     uint8_t get_fast_sampling_rate() const {
         return (1 << uint8_t(_imu._fast_sampling_rate));

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