[BACKPORT] px4_fmu-v6c:Add Mini & fix Rev 1 (#21227)

This should be identical but now cleanly pointing to the
px4_firmware_nuttx-10.1.0+ branch.

ROMFS/px4fmu_common/init.d/rcS

@@ -171,7 +171,7 @@ else
 		param select-backup /fs/microsd/parameters_backup.bson
 	fi
 
-	if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X
+	if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X
 	then
 		netman update -i eth0
 	fi
@@ -282,28 +282,6 @@ else
 		param set SYS_AUTOCONFIG 0
 	fi
 
-	#
-	# Check if UAVCAN is enabled, default to it for ESCs.
-	#
-	if param greater -s UAVCAN_ENABLE 0
-	then
-		# Start core UAVCAN module.
-		if uavcan start
-		then
-			if param greater UAVCAN_ENABLE 2
-			then
-				set OUTPUT_MODE uavcan_esc
-			fi
-		else
-			tune_control play error
-		fi
-	else
-		if param greater -s UAVCAN_V1_ENABLE 0
-		then
-			uavcan_v1 start
-		fi
-	fi
-
 	#
 	# Check if PX4IO present and update firmware if needed.
 	# Assumption IOFW set to firmware file and IO_PRESENT = no
@@ -556,6 +534,28 @@ else
 	fi
 	unset BOARD_BOOTLOADER_UPGRADE
 
+	#
+	# Check if UAVCAN is enabled, default to it for ESCs.
+	#
+	if param greater -s UAVCAN_ENABLE 0
+	then
+		# Start core UAVCAN module.
+		if uavcan start
+		then
+			if param greater UAVCAN_ENABLE 2
+			then
+				set OUTPUT_MODE uavcan_esc
+			fi
+		else
+			tune_control play error
+		fi
+	else
+		if param greater -s UAVCAN_V1_ENABLE 0
+		then
+			uavcan_v1 start
+		fi
+	fi
+
 #
 # End of autostart.
 #

boards/ark/fmu-v6x/default.px4board

@@ -6,6 +6,7 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -85,6 +86,7 @@ CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y

boards/ark/fmu-v6x/init/rc.board_defaults

@@ -16,4 +16,11 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1
 
+if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
+then
+	param set-default SYS_USE_IO 0
+else
+	param set-default SYS_USE_IO 1
+fi
+
 safety_button start

boards/ark/fmu-v6x/nuttx-config/include/board.h

@@ -379,7 +379,7 @@
 
 #define GPIO_UART5_RX    GPIO_UART5_RX_3    /* PD2  */
 #define GPIO_UART5_TX    GPIO_UART5_TX_3    /* PC12 */
-// GPIO_UART5_RTS   no remap                /* PC8  */
+// GPIO_UART5_RTS  No remap                 /* PC8  */
 // GPIO_UART5_CTS  No remap                 /* PC9  */
 
 #define GPIO_USART6_RX   GPIO_USART6_RX_1   /* PC7 */
@@ -387,8 +387,8 @@
 
 #define GPIO_UART7_RX    GPIO_UART7_RX_4    /* PF6  */
 #define GPIO_UART7_TX    GPIO_UART7_TX_3    /* PE8  */
-#define GPIO_UART7_RTS   GPIO_UART7_RTS_2   /* PF8  */
-#define GPIO_UART7_CTS   GPIO_UART7_CTS_1   /* PE10 */
+#define GPIO_UART7_RTS   (GPIO_UART7_RTS_2 | GPIO_PULLDOWN)   /* PF8  */
+#define GPIO_UART7_CTS   (GPIO_UART7_CTS_1 | GPIO_PULLDOWN)   /* PE10 */
 
 #define GPIO_UART8_RX    GPIO_UART8_RX_1    /* PE0 */
 #define GPIO_UART8_TX    GPIO_UART8_TX_1    /* PE1 */

boards/ark/fmu-v6x/src/board_config.h

@@ -326,7 +326,6 @@
 /* RC Serial port */
 
 #define RC_SERIAL_PORT                     "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE
 
 /* Input Capture Channels. */
 #define INPUT_CAP1_TIMER                  1

boards/ark/fmu-v6x/src/bootloader_main.c

@@ -46,6 +46,7 @@
 #include <stm32_uart.h>
 #include <arch/board/board.h>
 #include "arm_internal.h"
+#include <px4_platform/gpio.h>
 #include <px4_platform_common/init.h>
 
 extern int sercon_main(int c, char **argv);
@@ -54,6 +55,15 @@ __EXPORT void board_on_reset(int status) {}
 
 __EXPORT void stm32_boardinitialize(void)
 {
+	/* configure pins */
+	const uint32_t list[] = PX4_GPIO_INIT_LIST;
+
+	for (size_t gpio = 0; gpio < arraySize(list); gpio++) {
+		if (list[gpio] != 0) {
+			px4_arch_configgpio(list[gpio]);
+		}
+	}
+
 	/* configure USB interfaces */
 	stm32_usbinitialize();
 }

boards/ark/fmu-v6x/src/init.c

@@ -105,6 +105,7 @@ __EXPORT void board_peripheral_reset(int ms)
 {
 	/* set the peripheral rails off */
 
+	VDD_5V_HIPOWER_EN(false);
 	VDD_5V_PERIPH_EN(false);
 	board_control_spi_sensors_power(false, 0xffff);
 	VDD_3V3_SENSORS4_EN(false);
@@ -123,6 +124,7 @@ __EXPORT void board_peripheral_reset(int ms)
 	VDD_3V3_SPEKTRUM_POWER_EN(last);
 	board_control_spi_sensors_power(true, 0xffff);
 	VDD_3V3_SENSORS4_EN(true);
+	VDD_5V_HIPOWER_EN(true);
 	VDD_5V_PERIPH_EN(true);
 
 }
@@ -209,7 +211,6 @@ stm32_boardinitialize(void)
 __EXPORT int board_app_initialize(uintptr_t arg)
 {
 	/* Power on Interfaces */
-	VDD_3V3_SD_CARD_EN(true);
 	VDD_5V_PERIPH_EN(true);
 	VDD_5V_HIPOWER_EN(true);
 	VDD_3V3_SENSORS4_EN(true);

boards/ark/fmu-v6x/src/mtd.cpp

@@ -43,7 +43,6 @@ static const px4_mft_device_t i2c3 = {             // 24LC64T on Base  8K 32 X 2
 	.devid    = PX4_MK_I2C_DEVID(3, 0x51)
 };
 
-
 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
 	.npart = 2,

boards/mro/pixracerpro/src/board_config.h

@@ -86,9 +86,9 @@
 #define DIRECT_PWM_OUTPUT_CHANNELS  8
 
 /* Power supply control and monitoring GPIOs */
-#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_POWER_IN_A                 /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
 
-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
+#define GPIO_VDD_BRICK1_VALID           GPIO_POWER_IN_A /* Brick 1 Is Chosen */
 #define BOARD_NUMBER_BRICKS             1
 
 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
@@ -143,7 +143,7 @@
  */
 #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
 #define BOARD_ADC_USB_VALID     BOARD_ADC_USB_CONNECTED
-#define BOARD_ADC_BRICK_VALID   (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID))
+#define BOARD_ADC_BRICK_VALID   (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID))
 
 #define BOARD_NUM_IO_TIMERS 3
 #define BOARD_DMA_ALLOC_POOL_SIZE 5120 /* This board provides a DMA pool and APIs */
@@ -160,7 +160,7 @@
 		GPIO_CAN2_SILENT_S0,              \
 		GPIO_LEVEL_SHIFTER_OE,            \
 		GPIO_PWM_VOLT_SEL,                \
-		GPIO_nPOWER_IN_A,                 \
+		GPIO_POWER_IN_A,                  \
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D0), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D1), \

boards/mro/pixracerpro/src/init.c

@@ -195,5 +195,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	sdio_mediachange(sdio_dev, true);
 #endif /* CONFIG_MMCSD */
 
