AP_InertialSensor: added support for ICM42670 IMU

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -122,6 +122,7 @@ public:
         DEVTYPE_INS_IIM42652 = 0x37,
         DEVTYPE_BMI270       = 0x38,
         DEVTYPE_INS_BMI085   = 0x39,
+        DEVTYPE_INS_ICM42670 = 0x3A,
     };
 
 protected:

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

@@ -21,6 +21,7 @@
    ICM-42605
    ICM-40605
    IIM-42652
+   ICM-42670
 
   Note that this sensor includes 32kHz internal sampling and an
   anti-aliasing filter, which means this driver can be a lot simpler
@@ -48,7 +49,6 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
 
 // registers we use
 #define INV3REG_WHOAMI        0x75
-#define INV3REG_INT_CONFIG    0x14
 #define INV3REG_FIFO_CONFIG   0x16
 #define INV3REG_PWR_MGMT0     0x4e
 #define INV3REG_GYRO_CONFIG0  0x4f
@@ -56,22 +56,42 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
 #define INV3REG_FIFO_CONFIG1  0x5f
 #define INV3REG_FIFO_CONFIG2  0x60
 #define INV3REG_FIFO_CONFIG3  0x61
-#define INV3REG_INT_SOURCE0   0x65
 #define INV3REG_SIGNAL_PATH_RESET 0x4b
 #define INV3REG_INTF_CONFIG0  0x4c
 #define INV3REG_FIFO_COUNTH   0x2e
 #define INV3REG_FIFO_DATA     0x30
 #define INV3REG_BANK_SEL      0x76
 
+// registers for ICM-42670, multi-bank
+#define INV3REG_70_PWR_MGMT0  0x1F
+#define INV3REG_70_GYRO_CONFIG0  0x20
+#define INV3REG_70_ACCEL_CONFIG0 0x21
+#define INV3REG_70_FIFO_COUNTH   0x3D
+#define INV3REG_70_FIFO_DATA     0x3F
+#define INV3REG_70_INTF_CONFIG0  0x35
+#define INV3REG_70_MCLK_RDY      0x00
+#define INV3REG_70_SIGNAL_PATH_RESET 0x02
+#define INV3REG_70_FIFO_CONFIG1  0x28
+#define INV3REG_BLK_SEL_W 0x79
+#define INV3REG_BLK_SEL_R 0x7C
+#define INV3REG_MADDR_W   0x7A
+#define INV3REG_MADDR_R   0x7D
+#define INV3REG_M_W       0x7B
+#define INV3REG_M_R       0x7E
+#define INV3REG_BANK_MREG1 0x00
+#define INV3REG_BANK_MREG2 0x28
+#define INV3REG_BANK_MREG3 0x50
+
+#define INV3REG_MREG1_FIFO_CONFIG5 0x1
+#define INV3REG_MREG1_SENSOR_CONFIG3 0x06
+
 // WHOAMI values
 #define INV3_ID_ICM40605      0x33
 #define INV3_ID_ICM40609      0x3b
 #define INV3_ID_ICM42605      0x42
 #define INV3_ID_ICM42688      0x47
 #define INV3_ID_IIM42652      0x6f
