Kconfig

@@ -37,8 +37,8 @@ menu "Toolchain"
         help
             forces nolockstep behaviour, despite REPLAY env variable
 
-    config BOARD_LINUX_TARGET
-        bool "Linux OS Target"
+    config BOARD_LINUX
+        bool "Linux OS"
         depends on PLATFORM_POSIX
         help
             Board Platform is running the Linux operating system

ROMFS/px4fmu_common/init.d/rc.interface

@@ -17,21 +17,17 @@ set OUTPUT_DEV none
 set OUTPUT_AUX_DEV /dev/pwm_output1
 set OUTPUT_EXTRA_DEV /dev/pwm_output0
 
-# set ESC mask before starting the modules
-# setting the numeric parameter to "none" would make the startup script fail
+# set these before starting the modules
 if [ $PWM_AUX_OUT != none ]
 then
+
 	param set PWM_AUX_OUT ${PWM_AUX_OUT}
-else
-	param set PWM_AUX_OUT 0
 fi
 
 
 if [ $PWM_OUT != none ]
 then
 	param set PWM_MAIN_OUT ${PWM_OUT}
-else
-	param set PWM_AUX_OUT 0
 fi
 
 #

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 ARK_FMU_V6X
+	if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X
 	then
 		netman update -i eth0
 	fi
@@ -282,6 +282,28 @@ 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
@@ -534,28 +556,6 @@ 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,7 +6,6 @@ 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
@@ -86,7 +85,6 @@ 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,11 +16,4 @@ 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 | GPIO_PULLDOWN)   /* PF8  */
-#define GPIO_UART7_CTS   (GPIO_UART7_CTS_1 | GPIO_PULLDOWN)   /* PE10 */
+#define GPIO_UART7_RTS   GPIO_UART7_RTS_2   /* PF8  */
+#define GPIO_UART7_CTS   GPIO_UART7_CTS_1   /* 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,6 +326,7 @@
 /* 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,7 +46,6 @@
 #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);
@@ -55,15 +54,6 @@ __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,7 +105,6 @@ __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);
@@ -124,7 +123,6 @@ __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);
 
 }
@@ -211,6 +209,7 @@ 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,6 +43,7 @@ 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/beaglebone/blue/default.px4board

@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX_TARGET=y
+CONFIG_BOARD_LINUX=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a8"
 CONFIG_BOARD_TESTING=y

boards/emlid/navio2/default.px4board

@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX_TARGET=y
+CONFIG_BOARD_LINUX=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y

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_POWER_IN_A                 /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
 
-#define GPIO_VDD_BRICK1_VALID           GPIO_POWER_IN_A /* Brick 1 Is Chosen */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_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_VDD_BRICK1_VALID))
+#define BOARD_ADC_BRICK_VALID   (!px4_arch_gpioread(GPIO_nVDD_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_POWER_IN_A,                  \
+		GPIO_nPOWER_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,7 +195,5 @@ __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,13 +143,12 @@
 #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 3 // Rev 0, 10 and Mini Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 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,11 +79,6 @@ 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[] = {