+	px4_platform_configure();
+
 	return OK;
 }

boards/px4/fmu-v6c/src/board_config.h

@@ -143,12 +143,13 @@
 #define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
 #define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
 
-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0, 10 and Mini Sensor sets
 //                 Base/FMUM
 #define V6C00   HW_VER_REV(0x0,0x0) // FMUV6C,                 Rev 0  I2C4 External but with Internal devices
 #define V6C01   HW_VER_REV(0x0,0x1) // FMUV6C,                 Rev 1  I2C4 Internal I2C2 External
 #define V6C10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0  I2C4 External but with Internal devices
 #define V6C11   HW_VER_REV(0x1,0x1) // NO PX4IO,               Rev 1  I2C4 Internal I2C2 External
+#define V6C21   HW_VER_REV(0x2,0x1) // FMUV6CMini,             Rev 1  I2C4 Internal I2C2 External
 
 
 /* HEATER

boards/px4/fmu-v6c/src/manifest.c

@@ -79,6 +79,11 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
 		.mandatory   = 1,
 		.connection  = px4_hw_con_onboard,
 	},
+	{
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
 };
 
 static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
@@ -101,6 +106,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6C01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1
 	{V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 0 No PX4IO
 	{V6C11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 1 No PX4IO
+	{V6C21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1 MINI
 };
 
 /************************************************************************************

boards/px4/fmu-v6c/src/spi.cpp

@@ -46,7 +46,17 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
 		}),
 	}),
-	initSPIHWVersion(V6C10, {
+	initSPIHWVersion(V6C01, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
+		}, {GPIO::PortB, GPIO::Pin2}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
+		}),
+	}),
+	initSPIHWVersion(V6C21, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
 			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),

boards/px4/fmu-v6x/default.px4board

@@ -9,6 +9,7 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
+CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
@@ -23,6 +24,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y

boards/px4/fmu-v6x/init/rc.board_sensors

@@ -70,19 +70,34 @@ else
 fi
 
 # Internal magnetometer on I2c
-bmm150 -I start
+if ver hwtypecmp V6X21
+then
+	rm3100 -I -b 4 start
+else
+	bmm150 -I start
+fi
 
 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
 ist8310 -X -b 1 -R 10 start
 
 # Possible internal Baro
-bmp388 -I -a 0x77 start
+if ver hwtypecmp V6X21
+then
+	icp201xx -I -a 0x64 start
+else
+	bmp388 -I -a 0x77 start
+fi
 
 if ver hwtypecmp V6X00 V6X10
 then
 	bmp388 -I start
 else
-	bmp388 -X start
+	if ver hwtypecmp V6X21
+	then
+		icp201xx -X start
+	else
+		bmp388 -X start
+	fi
 fi
 
 # Baro on I2C3

boards/px4/fmu-v6x/nuttx-config/include/board.h

@@ -387,8 +387,8 @@
 
 #define GPIO_UART7_RX    GPIO_UART7_RX_4    /* PF6  */
 #define GPIO_UART7_TX    GPIO_UART7_TX_3    /* PE8  */
-#define GPIO_UART7_RTS   GPIO_UART7_RTS_2   /* PF8  */
-#define GPIO_UART7_CTS   GPIO_UART7_CTS_1   /* PE10 */
+#define GPIO_UART7_RTS   (GPIO_UART7_RTS_2 | GPIO_PULLDOWN)   /* PF8  */
+#define GPIO_UART7_CTS   (GPIO_UART7_CTS_1 | GPIO_PULLDOWN)   /* PE10 */
 
 #define GPIO_UART8_RX    GPIO_UART8_RX_1    /* PE0 */
 #define GPIO_UART8_TX    GPIO_UART8_TX_1    /* PE1 */

boards/px4/fmu-v6x/src/board_config.h

@@ -129,10 +129,14 @@
 
 /* I2C busses */
 
+#define BOARD_MTD_NUM_EEPROM        2 /* MTD: base_eeprom, imu_eeprom*/
+
 /* Devices on the onboard buses.
  *
  * Note that these are unshifted addresses.
  */
+#define BOARD_MTD_NUM_EEPROM        2 /* MTD: base_eeprom, imu_eeprom*/
+
 #define PX4_I2C_OBDEV_SE050         0x48
 
 #define GPIO_I2C2_DRDY1_BMP388      /* PG5  */  (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTG|GPIO_PIN5)
@@ -215,7 +219,8 @@
 #define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
 #define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
 
-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 3,4 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 9 // Rev 0 and Rev 3,4 Sensor sets
+
 //                 Base/FMUM
 #define V6X00   HW_VER_REV(0x0,0x0) // FMUV6X,                 Rev 0
 #define V6X01   HW_VER_REV(0x0,0x1) // FMUV6X,     BMI388 I2C2 Rev 1
@@ -224,6 +229,7 @@
 #define V6X10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
 #define V6X13   HW_VER_REV(0x1,0x3) // NO PX4IO,   Sensor Set  Rev 3
 #define V6X14   HW_VER_REV(0x1,0x4) // NO PX4IO,   Sensor Set  Rev 4
+#define V6X21   HW_VER_REV(0x2,0x1) // FMUV6X,     CUAV Sensor Set
 #define V6X40   HW_VER_REV(0x4,0x0) // FMUV6X,                    HB CM4 base Rev 0
 #define V6X41   HW_VER_REV(0x4,0x1) // FMUV6X,     BMI388 I2C2    HB CM4 base Rev 1
 #define V6X43   HW_VER_REV(0x4,0x3) // FMUV6X,     Sensor Set     HB CM4 base Rev 3

boards/px4/fmu-v6x/src/manifest.c

