AP_IntertialSensor: support ICM20601 IMU

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -107,6 +107,7 @@ public:
         DEVTYPE_INS_ICM20648 = 0x2D,
         DEVTYPE_INS_ICM20649 = 0x2E,
         DEVTYPE_INS_ICM20602 = 0x2F,
+        DEVTYPE_INS_ICM20601 = 0x30,
     };
 
 protected:

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -59,12 +59,6 @@ extern const AP_HAL::HAL& hal;
 #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
 #define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
 
-/*
- *  RM-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of
- *  gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
- */
-static const float GYRO_SCALE = (0.0174532f / 16.4f);
-
 /*
  *  RM-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of
  *  accel as 4096 LSB/mg at scale factor of +/- 8g (AFS_SEL==2)
@@ -205,6 +199,10 @@ void AP_InertialSensor_Invensense::start()
         gdev = DEVTYPE_INS_ICM20602;
         adev = DEVTYPE_INS_ICM20602;
         break;
+    case Invensense_ICM20601:
+        gdev = DEVTYPE_INS_ICM20601;
+        adev = DEVTYPE_INS_ICM20601;
+        break;
     case Invensense_MPU6000:
     case Invensense_MPU6500:
     case Invensense_ICM20608:
@@ -240,6 +238,7 @@ void AP_InertialSensor_Invensense::start()
 
     case Invensense_ICM20608:
     case Invensense_ICM20602:
+    case Invensense_ICM20601:
         temp_zero = 25;
         temp_sensitivity = 1.0/326.8; 
         break;
@@ -294,18 +293,27 @@ void AP_InertialSensor_Invensense::start()
         // Rev C has different scaling than rev D
         _register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
         _accel_scale = GRAVITY_MSS / 4096.f;
+        _gyro_scale = (radians(1) / 16.4f);
+    } else if (_mpu_type == Invensense_ICM20601) {
+        // Accel scale 32g (4096 LSB/g)
+        _register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
+        _accel_scale = GRAVITY_MSS / 4096.f;
+        _gyro_scale = (radians(1) / 8.2f);
+        _clip_limit = 29.5f * GRAVITY_MSS;
     } else {
         // Accel scale 16g (2048 LSB/g)
         _register_write(MPUREG_ACCEL_CONFIG,3<<3, true);
         _accel_scale = GRAVITY_MSS / 2048.f;
+        _gyro_scale = (radians(1) / 16.4f);
     }
     hal.scheduler->delay(1);
 
-	if (_mpu_type == Invensense_ICM20608 ||
-        _mpu_type == Invensense_ICM20602) {
+    if (_mpu_type == Invensense_ICM20608 ||
+        _mpu_type == Invensense_ICM20602 ||
+        _mpu_type == Invensense_ICM20601) {
         // this avoids a sensor bug, see description above
-		_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
-	}
+        _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
+    }
     
     // configure interrupt to fire when new data arrives
     _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
@@ -332,7 +340,7 @@ void AP_InertialSensor_Invensense::start()
 
     // setup scale factors for fifo data after downsampling
     _fifo_accel_scale = _accel_scale / (MAX(_fifo_downsample_rate,2)/2);
-    _fifo_gyro_scale = GYRO_SCALE / _fifo_downsample_rate;
+    _fifo_gyro_scale = _gyro_scale / _fifo_downsample_rate;
     
     // allocate fifo buffer
     _fifo_buffer = (uint8_t *)hal.util->malloc_type(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
@@ -429,7 +437,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
         gyro = Vector3f(int16_val(data, 5),
                         int16_val(data, 4),
                         -int16_val(data, 6));
-        gyro *= GYRO_SCALE;
+        gyro *= _gyro_scale;
 
         _rotate_and_correct_accel(_accel_instance, accel);
         _rotate_and_correct_gyro(_gyro_instance, gyro);
@@ -489,7 +497,7 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
                    int16_val(data, 4),
                    -int16_val(data, 6));
 
-        Vector3f g2 = g * GYRO_SCALE;
+        Vector3f g2 = g * _gyro_scale;
         _notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
 
         _accum.gyro += _accum.gyro_filter.apply(g);
@@ -709,7 +717,7 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
     config |= MPUREG_CONFIG_FIFO_MODE_STOP;
     _register_write(MPUREG_CONFIG, config, true);
 
-	if (_mpu_type != Invensense_MPU6000) {
+    if (_mpu_type != Invensense_MPU6000) {
         if (_fast_sampling) {
             // setup for 4kHz accels
             _register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_FCHOICE_B, true);
@@ -743,6 +751,9 @@ bool AP_InertialSensor_Invensense::_check_whoami(void)
     case MPU_WHOAMI_20602:
         _mpu_type = Invensense_ICM20602;
         return true;
+    case MPU_WHOAMI_20601:
+        _mpu_type = Invensense_ICM20601;
+        return true;
     case MPU_WHOAMI_ICM20789:
     case MPU_WHOAMI_ICM20789_R1:
         _mpu_type = Invensense_ICM20789;
@@ -832,11 +843,12 @@ bool AP_InertialSensor_Invensense::_hardware_init(void)
         return false;
     }
 
-	if (_mpu_type == Invensense_ICM20608 ||
-        _mpu_type == Invensense_ICM20602) {
+    if (_mpu_type == Invensense_ICM20608 ||
+        _mpu_type == Invensense_ICM20602 ||
+        _mpu_type == Invensense_ICM20601) {
         // this avoids a sensor bug, see description above
-		_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
-	}
+        _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
+    }
     _dev->get_semaphore()->give();
     
     return true;

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -62,6 +62,7 @@ public:
         Invensense_MPU9250,
         Invensense_ICM20608,
         Invensense_ICM20602,
+        Invensense_ICM20601,
         Invensense_ICM20789,
         Invensense_ICM20689,
     };
@@ -116,6 +117,7 @@ private:
     
     float _temp_filtered;
     float _accel_scale;
+    float _gyro_scale;
 
     float _fifo_accel_scale;
     float _fifo_gyro_scale;

libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h

@@ -169,6 +169,7 @@
 #define MPU_WHOAMI_6000			0x68
 #define MPU_WHOAMI_20608		0xaf
 #define MPU_WHOAMI_20602		0x12
+#define MPU_WHOAMI_20601		0xac
 #define MPU_WHOAMI_6500			0x70
 #define MPU_WHOAMI_MPU9250      0x71
 #define MPU_WHOAMI_MPU9255      0x73