Support ICM42670 on Pixhawk6X

Tools/scripts/decode_devid.py

@@ -98,6 +98,7 @@ def num(s):
     0x37 : "DEVTYPE_INS_IIM42652",
     0x38 : "DEVTYPE_INS_BMI270",
     0x39 : "DEVTYPE_INS_BMI085",
+    0x3A : "DEVTYPE_INS_ICM42670",
 }
 
 baro_types = {
@@ -114,6 +115,9 @@ def num(s):
     0x0B : "DEVTYPE_BARO_MS5611",
     0x0C : "DEVTYPE_BARO_SPL06",
     0x0D : "DEVTYPE_BARO_UAVCAN",
+    0x0E : "DEVTYPE_BARO_MSP",
+    0x0F : "DEVTYPE_BARO_ICP101XX",
+    0x10 : "DEVTYPE_BARO_ICP201XX",
 }
 
 airspeed_types = {

libraries/AP_BoardConfig/board_drivers.cpp

@@ -271,6 +271,7 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
 #define INV3REG_WHOAMI        0x75
 
 #define INV3_WHOAMI_ICM42688  0x47
+#define INV3_WHOAMI_ICM42670  0x67
 
 /*
   validation of the board type
@@ -489,17 +490,24 @@ void AP_BoardConfig::board_setup()
 
 
 #ifdef HAL_CHIBIOS_ARCH_FMUV6
+
+#define BMI088REG_CHIPID 0x00
+#define CHIPID_BMI088_G 0x0F
+
 /*
   detect which FMUV6 variant we are running on
  */
 void AP_BoardConfig::detect_fmuv6_variant()
 {
-    if ((spi_check_register_inv2("icm20649", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649) &&
-         spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688))) {
+    if (((spi_check_register_inv2("icm20649", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649) ||
+          spi_check_register("bmi088_g", BMI088REG_CHIPID, CHIPID_BMI088_G)) && // alternative config
+         spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688) &&
+         spi_check_register("icm42670", INV3REG_WHOAMI, INV3_WHOAMI_ICM42670))) {
         state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X);
         DEV_PRINTF("Detected Holybro 6X\n");
     } else if ((spi_check_register_inv2("icm20649_2", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649) &&
-                spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688))) {
+                spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688) &&
+                spi_check_register("bmi088_g", BMI088REG_CHIPID, CHIPID_BMI088_G))) {
         state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X);
         DEV_PRINTF("Detected CUAV 6X\n");
     }

libraries/AP_Compass/AP_Compass_AK09916.cpp

@@ -324,19 +324,6 @@ void AP_Compass_AK09916::_update()
     _make_adc_sensitivity_adjustment(raw_field);
     raw_field *= AK09916_MILLIGAUSS_SCALE;
 
-#ifdef HAL_AK09916_HEATER_OFFSET
-    /*
-      the internal AK09916 can be impacted by the magnetic field from
-      a heater. We use the heater duty cycle to correct for the error
-     */
-    if (AP_HAL::Device::devid_get_bus_type(_bus->get_bus_id()) == AP_HAL::Device::BUS_TYPE_SPI) {
-        auto *bc = AP::boardConfig();
-        if (bc) {
-            raw_field += HAL_AK09916_HEATER_OFFSET * bc->get_heater_duty_cycle() * 0.01;
-        }
-    }
-#endif
-
     accumulate_sample(raw_field, _compass_instance, 10);
 
 check_registers:

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -5,6 +5,7 @@
 
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -21,6 +22,30 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
     }
     mag.rotate(state.rotation);
 