@@ -154,6 +154,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3
 	{V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
 	{V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4
+	{V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
 };
 
 /************************************************************************************

boards/px4/fmu-v6x/src/spi.cpp

@@ -84,6 +84,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),
 
+	initSPIHWVersion(V6X21, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
 	initSPIHWVersion(V6X43, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),

msg/tools/generate_microRTPS_bridge.py

@@ -354,7 +354,7 @@ def generate_agent(out_dir):
     # the '-typeros2' option in fastrtpsgen.
     # .. note:: This is only available in FastRTPSGen 1.0.4 and above
     gen_ros2_typename = ""
-    if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"):
+    if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', 'humble', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"):
         gen_ros2_typename = "-typeros2 "
 
     idl_files = []

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit 91bece51afbe7da9db12e3695cdbb4f4bba4bc83
+Subproject commit 4a1dd8680cd29f51fb0fe66dcfbf6f69bec747cf

src/drivers/barometer/Kconfig

@@ -9,7 +9,9 @@ menu "barometer"
         select DRIVERS_BAROMETER_LPS33HW
         select DRIVERS_BAROMETER_MS5611
         select DRIVERS_BAROMETER_MAIERTEK_MPC2520
-	select DRIVERS_BAROMETER_GOERTEK_SPL06 
+        select DRIVERS_BAROMETER_GOERTEK_SPL06
+        select DRIVERS_BAROMETER_INVENSENSE_ICP101XX
+        select DRIVERS_BAROMETER_INVENSENSE_ICP201XX
         ---help---
             Enable default set of barometer drivers
     rsource "*/Kconfig"

src/drivers/barometer/invensense/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,5 +31,5 @@
 #
 ############################################################################
 
-add_subdirectory(icp10100)
-add_subdirectory(icp10111)
+add_subdirectory(icp101xx)
+add_subdirectory(icp201xx)

src/drivers/barometer/invensense/Kconfig

@@ -0,0 +1,3 @@
+menu "InvenSense"
+rsource "*/Kconfig"
+endmenu #InvenSense

src/drivers/barometer/invensense/icp10100/Kconfig

@@ -0,0 +1,7 @@
+menuconfig DRIVERS_BAROMETER_INVENSENSE_ICP10100
+	bool "icp10100"
+	default n
+	---help---
+		Enable support for icp10100
+
+

src/drivers/barometer/invensense/icp10111/ICP10111.cpp

@@ -1,378 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "ICP10111.hpp"
-
-using namespace time_literals;
-
-ICP10111::ICP10111(const I2CSPIDriverConfig &config) :
-	I2C(config),
-	I2CSPIDriver(config)
-{
-}
-
-ICP10111::~ICP10111()
-{
-	perf_free(_reset_perf);
-	perf_free(_sample_perf);
-	perf_free(_bad_transfer_perf);
-}
-
-int
-ICP10111::init()
-{
-	int ret = I2C::init();
-
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("I2C::init failed (%i)", ret);
-		return ret;
-	}
-
-	return Reset() ? 0 : -1;
-}
-
-bool
-ICP10111::Reset()
-{
-	_state = STATE::RESET;
-	ScheduleClear();
-	ScheduleNow();
-	return true;
-}
-
-void
-ICP10111::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_reset_perf);
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_bad_transfer_perf);
-}
-
-int
-ICP10111::probe()
-{
-	uint16_t ID = 0;
-	read_response(Cmd::READ_ID, (uint8_t *)&ID, 2);
-	uint8_t PROD_ID = (ID >> 8) & 0x3f;
-
-	if (PROD_ID != Product_ID) {
-		DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID);
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
-void
-ICP10111::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// Software Reset
-		send_command(Cmd::SOFT_RESET);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		perf_count(_reset_perf);
-		ScheduleDelayed(100_ms); // Power On Reset: max 100ms
-		break;
-
-	case STATE::WAIT_FOR_RESET: {
-			// check product id
-			uint16_t ID = 0;
-			read_response(Cmd::READ_ID, (uint8_t *)&ID, 2);
-			uint8_t PROD_ID = (ID >> 8) & 0x3f; // Product ID Bits 5:0
-
-			if (PROD_ID == Product_ID) {
-				// if reset succeeded then read otp
-				_state = STATE::READ_OTP;
-				ScheduleDelayed(10_ms); // Time to coefficients are available.
-
-			} else {
-				// RESET not complete
-				if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-					PX4_DEBUG("Reset failed, retrying");
-					_state = STATE::RESET;
-					ScheduleDelayed(100_ms);
-
-				} else {
-					PX4_DEBUG("Reset not complete, check again in 10 ms");
-					ScheduleDelayed(10_ms);
-				}
-			}
-		}
-		break;
-
-	case STATE::READ_OTP: {
-			// read otp
-			uint8_t addr_otp_cmd[3] = {0x00, 0x66, 0x9c};
-			uint8_t otp_buf[3];
-			uint8_t crc;
-			bool success = true;
-
-			send_command(Cmd::SET_ADDR, addr_otp_cmd, 3);
-
-			for (uint8_t i = 0; i < 4; i++) {
-				read_response(Cmd::READ_OTP, otp_buf, 3);
-
-				crc = 0xFF;
-
-				for (int j = 0; j < 2; j++) {
-					crc = (uint8_t)cal_crc(crc, otp_buf[j]);
-				}
-
-				if (crc != otp_buf[2]) {
-					success = false;
-					break;
-				}
-
-				_scal[i] = (otp_buf[0] << 8) | otp_buf[1];
-			}
-
-			if (success) {
-				_state = STATE::MEASURE;
-
-			} else {
-				_state = STATE::RESET;
-			}
-
-			ScheduleDelayed(10_ms);
-		}
-		break;
-
-	case STATE::MEASURE:
-		if (Measure()) {
-			// if configure succeeded then start measurement cycle
-			_state = STATE::READ;
-			perf_begin(_sample_perf);
-			ScheduleDelayed(_measure_interval);
-
-		} else {
-			// MEASURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Measure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Measure failed, retrying");
-			}
-
-			ScheduleDelayed(_measure_interval);
-		}
-
-		break;
-
-	case STATE::READ: {
-			uint8_t comp_data[9] {};
-			bool success = false;
-
-			if (read_measure_results(comp_data, 9) == PX4_OK) {
-				perf_end(_sample_perf);
-
-				uint16_t _raw_t = (comp_data[0] << 8) | comp_data[1];
-				uint32_t L_res_buf3 = comp_data[3];	// expand result bytes to 32bit to fix issues on 8-bit MCUs
-				uint32_t L_res_buf4 = comp_data[4];
-				uint32_t L_res_buf6 = comp_data[6];
-				uint32_t _raw_p = (L_res_buf3 << 16) | (L_res_buf4 << 8) | L_res_buf6;
-
-				// constants for presure calculation
-				static constexpr float _pcal[3] = { 45000.0, 80000.0, 105000.0 };
-				static constexpr float _lut_lower = 3.5 * 0x100000;	// 1<<20
-				static constexpr float _lut_upper = 11.5 * 0x100000;	// 1<<20
-				static constexpr float _quadr_factor = 1 / 16777216.0;
-				static constexpr float _offst_factor = 2048.0;
-
-				// calculate temperature
-				float _temperature_C = -45.f + 175.f / 65536.f * _raw_t;
-
-				// calculate pressure
-				float t = (float)(_raw_t - 32768);
-				float s1 = _lut_lower + (float)(_scal[0] * t * t) * _quadr_factor;
-				float s2 = _offst_factor * _scal[3] + (float)(_scal[1] * t * t) * _quadr_factor;
-				float s3 = _lut_upper + (float)(_scal[2] * t * t) * _quadr_factor;
-				float c = (s1 * s2 * (_pcal[0] - _pcal[1]) +
-					   s2 * s3 * (_pcal[1] - _pcal[2]) +
-					   s3 * s1 * (_pcal[2] - _pcal[0])) /
-					  (s3 * (_pcal[0] - _pcal[1]) +
-					   s1 * (_pcal[1] - _pcal[2]) +
-					   s2 * (_pcal[2] - _pcal[0]));
-				float a = (_pcal[0] * s1 - _pcal[1] * s2 - (_pcal[1] - _pcal[0]) * c) / (s1 - s2);
-				float b = (_pcal[0] - a) * (s1 + c);
-				float _pressure_Pa = a + b / (c + _raw_p);
-
-				float temperature = _temperature_C;
-				float pressure = _pressure_Pa; // to Pascal
-
-				// publish
-				sensor_baro_s sensor_baro{};
-				sensor_baro.timestamp_sample = now;
-				sensor_baro.device_id = get_device_id();
-				sensor_baro.pressure = pressure;
-				sensor_baro.temperature = temperature;
-				sensor_baro.error_count = perf_event_count(_bad_transfer_perf);
-				sensor_baro.timestamp = hrt_absolute_time();
-				_sensor_baro_pub.publish(sensor_baro);
-
-				success = true;
-
-				if (_failure_count > 0) {
-					_failure_count--;
-				}
-
-				_state = STATE::MEASURE;
-
-			} else {
-				perf_count(_bad_transfer_perf);
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			ScheduleDelayed(1000_ms / 8 - _measure_interval); // 8Hz
-		}
-
-		break;
-	}
-}
-
-bool
-ICP10111::Measure()
-{
-	/*
-	  From ds-000186-icp-101xx-v1.0.pdf, page 6, table 1
-
-	  Sensor                  Measurement       Max Time
-	  Mode                    Time (Forced)
-	  Low Power (LP)             1.6 ms          1.8 ms
-	  Normal (N)                 5.6 ms          6.3 ms
-	  Low Noise (LN)             20.8 ms         23.8 ms
-	  Ultra Low Noise(ULN)       83.2 ms         94.5 ms
-	*/
-	Cmd cmd;
-
-	switch (_mode) {
-	case MODE::FAST:
-		cmd = Cmd::MEAS_LP;
-		_measure_interval = 2_ms;
-		break;
-
-	case MODE::ACCURATE:
-		cmd = Cmd::MEAS_LN;
-		_measure_interval = 24_ms;
-		break;
-
-	case MODE::VERY_ACCURATE:
-		cmd = Cmd::MEAS_ULN;
-		_measure_interval = 95_ms;
-		break;
-
-	case MODE::NORMAL:
-	default:
-		cmd = Cmd::MEAS_N;
-		_measure_interval = 7_ms;
-		break;
-	}
-
-	if (send_command(cmd) != PX4_OK) {
-		return false;
-	}
-
-	return true;
-}
-
-int8_t
-ICP10111::cal_crc(uint8_t seed, uint8_t data)
-{
-	int8_t poly = 0x31;
-	int8_t var2;
-	uint8_t i;
-
-	for (i = 0; i < 8; i++) {
-		if ((seed & 0x80) ^ (data & 0x80)) {
-			var2 = 1;
-
-		} else {
-			var2 = 0;
-		}
-
-		seed = (seed & 0x7F) << 1;
-		data = (data & 0x7F) << 1;
-		seed = seed ^ (uint8_t)(poly * var2);
-	}
-
-	return (int8_t)seed;
-}
-
-int
-ICP10111::read_measure_results(uint8_t *buf, uint8_t len)
-{
-	return transfer(nullptr, 0, buf, len);
-}
-
-int
-ICP10111::read_response(Cmd cmd, uint8_t *buf, uint8_t len)
-{
-	uint8_t buff[2];
-	buff[0] = ((uint16_t)cmd >> 8) & 0xff;
-	buff[1] = (uint16_t)cmd & 0xff;
-	return transfer(&buff[0], 2, buf, len);
-}
-
-int
-ICP10111::send_command(Cmd cmd)
-{
-	uint8_t buf[2];
-	buf[0] = ((uint16_t)cmd >> 8) & 0xff;
-	buf[1] = (uint16_t)cmd & 0xff;
-	return transfer(buf, sizeof(buf), nullptr, 0);
-}
-
-int
-ICP10111::send_command(Cmd cmd, uint8_t *data, uint8_t len)
-{
-	uint8_t buf[5];
-	buf[0] = ((uint16_t)cmd >> 8) & 0xff;
-	buf[1] = (uint16_t)cmd & 0xff;
-	memcpy(&buf[2], data, len);
-	return transfer(&buf[0], len + 2, nullptr, 0);
-}