@@ -106,7 +101,6 @@ 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,17 +46,7 @@ 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(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, {
+	initSPIHWVersion(V6C10, {
 		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,7 +9,6 @@ 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
@@ -24,7 +23,6 @@ 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,34 +70,19 @@ else
 fi
 
 # Internal magnetometer on I2c
-if ver hwtypecmp V6X21
-then
-	rm3100 -I -b 4 start
-else
-	bmm150 -I start
-fi
+bmm150 -I start
 
 # 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
-if ver hwtypecmp V6X21
-then
-	icp201xx -I -a 0x64 start
-else
-	bmp388 -I -a 0x77 start
-fi
+bmp388 -I -a 0x77 start
 
 if ver hwtypecmp V6X00 V6X10
 then
 	bmp388 -I start
 else
-	if ver hwtypecmp V6X21
-	then
-		icp201xx -X start
-	else
-		bmp388 -X start
-	fi
+	bmp388 -X start
 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 | GPIO_PULLDOWN)   /* PF8  */
-#define GPIO_UART7_CTS   (GPIO_UART7_CTS_1 | GPIO_PULLDOWN)   /* PE10 */
+#define GPIO_UART7_RTS   GPIO_UART7_RTS_2   /* PF8  */
+#define GPIO_UART7_CTS   GPIO_UART7_CTS_1   /* 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,14 +129,10 @@
 
 /* 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)
@@ -219,8 +215,7 @@
 #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 9 // Rev 0 and Rev 3,4 Sensor sets
-
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // 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
@@ -229,7 +224,6 @@
 #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,7 +154,6 @@ 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,30 +84,6 @@ 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}),

boards/px4/raspberrypi/cmake/upload.cmake

@@ -38,7 +38,7 @@ else()
 endif()
 
 add_custom_target(upload
-	COMMAND rsync -arh --progress -e "ssh -o StrictHostKeyChecking=no"
+	COMMAND rsync -arh --progress
 			${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/rpi/*.config ${PX4_BINARY_DIR}/etc # source
 			pi@${AUTOPILOT_HOST}:/home/pi/px4 # destination
 	DEPENDS px4

boards/px4/raspberrypi/default.px4board

@@ -1,18 +1,18 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX_TARGET=y
+CONFIG_BOARD_LINUX=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_ADC_ADS1115=y
-CONFIG_COMMON_BAROMETERS=y
+CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_COMMON_IMU=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
-CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
+CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_RC_INPUT=y

boards/scumaker/pilotpi/default.px4board

@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX_TARGET=y
+CONFIG_BOARD_LINUX=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y

cmake/kconfig.cmake

@@ -341,7 +341,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
         add_definitions( ${COMPILE_DEFINITIONS})
 	endif()
 
-	if(LINUX_TARGET)
+	if(LINUX)
         add_definitions( "-D__PX4_LINUX" )
 	endif()
 

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', 'humble', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"):
+    if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"):
         gen_ros2_typename = "-typeros2 "
 
     idl_files = []

platforms/nuttx/NuttX/nuttx

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

src/drivers/barometer/Kconfig

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

src/drivers/barometer/invensense/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   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
@@ -31,5 +31,5 @@
 #
 ############################################################################
 
-add_subdirectory(icp101xx)
-add_subdirectory(icp201xx)
+add_subdirectory(icp10100)
+add_subdirectory(icp10111)

src/drivers/barometer/invensense/Kconfig

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

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

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   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
@@ -32,14 +32,14 @@
 ############################################################################
 
 px4_add_module(
-	MODULE drivers__invensense__icp201xx
-	MAIN icp201xx
+	MODULE drivers__invensense__icp10100
+	MAIN icp10100
 	COMPILE_FLAGS
 	SRCS
-		Inven_Sense_ICP201XX_registers.hpp
-		ICP201XX.cpp
-		ICP201XX.hpp
-		icp201xx_main.cpp
+		Inven_Sense_ICP10100_registers.hpp
+		ICP10100.cpp
+		ICP10100.hpp
+		icp10100_main.cpp
 	DEPENDS
 		px4_work_queue
 	)

src/drivers/barometer/invensense/icp10100/ICP10100.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,17 +31,17 @@
  *
  ****************************************************************************/
 
-#include "ICP101XX.hpp"
+#include "ICP10100.hpp"
 
 using namespace time_literals;
 
-ICP101XX::ICP101XX(const I2CSPIDriverConfig &config) :
+ICP10100::ICP10100(const I2CSPIDriverConfig &config) :
 	I2C(config),
 	I2CSPIDriver(config)
 {
 }
 
-ICP101XX::~ICP101XX()
+ICP10100::~ICP10100()
 {
 	perf_free(_reset_perf);
 	perf_free(_sample_perf);
@@ -49,7 +49,7 @@ ICP101XX::~ICP101XX()
 }
 
 int
-ICP101XX::init()
+ICP10100::init()
 {
 	int ret = I2C::init();
 
@@ -62,7 +62,7 @@ ICP101XX::init()
 }
 
 bool
-ICP101XX::Reset()
+ICP10100::Reset()
 {
 	_state = STATE::RESET;
 	ScheduleClear();
@@ -71,7 +71,7 @@ ICP101XX::Reset()
 }
 
 void
-ICP101XX::print_status()
+ICP10100::print_status()
 {
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_reset_perf);
@@ -80,7 +80,7 @@ ICP101XX::print_status()
 }
 
 int
-ICP101XX::probe()
+ICP10100::probe()
 {
 	uint16_t ID = 0;
 	read_response(Cmd::READ_ID, (uint8_t *)&ID, 2);
@@ -95,7 +95,7 @@ ICP101XX::probe()
 }
 
 void
-ICP101XX::RunImpl()
+ICP10100::RunImpl()
 {
 	const hrt_abstime now = hrt_absolute_time();
 
@@ -233,6 +233,7 @@ ICP101XX::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;
 
@@ -276,7 +277,7 @@ ICP101XX::RunImpl()
 }
 
 bool
-ICP101XX::Measure()
+ICP10100::Measure()
 {
 	/*
 	  From ds-000186-icp-101xx-v1.0.pdf, page 6, table 1
@@ -321,7 +322,7 @@ ICP101XX::Measure()
 }
 
 int8_t
-ICP101XX::cal_crc(uint8_t seed, uint8_t data)
+ICP10100::cal_crc(uint8_t seed, uint8_t data)
 {
 	int8_t poly = 0x31;
 	int8_t var2;
@@ -344,13 +345,13 @@ ICP101XX::cal_crc(uint8_t seed, uint8_t data)
 }
 
 int
-ICP101XX::read_measure_results(uint8_t *buf, uint8_t len)
+ICP10100::read_measure_results(uint8_t *buf, uint8_t len)
 {
 	return transfer(nullptr, 0, buf, len);
 }
 
 int
-ICP101XX::read_response(Cmd cmd, uint8_t *buf, uint8_t len)
+ICP10100::read_response(Cmd cmd, uint8_t *buf, uint8_t len)
 {
 	uint8_t buff[2];
 	buff[0] = ((uint16_t)cmd >> 8) & 0xff;
@@ -359,7 +360,7 @@ ICP101XX::read_response(Cmd cmd, uint8_t *buf, uint8_t len)
 }
 
 int
-ICP101XX::send_command(Cmd cmd)
+ICP10100::send_command(Cmd cmd)
 {
 	uint8_t buf[2];
 	buf[0] = ((uint16_t)cmd >> 8) & 0xff;
@@ -368,7 +369,7 @@ ICP101XX::send_command(Cmd cmd)
 }
 
 int
-ICP101XX::send_command(Cmd cmd, uint8_t *data, uint8_t len)
+ICP10100::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/icp10100/ICP10100.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "Inven_Sense_ICP101XX_registers.hpp"
+#include "Inven_Sense_ICP10100_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_ICP101XX;
+using namespace Inven_Sense_ICP10100;
 
-class ICP101XX : public device::I2C, public I2CSPIDriver<ICP101XX>
+class ICP10100 : public device::I2C, public I2CSPIDriver<ICP10100>
 {
 public:
-	ICP101XX(const I2CSPIDriverConfig &config);
-	~ICP101XX() override;
+	ICP10100(const I2CSPIDriverConfig &config);
+	~ICP10100() override;
 
 	static void print_usage();
 

src/drivers/barometer/invensense/icp10100/Inven_Sense_ICP10100_registers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file icp101xx_registers.hpp
+ * @file icp10100_registers.hpp
  *
- * icp101xx registers.
+ * icp10100 registers.
  *
  */
 
@@ -42,7 +42,7 @@
 
 #include <cstdint>
 
-namespace Inven_Sense_ICP101XX
+namespace Inven_Sense_ICP10100
 {
 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_ICP101XX
+} // namespace Inven_Sense_ICP10100

src/drivers/barometer/invensense/icp10100/Kconfig

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

src/drivers/barometer/invensense/icp10100/icp10100_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -35,12 +35,12 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-#include "ICP201XX.hpp"
+#include "ICP10100.hpp"
 
 void
-ICP201XX::print_usage()
+ICP10100::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("icp201xx", "driver");
+	PRINT_MODULE_USAGE_NAME("icp10100", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
@@ -48,9 +48,9 @@ ICP201XX::print_usage()
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int icp201xx_main(int argc, char *argv[])
+extern "C" int icp10100_main(int argc, char *argv[])
 {
-	using ThisDriver = ICP201XX;
+	using ThisDriver = ICP10100;
 	BusCLIArguments cli{true, false};
 	cli.i2c_address = I2C_ADDRESS_DEFAULT;
 	cli.default_i2c_frequency = I2C_SPEED;
@@ -62,7 +62,7 @@ extern "C" int icp201xx_main(int argc, char *argv[])
 		return -1;
 	}
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP201XX);
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP10100);
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);

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

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+#   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
@@ -32,14 +32,14 @@
 ############################################################################
 
 px4_add_module(
-	MODULE drivers__invensense__icp101xx
-	MAIN icp101xx
+	MODULE drivers__invensense__icp10111
+	MAIN icp10111
 	COMPILE_FLAGS
 	SRCS
-		Inven_Sense_ICP101XX_registers.hpp
-		ICP101XX.cpp
-		ICP101XX.hpp
-		icp101xx_main.cpp
+		Inven_Sense_ICP10111_registers.hpp
+		ICP10111.cpp
+		ICP10111.hpp
+		icp10111_main.cpp
 	DEPENDS
 		px4_work_queue
 	)

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

@@ -0,0 +1,378 @@
+/****************************************************************************
+ *
+ *   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/ICP10111.hpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "Inven_Sense_ICP10111_registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/device/i2c.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+
+using namespace Inven_Sense_ICP10111;
+
+class ICP10111 : public device::I2C, public I2CSPIDriver<ICP10111>
+{
+public:
+	ICP10111(const I2CSPIDriverConfig &config);
+	~ICP10111() override;
+
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+	void print_status() override;
+
+private:
+	int probe() override;
+
+	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);
+
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+
+	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+
+	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,
+		READ
+	} _state{STATE::RESET};
+
+	enum class MODE : uint8_t {
+		FAST,
+		NORMAL,
+		ACCURATE,
+		VERY_ACCURATE
+	} _mode{MODE::VERY_ACCURATE};
+};

src/drivers/barometer/invensense/icp10111/Inven_Sense_ICP10111_registers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file icp201xx_registers.hpp
+ * @file icp10111_registers.hpp
  *
- * icp201xx registers.
+ * icp10111 registers.
  *
  */
 
@@ -42,45 +42,21 @@
 
 #include <cstdint>
 
-namespace Inven_Sense_ICP201XX
+namespace Inven_Sense_ICP10111
 {
 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;
 
-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
+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
 };
-} // namespace Inven_Sense_ICP201XX
+} // namespace Inven_Sense_ICP10111