-
+#define INV3_ID_ICM42670      0x67
-// run output data at 2kHz
-#define INV3_ODR 2000
 
 /*
   really nice that this sensor has an option to request little-endian
@@ -128,6 +148,10 @@ AP_InertialSensor_Backend *AP_InertialSensor_Invensensev3::probe(AP_InertialSens
 
 void AP_InertialSensor_Invensensev3::fifo_reset()
 {
+    if (inv3_type == Invensensev3_Type::ICM42670) {
+        // FIFO_FLUSH
+        register_write(INV3REG_70_SIGNAL_PATH_RESET, 0x04);
+    } else {
         // FIFO_MODE stop-on-full
         register_write(INV3REG_FIFO_CONFIG, 0x80);
         // FIFO partial disable, enable accel, gyro, temperature
@@ -135,6 +159,7 @@ void AP_InertialSensor_Invensensev3::fifo_reset()
         // little-endian, fifo count in records, last data hold for ODR mismatch
         register_write(INV3REG_INTF_CONFIG0, 0xC0);
         register_write(INV3REG_SIGNAL_PATH_RESET, 2);
+    }
 
     notify_accel_fifo_reset(accel_instance);
     notify_gyro_fifo_reset(gyro_instance);
@@ -170,6 +195,10 @@ void AP_InertialSensor_Invensensev3::start()
         fifo_config1 = 0x0F;
         temp_sensitivity = 1.0 * 128 / 115.49;
         break;
+    case Invensensev3_Type::ICM42670:
+        devtype = DEVTYPE_INS_ICM42670;
+        temp_sensitivity = 1.0 / 2.0;
+        break;
     case Invensensev3_Type::ICM40609:
     default:
         devtype = DEVTYPE_INS_ICM40609;
@@ -181,8 +210,8 @@ void AP_InertialSensor_Invensensev3::start()
     // always use FIFO
     fifo_reset();
 
-    if (!_imu.register_gyro(gyro_instance, INV3_ODR, dev->get_bus_id_devtype(devtype)) ||
+    if (!_imu.register_gyro(gyro_instance, expected_ODR, dev->get_bus_id_devtype(devtype)) ||
-        !_imu.register_accel(accel_instance, INV3_ODR, dev->get_bus_id_devtype(devtype))) {
+        !_imu.register_accel(accel_instance, expected_ODR, dev->get_bus_id_devtype(devtype))) {
         return;
     }
 
@@ -190,8 +219,8 @@ void AP_InertialSensor_Invensensev3::start()
     set_filter_and_scaling();
 
     // update backend sample rate
-    _set_accel_raw_sample_rate(accel_instance, INV3_ODR);
+    _set_accel_raw_sample_rate(accel_instance, expected_ODR);
-    _set_gyro_raw_sample_rate(gyro_instance, INV3_ODR);
+    _set_gyro_raw_sample_rate(gyro_instance, expected_ODR);
 
     // indicate what multiplier is appropriate for the sensors'
     // readings to fit them into an int16_t:
@@ -211,7 +240,7 @@ void AP_InertialSensor_Invensensev3::start()
     }
 
     // start the timer process to read samples
-    dev->register_periodic_callback(1e6 / INV3_ODR, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
+    dev->register_periodic_callback(1e6 / expected_ODR, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
 }
 
 /*
@@ -241,10 +270,18 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui
 
         // we have a header to confirm we don't have FIFO corruption! no more mucking
         // about with the temperature registers
+        if (inv3_type == Invensensev3_Type::ICM42670) {
+            if ((d.header & 0xFC) != 0x68) {
+                // no or bad data
+                return false;
+            }
+
+        } else {
             if ((d.header & 0xF8) != 0x68) {
                 // no or bad data
                 return false;
             }
+        }
 
         Vector3f accel{float(d.accel[0]), float(d.accel[1]), float(d.accel[2])};
         Vector3f gyro{float(d.gyro[0]), float(d.gyro[1]), float(d.gyro[2])};
@@ -272,8 +309,10 @@ void AP_InertialSensor_Invensensev3::read_fifo()
 {
     bool need_reset = false;
     uint16_t n_samples;
+    const uint8_t reg_counth = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_COUNTH:INV3REG_FIFO_COUNTH;
+    const uint8_t reg_data   = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_DATA:INV3REG_FIFO_DATA;
 
-    if (!block_read(INV3REG_FIFO_COUNTH, (uint8_t*)&n_samples, 2)) {
+    if (!block_read(reg_counth, (uint8_t*)&n_samples, 2)) {
         goto check_registers;
     }
 
@@ -284,7 +323,7 @@ void AP_InertialSensor_Invensensev3::read_fifo()
 
     while (n_samples > 0) {
         uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN);
-        if (!block_read(INV3REG_FIFO_DATA, (uint8_t*)fifo_buffer, n * INV3_SAMPLE_SIZE)) {
+        if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * INV3_SAMPLE_SIZE)) {
             goto check_registers;
         }
 
@@ -328,11 +367,67 @@ void AP_InertialSensor_Invensensev3::register_write(uint8_t reg, uint8_t val, bo
     dev->write_register(reg, val, checked);
 }
 
+/*
+  read a bank register, only used on startup
+ */
+uint8_t AP_InertialSensor_Invensensev3::register_read_bank(uint8_t bank, uint8_t reg)
+{
+    register_write(INV3REG_BLK_SEL_R, bank);
+    register_write(INV3REG_MADDR_R, reg);
+    hal.scheduler->delay_microseconds(10);
+    const uint8_t val = register_read(INV3REG_M_R);
+    hal.scheduler->delay_microseconds(10);
+    register_write(INV3REG_BLK_SEL_R, 0);
+    return val;
+}
+
+/*
+  write to a bank register. This is only used on startup, so can use
+  sleeps to wait for success
+ */
+void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t reg, uint8_t val)
+{
+    // wait for clock ready
+    for (uint8_t wait_tries=0; wait_tries<10; wait_tries++) {
+        if (register_read(INV3REG_70_MCLK_RDY) != 0) {
+            break;
+        }
+        hal.scheduler->delay_microseconds(10);
+    }
+    if (register_read(INV3REG_70_MCLK_RDY) == 0) {
+        return;
+    }
+    for (uint8_t tries=0; tries<10; tries++) {
+        register_write(INV3REG_BLK_SEL_W, bank);
+        register_write(INV3REG_MADDR_W, reg);
+        register_write(INV3REG_M_W, val);
+        hal.scheduler->delay_microseconds(10);
+        register_write(INV3REG_BLK_SEL_W, 0);
+        hal.scheduler->delay_microseconds(10);
+        const uint8_t val2 = register_read_bank(bank, reg);
+        if (val == val2) {
+            break;
+        }
+        hal.scheduler->delay_microseconds(100);
+    }
+}
+
 /*
   set the filter frequencies and scaling
  */
 void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
 {
+    if (inv3_type == Invensensev3_Type::ICM42670) {
+        // use low-noise mode
+        register_write(INV3REG_70_PWR_MGMT0, 0x0f);
+        hal.scheduler->delay_microseconds(300);
+
+        // setup gyro for 1.6kHz
+        register_write(INV3REG_70_GYRO_CONFIG0, 0x05);
+
+        // setup accel for 1.6kHz
+        register_write(INV3REG_70_ACCEL_CONFIG0, 0x05);
+    } else {
         // enable gyro and accel in low-noise modes
         register_write(INV3REG_PWR_MGMT0, 0x0F);
         hal.scheduler->delay_microseconds(300);
@@ -343,6 +438,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
         // setup accel for 2kHz
         register_write(INV3REG_ACCEL_CONFIG0, 0x05);
     }
+}
 
 /*
   check whoami for sensor type
@@ -350,6 +446,8 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
 bool AP_InertialSensor_Invensensev3::check_whoami(void)
 {
     uint8_t whoami = register_read(INV3REG_WHOAMI);
+    expected_ODR = 2000;
+
     switch (whoami) {
     case INV3_ID_ICM40609:
         inv3_type = Invensensev3_Type::ICM40609;
@@ -371,6 +469,11 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void)
         inv3_type = Invensensev3_Type::IIM42652;
         accel_scale = (GRAVITY_MSS / 2048);
         return true;
+    case INV3_ID_ICM42670:
+        inv3_type = Invensensev3_Type::ICM42670;
+        accel_scale = (GRAVITY_MSS / 2048);
+        expected_ODR = 1600;
+        return true;
     }
     // not a value WHOAMI result
     return false;
@@ -399,9 +502,39 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
     case Invensensev3_Type::IIM42652:
     case Invensensev3_Type::ICM42605:
     case Invensensev3_Type::ICM40605:
+    case Invensensev3_Type::ICM42670:
         _clip_limit = 15.5f * GRAVITY_MSS;
         break;
     }
 
+    if (inv3_type == Invensensev3_Type::ICM42670) {
+        // the ICM-42670 needs some more power-up config
+        for (uint8_t tries=0; tries<50; tries++) {
+            // initiate a power up sequence
+            register_write(INV3REG_70_SIGNAL_PATH_RESET, 0x10);
+            hal.scheduler->delay_microseconds(1000);
+            register_write(INV3REG_70_PWR_MGMT0, 0x0f, true);
+            if (register_read(INV3REG_70_MCLK_RDY) != 0) {
+                break;
+            }
+            hal.scheduler->delay(5);
+        }
+        if (register_read(INV3REG_70_MCLK_RDY) == 0) {
+            return false;
+        }
+
+        // disable APEX for larger FIFO
+        register_write_bank(INV3REG_BANK_MREG1, INV3REG_MREG1_SENSOR_CONFIG3, 0x40);
+
+        // use 16 bit data, gyro+accel
+        register_write_bank(INV3REG_BANK_MREG1, INV3REG_MREG1_FIFO_CONFIG5, 0x3);
+
+        // FIFO stop-on-full, disable bypass
+        register_write(INV3REG_70_FIFO_CONFIG1, 0x2, true);
+        
+        // little-endian, fifo count in records
+        register_write(INV3REG_70_INTF_CONFIG0, 0x40, true);
+    }
+
     return true;
 }

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -35,6 +35,7 @@ public:
         ICM42605,
         ICM40605,
         IIM42652,
+        ICM42670,
     };
 
     // acclerometers on Invensense sensors will return values up to 32G
@@ -59,6 +60,9 @@ private:
     uint8_t register_read(uint8_t reg);
     void register_write(uint8_t reg, uint8_t val, bool checked=false);
 
+    uint8_t register_read_bank(uint8_t bank, uint8_t reg);
+    void register_write_bank(uint8_t bank, uint8_t reg, uint8_t val);
+    
     bool accumulate_samples(const struct FIFOData *data, uint8_t n_samples);
 
     // instance numbers of accel and gyro data
@@ -75,6 +79,7 @@ private:
     const enum Rotation rotation;
 
     float accel_scale;
+    float expected_ODR;
 
     AP_HAL::OwnPtr<AP_HAL::Device> dev;