src/drivers/barometer/invensense/icp10111/Kconfig

@@ -0,0 +1,6 @@
+menuconfig DRIVERS_BAROMETER_INVENSENSE_ICP10111
+	bool "icp10100"
+	default n
+	---help---
+		Enable support for icp10111
+

src/drivers/barometer/invensense/icp101xx/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -32,14 +32,14 @@
 ############################################################################
 
 px4_add_module(
-	MODULE drivers__invensense__icp10111
-	MAIN icp10111
+	MODULE drivers__invensense__icp101xx
+	MAIN icp101xx
 	COMPILE_FLAGS
 	SRCS
-		Inven_Sense_ICP10111_registers.hpp
-		ICP10111.cpp
-		ICP10111.hpp
-		icp10111_main.cpp
+		Inven_Sense_ICP101XX_registers.hpp
+		ICP101XX.cpp
+		ICP101XX.hpp
+		icp101xx_main.cpp
 	DEPENDS
 		px4_work_queue
 	)

src/drivers/barometer/invensense/icp101xx/ICP101XX.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,17 +31,17 @@
  *
  ****************************************************************************/
 
-#include "ICP10100.hpp"
+#include "ICP101XX.hpp"
 
 using namespace time_literals;
 
-ICP10100::ICP10100(const I2CSPIDriverConfig &config) :
+ICP101XX::ICP101XX(const I2CSPIDriverConfig &config) :
 	I2C(config),
 	I2CSPIDriver(config)
 {
 }
 
-ICP10100::~ICP10100()
+ICP101XX::~ICP101XX()
 {
 	perf_free(_reset_perf);
 	perf_free(_sample_perf);
@@ -49,7 +49,7 @@ ICP10100::~ICP10100()
 }
 
 int
-ICP10100::init()
+ICP101XX::init()
 {
 	int ret = I2C::init();
 
@@ -62,7 +62,7 @@ ICP10100::init()
 }
 
 bool
-ICP10100::Reset()
+ICP101XX::Reset()
 {
 	_state = STATE::RESET;
 	ScheduleClear();
@@ -71,7 +71,7 @@ ICP10100::Reset()
 }
 
 void
-ICP10100::print_status()
+ICP101XX::print_status()
 {
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_reset_perf);
@@ -80,7 +80,7 @@ ICP10100::print_status()
 }
 
 int
-ICP10100::probe()
+ICP101XX::probe()
 {
 	uint16_t ID = 0;
 	read_response(Cmd::READ_ID, (uint8_t *)&ID, 2);
@@ -95,7 +95,7 @@ ICP10100::probe()
 }
 
 void
-ICP10100::RunImpl()
+ICP101XX::RunImpl()
 {
 	const hrt_abstime now = hrt_absolute_time();
 
@@ -233,7 +233,6 @@ ICP10100::RunImpl()
 				float b = (_pcal[0] - a) * (s1 + c);
 				float _pressure_Pa = a + b / (c + _raw_p);
 
-				const hrt_abstime nowx = hrt_absolute_time();
 				float temperature = _temperature_C;
 				float pressure = _pressure_Pa;
 
@@ -277,7 +276,7 @@ ICP10100::RunImpl()
 }
 
 bool
-ICP10100::Measure()
+ICP101XX::Measure()
 {
 	/*
 	  From ds-000186-icp-101xx-v1.0.pdf, page 6, table 1
@@ -322,7 +321,7 @@ ICP10100::Measure()
 }
 
 int8_t
-ICP10100::cal_crc(uint8_t seed, uint8_t data)
+ICP101XX::cal_crc(uint8_t seed, uint8_t data)
 {
 	int8_t poly = 0x31;
 	int8_t var2;
@@ -345,13 +344,13 @@ ICP10100::cal_crc(uint8_t seed, uint8_t data)
 }
 
 int
-ICP10100::read_measure_results(uint8_t *buf, uint8_t len)
+ICP101XX::read_measure_results(uint8_t *buf, uint8_t len)
 {
 	return transfer(nullptr, 0, buf, len);
 }
 
 int
-ICP10100::read_response(Cmd cmd, uint8_t *buf, uint8_t len)
+ICP101XX::read_response(Cmd cmd, uint8_t *buf, uint8_t len)
 {
 	uint8_t buff[2];
 	buff[0] = ((uint16_t)cmd >> 8) & 0xff;
@@ -360,7 +359,7 @@ ICP10100::read_response(Cmd cmd, uint8_t *buf, uint8_t len)
 }
 
 int
-ICP10100::send_command(Cmd cmd)
+ICP101XX::send_command(Cmd cmd)
 {
 	uint8_t buf[2];
 	buf[0] = ((uint16_t)cmd >> 8) & 0xff;
@@ -369,7 +368,7 @@ ICP10100::send_command(Cmd cmd)
 }
 
 int
-ICP10100::send_command(Cmd cmd, uint8_t *data, uint8_t len)
+ICP101XX::send_command(Cmd cmd, uint8_t *data, uint8_t len)
 {
 	uint8_t buf[5];
 	buf[0] = ((uint16_t)cmd >> 8) & 0xff;

src/drivers/barometer/invensense/icp101xx/ICP101XX.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "Inven_Sense_ICP10111_registers.hpp"
+#include "Inven_Sense_ICP101XX_registers.hpp"
 
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/device/i2c.h>
@@ -42,13 +42,13 @@
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 
-using namespace Inven_Sense_ICP10111;
+using namespace Inven_Sense_ICP101XX;
 
-class ICP10111 : public device::I2C, public I2CSPIDriver<ICP10111>
+class ICP101XX : public device::I2C, public I2CSPIDriver<ICP101XX>
 {
 public:
-	ICP10111(const I2CSPIDriverConfig &config);
-	~ICP10111() override;
+	ICP101XX(const I2CSPIDriverConfig &config);
+	~ICP101XX() override;
 
 	static void print_usage();
 

src/drivers/barometer/invensense/icp101xx/Inven_Sense_ICP101XX_registers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file icp10111_registers.hpp
+ * @file icp101xx_registers.hpp
  *
- * icp10111 registers.
+ * icp101xx registers.
  *
  */
 
@@ -42,7 +42,7 @@
 
 #include <cstdint>
 
-namespace Inven_Sense_ICP10111
+namespace Inven_Sense_ICP101XX
 {
 static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface
 static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x63;
@@ -59,4 +59,4 @@ enum class Cmd : uint16_t {
 	MEAS_ULN  	= 0x7866,
 	SOFT_RESET 	= 0x805d
 };
-} // namespace Inven_Sense_ICP10111
+} // namespace Inven_Sense_ICP101XX