src/drivers/barometer/invensense/icp10111/Kconfig

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

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -35,12 +35,12 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-#include "ICP101XX.hpp"
+#include "ICP10111.hpp"
 
 void
-ICP101XX::print_usage()
+ICP10111::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("icp101xx", "driver");
+	PRINT_MODULE_USAGE_NAME("icp10111", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
@@ -48,9 +48,9 @@ ICP101XX::print_usage()
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int icp101xx_main(int argc, char *argv[])
+extern "C" int icp10111_main(int argc, char *argv[])
 {
-	using ThisDriver = ICP101XX;
+	using ThisDriver = ICP10111;
 	BusCLIArguments cli{true, false};
 	cli.i2c_address = I2C_ADDRESS_DEFAULT;
 	cli.default_i2c_frequency = I2C_SPEED;
@@ -62,7 +62,7 @@ extern "C" int icp101xx_main(int argc, char *argv[])
 		return -1;
 	}
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP101XX);
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP10111);
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);

src/drivers/barometer/invensense/icp101xx/Kconfig

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

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

@@ -1,521 +0,0 @@
-/****************************************************************************
- *
- *   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,117 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include "Inven_Sense_ICP201XX_registers.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <lib/drivers/device/i2c.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/sensor_baro.h>
-#include <lib/perf/perf_counter.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-
-using namespace Inven_Sense_ICP201XX;
-
-class ICP201XX : public device::I2C, public I2CSPIDriver<ICP201XX>
-{
-public:
-	ICP201XX(const I2CSPIDriverConfig &config);
-	~ICP201XX() override;
-
-	static void print_usage();
-
-	void RunImpl();
-
-	int init() override;
-	void print_status() override;
-
-private:
-	int probe() override;
-
-	bool Reset();
-
-	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)};
-
-	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-
-	hrt_abstime _reset_timestamp{0};
-	int _failure_count{0};
-
-	enum class STATE : uint8_t {
-		SOFT_RESET = 0,
-		OTP_BOOTUP_CFG,
-		CONFIG,
-		WAIT_READ,
-		READ
-	} _state{STATE::SOFT_RESET};
-
-	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/Kconfig

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

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_ICP101XX 0xB7
-#define DRV_BARO_DEVTYPE_ICP201XX 0xB8
+#define DRV_BARO_DEVTYPE_ICP10100 0xC0
+#define DRV_BARO_DEVTYPE_ICP10111 0xC1
 
 #define DRV_DEVTYPE_UNUSED		0xff
 

src/drivers/imu/adis16497/ADIS16497.cpp

@@ -342,9 +342,9 @@ ADIS16497::read_reg16(uint8_t reg)
 
 	cmd[0] = ((reg | DIR_READ) << 8) & 0xff00;
 	transferhword(cmd, nullptr, 1);
-	px4_udelay(T_STALL);
+	up_udelay(T_STALL);
 	transferhword(nullptr, cmd, 1);
-	px4_udelay(T_STALL);
+	up_udelay(T_STALL);
 
 	return cmd[0];
 }
@@ -367,9 +367,9 @@ ADIS16497::write_reg16(uint8_t reg, uint16_t value)
 	cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8);
 
 	transferhword(cmd, nullptr, 1);
-	px4_udelay(T_STALL);
+	up_udelay(T_STALL);
 	transferhword(cmd + 1, nullptr, 1);
-	px4_udelay(T_STALL);
+	up_udelay(T_STALL);
 }
 
 void

src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp

@@ -33,6 +33,8 @@
 
 #include "BMI055_Gyroscope.hpp"
 
+#include <px4_platform/board_dma_alloc.h>
+
 using namespace time_literals;
 
 namespace Bosch::BMI055::Gyroscope

src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp

@@ -33,6 +33,8 @@
 
 #include "BMI088_Gyroscope.hpp"
 
+#include <px4_platform/board_dma_alloc.h>
+
 using namespace time_literals;
 
 namespace Bosch::BMI088::Gyroscope

src/drivers/linux_pwm_out/CMakeLists.txt

@@ -40,5 +40,5 @@ px4_add_module(
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
-		mixer_module
 	)
+

src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp

@@ -40,6 +40,7 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
+#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp

@@ -40,6 +40,7 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
+#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp

@@ -40,6 +40,7 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
+#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp

@@ -40,6 +40,7 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
+#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

src/drivers/magnetometer/rm3100/rm3100_i2c.cpp

@@ -40,6 +40,7 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
+#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

src/drivers/magnetometer/rm3100/rm3100_spi.cpp

@@ -40,6 +40,7 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
+#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

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

src/lib/sensor_calibration/Gyroscope.hpp

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

src/lib/sensor_calibration/Magnetometer.hpp

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

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -148,11 +148,6 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 	}
 
 	if ((gps.time_usec - _time_last_gps) > _min_obs_interval_us) {
-
-		if (!gps.vel_ned_valid || (gps.fix_type == 0)) {
-			return;
-		}
-
 		_time_last_gps = gps.time_usec;
 
 		gpsSample gps_sample_new;

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -79,7 +79,6 @@ public:
 TEST_F(EkfGpsTest, gpsTimeout)
 {
 	// GIVEN:EKF that fuses GPS
-	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
 
 	// WHEN: setting the PDOP to high
 	_sensor_simulator._gps.setNumberOfSatellites(3);
@@ -91,24 +90,6 @@ TEST_F(EkfGpsTest, gpsTimeout)
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
 }
 
-TEST_F(EkfGpsTest, gpsFixLoss)
-{
-	// GIVEN:EKF that fuses GPS
-	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
-
-	// WHEN: the fix is loss
-	_sensor_simulator._gps.setFixType(0);
-
-	// THEN: after dead-reconing for a couple of seconds, the local position gets invalidated
-	_sensor_simulator.runSeconds(5);
-	EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning);
-	EXPECT_FALSE(_ekf->local_position_is_valid());
-
-	// The control logic takes a bit more time to deactivate the GNSS fusion completely
-	_sensor_simulator.runSeconds(5);
-	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
-}
-
 TEST_F(EkfGpsTest, resetToGpsVelocity)
 {
 	ResetLoggingChecker reset_logging_checker(_ekf);

src/modules/gyro_calibration/GyroCalibration.cpp

@@ -153,7 +153,6 @@ 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,6 +110,8 @@ 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/mc_pos_control/MulticopterPositionControl.cpp

@@ -443,6 +443,7 @@ void MulticopterPositionControl::Run()
 
 			// Allow ramping from zero thrust on takeoff
 			const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
+
 			_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
 
 			_control.setVelocityLimits(

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -79,11 +79,10 @@ void PositionControl::updateHoverThrust(const float hover_thrust_new)
 	// T' = T => a_sp' * Th' / g - Th' = a_sp * Th / g - Th
 	// so a_sp' = (a_sp - g) * Th / Th' + g
 	// we can then add a_sp' - a_sp to the current integrator to absorb the effect of changing Th by Th'
-	const float previous_hover_thrust = _hover_thrust;
-	setHoverThrust(hover_thrust_new);
-
-	_vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust
-		       + CONSTANTS_ONE_G - _acc_sp(2);
+	if (hover_thrust_new > FLT_EPSILON) {
+		_vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * _hover_thrust / hover_thrust_new + CONSTANTS_ONE_G - _acc_sp(2);
+		setHoverThrust(hover_thrust_new);
+	}
 }
 
 void PositionControl::setState(const PositionControlStates &states)
@@ -140,9 +139,6 @@ void PositionControl::_positionControl()
 
 void PositionControl::_velocityControl(const float dt)
 {
-	// Constrain vertical velocity integral
-	_vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);
-
 	// PID velocity control
 	Vector3f vel_error = _vel_sp - _vel;
 	Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
@@ -193,6 +189,9 @@ void PositionControl::_velocityControl(const float dt)
 	ControlMath::setZeroIfNanVector3f(vel_error);
 	// Update integral part of velocity control
 	_vel_int += vel_error.emult(_gain_vel_i) * dt;
+
+	// limit thrust integral
+	_vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2));
 }
 
 void PositionControl::_accelerationControl()

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -121,9 +121,9 @@ public:
 
 	/**
 	 * Set the normalized hover thrust
-	 * @param hover_thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation
+	 * @param thrust [0.1, 0.9] with which the vehicle hovers not acelerating down or up with level orientation
 	 */