+#ifdef HAL_HEATER_MAG_OFFSET
+    /*
+      apply compass compensations for boards that have a heater which
+      interferes with an internal compass. This needs to be applied
+      before the board orientation so it is independent of
+      AHRS_ORIENTATION
+     */
+    if (!is_external(instance)) {
+        const uint32_t dev_id = uint32_t(_compass._state[Compass::StateIndex(instance)].dev_id);
+        static const struct offset {
+            uint32_t dev_id;
+            Vector3f ofs;
+        } offsets[] = HAL_HEATER_MAG_OFFSET;
+        const auto *bc = AP::boardConfig();
+        if (bc) {
+            for (const auto &o : offsets) {
+                if (o.dev_id == dev_id) {
+                    mag += o.ofs * bc->get_heater_duty_cycle() * 0.01;
+                }
+            }
+        }
+    }
+#endif
+
     if (!state.external) {
         mag.rotate(_compass._board_orientation);
     } else {

libraries/AP_Compass/AP_Compass_IST8310.cpp

@@ -229,19 +229,6 @@ void AP_Compass_IST8310::timer()
     /* Resolution: 0.3 µT/LSB - already convert to milligauss */
     Vector3f field = Vector3f{x * 3.0f, y * 3.0f, z * 3.0f};
 
-#ifdef HAL_IST8310_I2C_HEATER_OFFSET
-    /*
-      the internal IST8310 can be impacted by the magnetic field from
-      a heater. We use the heater duty cycle to correct for the error
-     */
-    if (!is_external(_instance) && AP_HAL::Device::devid_get_bus_type(_dev->get_bus_id()) == AP_HAL::Device::BUS_TYPE_I2C) {
-        const auto *bc = AP::boardConfig();
-        if (bc) {
-            field += HAL_IST8310_I2C_HEATER_OFFSET * bc->get_heater_duty_cycle() * 0.01;
-        }
-    }
-#endif
-
     accumulate_sample(field, _instance);
 }
 

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc

@@ -279,8 +279,8 @@ COMPASS AK8963:probe_mpu9250 1 ROTATION_YAW_270
 COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90
 
 # offset the internal compass for EM impact of the IMU heater
-# this is in sensor frame mGauss
-define HAL_AK09916_HEATER_OFFSET Vector3f(30,10,235)
+# this is in body frame mGauss
+define HAL_HEATER_MAG_OFFSET {AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SPI,4,1,9),Vector3f(10,30,-235)}
 
 # also probe for external compasses
 define HAL_PROBE_EXTERNAL_I2C_COMPASSES

libraries/AP_HAL_ChibiOS/hwdef/Pix32v5/hwdef.dat

@@ -17,5 +17,5 @@ define HAL_IMUHEAT_I_DEFAULT 0.07
 PG14 USART6_TX USART6 NODMA
 
 # offset the internal compass for EM impact of the IMU heater
-# this is in sensor frame mGauss
-define HAL_IST8310_I2C_HEATER_OFFSET Vector3f(-3,14,-6)
+# this is in body frame mGauss
+define HAL_HEATER_MAG_OFFSET {AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C,0,0xe,0xa),Vector3f(18,0,8)}

libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C/hwdef.dat

@@ -144,6 +144,12 @@ PD13 I2C4_SDA I2C4
 I2C_ORDER I2C4 I2C1 I2C2
 define HAL_I2C_INTERNAL_MASK 1
 
+# this board is tight on DMA channels. To allow for more UART DMA we
+# disable DMA on I2C. This also prevents a problem with DMA on I2C
+# interfering with IMUs
+NODMA I2C*
+define STM32_I2C_USE_DMA FALSE
+
 # heater
 PB9 HEATER_EN OUTPUT LOW GPIO(80)
 define HAL_HEATER_GPIO_PIN 80
@@ -215,9 +221,14 @@ define HAL_BARO_ALLOW_INIT_NO_BARO 1
 define HAL_PROBE_EXTERNAL_I2C_COMPASSES
 COMPASS IST8310 I2C:0:0x0C false ROTATION_NONE
 
+# compensate for magnetic field generated by the heater on 6C IST8310
+define HAL_HEATER_MAG_OFFSET {AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C,0,0xc,0xa),Vector3f(17,14,0)}
+
+
 # we need to stop the probe of an IST8310 as an internal compass with PITCH_180
 define HAL_COMPASS_DISABLE_IST8310_INTERNAL_PROBE
 
+
 # SPI devices
 SPIDEV bmi055_g SPI1 DEVID1  BMI055_G_CS  MODE3 10*MHZ 10*MHZ
 SPIDEV bmi055_a SPI1 DEVID2  BMI055_A_CS  MODE3 10*MHZ 10*MHZ

libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat

@@ -104,6 +104,7 @@ IOMCU_UART USART6
 PA0 SCALED1_V3V3 ADC1 SCALE(2)
 PA4 SCALED2_V3V3 ADC1 SCALE(2)
 PB0 SCALED3_V3V3 ADC1 SCALE(2)