src/drivers/barometer/invensense/icp101xx/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_BAROMETER_INVENSENSE_ICP101XX
+	bool "icp101xx"
+	default n
+	---help---
+		Enable support for icp101xx

src/drivers/barometer/invensense/icp101xx/icp101xx_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,12 +35,12 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-#include "ICP10111.hpp"
+#include "ICP101XX.hpp"
 
 void
-ICP10111::print_usage()
+ICP101XX::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("icp10111", "driver");
+	PRINT_MODULE_USAGE_NAME("icp101xx", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
@@ -48,9 +48,9 @@ ICP10111::print_usage()
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int icp10111_main(int argc, char *argv[])
+extern "C" int icp101xx_main(int argc, char *argv[])
 {
-	using ThisDriver = ICP10111;
+	using ThisDriver = ICP101XX;
 	BusCLIArguments cli{true, false};
 	cli.i2c_address = I2C_ADDRESS_DEFAULT;
 	cli.default_i2c_frequency = I2C_SPEED;
@@ -62,7 +62,7 @@ extern "C" int icp10111_main(int argc, char *argv[])
 		return -1;
 	}
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP10111);
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP101XX);
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);

src/drivers/barometer/invensense/icp201xx/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -32,14 +32,14 @@
 ############################################################################
 
 px4_add_module(
-	MODULE drivers__invensense__icp10100
-	MAIN icp10100
+	MODULE drivers__invensense__icp201xx
+	MAIN icp201xx
 	COMPILE_FLAGS
 	SRCS
-		Inven_Sense_ICP10100_registers.hpp
-		ICP10100.cpp
-		ICP10100.hpp
-		icp10100_main.cpp
+		Inven_Sense_ICP201XX_registers.hpp
+		ICP201XX.cpp
+		ICP201XX.hpp
+		icp201xx_main.cpp
 	DEPENDS
 		px4_work_queue
 	)

src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp

@@ -0,0 +1,521 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ICP201XX.hpp"
+#include <unistd.h>
+
+using namespace time_literals;
+
+ICP201XX::ICP201XX(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config)
+{
+}
+
+ICP201XX::~ICP201XX()
+{
+	perf_free(_reset_perf);
+	perf_free(_sample_perf);
+	perf_free(_bad_transfer_perf);
+}
+
+int
+ICP201XX::init()
+{
+	int ret = I2C::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("I2C::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool
+ICP201XX::Reset()
+{
+	_state = STATE::SOFT_RESET;
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void
+ICP201XX::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_reset_perf);
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_bad_transfer_perf);
+}
+
+int
+ICP201XX::probe()
+{
+	uint8_t device_id = 0;
+	uint8_t ver = 0xFF;
+	read_reg(Register::DEVICE_ID, &device_id);
+	read_reg(Register::VERSION, &ver);
+
+	if (device_id != EXPECTED_DEVICE_ID) {
+		DEVICE_DEBUG("unexpected device id 0x%02x", device_id);
+		return PX4_ERROR;
+	}
+
+	if (ver != 0x00 && ver != 0xB2) {
+		DEVICE_DEBUG("unexpected version 0x%02x", ver);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+void
+ICP201XX::RunImpl()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::SOFT_RESET: {
+			/* Stop the measurement */
+			mode_select(0x00);
+
+			ScheduleDelayed(2_ms);
+
+			/* Flush FIFO */
+			flush_fifo();
+
+			/* Mask all interrupts */
+			write_reg(Register::FIFO_CONFIG, 0x00);
+			write_reg(Register::INTERRUPT_MASK, 0xFF);
+
+			_reset_timestamp = now;
+			_failure_count = 0;
+			perf_count(_reset_perf);
+			_state = STATE::OTP_BOOTUP_CFG;
+			ScheduleDelayed(10_ms);
+		}
+		break;
+
+	case STATE::OTP_BOOTUP_CFG: {
+			uint8_t reg_value = 0;
+			uint8_t offset = 0, gain = 0, Hfosc = 0;
+			uint8_t version = 0;
+			uint8_t bootup_status = 0;
+			int ret = 0;
+
+			/*  read version register */
+			if (read_reg(Register::VERSION, &version) != PX4_OK) {
+				ScheduleDelayed(10_ms);
+				break;
+			}
+
+			if (version == 0xB2) {
+				/* B2 version Asic is detected. Boot up sequence is not required for B2 Asic, so returning */
+				_state = STATE::CONFIG;
+				ScheduleDelayed(10_ms);
+			}
+
+			/* Read boot up status and avoid re running boot up sequence if it is already done */
+			if (read_reg(Register::OTP_MTP_OTP_STATUS2, &bootup_status) != PX4_OK) {
+				ScheduleDelayed(10_ms);
+				break;
+			}
+
+			if (bootup_status & 0x01) {
+				/* Boot up sequence is already done, not required to repeat boot up sequence */
+				_state = STATE::CONFIG;
+				ScheduleDelayed(10_ms);
+				break;
+			}
+
+			/* Bring the ASIC in power mode to activate the OTP power domain and get access to the main registers */
+			mode_select(0x04);
+			usleep(4_ms);
+
+			/* Unlock the main registers */
+			write_reg(Register::MASTER_LOCK, 0x1F);
+
+			/* Enable the OTP and the write switch */
+			read_reg(Register::OTP_MTP_OTP_CFG1, &reg_value);
+			reg_value |= 0x03;
+			write_reg(Register::OTP_MTP_OTP_CFG1, reg_value);
+			usleep(10_us);
+
+			/* Toggle the OTP reset pin */
+			read_reg(Register::OTP_DEBUG2, &reg_value);
+			reg_value |= 1 << 7;
+			write_reg(Register::OTP_DEBUG2, reg_value);
+			usleep(10_us);
+
+			read_reg(Register::OTP_DEBUG2, &reg_value);
+			reg_value &= ~(1 << 7);
+			write_reg(Register::OTP_DEBUG2, reg_value);
+			usleep(10_us);
+
+			/* Program redundant read */
+			write_reg(Register::OTP_MTP_MRA_LSB, 0x04);
+			write_reg(Register::OTP_MTP_MRA_MSB, 0x04);
+			write_reg(Register::OTP_MTP_MRB_LSB, 0x21);
+			write_reg(Register::OTP_MTP_MRB_MSB, 0x20);
+			write_reg(Register::OTP_MTP_MR_LSB, 0x10);
+			write_reg(Register::OTP_MTP_MR_MSB, 0x80);
+
+			/* Read the data from register */
+			ret |= read_otp_data(0xF8, 0x10, &offset);
+			ret |= read_otp_data(0xF9, 0x10, &gain);
+			ret |= read_otp_data(0xFA, 0x10, &Hfosc);
+			ScheduleDelayed(10_us);
+
+			/* Write OTP values to main registers */
+			ret |= read_reg(Register::TRIM1_MSB, &reg_value);
+
+			if (ret == 0) {
+				reg_value = (reg_value & (~0x3F)) | (offset & 0x3F);
+				ret |= write_reg(Register::TRIM1_MSB, reg_value);
+			}
+
+			ret |= read_reg(Register::TRIM2_MSB, &reg_value);
+
+			if (ret == 0) {
+				reg_value = (reg_value & (~0x70)) | ((gain & 0x07) << 4);
+				ret |= write_reg(Register::TRIM2_MSB, reg_value);
+			}
+
+			ret |= read_reg(Register::TRIM2_LSB, &reg_value);
+
+			if (ret == 0) {
+				reg_value = (reg_value & (~0x7F)) | (Hfosc & 0x7F);
+				ret |= write_reg(Register::TRIM2_LSB, reg_value);
+			}
+
+			ScheduleDelayed(10_us);
+
+			/* Update boot up status to 1 */
+			if (ret == 0) {
+				ret |= read_reg(Register::OTP_MTP_OTP_STATUS2, &reg_value);
+
+				if (ret == 0) {
+					reg_value |= 0x01;
+					ret |= write_reg(Register::OTP_MTP_OTP_STATUS2, reg_value);
+				}
+			}
+
+			/* Disable OTP and write switch */
+			read_reg(Register::OTP_MTP_OTP_CFG1, &reg_value);
+			reg_value &= ~0x03;
+			write_reg(Register::OTP_MTP_OTP_CFG1, reg_value);
+
+			/* Lock the main register */
+			write_reg(Register::MASTER_LOCK, 0x00);
+
+			/* Move to standby */
+			mode_select(0x00);
+
+			ScheduleDelayed(10_ms);
+		}
+		break;
+
+	case STATE::CONFIG: {
+			if (configure()) {
+				_state = STATE::WAIT_READ;
+				ScheduleDelayed(10_ms);
+
+			} else {
+				if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+					PX4_WARN("Configure failed, resetting");
+					_state = STATE::SOFT_RESET;
+
+				} else {
+					PX4_WARN("Configure failed, retrying");
+				}
+
+				ScheduleDelayed(100_ms);
+			}
+		}
+		break;
+
+	case STATE::WAIT_READ: {
+			/*
+			* If FIR filter is enabled, it will cause a settling effect on the first 14 pressure values.
+			* Therefore the first 14 pressure output values are discarded.
+			**/
+			uint8_t fifo_packets = 0;
+			uint8_t fifo_packets_to_skip = 14;
+
+			do {
+				ScheduleDelayed(10_ms);
+				read_reg(Register::FIFO_FILL, &fifo_packets);
+				fifo_packets = (uint8_t)(fifo_packets & 0x1F);
+			} while (fifo_packets >= fifo_packets_to_skip);
+
+			flush_fifo();
+			fifo_packets = 0;
+
+			do {
+				ScheduleDelayed(10_ms);
+				read_reg(Register::FIFO_FILL, &fifo_packets);
+				fifo_packets = (uint8_t)(fifo_packets & 0x1F);
+			} while (fifo_packets == 0);
+
+			_state = STATE::READ;
+			perf_begin(_sample_perf);
+			ScheduleOnInterval(1_s / 30);
+		}
+		break;
+
+	case STATE::READ: {
+			bool success = false;
+			float pressure, temperature;
+
+			if (get_sensor_data(&pressure, &temperature)) {
+				perf_end(_sample_perf);
+				perf_begin(_sample_perf);
+
+				sensor_baro_s sensor_baro{};
+				sensor_baro.timestamp_sample = now;
+				sensor_baro.device_id = get_device_id();
+				sensor_baro.pressure = pressure;
+				sensor_baro.temperature = temperature;
+				sensor_baro.error_count = perf_event_count(_bad_transfer_perf);
+				sensor_baro.timestamp = hrt_absolute_time();
+				_sensor_baro_pub.publish(sensor_baro);
+
+				success = true;
+
+				if (_failure_count > 0) {
+					_failure_count--;
+				}
+
+			} else {
+				perf_count(_bad_transfer_perf);
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				if (_failure_count > 10) {
+					Reset();
+					return;
+				}
+			}
+		}
+		break;
+	}
+}
+
+void
+ICP201XX::dummy_reg()
+{
+	do {
+		uint8_t reg = (uint8_t)Register::EMPTY;
+		uint8_t val = 0;
+		transfer((uint8_t *)&reg, 1, &val, 1);
+	} while (0);
+}
+
+int
+ICP201XX::read_reg(Register reg, uint8_t *buf, uint8_t len)
+{
+	int ret;
+	ret = transfer((uint8_t *)&reg, 1, buf, len);
+	dummy_reg();
+	return ret;
+}
+
+int
+ICP201XX::read_reg(Register reg, uint8_t *val)
+{
+	return read_reg(reg, val, 1);
+}
+
+int
+ICP201XX::write_reg(Register reg, uint8_t val)
+{
+	uint8_t data[2] = { (uint8_t)reg, val };
+	int ret;
+	ret = transfer(data, sizeof(data), nullptr, 0);
+	dummy_reg();
+	return ret;
+}
+
+int
+ICP201XX::mode_select(uint8_t mode)
+{
+	uint8_t mode_sync_status = 0;
+
+	do {
+		read_reg(Register::DEVICE_STATUS, &mode_sync_status, 1);
+
+		if (mode_sync_status & 0x01) {
+			break;
+		}
+
+		ScheduleDelayed(500_us);
+	} while (1);
+
+	if (write_reg(Register::MODE_SELECT, mode) != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int
+ICP201XX::read_otp_data(uint8_t addr, uint8_t cmd, uint8_t *val)
+{
+	uint8_t otp_status = 0xFF;
+
+	/* Write the address content and read command */
+	if (write_reg(Register::OTP_MTP_OTP_ADDR, addr) != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	if (write_reg(Register::OTP_MTP_OTP_CMD, cmd) != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	/* Wait for the OTP read to finish Monitor otp_status */
+	do 	{
+		read_reg(Register::OTP_MTP_OTP_STATUS, &otp_status);
+
+		if (otp_status == 0) {
+			break;
+		}
+
+		ScheduleDelayed(1_us);
+	} while (1);
+
+	/* Read the data from register */
+	if (read_reg(Register::OTP_MTP_RD_DATA, val) != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+bool
+ICP201XX::get_sensor_data(float *pressure, float *temperature)
+{
+	bool success = false;
+	uint8_t fifo_packets = 0;
+	uint8_t fifo_data[96] {0};
+	int32_t data_temp[16] {0};
+	int32_t data_press[16] {0};
+
+	if (read_reg(Register::FIFO_FILL, &fifo_packets) == PX4_OK) {
+		fifo_packets  = (uint8_t)(fifo_packets & 0x1F);
+
+		if (fifo_packets > 0 && fifo_packets <= 16 && !read_reg(Register::FIFO_BASE, fifo_data, fifo_packets * 2 * 3)) {
+			uint8_t offset = 0;
+
+			for (uint8_t i = 0; i < fifo_packets; i++) {
+				data_press[i] = (int32_t)(((fifo_data[offset + 2] & 0x0f) << 16) | (fifo_data[offset + 1] << 8) | fifo_data[offset]) ;
+				offset += 3;
+				data_temp[i] = (int32_t)(((fifo_data[offset + 2] & 0x0f) << 16) | (fifo_data[offset + 1] << 8) | fifo_data[offset]) ;
+				offset += 3;
+			}
+
+			*pressure = 0;
+			*temperature = 0;
+
+			for (uint8_t i = 0; i < fifo_packets; i++) {
+				/** P = (POUT/2^17)*40kPa + 70kPa **/
+				if (data_press[i] & 0x080000) {
+					data_press[i] |= 0xFFF00000;
+				}
+
+				*pressure += ((float)(data_press[i]) * 40 / 131072) + 70;
+
+				/* T = (TOUT/2^18)*65C + 25C */
+				if (data_temp[i] & 0x080000) {
+					data_temp[i] |= 0xFFF00000;
+				}
+
+				*temperature += ((float)(data_temp[i]) * 65 / 262144) + 25;
+			}
+
+			*pressure = *pressure * 1000 / fifo_packets;
+			*temperature = *temperature / fifo_packets;
+			success = true;
+		}
+	}
+
+	return success;
+}
+
+bool
+ICP201XX::configure()
+{
+	uint8_t reg_value = 0;
+
+	/* Initiate Triggered Operation: Stay in Standby mode */
+	reg_value |= (reg_value & (~0x10)) | ((uint8_t)_forced_meas_trigger << 4);
+
+	/* Power Mode Selection: Normal Mode */
+	reg_value |= (reg_value & (~0x04)) | ((uint8_t)_power_mode << 2);
+
+	/* FIFO Readout Mode Selection: Pressure first. */
+	reg_value |= (reg_value & (~0x03)) | ((uint8_t)(_fifo_readout_mode));
+
+	/* Measurement Configuration: Mode2*/
+	reg_value |= (reg_value & (~0xE0)) | (((uint8_t)_op_mode) << 5);
+
+	/* Measurement Mode Selection: Continuous Measurements (duty cycled) */
+	reg_value |= (reg_value & (~0x08)) | ((uint8_t)_meas_mode << 3);
+
+	if (mode_select(reg_value) != PX4_OK) {
+		return false;
+	}
+
+	return true;
+}
+
+bool
+ICP201XX::flush_fifo()
+{
+	uint8_t reg_value;
+
+	if (read_reg(Register::FIFO_FILL, &reg_value) != PX4_OK) {
+		return false;
+	}
+
+	reg_value |= 0x80;
+
+	if (write_reg(Register::FIFO_FILL, reg_value) != PX4_OK) {
+		return false;
+	}
+
+	return true;
+}

src/drivers/barometer/invensense/icp201xx/ICP201XX.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "Inven_Sense_ICP10100_registers.hpp"
+#include "Inven_Sense_ICP201XX_registers.hpp"
 
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/device/i2c.h>
@@ -42,13 +42,13 @@
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 
-using namespace Inven_Sense_ICP10100;
+using namespace Inven_Sense_ICP201XX;
 
-class ICP10100 : public device::I2C, public I2CSPIDriver<ICP10100>
+class ICP201XX : public device::I2C, public I2CSPIDriver<ICP201XX>
 {
 public:
-	ICP10100(const I2CSPIDriverConfig &config);
-	~ICP10100() override;
+	ICP201XX(const I2CSPIDriverConfig &config);
+	~ICP201XX() override;
 
 	static void print_usage();
 
@@ -62,13 +62,15 @@ private:
 
 	bool Reset();
 
-	bool Measure();
-
-	int8_t cal_crc(uint8_t seed, uint8_t data);
-	int read_measure_results(uint8_t *buf, uint8_t len);
-	int read_response(Cmd cmd, uint8_t *buf, uint8_t len);
-	int send_command(Cmd cmd);
-	int send_command(Cmd cmd, uint8_t *data, uint8_t len);
+	int read_reg(Register reg, uint8_t *val);
+	int read_reg(Register reg, uint8_t *buf, uint8_t len);
+	int write_reg(Register reg, uint8_t val);
+	int read_otp_data(uint8_t addr, uint8_t cmd, uint8_t *dat);
+	bool get_sensor_data(float *pressure, float *temperature);
+	int mode_select(uint8_t mode);
+	void dummy_reg();
+	bool flush_fifo();
+	bool configure();
 
 	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
@@ -79,21 +81,37 @@ private:
 	hrt_abstime _reset_timestamp{0};
 	int _failure_count{0};
 
-	unsigned _measure_interval{0};
-	int16_t _scal[4];
-
 	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		READ_OTP,
-		MEASURE,
+		SOFT_RESET = 0,
+		OTP_BOOTUP_CFG,
+		CONFIG,
+		WAIT_READ,
 		READ
-	} _state{STATE::RESET};
+	} _state{STATE::SOFT_RESET};
 
-	enum class MODE : uint8_t {
-		FAST,
-		NORMAL,
-		ACCURATE,
-		VERY_ACCURATE
-	} _mode{MODE::VERY_ACCURATE};
+	enum class OP_MODE : uint8_t {
+		OP_MODE0 = 0,   /* Mode 0: Bw:6.25 Hz ODR: 25Hz */
+		OP_MODE1,       /* Mode 1: Bw:30 Hz ODR: 120Hz */
+		OP_MODE2,       /* Mode 2: Bw:10 Hz ODR: 40Hz */
+		OP_MODE3,       /* Mode 3: Bw:0.5 Hz ODR: 2Hz */
+		OP_MODE4,       /* Mode 4: User configurable Mode */
+	} _op_mode{OP_MODE::OP_MODE2};
+	enum class FIFO_READOUT_MODE : uint8_t {
+		FIFO_READOUT_MODE_PRES_TEMP = 0,   /* Pressure and temperature as pair and address wraps to the start address of the Pressure value ( pressure first ) */
+		FIFO_READOUT_MODE_TEMP_ONLY = 1,   /* Temperature only reporting */
+		FIFO_READOUT_MODE_TEMP_PRES = 2,   /* Pressure and temperature as pair and address wraps to the start address of the Temperature value ( Temperature first ) */
+		FIFO_READOUT_MODE_PRES_ONLY = 3    /* Pressure only reporting */
+	} _fifo_readout_mode{FIFO_READOUT_MODE::FIFO_READOUT_MODE_PRES_TEMP};
+	enum class POWER_MODE : uint8_t {
+		POWER_MODE_NORMAL = 0,  /* Normal Mode: Device is in standby and goes to active mode during the execution of a measurement */
+		POWER_MODE_ACTIVE = 1   /* Active Mode: Power on DVDD and enable the high frequency clock */
+	} _power_mode{POWER_MODE::POWER_MODE_NORMAL};
+	enum MEAS_MODE : uint8_t {
+		MEAS_MODE_FORCED_TRIGGER = 0, /* Force trigger mode based on icp201xx_forced_meas_trigger_t **/
+		MEAS_MODE_CONTINUOUS = 1   /* Continuous measurements based on selected mode ODR settings*/
+	} _meas_mode{MEAS_MODE::MEAS_MODE_CONTINUOUS};
+	enum FORCED_MEAS_TRIGGER : uint8_t {
+		FORCE_MEAS_STANDBY = 0,			/* Stay in Stand by */
+		FORCE_MEAS_TRIGGER_FORCE_MEAS = 1	/* Trigger for forced measurements */
+	} _forced_meas_trigger{FORCED_MEAS_TRIGGER::FORCE_MEAS_STANDBY};
 };

src/drivers/barometer/invensense/icp201xx/Inven_Sense_ICP201XX_registers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file icp10100_registers.hpp
+ * @file icp201xx_registers.hpp
  *
- * icp10100 registers.
+ * icp201xx registers.
  *
  */
 
@@ -42,21 +42,45 @@
 
 #include <cstdint>
 
-namespace Inven_Sense_ICP10100
+namespace Inven_Sense_ICP201XX
 {
 static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface
 static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x63;
+static constexpr uint8_t EXPECTED_DEVICE_ID = 0x63;
 
-static constexpr uint8_t Product_ID = 0x08;
-
-enum class Cmd : uint16_t {
-	READ_ID		= 0xefc8,
-	SET_ADDR 	= 0xc595,
-	READ_OTP 	= 0xc7f7,
-	MEAS_LP 	= 0x609c,
-	MEAS_N 		= 0x6825,
-	MEAS_LN 	= 0x70df,
-	MEAS_ULN  	= 0x7866,
-	SOFT_RESET 	= 0x805d
+enum class Register : uint8_t {
+	EMPTY = 0x00,
+	TRIM1_MSB = 0x05,
+	TRIM2_LSB = 0x06,
+	TRIM2_MSB = 0x07,
+	DEVICE_ID = 0x0C,
+	OTP_MTP_OTP_CFG1 = 0xAC,
+	OTP_MTP_MR_LSB = 0xAD,
+	OTP_MTP_MR_MSB = 0xAE,
+	OTP_MTP_MRA_LSB = 0xAF,
+	OTP_MTP_MRA_MSB = 0xB0,
+	OTP_MTP_MRB_LSB = 0xB1,
+	OTP_MTP_MRB_MSB = 0xB2,
+	OTP_MTP_OTP_ADDR = 0xB5,
+	OTP_MTP_OTP_CMD = 0xB6,
+	OTP_MTP_RD_DATA = 0xB8,
+	OTP_MTP_OTP_STATUS = 0xB9,
+	OTP_DEBUG2 = 0xBC,
+	MASTER_LOCK = 0xBE,
+	OTP_MTP_OTP_STATUS2 = 0xBF,
+	MODE_SELECT = 0xC0,
+	INTERRUPT_STATUS = 0xC1,
+	INTERRUPT_MASK = 0xC2,
+	FIFO_CONFIG = 0xC3,
+	FIFO_FILL = 0xC4,
+	SPI_MODE = 0xC5,
+	PRESS_ABS_LSB = 0xC7,
+	PRESS_ABS_MSB = 0xC8,
+	PRESS_DELTA_LSB = 0xC9,
+	PRESS_DELTA_MSB = 0xCA,
+	DEVICE_STATUS = 0xCD,
+	I3C_INFO = 0xCE,
+	VERSION = 0xD3,
+	FIFO_BASE = 0xFA
 };
-} // namespace Inven_Sense_ICP10100
+} // namespace Inven_Sense_ICP201XX

src/drivers/barometer/invensense/icp201xx/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_BAROMETER_INVENSENSE_ICP201XX
+	bool "icp201xx"
+	default n
+	---help---
+		Enable support for icp201xx

src/drivers/barometer/invensense/icp201xx/icp201xx_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,12 +35,12 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-#include "ICP10100.hpp"
+#include "ICP201XX.hpp"
 
 void
-ICP10100::print_usage()
+ICP201XX::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("icp10100", "driver");
+	PRINT_MODULE_USAGE_NAME("icp201xx", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
@@ -48,9 +48,9 @@ ICP10100::print_usage()
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int icp10100_main(int argc, char *argv[])
+extern "C" int icp201xx_main(int argc, char *argv[])
 {
-	using ThisDriver = ICP10100;
+	using ThisDriver = ICP201XX;
 	BusCLIArguments cli{true, false};
 	cli.i2c_address = I2C_ADDRESS_DEFAULT;
 	cli.default_i2c_frequency = I2C_SPEED;
@@ -62,7 +62,7 @@ extern "C" int icp10100_main(int argc, char *argv[])
 		return -1;
 	}
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP10100);
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP201XX);
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);

src/drivers/drv_sensor.h

@@ -216,8 +216,8 @@
 #define DRV_FLOW_DEVTYPE_PX4FLOW 0xB5
 #define DRV_FLOW_DEVTYPE_PAA3905 0xB6
 
-#define DRV_BARO_DEVTYPE_ICP10100 0xC0
-#define DRV_BARO_DEVTYPE_ICP10111 0xC1
+#define DRV_BARO_DEVTYPE_ICP101XX 0xB7
+#define DRV_BARO_DEVTYPE_ICP201XX 0xB8
 
 #define DRV_DEVTYPE_UNUSED		0xff
 

src/lib/mixer_module/mixer_module.cpp

@@ -103,12 +103,12 @@ _param_prefix(param_prefix)
 		}
 
 		updateParams();
+		_outputs_pub.advertise();
 
 	} else {
 		_control_allocator_status_pub.advertise();
 	}
 