-	void setHoverThrust(const float hover_thrust) { _hover_thrust = math::constrain(hover_thrust, HOVER_THRUST_MIN, HOVER_THRUST_MAX); }
+	void setHoverThrust(const float hover_thrust) { _hover_thrust = math::constrain(hover_thrust, 0.1f, 0.9f); }
 
 	/**
 	 * Update the hover thrust without immediately affecting the output
@@ -179,10 +179,6 @@ public:
 	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
 private:
-	// The range limits of the hover thrust configuration/estimate
-	static constexpr float HOVER_THRUST_MIN = 0.05f;
-	static constexpr float HOVER_THRUST_MAX = 0.9f;
-
 	bool _inputValid();
 
 	void _positionControl(); ///< Position proportional control
@@ -204,7 +200,7 @@ private:
 	float _lim_thr_xy_margin{}; ///< Margin to keep for horizontal control when saturating prioritized vertical thrust
 	float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have
 
-	float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation
+	float _hover_thrust{}; ///< Thrust [0.1, 0.9] with which the vehicle hovers not accelerating down or up with level orientation
 
 	// States
 	matrix::Vector3f _pos; /**< current position */

src/modules/temperature_compensation/temperature_calibration/accel.cpp

@@ -225,16 +225,5 @@ 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,10 +209,5 @@ 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,11 +211,5 @@ 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;
 }