+PF12 SCALED4_V3V3 ADC1 SCALE(2)
 
 PB1 VDD_5V_SENS  ADC1 SCALE(2)
 
@@ -200,6 +201,12 @@ PF15 I2C4_SDA I2C4
 I2C_ORDER I2C4 I2C1 I2C2 I2C3
 define HAL_I2C_INTERNAL_MASK 1
 
+# this board is very tight on DMA channels. To allow for more UART DMA
+# we disable DMA on I2C. This also prevents a problem with DMA on I2C
+# interfering with IMUs
+NODMA I2C*
+define STM32_I2C_USE_DMA FALSE
+
 # heater
 PB10 HEATER_EN OUTPUT LOW GPIO(80)
 define HAL_HEATER_GPIO_PIN 80
@@ -260,11 +267,14 @@ PE3 LED_RED OUTPUT OPENDRAIN GPIO(90) HIGH
 PE4 LED_GREEN OUTPUT OPENDRAIN GPIO(91) HIGH
 PE5 LED_BLUE OUTPUT OPENDRAIN GPIO(92) HIGH
 
-# setup for BoardLED2
+# setup for "pixracer" RGB LEDs
 define HAL_GPIO_A_LED_PIN 90
-define HAL_GPIO_B_LED_PIN 92
+define HAL_GPIO_B_LED_PIN 91
+define HAL_GPIO_C_LED_PIN 92
 define HAL_GPIO_LED_ON 0
 
+define HAL_HAVE_PIXRACER_LED
+
 # ID pins
 PG0 HW_VER_REV_DRIVE OUTPUT LOW
 # PH3 HW_VER_SENS ADC3 SCALE(1)
@@ -293,13 +303,21 @@ COMPASS BMM150 I2C:0:0x10 false ROTATION_NONE
 # builtin compass on CUAV 6X
 COMPASS RM3100 I2C:0:0x20 false ROTATION_PITCH_180
 
+# compensate for magnetic field generated by the heater on CUAV-6X RM3100
+define HAL_HEATER_MAG_OFFSET_RM3100 AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C,0,0x20,0x11),Vector3f(-19,37,-24)
+
+# compensate for magnetic field generated by the heater on Holybro6X BMM150
+define HAL_HEATER_MAG_OFFSET_BMM150 AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C,0,0x10,0x05),Vector3f(12,-38,23)
+
+define HAL_HEATER_MAG_OFFSET {HAL_HEATER_MAG_OFFSET_RM3100, HAL_HEATER_MAG_OFFSET_BMM150}
+
 # IMU devices for Holybro6X
 SPIDEV bmi088_g SPI3 DEVID1  BMI088_G_CS  MODE3 10*MHZ 10*MHZ
 SPIDEV bmi088_a SPI3 DEVID2  BMI088_A_CS  MODE3 10*MHZ 10*MHZ
 # alternative to bmi088
 SPIDEV icm20649  SPI3 DEVID1  BMI088_A_CS MODE3  10*MHZ 10*MHZ
 SPIDEV icm42688  SPI2 DEVID1  ICM42688_CS MODE3  2*MHZ  8*MHZ
-SPIDEV imu_spi1  SPI1 DEVID1  IMU1_CS MODE3  2*MHZ  8*MHZ
+SPIDEV icm42670  SPI1 DEVID1  IMU1_CS MODE3  2*MHZ  8*MHZ
 
 # IMU devices for CUAV-6X. The CUAV board has a BMI088, ICM20649 and
 # ICM42688 the ICM42688 and BMI088 are on the same SPI buses and CS
@@ -311,9 +329,9 @@ SPIDEV ramtron   SPI5 DEVID1  FRAM_CS     MODE3  8*MHZ  8*MHZ
 
 # Holybro6X 3 IMUs
 IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_90 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X)
-IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_NONE      BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X)
+IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_180 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X)
 IMU Invensensev2 SPI:icm20649 ROTATION_YAW_180          BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X) # alternative to BMI088
-IMU Invensensev3 SPI:imu_spi1 ROTATION_PITCH_180_YAW_90 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X)
+IMU Invensensev3 SPI:icm42670 ROTATION_YAW_90           BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X)
 
 # CUAV-6X 3 IMUs
 IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180_YAW_270 BOARD_MATCH(FMUV6_BOARD_CUAV_6X)

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

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