-	_outputs_pub.advertise();
 }
 
 MixingOutput::~MixingOutput()
@@ -1239,6 +1239,7 @@ int MixingOutput::loadMixer(const char *buf, unsigned len)
 	_mixers->groups_required(_groups_required);
 	PX4_DEBUG("loaded mixers \n%s\n", buf);
 
+	_outputs_pub.advertise();
 	updateParams();
 	_interface.mixerChanged();
 	return ret;

src/lib/sensor_calibration/Accelerometer.hpp

@@ -87,7 +87,8 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		return (_rotation.I() * bias).edivide(_scale) + _thermal_offset + _offset;
+		// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
+		return _offset + (_rotation.I() * bias).edivide(_scale);
 	}
 
 	bool ParametersLoad();

src/lib/sensor_calibration/Gyroscope.hpp

@@ -91,7 +91,8 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		return (_rotation.I() * bias) + _thermal_offset + _offset;
+		// updated calibration offset = existing offset + bias rotated to sensor frame
+		return _offset + (_rotation.I() * bias);
 	}
 
 	bool ParametersLoad();

src/lib/sensor_calibration/Magnetometer.hpp

@@ -89,7 +89,8 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		return _scale.I() * _rotation.I() * bias + _offset;
+		// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
+		return _offset + (_scale.I() * _rotation.I() * bias);
 	}
 
 	bool ParametersLoad();

src/modules/gyro_calibration/GyroCalibration.cpp

@@ -153,6 +153,7 @@ void GyroCalibration::Run()
 			}
 
 			if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) {
+				_gyro_calibration[gyro].SensorCorrectionsUpdate();
 				const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()};
 				_gyro_mean[gyro].update(val);
 				_gyro_last_update[gyro] = sensor_gyro.timestamp;

src/modules/mavlink/mavlink_shell.cpp

@@ -110,8 +110,6 @@ int MavlinkShell::start()
 #ifdef __PX4_NUTTX
 	sched_lock();
 #endif /* __PX4_NUTTX */
-	fflush(stdout);
-	fflush(stderr);
 
 	int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task
 

src/modules/temperature_compensation/temperature_calibration/accel.cpp

@@ -225,5 +225,16 @@ int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int
 	set_parameter("TC_A%d_TMAX", sensor_index, &data.high_temp);
 	set_parameter("TC_A%d_TMIN", sensor_index, &data.low_temp);
 	set_parameter("TC_A%d_TREF", sensor_index, &data.ref_temp);
+
+	// reset current calibration (covered by TC parameters)
+	float offset = 0.0f;
+	float scale = 1.0f;
+	set_parameter("CAL_ACC%u_XOFF", sensor_index, &offset);
+	set_parameter("CAL_ACC%u_YOFF", sensor_index, &offset);
+	set_parameter("CAL_ACC%u_ZOFF", sensor_index, &offset);
+	set_parameter("CAL_ACC%u_XSCALE", sensor_index, &scale);
+	set_parameter("CAL_ACC%u_YSCALE", sensor_index, &scale);
+	set_parameter("CAL_ACC%u_ZSCALE", sensor_index, &scale);
+
 	return 0;
 }

src/modules/temperature_compensation/temperature_calibration/baro.cpp

@@ -209,5 +209,10 @@ int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int
 	set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp);
 	set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp);
 	set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp);
+
+	// reset current calibration (covered by TC parameters)
+	float offset = 0.0f;
+	set_parameter("CAL_BARO%u_OFF", sensor_index, &offset);
+
 	return 0;
 }

src/modules/temperature_compensation/temperature_calibration/gyro.cpp

@@ -211,5 +211,11 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int
 	set_parameter("TC_G%d_TMIN", sensor_index, &data.low_temp);
 	set_parameter("TC_G%d_TREF", sensor_index, &data.ref_temp);
 
+	// reset current calibration (covered by TC parameters)
+	float offset = 0.0f;
+	set_parameter("CAL_GYRO%u_XOFF", sensor_index, &offset);
+	set_parameter("CAL_GYRO%u_YOFF", sensor_index, &offset);
+	set_parameter("CAL_GYRO%u_ZOFF", sensor_index, &offset);
+
 	return 0;
 }