Merge remote-tracking branch 'px4/master' into pr-land_detector_vehicle_at_rest

2026-05-18 05:47:35 +08:00 · 2022-02-01 11:29:21 -05:00
parent 27607d470b e387f302f9
commit 55224b85e7
229 changed files with 7875 additions and 3591 deletions
@@ -125,6 +125,11 @@ define_property(GLOBAL PROPERTY PX4_MODULE_LIBRARIES
                 FULL_DOCS "List of all PX4 module libraries"
                 )

+define_property(GLOBAL PROPERTY PX4_KERNEL_MODULE_LIBRARIES
+                 BRIEF_DOCS "PX4 kernel side module libs"
+                 FULL_DOCS "List of all PX4 kernel module libraries"
+                 )
+
 define_property(GLOBAL PROPERTY PX4_MODULE_PATHS
                 BRIEF_DOCS "PX4 module paths"
                 FULL_DOCS "List of paths to all PX4 modules"
@@ -142,6 +147,7 @@ set(CONFIG "px4_sitl_default" CACHE STRING "desired configuration")

 include(px4_add_module)
 set(config_module_list)
+set(config_kernel_list)

 # Find Python
 # If using catkin, Python 2 is found since it points
@@ -118,6 +118,11 @@ config BOARD_CRYPTO
    help
        Enable PX4 Crypto Support. Select the implementation under drivers

+config BOARD_PROTECTED
+    bool "Memory protection"
+    help
+        Enable memory protection via MPU/MMU
+
 menu "Serial ports"

    config BOARD_SERIAL_URT6
@@ -10,7 +10,7 @@ fi

 if [ ! -f replay_params.txt ]; then
 	echo "Creating $(pwd)/replay_params.txt"
-	ulog_params -i "${replay}" -d ' ' | grep -e '^EKF2' > replay_params.txt
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
 fi

 publisher_rules_file="orb_publisher.rules"
@@ -21,6 +21,7 @@ ignore_others: false
 EOF

 param set SDLOG_DIRS_MAX 7
+param set SDLOG_PROFILE 3

 # apply all params before ekf starts, as some params cannot be changed after startup
 replay tryapplyparams
@@ -0,0 +1,23 @@
+#!/bin/sh
+#
+# @name Holybro X500 V2
+#
+# @type Quadrotor x
+# @class Copter
+#
+# @maintainer Farhang Naderi <farhang.nba@gmail.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default IMU_GYRO_CUTOFF 30
+
+param set-default MC_ROLLRATE_P 0.14
+param set-default MC_PITCHRATE_P 0.14
+param set-default MC_ROLLRATE_I 0.3
+param set-default MC_PITCHRATE_I 0.3
+param set-default MC_ROLLRATE_D 0.004
+param set-default MC_PITCHRATE_D 0.004
@@ -73,6 +73,7 @@ px4_add_romfs_files(
 	4016_holybro_px4vision
 	4017_nxp_hovergames
 	4018_s500_ctrlalloc
+	4019_x500_v2
 	4030_3dr_solo
 	4031_3dr_quad
 	4040_reaper
@@ -130,6 +130,13 @@ then
 	fi
 fi

+# SHT3x temperature and hygrometer sensor, external I2C
+if param compare -s SENS_EN_SHT3X 1
+then
+	sht3x start -X
+	sht3x start -X -a 0x45
+fi
+
 # TE MS4525 differential pressure sensor external I2C
 if param compare -s SENS_EN_MS4525 1
 then
@@ -39,7 +39,7 @@ function spawn_model() {

 	echo "Spawning ${MODEL}_${N} at ${X} ${Y}"

-	gz model --spawn-file=/tmp/${MODEL}_${N}.sdf --model-name=${MODEL}_${N} -x ${X} -y ${Y} -z 0.0
+	gz model --spawn-file=/tmp/${MODEL}_${N}.sdf --model-name=${MODEL}_${N} -x ${X} -y ${Y} -z 0.83

 	popd &>/dev/null

@@ -56,6 +56,21 @@ def get_N_colors(N, s=0.8, v=0.9):
        hex_out.append("#"+"".join(map(lambda x: format(x, '02x'), rgb)))
    return hex_out

+def topic_filename(topic):
+    MSG_PATH = 'msg/'
+
+    file_list = os.listdir(MSG_PATH)
+    msg_files = [file.replace('.msg', '') for file in file_list if file.endswith(".msg")]
+
+    if topic in msg_files:
+        return topic
+    else:
+        for msg_file in msg_files:
+            with open(f'{MSG_PATH}/{msg_file}.msg') as f:
+                ret = re.findall(f'^# TOPICS.*{topic}.*',f.read(),re.MULTILINE)
+                if len(ret) > 0:
+                    return msg_file
+    return "no_file"

 class PubSub(object):
    """ Collects either publication or subscription information for nodes
@@ -679,8 +694,7 @@ class OutputJSON(object):
            node['type'] = 'topic'
            node['color'] = topic_colors[topic]
            # url is opened when double-clicking on the node
-            # TODO: does not work for multi-topics
-            node['url'] = 'https://github.com/PX4/PX4-Autopilot/blob/master/msg/'+topic+'.msg'
+            node['url'] = 'https://github.com/PX4/PX4-Autopilot/blob/master/msg/'+topic_filename(topic)+'.msg'
            nodes.append(node)

        data['nodes'] = nodes
@@ -61,6 +61,5 @@ else()
 			nuttx_arch
 			nuttx_drivers
 			px4_layer
-			arch_io_pins
 	)
 endif()
@@ -63,6 +63,5 @@ else()
 			nuttx_arch
 			nuttx_drivers
 			px4_layer
-			arch_io_pins
 	)
 endif()
@@ -63,6 +63,5 @@ else()
 			nuttx_arch
 			nuttx_drivers
 			px4_layer
-			arch_io_pins
 	)
 endif()
@@ -61,6 +61,5 @@ else()
 			nuttx_arch
 			nuttx_drivers
 			px4_layer
-			arch_io_pins
 	)
 endif()
@@ -149,7 +149,6 @@
 #define BOARD_HAS_STATIC_MANIFEST 1


-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -144,7 +144,6 @@
 #define BOARD_HAS_STATIC_MANIFEST 1


-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -62,6 +62,5 @@ else()
 			nuttx_arch
 			nuttx_drivers
 			px4_layer
-			arch_io_pins
 	)
 endif()
@@ -68,6 +68,5 @@ else()
 			nuttx_arch
 			nuttx_drivers
 			px4_layer
-			arch_io_pins
 	)
 endif()
@@ -181,7 +181,6 @@

 #define BOARD_NUM_IO_TIMERS 4

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 9, 8};

 /* Power supply control and monitoring GPIOs */

@@ -430,7 +430,6 @@

 #define BOARD_NUM_IO_TIMERS 5

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8, 9, 10};

 __BEGIN_DECLS

@@ -262,7 +262,6 @@

 #define BOARD_NUM_IO_TIMERS 5

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7};

 __BEGIN_DECLS

@@ -199,7 +199,6 @@
 */
 #define DIRECT_PWM_OUTPUT_CHANNELS   8

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7};

 /* Power supply control and monitoring GPIOs */
 #define GPIO_nVDD_USB_VALID             /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) /* Low for USB power, High for DC power */
@@ -157,7 +157,6 @@

 #define BOARD_NUM_IO_TIMERS 3

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7};

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -157,7 +157,6 @@

 #define BOARD_NUM_IO_TIMERS 3

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7};

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -146,7 +146,6 @@
 #define BOARD_ADC_BRICK_VALID   (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID))

 #define BOARD_NUM_IO_TIMERS 3
-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7};
 #define BOARD_DMA_ALLOC_POOL_SIZE 5120 /* This board provides a DMA pool and APIs */
 #define BOARD_HAS_ON_RESET 1 /* This board provides the board_on_reset interface */
 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -46,7 +46,6 @@ px4_add_library(drivers_board
 	imxrt_flexspi_nor_boot.c
 	imxrt_flexspi_nor_flash.c
 )
-add_dependencies(drivers_board arch_board_hw_info)

 target_link_libraries(drivers_board
 	PRIVATE
@@ -45,7 +45,6 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "canbootloader")
 			nuttx_arch
 			nuttx_drivers
 			canbootloader
-			arch_io_pins
 			arch_led_pwm
 	)
 	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader)
@@ -73,6 +72,6 @@ else()
 			nuttx_drivers # sdio
 			drivers__led # drv_led_start
 			px4_layer
-			arch_io_pins
+			px4_platform # I2CBusIterator
 		)
 endif()
@@ -147,7 +147,6 @@
 #define BOARD_ENABLE_CONSOLE_BUFFER
 #define BOARD_CONSOLE_BUFFER_SIZE (1024*3)

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {0, 1, 3, 2};

 __BEGIN_DECLS

@@ -158,7 +158,6 @@

 #define BOARD_HAS_ON_RESET 1

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

 __BEGIN_DECLS

@@ -158,7 +158,6 @@

 #define BOARD_HAS_ON_RESET 1

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

 /* Internal IMU Heater
 *
@@ -48,5 +48,4 @@ target_link_libraries(drivers_board
 		nuttx_arch # sdio
 		nuttx_drivers # sdio
 		px4_layer
-		arch_io_pins
 )
@@ -179,7 +179,6 @@

 #define BOARD_HAS_ON_RESET 1

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

 __BEGIN_DECLS

@@ -12,6 +12,7 @@ CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
+CONFIG_COMMON_HYGROMETERS=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
@@ -81,8 +82,8 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -0,0 +1,146 @@
+/****************************************************************************
+ * kernel-space.ld
+ *
+ *   Copyright (C) 2015 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* NOTE:  This depends on the memory.ld script having been included prior to
+ * this script.
+ */
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+ENTRY(_stext)
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+EXTERN(_bootdelay_signature)
+/*
+ * TODO: Fill in the signature location into TOC from user-space elf
+EXTERN(_main_toc)
+*/
+
+SECTIONS
+{
+    .text : {
+        _stext = ABSOLUTE(.);
+        *(.vectors)
+	. = ALIGN(32);
+	/*
+	This signature provides the bootloader with a way to delay booting
+	*/
+	_bootdelay_signature = ABSOLUTE(.);
+	FILL(0xffecc2925d7d05c5)
+	. += 8;
+        /*
+        *(.main_toc)
+        */
+        *(.text .text.*)
+        *(.fixup)
+        *(.gnu.warning)
+        *(.rodata .rodata.*)
+        *(.gnu.linkonce.t.*)
+        *(.glue_7)
+        *(.glue_7t)
+        *(.got)
+        *(.gcc_except_table)
+        *(.gnu.linkonce.r.*)
+        _etext = ABSOLUTE(.);
+    } > kflash
+
+    /*
+     * Init functions (static constructors and the like)
+     */
+    .init_section : {
+        _sinit = ABSOLUTE(.);
+	KEEP(*(.init_array .init_array.*))
+        _einit = ABSOLUTE(.);
+    } > kflash
+
+
+    .ARM.extab : {
+        *(.ARM.extab*)
+    } > kflash
+
+    __exidx_start = ABSOLUTE(.);
+    .ARM.exidx : {
+        *(.ARM.exidx*)
+    } > kflash
+
+    __exidx_end = ABSOLUTE(.);
+
+    _eronly = ABSOLUTE(.);
+
+    .data : {
+        _sdata = ABSOLUTE(.);
+        *(.data .data.*)
+        *(.gnu.linkonce.d.*)
+        CONSTRUCTORS
+        _edata = ABSOLUTE(.);
+    } > ksram AT > kflash
+
+    .bss : {
+        _sbss = ABSOLUTE(.);
+        *(.bss .bss.*)
+        *(.gnu.linkonce.b.*)
+        *(COMMON)
+        . = ALIGN(4);
+        _ebss = ABSOLUTE(.);
+    } > ksram
+
+    /* Stabs debugging sections */
+
+    .stab 0 : { *(.stab) }
+    .stabstr 0 : { *(.stabstr) }
+    .stab.excl 0 : { *(.stab.excl) }
+    .stab.exclstr 0 : { *(.stab.exclstr) }
+    .stab.index 0 : { *(.stab.index) }
+    .stab.indexstr 0 : { *(.stab.indexstr) }
+    .comment 0 : { *(.comment) }
+    .debug_abbrev 0 : { *(.debug_abbrev) }
+    .debug_info 0 : { *(.debug_info) }
+    .debug_line 0 : { *(.debug_line) }
+    .debug_pubnames 0 : { *(.debug_pubnames) }
+    .debug_aranges 0 : { *(.debug_aranges) }
+
+    .ramfunc : {
+        _sramfuncs = .;
+	*(.ramfunc  .ramfunc.*)
+	. = ALIGN(4);
+	_eramfuncs = .;
+    } > ksram AT > kflash
+
+    _framfuncs = LOADADDR(.ramfunc);
+}
@@ -0,0 +1,98 @@
+/****************************************************************************
+ * scripts/memory.ld
+ *
+ *   Copyright (C) 2016 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F765IIT6 has 2048 KiB of main FLASH memory.  This FLASH memory
+ * can be accessed from either the AXIM interface at address 0x0800:0000 or
+ * from the ITCM interface at address 0x0020:0000.
+ *
+ * Additional information, including the option bytes, is available at at
+ * FLASH at address 0x1ff0:0000 (AXIM) or 0x0010:0000 (ITCM).
+ *
+ * In the STM32F765IIT6, two different boot spaces can be selected through
+ * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and
+ * BOOT_ADD1 option bytes:
+ *
+ *   1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0].
+ *      ST programmed value: Flash on ITCM at 0x0020:0000
+ *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
+ *      ST programmed value: System bootloader at 0x0010:0000
+ *
+ * NuttX does not modify these option byes.  On the unmodified NUCLEO-144
+ * board, the BOOT0 pin is at ground so by default, the STM32F765IIT6 will
+ * boot from address 0x0020:0000 in ITCM FLASH.
+ *
+ * The STM32F765IIT6 also has 512 KiB of data SRAM (in addition to ITCM SRAM).
+ * SRAM is split up into three blocks:
+ *
+ *   1) 128 KiB of DTCM SRM beginning at address 0x2000:0000
+ *   2) 368 KiB of SRAM1 beginning at address 0x2002:0000
+ *   3)  16 KiB of SRAM2 beginning at address 0x2007:c000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * Bootloader reserves the first 32K bank (2 Mbytes Flash memory single bank)
+ * organization (256 bits read width)
+ */
+
+MEMORY
+{
+  /* ITCM boot address */
+
+  itcm  (rwx) : ORIGIN = 0x00208000, LENGTH = 2048K-32K
+
+  /* 2048KB FLASH, bootloader reserves the first 32kb */
+
+  kflash (rx) : ORIGIN = 0x08008000, LENGTH = 1024K-32K
+  uflash (rx) : ORIGIN = 0x08100000, LENGTH = 1024K
+
+  /* ITCM RAM */
+
+  itcm_ram  (rwx) : ORIGIN = 0x00000000, LENGTH = 16K
+
+  /* DTCM SRAM */
+
+  dtcm_ram  (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+
+  /* 368KB of contiguous SRAM1 */
+
+  ksram (rwx) : ORIGIN = 0x20020000, LENGTH = 128K
+  usram (rwx) : ORIGIN = 0x20040000, LENGTH = 368K-128K
+
+  /* 16KB of SRAM2 */
+
+  sram2 (rwx) : ORIGIN = 0x2007c000, LENGTH = 16K
+}
@@ -0,0 +1,124 @@
+/****************************************************************************
+ * user-space.ld
+ *
+ *   Copyright (C) 2015 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* NOTE:  This depends on the memory.ld script having been included prior to
+ * this script.
+ */
+EXTERN(userspace)
+OUTPUT_ARCH(arm)
+SECTIONS
+{
+    .userspace : {
+        *(.userspace)
+    } > uflash
+
+    .text : {
+        _stext = ABSOLUTE(.);
+        *(.text .text.*)
+        *(.fixup)
+        *(.gnu.warning)
+        *(.rodata .rodata.*)
+        *(.gnu.linkonce.t.*)
+        *(.glue_7)
+        *(.glue_7t)
+        *(.got)
+        *(.gcc_except_table)
+        *(.gnu.linkonce.r.*)
+        _etext = ABSOLUTE(.);
+    } > uflash
+
+    /*
+     * Init functions (static constructors and the like)
+     */
+    .init_section : {
+        _sinit = ABSOLUTE(.);
+        KEEP(*(.init_array .init_array.*))
+        _einit = ABSOLUTE(.);
+    } > uflash
+
+
+    .ARM.extab : {
+        *(.ARM.extab*)
+    } > uflash
+
+    __exidx_start = ABSOLUTE(.);
+    .ARM.exidx : {
+        *(.ARM.exidx*)
+    } > uflash
+
+    __exidx_end = ABSOLUTE(.);
+
+    _eronly = ABSOLUTE(.);
+
+    .data : {
+        _sdata = ABSOLUTE(.);
+        *(.data .data.*)
+        *(.gnu.linkonce.d.*)
+        CONSTRUCTORS
+        _edata = ABSOLUTE(.);
+    } > usram AT > uflash
+
+    .bss : {
+        _sbss = ABSOLUTE(.);
+        *(.bss .bss.*)
+        *(.gnu.linkonce.b.*)
+        *(COMMON)
+        /* Kernel heap start at _ebss, make _ebss MPU-friendly aligned */
+        . = ALIGN(0x1000);
+        _ebss = ABSOLUTE(.);
+    } > usram
+
+    /* Stabs debugging sections */
+
+    .stab 0 : { *(.stab) }
+    .stabstr 0 : { *(.stabstr) }
+    .stab.excl 0 : { *(.stab.excl) }
+    .stab.exclstr 0 : { *(.stab.exclstr) }
+    .stab.index 0 : { *(.stab.index) }
+    .stab.indexstr 0 : { *(.stab.indexstr) }
+    .comment 0 : { *(.comment) }
+    .debug_abbrev 0 : { *(.debug_abbrev) }
+    .debug_info 0 : { *(.debug_info) }
+    .debug_line 0 : { *(.debug_line) }
+    .debug_pubnames 0 : { *(.debug_pubnames) }
+    .debug_aranges 0 : { *(.debug_aranges) }
+
+    /* Start of the image signature. This
+     *  has to be in the end of the image
+     */
+     .signature : {
+         _boot_signature = ALIGN(4);
+     } > uflash
+}
@@ -433,7 +433,6 @@

 #define BOARD_NUM_IO_TIMERS 5

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8, 9, 10};

 __BEGIN_DECLS

@@ -1,4 +1,5 @@
 # CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
+CONFIG_COMMON_HYGROMETERS=n
 CONFIG_DRIVERS_BATT_SMBUS=n
 CONFIG_DRIVERS_CAMERA_CAPTURE=n
 CONFIG_DRIVERS_CAMERA_TRIGGER=n
@@ -43,7 +43,8 @@ add_library(drivers_board
 	timer_config.cpp
 	usb.c
 )
-add_dependencies(drivers_board arch_board_hw_info platform_gpio_mcp23009)
+
+add_dependencies(drivers_board platform_gpio_mcp23009)

 target_link_libraries(drivers_board
 	PRIVATE
@@ -218,7 +218,6 @@
 */
 #define DIRECT_PWM_OUTPUT_CHANNELS  9

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8};

 /* Power supply control and monitoring GPIOs */

@@ -196,7 +196,6 @@
 */
 #define DIRECT_PWM_OUTPUT_CHANNELS   9

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8};

 /* Power supply control and monitoring GPIOs */

@@ -236,7 +236,6 @@
 */
 #define DIRECT_PWM_OUTPUT_CHANNELS   9

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8};

 /* Power supply control and monitoring GPIOs */

@@ -48,5 +48,4 @@ target_link_libraries(drivers_board
 		nuttx_arch # sdio
 		nuttx_drivers # sdio
 		px4_layer
-		arch_io_pins
 )
@@ -133,7 +133,6 @@

 #define BOARD_NUM_IO_TIMERS 3

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {1, 2, 3, 0, 4, 5, 6, 7};

 __BEGIN_DECLS

@@ -48,5 +48,4 @@ target_link_libraries(drivers_board
 		nuttx_arch # sdio
 		nuttx_drivers # sdio
 		px4_layer
-		arch_io_pins
 )
@@ -178,7 +178,6 @@

 #define BOARD_HAS_ON_RESET 1

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

 #define BOARD_ENABLE_CONSOLE_BUFFER
 #define BOARD_CONSOLE_BUFFER_SIZE (1024*3)
@@ -26,6 +26,8 @@ set(COMMON_KCONFIG_ENV_SETTINGS
 	ROMFSROOT=${config_romfs_root}
 )

+set(config_user_list)
+
 if(EXISTS ${BOARD_DEFCONFIG})

    # Depend on BOARD_DEFCONFIG so that we reconfigure on config change
@@ -79,6 +81,17 @@ if(EXISTS ${BOARD_DEFCONFIG})
            endif()
        endif()

+        # Find variable name
+        string(REGEX MATCH "^CONFIG_USER[^=]+" Userspace ${NameAndValue})
+
+        if(Userspace)
+            # Find the value
+            string(REPLACE "${Name}=" "" Value ${NameAndValue})
+            string(REPLACE "CONFIG_USER_" "" module ${Name})
+            string(TOLOWER ${module} module)
+            list(APPEND config_user_list ${module})
+        endif()
+
        # Find variable name
        string(REGEX MATCH "^CONFIG_DRIVERS[^=]+" Drivers ${NameAndValue})

@@ -171,6 +184,16 @@ if(EXISTS ${BOARD_DEFCONFIG})

    endforeach()

+    # Put every module not in userspace also to kernel list
+    foreach(modpath ${config_module_list})
+        get_filename_component(module ${modpath} NAME)
+        list(FIND config_user_list ${module} _index)
+
+        if (${_index} EQUAL -1)
+            list(APPEND config_kernel_list ${modpath})
+        endif()
+    endforeach()
+
    if(PLATFORM)
        # set OS, and append specific platform module path
        set(PX4_PLATFORM ${PLATFORM} CACHE STRING "PX4 board OS" FORCE)
@@ -336,6 +359,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	list(APPEND config_module_list ${board_support_src_rel}/src)

 	set(config_module_list ${config_module_list})
+	set(config_kernel_list ${config_kernel_list})

 endif()

@@ -44,8 +44,8 @@ function(px4_add_library target)
 	target_compile_definitions(${target} PRIVATE MODULE_NAME="${target}")

 	# all PX4 libraries have access to parameters and uORB
-	add_dependencies(${target} uorb_headers)
-	target_link_libraries(${target} PRIVATE prebuild_targets parameters_interface px4_platform)
+	add_dependencies(${target} uorb_headers parameters)
+	target_link_libraries(${target} PRIVATE prebuild_targets)

 	set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
 	px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${ARGN})
@@ -154,9 +154,29 @@ function(px4_add_module)
 	# all modules can potentially use parameters and uORB
 	add_dependencies(${MODULE} uorb_headers)

+	# Check if the modules source dir exists in config_kernel_list
+	# in this case, treat is as a kernel side component for
+	# protected build
+	get_target_property(MODULE_SOURCE_DIR ${MODULE} SOURCE_DIR)
+	file(RELATIVE_PATH module ${PROJECT_SOURCE_DIR}/src ${MODULE_SOURCE_DIR})
+
+	list (FIND config_kernel_list ${module} _index)
+	if (${_index} GREATER -1)
+		set (KERNEL TRUE)
+	endif()
+
 	if(NOT DYNAMIC)
-		target_link_libraries(${MODULE} PRIVATE prebuild_targets parameters_interface px4_layer px4_platform systemlib)
-		set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})
+		target_link_libraries(${MODULE} PRIVATE prebuild_targets parameters_interface px4_platform systemlib perf)
+		if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL)
+			target_link_libraries(${MODULE} PRIVATE px4_kernel_layer uORB_kernel)
+			set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE})
+		else()
+			target_link_libraries(${MODULE} PRIVATE px4_layer uORB)
+			set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})
+		endif()
+		set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
+		px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${SRCS})
+		set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS})
 	endif()

 	set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
@@ -197,6 +217,10 @@ function(px4_add_module)
 		target_compile_options(${MODULE} PRIVATE ${COMPILE_FLAGS})
 	endif()

+	if (KERNEL)
+		target_compile_options(${MODULE} PRIVATE -D__KERNEL__)
+	endif()
+
 	if(INCLUDES)
 		target_include_directories(${MODULE} PRIVATE ${INCLUDES})
 	endif()
@@ -108,6 +108,7 @@ set(msg_files
 	manual_control_setpoint.msg
 	manual_control_switches.msg
 	mavlink_log.msg
+	mavlink_tunnel.msg
 	mission.msg
 	mission_result.msg
 	mount_orientation.msg
@@ -70,7 +70,8 @@ uint8 MAX_INSTANCES = 4

 float32 average_power               # The average power of the current discharge
 float32 available_energy            # The predicted charge or energy remaining in the battery
-float32 remaining_capacity          # The compensated battery capacity remaining
+float32 full_charge_capacity_wh     # The compensated battery capacity
+float32 remaining_capacity_wh       # The compensated battery capacity remaining
 float32 design_capacity             # The design capacity of the battery
 uint16 average_time_to_full         # The predicted remaining time until the battery reaches full charge, in minutes
 uint16 over_discharge_count         # Number of battery overdischarge
@@ -1,12 +1,12 @@
-uint64 timestamp			    # time since system start (microseconds)
-uint8 GF_ACTION_NONE = 0                    # no action on geofence violation
-uint8 GF_ACTION_WARN = 1                    # critical mavlink message
-uint8 GF_ACTION_LOITER = 2                  # switch to AUTO|LOITER
-uint8 GF_ACTION_RTL = 3                     # switch to AUTO|RTL
-uint8 GF_ACTION_TERMINATE = 4               # flight termination
-uint8 GF_ACTION_LAND = 5		    # switch to AUTO|LAND
+uint64 timestamp                # time since system start (microseconds)
+uint8 GF_ACTION_NONE = 0        # no action on geofence violation
+uint8 GF_ACTION_WARN = 1        # critical mavlink message
+uint8 GF_ACTION_LOITER = 2      # switch to AUTO|LOITER
+uint8 GF_ACTION_RTL = 3         # switch to AUTO|RTL
+uint8 GF_ACTION_TERMINATE = 4   # flight termination
+uint8 GF_ACTION_LAND = 5        # switch to AUTO|LAND

-bool geofence_violated		# true if the geofence is violated
-uint8 geofence_action       	# action to take when geofence is violated
+bool geofence_violated          # true if the geofence is violated
+uint8 geofence_action           # action to take when geofence is violated

-bool home_required		# true if the geofence requires a valid home position
+bool home_required              # true if the geofence requires a valid home position
@@ -0,0 +1,20 @@
+# MAV_TUNNEL_PAYLOAD_TYPE enum
+
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_UNKNOWN = 0                # Encoding of payload unknown
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED0 = 200    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED1 = 201    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED2 = 202    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED3 = 203    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED4 = 204    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED5 = 205    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED6 = 206    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED7 = 207    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED8 = 208    # Registered for STorM32 gimbal controller
+uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED9 = 209    # Registered for STorM32 gimbal controller
+
+uint64 timestamp	     # Time since system start (microseconds)
+uint16 payload_type      # A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase.
+uint8 target_system      # System ID (can be 0 for broadcast, but this is discouraged)
+uint8 target_component   # Component ID (can be 0 for broadcast, but this is discouraged)
+uint8 payload_length     # Length of the data transported in payload
+uint8[128] payload       # Data itself
@@ -62,5 +62,7 @@ float32 yawspeed		# Angular velocity about Z body axis
 # If angular velocity covariance invalid/unknown, 16th cell is NaN
 float32[21] velocity_covariance

+uint8 reset_counter
+
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
 # TOPICS estimator_odometry estimator_visual_odometry_aligned
@@ -52,11 +52,10 @@ add_library(px4_platform
 	spi.cpp
 	${SRCS}
 )
-add_dependencies(px4_platform prebuild_targets)
+target_link_libraries(px4_platform prebuild_targets px4_work_queue)

 if (NOT "${PX4_BOARD}" MATCHES "io-v2")
 	add_subdirectory(uORB)
-	target_link_libraries(px4_platform PRIVATE uORB)
 endif()

 add_subdirectory(px4_work_queue)
@@ -76,16 +76,18 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};

 static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19};

-static constexpr wq_config_t UART0{"wq:UART0", 1632, -21};
-static constexpr wq_config_t UART1{"wq:UART1", 1632, -22};
-static constexpr wq_config_t UART2{"wq:UART2", 1632, -23};
-static constexpr wq_config_t UART3{"wq:UART3", 1632, -24};
-static constexpr wq_config_t UART4{"wq:UART4", 1632, -25};
-static constexpr wq_config_t UART5{"wq:UART5", 1632, -26};
-static constexpr wq_config_t UART6{"wq:UART6", 1632, -27};
-static constexpr wq_config_t UART7{"wq:UART7", 1632, -28};
-static constexpr wq_config_t UART8{"wq:UART8", 1632, -29};
-static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1632, -30};
+static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21};
+static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22};
+static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23};
+static constexpr wq_config_t ttyS3{"wq:ttyS3", 1632, -24};
+static constexpr wq_config_t ttyS4{"wq:ttyS4", 1632, -25};
+static constexpr wq_config_t ttyS5{"wq:ttyS5", 1632, -26};
+static constexpr wq_config_t ttyS6{"wq:ttyS6", 1632, -27};
+static constexpr wq_config_t ttyS7{"wq:ttyS7", 1632, -28};
+static constexpr wq_config_t ttyS8{"wq:ttyS8", 1632, -29};
+static constexpr wq_config_t ttyS9{"wq:ttyS9", 1632, -30};
+static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1632, -31};
+static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1632, -32};

 static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};

@@ -44,4 +44,3 @@ if(PX4_TESTING)
 endif()

 target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_link_libraries(px4_work_queue PRIVATE px4_platform)
@@ -166,39 +166,45 @@ const wq_config_t &
 serial_port_to_wq(const char *serial)
 {
 	if (serial == nullptr) {
-		return wq_configurations::hp_default;
+		return wq_configurations::ttyUnknown;

 	} else if (strstr(serial, "ttyS0")) {
-		return wq_configurations::UART0;
+		return wq_configurations::ttyS0;

 	} else if (strstr(serial, "ttyS1")) {
-		return wq_configurations::UART1;
+		return wq_configurations::ttyS1;

 	} else if (strstr(serial, "ttyS2")) {
-		return wq_configurations::UART2;
+		return wq_configurations::ttyS2;

 	} else if (strstr(serial, "ttyS3")) {
-		return wq_configurations::UART3;
+		return wq_configurations::ttyS3;

 	} else if (strstr(serial, "ttyS4")) {
-		return wq_configurations::UART4;
+		return wq_configurations::ttyS4;

 	} else if (strstr(serial, "ttyS5")) {
-		return wq_configurations::UART5;
+		return wq_configurations::ttyS5;

 	} else if (strstr(serial, "ttyS6")) {
-		return wq_configurations::UART6;
+		return wq_configurations::ttyS6;

 	} else if (strstr(serial, "ttyS7")) {
-		return wq_configurations::UART7;
+		return wq_configurations::ttyS7;

 	} else if (strstr(serial, "ttyS8")) {
-		return wq_configurations::UART8;
+		return wq_configurations::ttyS8;
+
+	} else if (strstr(serial, "ttyS9")) {
+		return wq_configurations::ttyS9;
+
+	} else if (strstr(serial, "ttyACM0")) {
+		return wq_configurations::ttyACM0;
 	}

 	PX4_DEBUG("unknown serial port: %s", serial);

-	return wq_configurations::UART_UNKNOWN;
+	return wq_configurations::ttyUnknown;
 }

 const wq_config_t &ins_instance_to_wq(uint8_t instance)
@@ -439,7 +439,7 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool
 			}

 			memcpy(topic_name, meta->o_fields + format_idx, topic_name_len);
-			field_name[topic_name_len] = '\0';
+			topic_name[topic_name_len] = '\0';

 			// find the metadata
 			const orb_metadata *const *topics = orb_get_topics();
@@ -44,6 +44,14 @@ add_dependencies(px4 git_nuttx)

 get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES)

+if (NOT CONFIG_BUILD_FLAT)
+	add_executable(px4_kernel ${PX4_SOURCE_DIR}/platforms/common/empty.c)
+	set(KERNEL_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}_kernel.elf)
+	set_target_properties(px4_kernel PROPERTIES OUTPUT_NAME ${KERNEL_NAME})
+	add_dependencies(px4_kernel git_nuttx)
+	get_property(kernel_module_libraries GLOBAL PROPERTY PX4_KERNEL_MODULE_LIBRARIES)
+endif()
+
 # build NuttX
 add_subdirectory(NuttX ${PX4_BINARY_DIR}/NuttX)

@@ -81,18 +89,31 @@ else()
 endif()

 list(APPEND nuttx_libs
-	nuttx_apps
-	nuttx_arch
-	nuttx_binfmt
-	nuttx_c
 	nuttx_boards
-	nuttx_xx
 	nuttx_drivers
 	nuttx_fs
-	nuttx_mm
 	nuttx_sched
 	nuttx_crypto
-)
+	nuttx_binfmt
+	nuttx_xx
+	)
+
+if (NOT CONFIG_BUILD_FLAT)
+	list(APPEND nuttx_libs
+		px4_board_ctrl
+		nuttx_karch
+		nuttx_kmm
+		nuttx_stubs
+		nuttx_kc
+		)
+else()
+	list(APPEND nuttx_libs
+		nuttx_apps
+		nuttx_arch
+		nuttx_mm
+		nuttx_c
+		)
+endif()

 if(CONFIG_NET)
 	list(APPEND nuttx_libs nuttx_net)
@@ -105,50 +126,6 @@ file(RELATIVE_PATH PX4_BINARY_DIR_REL ${CMAKE_CURRENT_BINARY_DIR} ${PX4_BINARY_D
 # because even relative linker script paths are different for linux, mac and windows
 CYGPATH(NUTTX_CONFIG_DIR NUTTX_CONFIG_DIR_CYG)

-target_link_libraries(nuttx_arch
-	INTERFACE
-		drivers_board
-		arch_hrt
-)
-
-target_link_libraries(nuttx_c INTERFACE nuttx_drivers)
-target_link_libraries(nuttx_drivers INTERFACE nuttx_c)
-target_link_libraries(nuttx_xx INTERFACE nuttx_c)
-target_link_libraries(nuttx_fs INTERFACE nuttx_c)
-
-target_link_libraries(px4 PRIVATE
-
-	-nostartfiles
-	-nodefaultlibs
-	-nostdlib
-	-nostdinc++
-
-	-fno-exceptions
-	-fno-rtti
-	-Wl,--script=${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX}script.ld
-	-Wl,-Map=${PX4_CONFIG}.map
-	-Wl,--warn-common
-	-Wl,--gc-sections
-
-	-Wl,--start-group
-		${nuttx_libs}
-	-Wl,--end-group
-
-	m
-	gcc
-	)
-
-if(NOT USE_LD_GOLD)
-	target_link_libraries(px4 PRIVATE -Wl,--print-memory-usage)
-endif()
-
-target_link_libraries(px4 PRIVATE ${module_libraries})
-
-if(config_romfs_root)
-	add_subdirectory(${PX4_SOURCE_DIR}/ROMFS ${PX4_BINARY_DIR}/ROMFS)
-	target_link_libraries(px4 PRIVATE romfs)
-endif()
-
 if((DEFINED ENV{SIGNING_TOOL}) AND (NOT NUTTX_DIR MATCHES "external"))
 	set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR}/${PX4_CONFIG}_unsigned.bin)

@@ -161,10 +138,154 @@ else()
 	set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_CONFIG}.bin)
 endif()

-add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT}
-	COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT}
-	DEPENDS px4
-)
+if (NOT CONFIG_BUILD_FLAT)
+
+	target_link_libraries(nuttx_karch
+		INTERFACE
+			drivers_board
+			arch_hrt
+			)
+
+	target_link_libraries(px4_kernel PRIVATE
+
+		-nostartfiles
+		-nodefaultlibs
+		-nostdlib
+		-nostdinc++
+
+		-fno-exceptions
+		-fno-rtti
+
+		-Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}kernel-space.ld
+
+		-Wl,-Map=${PX4_CONFIG}_kernel.map
+		-Wl,--warn-common
+		-Wl,--gc-sections
+
+		-Wl,--start-group
+			${nuttx_libs}
+			${kernel_module_libraries}
+			px4_work_queue # TODO, this shouldn't be needed here?
+		-Wl,--end-group
+
+		m
+		gcc
+		)
+
+	if (config_romfs_root)
+		add_subdirectory(${PX4_SOURCE_DIR}/ROMFS ${PX4_BINARY_DIR}/ROMFS)
+		target_link_libraries(px4_kernel PRIVATE romfs)
+	endif()
+
+	target_link_libraries(px4_kernel PRIVATE -Wl,--print-memory-usage)
+
+	set(nuttx_userspace)
+
+	list(APPEND nuttx_userspace
+		drivers_userspace
+		nuttx_arch
+		nuttx_apps
+		nuttx_mm
+		nuttx_proxies
+		nuttx_c
+		nuttx_xx
+		)
+
+	target_link_libraries(nuttx_c INTERFACE nuttx_proxies)
+
+	target_link_libraries(px4 PRIVATE
+
+		-nostartfiles
+		-nodefaultlibs
+		-nostdlib
+		-nostdinc++
+
+		-fno-exceptions
+		-fno-rtti
+
+		-Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}user-space.ld
+
+		-Wl,-Map=${PX4_CONFIG}.map
+		-Wl,--warn-common
+		-Wl,--gc-sections
+
+		-Wl,--start-group
+		${nuttx_userspace}
+		-Wl,--end-group
+
+		m
+		gcc
+		)
+
+	target_link_libraries(px4 PRIVATE -Wl,--print-memory-usage)
+
+	target_link_libraries(px4 PRIVATE
+		${module_libraries}
+	)
+
+	add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT}
+		COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_DIR_REL}/${PX4_BOARD}_user.bin
+		COMMAND ${CMAKE_OBJCOPY} --gap-fill 0xFF --pad-to ${CONFIG_NUTTX_USERSPACE} -O binary ${PX4_BINARY_DIR_REL}/${KERNEL_NAME} ${PX4_BINARY_OUTPUT}
+		COMMAND cat ${PX4_BINARY_DIR_REL}/${PX4_BOARD}_user.bin >> ${PX4_BINARY_OUTPUT}
+
+		DEPENDS px4 px4_kernel
+	)
+
+else()
+
+	target_link_libraries(nuttx_c INTERFACE nuttx_sched) # nxsched_get_streams
+
+	target_link_libraries(nuttx_arch
+		INTERFACE
+			drivers_board
+			arch_hrt
+			arch_board_reset
+	)
+
+	target_link_libraries(nuttx_c INTERFACE nuttx_drivers)
+	target_link_libraries(nuttx_drivers INTERFACE nuttx_c)
+	target_link_libraries(nuttx_xx INTERFACE nuttx_c)
+	target_link_libraries(nuttx_fs INTERFACE nuttx_c)
+
+	target_link_libraries(px4 PRIVATE
+
+		-nostartfiles
+		-nodefaultlibs
+		-nostdlib
+		-nostdinc++
+
+		-fno-exceptions
+		-fno-rtti
+		-Wl,--script=${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX}script.ld
+		-Wl,-Map=${PX4_CONFIG}.map
+		-Wl,--warn-common
+		-Wl,--gc-sections
+
+		-Wl,--start-group
+			${nuttx_libs}
+		-Wl,--end-group
+
+		m
+		gcc
+		)
+
+	if(NOT USE_LD_GOLD)
+		target_link_libraries(px4 PRIVATE -Wl,--print-memory-usage)
+	endif()
+
+	target_link_libraries(px4 PRIVATE ${module_libraries})
+
+	if(config_romfs_root)
+		add_subdirectory(${PX4_SOURCE_DIR}/ROMFS ${PX4_BINARY_DIR}/ROMFS)
+		target_link_libraries(px4 PRIVATE romfs)
+	endif()
+
+	add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT}
+		COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT}
+		DEPENDS px4
+	)
+
+endif()

 # create .px4 with parameter and airframe metadata
 if (TARGET parameters_xml AND TARGET airframes_xml)
@@ -117,7 +117,13 @@ set_property(TARGET nuttx_apps PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY
 add_dependencies(nuttx_apps nuttx_apps_build)

 # helper for all targets
-function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra)
+function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra target)
+	if (${target} STREQUAL all)
+		set(nuttx_lib_target all)
+	else()
+		set(nuttx_lib_target lib${target}.a)
+	endif()
+
 	file(GLOB_RECURSE nuttx_lib_files LIST_DIRECTORIES false
 		${CMAKE_CURRENT_SOURCE_DIR}/nuttx/${nuttx_lib_dir}/*.c
 		${CMAKE_CURRENT_SOURCE_DIR}/nuttx/${nuttx_lib_dir}/*.h
@@ -126,7 +132,7 @@ function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra)
 	add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/nuttx/${nuttx_lib_dir}/lib${nuttx_lib}.a
 		COMMAND ${CMAKE_COMMAND} -E remove -f ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a
 		COMMAND find ${nuttx_lib_dir} -type f -name \*.o -delete
-		COMMAND make -C ${nuttx_lib_dir} --no-print-directory --silent all TOPDIR="${NUTTX_DIR}" KERNEL=${kernel} EXTRAFLAGS=${extra}
+		COMMAND make -C ${nuttx_lib_dir} --no-print-directory --silent ${nuttx_lib_target} TOPDIR="${NUTTX_DIR}" KERNEL=${kernel} EXTRAFLAGS=${extra}
 		COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a ${CMAKE_CURRENT_BINARY_DIR}/nuttx/${nuttx_lib_dir}/lib${nuttx_lib}.a
 		DEPENDS
 			${nuttx_lib_files}
@@ -142,19 +148,35 @@ function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra)
 endfunction()

 # add_nuttx_dir(NAME DIRECTORY KERNEL EXTRA)
-add_nuttx_dir(arch arch/${CONFIG_ARCH}/src y -D__KERNEL__)
-add_nuttx_dir(binfmt binfmt y -D__KERNEL__)
-add_nuttx_dir(boards boards y -D__KERNEL__)
-add_nuttx_dir(drivers drivers y -D__KERNEL__)
-add_nuttx_dir(fs fs y -D__KERNEL__)
-add_nuttx_dir(sched sched y -D__KERNEL__)
-add_nuttx_dir(c libs/libc n "")
-add_nuttx_dir(xx libs/libxx n "")
-add_nuttx_dir(mm mm n "")
-add_nuttx_dir(crypto crypto y -D__KERNEL__)
+add_nuttx_dir(binfmt binfmt y -D__KERNEL__ all)
+add_nuttx_dir(boards boards y -D__KERNEL__ all)
+add_nuttx_dir(drivers drivers y -D__KERNEL__ all)
+add_nuttx_dir(fs fs y -D__KERNEL__ all)
+add_nuttx_dir(sched sched y -D__KERNEL__ all)
+add_nuttx_dir(xx libs/libxx n "" all)
+add_nuttx_dir(crypto crypto y -D__KERNEL__ all)
+
+if (NOT CONFIG_BUILD_FLAT)
+	add_nuttx_dir(arch arch/${CONFIG_ARCH}/src n "" arch)
+	add_dependencies(nuttx_arch_build nuttx_karch_build) # can't build these in parallel
+	add_nuttx_dir(karch arch/arm/src y -D__KERNEL__ karch)
+	add_nuttx_dir(c libs/libc n "" c)
+	add_dependencies(nuttx_c_build nuttx_kc_build) # can't build these in parallel
+	add_nuttx_dir(kc libs/libc y -D__KERNEL__ kc)
+	add_nuttx_dir(mm mm n "" mm)
+	add_dependencies(nuttx_mm_build nuttx_kmm_build) # can't build these in parallel
+	add_nuttx_dir(kmm mm y -D__KERNEL__ kmm)
+	add_nuttx_dir(proxies syscall n "" proxies)
+	add_dependencies(nuttx_proxies_build nuttx_stubs_build) # can't build these in parallel
+	add_nuttx_dir(stubs syscall y -D__KERNEL__ stubs)
+else()
+	add_nuttx_dir(arch arch/${CONFIG_ARCH}/src y -D__KERNEL__ all)
+	add_nuttx_dir(c libs/libc n "" all)
+	add_nuttx_dir(mm mm n "" mm)
+endif()

 if(CONFIG_NET)
-	add_nuttx_dir(net net y -D__KERNEL__)
+	add_nuttx_dir(net net y -D__KERNEL__ all)
 endif()

 ###############################################################################
@@ -177,7 +177,7 @@ function(px4_os_prebuild_targets)
 	endif()

 	add_library(prebuild_targets INTERFACE)
-	target_link_libraries(prebuild_targets INTERFACE nuttx_xx nuttx_c nuttx_fs nuttx_mm nuttx_sched m gcc)
+	target_link_libraries(prebuild_targets INTERFACE nuttx_xx m gcc)
 	add_dependencies(prebuild_targets DEPENDS nuttx_context uorb_headers)

 endfunction()
@@ -39,4 +39,5 @@ px4_add_library(arch_bootloader
 target_link_libraries(arch_bootloader
 	PRIVATE
 		bootloader_lib
+		nuttx_arch
 )
@@ -33,8 +33,8 @@

 # skip for px4_layer support on an IO board
 if(NOT PX4_BOARD MATCHES "io-v2")
-
-	add_library(px4_layer
+	# Kernel side & nuttx flat build common sources
+	set(KERNEL_SRCS
 		board_crashdump.c
 		board_dma_alloc.c
 		board_fat_dma_alloc.c
@@ -51,31 +51,36 @@ if(NOT PX4_BOARD MATCHES "io-v2")
 		px4_24xxxx_mtd.c
 		px4_crypto.cpp
 	)
-	target_link_libraries(px4_layer
-		PRIVATE
-			arch_board_reset
-			arch_board_critmon
-			arch_version
-			nuttx_apps
-			nuttx_sched
-			px4_work_queue
-			uORB
+
+	# Kernel side & nuttx flat build common libraries
+	set(KERNEL_LIBS
+		arch_board_reset
+		arch_board_critmon
+		arch_version
+		nuttx_sched
 	)
+
+if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
+	# Build the NuttX user and kernel space px4 layers
+	include(${CMAKE_CURRENT_SOURCE_DIR}/px4_protected_layers.cmake)
+
 else()
+	# Build the flat build px4_layer
+	include(${CMAKE_CURRENT_SOURCE_DIR}/px4_layer.cmake)
+endif()
+
+else()
+	# Build the px4 layer for io_v2
 	add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/common/empty.c)
 endif()
+
 add_dependencies(px4_layer prebuild_targets)

 add_subdirectory(gpio)
 add_subdirectory(srgbled)

+# Build px4_random
 if (DEFINED PX4_CRYPTO)
 	add_library(px4_random nuttx_random.c)
-	add_dependencies(px4_random nuttx_context)
 	target_link_libraries(px4_random PRIVATE nuttx_crypto)
-
-	target_link_libraries(px4_layer
-		PUBLIC
-			crypto_backend
-	)
 endif()
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 Technology Innovation Institute. 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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/console_buffer.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/sem.h>
+#include <pthread.h>
+#include <string.h>
+#include <fcntl.h>
+
+#ifdef BOARD_ENABLE_CONSOLE_BUFFER
+#ifndef BOARD_CONSOLE_BUFFER_SIZE
+# define BOARD_CONSOLE_BUFFER_SIZE (1024*4) // default buffer size
+#endif
+
+
+// TODO: User side implementation of px4_console_buffer
+
+int px4_console_buffer_init()
+{
+	return 0;
+}
+
+int px4_console_buffer_size()
+{
+	return 0;
+}
+
+int px4_console_buffer_read(char *buffer, int buffer_length, int *offset)
+{
+	return 0;
+}
+
+#endif /* BOARD_ENABLE_CONSOLE_BUFFER */
@@ -58,6 +58,8 @@ static inline constexpr io_timers_channel_mapping_t initIOTimerChannelMapping(co
 		}

 		uint32_t first_channel = UINT32_MAX;
+		uint32_t min_timer_channel = UINT32_MAX;
+		uint32_t max_timer_channel = 0;
 		uint32_t channel_count = 0;

 		for (uint32_t channel = 0; channel < MAX_TIMER_IO_CHANNELS; ++channel) {
@@ -74,11 +76,23 @@ static inline constexpr io_timers_channel_mapping_t initIOTimerChannelMapping(co
 				}

 				++channel_count;
+
+				if (timer_io_channels_conf[channel].timer_channel < min_timer_channel) {
+					min_timer_channel = timer_io_channels_conf[channel].timer_channel;
+				}
+
+				if (timer_io_channels_conf[channel].timer_channel > max_timer_channel) {
+					max_timer_channel = timer_io_channels_conf[channel].timer_channel;
+				}
 			}
 		}

 		if (first_channel == UINT32_MAX) { //unused timer, channel_count is 0
 			first_channel = 0;
+
+		} else {
+			ret.element[i].lowest_timer_channel = min_timer_channel;
+			ret.element[i].channel_count_including_gaps = max_timer_channel - min_timer_channel + 1;
 		}

 		ret.element[i].first_channel_index = first_channel;
@@ -0,0 +1,22 @@
+# Build the px4 layer for nuttx flat build
+
+add_library(px4_layer
+		${KERNEL_SRCS}
+		cdc_acm_check.cpp
+	)
+
+target_link_libraries(px4_layer
+	PRIVATE
+		${KERNEL_LIBS}
+		nuttx_c
+		nuttx_arch
+		nuttx_mm
+	)
+
+
+if (DEFINED PX4_CRYPTO)
+	target_link_libraries(px4_layer
+		PUBLIC
+			crypto_backend
+	)
+endif()
@@ -0,0 +1,41 @@
+
+# Build the user side px4_layer
+
+add_library(px4_layer
+	tasks.cpp
+	console_buffer_usr.cpp
+	usr_mcu_version.cpp
+	cdc_acm_check.cpp
+	${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp
+	${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp
+)
+
+target_link_libraries(px4_layer
+	PRIVATE
+		m
+		nuttx_c
+		nuttx_xx
+		nuttx_mm
+)
+
+# Build the kernel side px4_kernel_layer
+
+add_library(px4_kernel_layer
+		${KERNEL_SRCS}
+)
+
+target_link_libraries(px4_kernel_layer
+	PRIVATE
+		${KERNEL_LIBS}
+		nuttx_kc
+		nuttx_karch
+		nuttx_kmm
+)
+
+if (DEFINED PX4_CRYPTO)
+	target_link_libraries(px4_kernel_layer PUBLIC crypto_backend)
+endif()
+
+target_compile_options(px4_kernel_layer PRIVATE -D__KERNEL__)
+
+add_dependencies(px4_kernel_layer prebuild_targets)
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 Technology Innovation Institute. All rights reserved.
+ *   Author: @author Jukka Laitinen <jukkax@ssrc.tii.ae>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file usr_mcu_version.c
+ * Implementation of generic user-space version API
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+
+#include "board_config.h"
+
+static int hw_version = 0;
+static int hw_revision = 0;
+static char hw_info[] = HW_INFO_INIT;
+
+__EXPORT const char *board_get_hw_type_name(void)
+{
+	return (const char *) hw_info;
+}
+
+__EXPORT int board_get_hw_version(void)
+{
+	return  hw_version;
+}
+
+__EXPORT int board_get_hw_revision()
+{
+	return  hw_revision;
+}
+
+__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words)
+{
+	/* TODO: This is a stub for userspace build. Use some proper interface
+	 * to fetch the uuid32 from the kernel
+	 */
+	uint32_t chip_uuid[PX4_CPU_UUID_WORD32_LENGTH];
+	memset((uint8_t *)chip_uuid, 0, PX4_CPU_UUID_WORD32_LENGTH * 4);
+
+	for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
+		uuid_words[i] = chip_uuid[i];
+	}
+}
+
+int board_mcu_version(char *rev, const char **revstr, const char **errata)
+{
+	/* TODO: This is a stub for userspace build. Use some proper interface
+	 * to fetch the version from the kernel
+	 */
+	return -1;
+}
+
+int board_get_px4_guid(px4_guid_t px4_guid)
+{
+	/* TODO: This is a stub for userspace build. Use some proper interface
+	 * to fetch the guid from the kernel
+	 */
+	uint8_t  *pb = (uint8_t *) &px4_guid[0];
+	memset(pb, 0, PX4_GUID_BYTE_LENGTH);
+
+	return PX4_GUID_BYTE_LENGTH;
+}
+
+int board_get_px4_guid_formated(char *format_buffer, int size)
+{
+	px4_guid_t px4_guid;
+	board_get_px4_guid(px4_guid);
+	int offset  = 0;
+
+	/* size should be 2 per byte + 1 for termination
+	 * So it needs to be odd
+	 */
+	size = size & 1 ? size : size - 1;
+
+	/* Discard from MSD */
+	for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) {
+		offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]);
+	}
+
+	return offset;
+}
+
@@ -180,7 +180,7 @@ uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
 		/* don't wait for more than 10us, since that means something broke
 		 *  should reset here if we see this
 		 */
-		if ((hrt_absolute_time() - now) > 10) {
+		if ((hrt_absolute_time() - now) > 30) {
 			px4_leave_critical_section(flags);
 			return UINT32_MAX;
 		}
@@ -34,3 +34,10 @@
 px4_add_library(arch_board_reset
 	board_reset.cpp
 )
+
+# up_systemreset
+if (NOT DEFINED CONFIG_BUILD_FLAT)
+	target_link_libraries(arch_board_reset PRIVATE nuttx_karch)
+else()
+	target_link_libraries(arch_board_reset PRIVATE nuttx_arch)
+endif()
@@ -86,6 +86,8 @@ typedef struct io_timers_t {
 typedef struct io_timers_channel_mapping_element_t {
 	uint32_t first_channel_index;
 	uint32_t channel_count;
+	uint32_t lowest_timer_channel;
+	uint32_t channel_count_including_gaps;
 } io_timers_channel_mapping_element_t;

 /* mapping for each io_timers to timer_io_channels */
@@ -102,6 +104,7 @@ typedef struct timer_io_channels_t {
 	uint8_t   val_offset;          /* IMXRT_FLEXPWM_SM0VAL3_OFFSET or IMXRT_FLEXPWM_SM0VAL5_OFFSET */
 	uint8_t   sub_module;          /* 0 based sub module offset */
 	uint8_t   sub_module_bits;     /* LDOK and CLDOK bits */
+	uint8_t   timer_channel;       /* Unused */
 } timer_io_channels_t;

 #define SM0           0
@@ -34,3 +34,10 @@
 px4_add_library(arch_board_reset
 	board_reset.cpp
 )
+
+# up_systemreset
+if (NOT DEFINED CONFIG_BUILD_FLAT)
+	target_link_libraries(arch_board_reset PRIVATE nuttx_karch)
+else()
+	target_link_libraries(arch_board_reset PRIVATE nuttx_arch)
+endif()
@@ -91,6 +91,8 @@ typedef struct io_timers_t {
 typedef struct io_timers_channel_mapping_element_t {
 	uint32_t first_channel_index;
 	uint32_t channel_count;
+	uint32_t lowest_timer_channel;
+	uint32_t channel_count_including_gaps;
 } io_timers_channel_mapping_element_t;

 /* mapping for each io_timers to timer_io_channels */
@@ -34,3 +34,10 @@
 px4_add_library(arch_board_reset
 	board_reset.cpp
 )
+
+# up_systemreset
+if (NOT DEFINED CONFIG_BUILD_FLAT)
+	target_link_libraries(arch_board_reset PRIVATE nuttx_karch)
+else()
+	target_link_libraries(arch_board_reset PRIVATE nuttx_arch)
+endif()
@@ -87,6 +87,8 @@ typedef struct io_timers_t {
 typedef struct io_timers_channel_mapping_element_t {
 	uint32_t first_channel_index;
 	uint32_t channel_count;
+	uint32_t lowest_timer_channel;
+	uint32_t channel_count_including_gaps;
 } io_timers_channel_mapping_element_t;

 /* mapping for each io_timers to timer_io_channels */
@@ -34,3 +34,5 @@
 px4_add_library(arch_led_pwm
 	led_pwm.cpp
 )
+
+target_link_libraries(arch_led_pwm PRIVATE arch_io_pins) # io_timer_init_timer
@@ -34,3 +34,10 @@
 px4_add_library(arch_board_reset
 	board_reset.cpp
 )
+
+# up_systemreset
+if (NOT DEFINED CONFIG_BUILD_FLAT)
+	target_link_libraries(arch_board_reset PRIVATE nuttx_karch)
+else()
+	target_link_libraries(arch_board_reset PRIVATE nuttx_arch)
+endif()
@@ -97,6 +97,8 @@ typedef struct io_timers_t {
 typedef struct io_timers_channel_mapping_element_t {
 	uint32_t first_channel_index;
 	uint32_t channel_count;
+	uint32_t lowest_timer_channel;
+	uint32_t channel_count_including_gaps;
 } io_timers_channel_mapping_element_t;

 /* mapping for each io_timers to timer_io_channels */
@@ -40,3 +40,5 @@ px4_add_library(arch_io_pins
 )

 target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+
+target_link_libraries(arch_io_pins PRIVATE drivers_board)
@@ -34,3 +34,10 @@
 px4_add_library(arch_board_reset
 	board_reset.cpp
 )
+
+# up_systemreset
+if (NOT DEFINED CONFIG_BUILD_FLAT)
+	target_link_libraries(arch_board_reset PRIVATE nuttx_karch)
+else()
+	target_link_libraries(arch_board_reset PRIVATE nuttx_arch)
+endif()
@@ -82,10 +82,6 @@ static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(M
 px4_cache_aligned_data() = {};
 static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {};

-#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT
-static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNMENT;
-#endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */
-
 int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 {
 	unsigned buffer_offset = 0;
@@ -104,7 +100,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 #pragma GCC diagnostic ignored "-Wcast-align"
 		dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset];
 #pragma GCC diagnostic pop
-		buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count);
+		buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps);

 		if (buffer_offset > sizeof(dshot_burst_buffer_array)) {
 			return -EINVAL; // something is wrong with the board configuration or some other logic
@@ -140,9 +136,10 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 	for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) {

 		if (true == dshot_handler[timer_index].init) {
-			dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count *
+			dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps *
 							      ONE_MOTOR_BUFF_SIZE;
-			io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, io_timers_channel_mapping.element[timer_index].channel_count);
+			io_timer_set_dshot_mode(timer_index, dshot_pwm_freq,
+						io_timers_channel_mapping.element[timer_index].channel_count_including_gaps);

 			dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap);

@@ -164,7 +161,7 @@ void up_dshot_trigger(void)
 			// Flush cache so DMA sees the data
 			up_clean_dcache((uintptr_t)dshot_burst_buffer[timer],
 					(uintptr_t)dshot_burst_buffer[timer] +
-					DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count));
+					DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps));

 			px4_stm32_dmasetup(dshot_handler[timer].dma_handle,
 					   io_timers[timer].base + STM32_GTIM_DMAR_OFFSET,
@@ -192,10 +189,6 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet
 	uint16_t packet = 0;
 	uint16_t checksum = 0;

-#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT
-	motor_number = motor_assignment[motor_number];
-#endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */
-
 	packet |= throttle << DSHOT_THROTTLE_POSITION;
 	packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION;

@@ -213,8 +206,9 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet

 	unsigned timer = timer_io_channels[motor_number].timer_index;
 	uint32_t *buffer = dshot_burst_buffer[timer];
-	unsigned num_motors = io_timers_channel_mapping.element[timer].channel_count;
-	unsigned timer_channel_index = motor_number - io_timers_channel_mapping.element[timer].first_channel_index;
+	const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer];
+	unsigned num_motors = mapping->channel_count_including_gaps;
+	unsigned timer_channel_index = timer_io_channels[motor_number].timer_channel - mapping->lowest_timer_channel;

 	for (unsigned motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) {
 		buffer[motor_data_index * num_motors + timer_channel_index] =
@@ -102,6 +102,8 @@ typedef struct io_timers_t {
 typedef struct io_timers_channel_mapping_element_t {
 	uint32_t first_channel_index;
 	uint32_t channel_count;
+	uint32_t lowest_timer_channel;
+	uint32_t channel_count_including_gaps;
 } io_timers_channel_mapping_element_t;

 /* mapping for each io_timers to timer_io_channels */
@@ -38,3 +38,5 @@ px4_add_library(arch_io_pins
 	pwm_trigger.c
 )
 target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+
+target_link_libraries(arch_io_pins PRIVATE drivers_board) # timer_io_channels
@@ -35,3 +35,5 @@ px4_add_library(arch_spi
 	spi.cpp
 )
 target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+
+target_link_libraries (arch_spi PRIVATE drivers_board) # px4_spi_buses
@@ -225,6 +225,9 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
+	// to be paranoid ensure all previous DMA transfers are cleared
+	_abort_dma();
+
 	_current_packet = _packet;

 	/* clear any lingering error status */
@@ -237,6 +237,9 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
+	// to be paranoid ensure all previous DMA transfers are cleared
+	_abort_dma();
+
 	_current_packet = _packet;

 	/* clear data that may be in the RDR and clear overrun error: */
@@ -275,6 +275,9 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
+	// to be paranoid ensure all previous DMA transfers are cleared
+	_abort_dma();
+
 	_current_packet = _packet;

 	/* clear data that may be in the RDR and clear overrun error: */
@@ -41,10 +41,6 @@ target_link_libraries(px4
 	PRIVATE
 		${module_libraries}
 		m
-
-		# horrible circular dependencies that need to be teased apart
-		px4_layer px4_platform
-		work_queue
 		parameters
 )

@@ -301,6 +301,7 @@ function(px4_os_prebuild_targets)
 			ARGN ${ARGN})

 	add_library(prebuild_targets INTERFACE)
+	target_link_libraries(prebuild_targets INTERFACE px4_layer drivers_board)
 	add_dependencies(prebuild_targets DEPENDS uorb_headers)

 endfunction()
@@ -41,3 +41,4 @@ add_subdirectory(lps33hw)
 add_subdirectory(maiertek)
 add_subdirectory(ms5611)
 #add_subdirectory(tcbp001ta) # only for users who really need this
+add_subdirectory(invensense)
@@ -0,0 +1,35 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(icp10100)
+add_subdirectory(icp10111)
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__invensense__icp10100
+	MAIN icp10100
+	COMPILE_FLAGS
+	SRCS
+		Inven_Sense_ICP10100_registers.hpp
+		ICP10100.cpp
+		ICP10100.hpp
+		icp10100_main.cpp
+	DEPENDS
+		drivers_barometer
+		px4_work_queue
+	)
@@ -0,0 +1,375 @@
+/****************************************************************************
+ *
+ *   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 "ICP10100.hpp"
+
+using namespace time_literals;
+
+ICP10100::ICP10100(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config),
+	_px4_baro(get_device_id())
+{
+}
+
+ICP10100::~ICP10100()
+{
+	perf_free(_reset_perf);
+	perf_free(_sample_perf);
+	perf_free(_bad_transfer_perf);
+}
+
+int
+ICP10100::init()
+{
+	int ret = I2C::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("I2C::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool
+ICP10100::Reset()
+{
+	_state = STATE::RESET;
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void
+ICP10100::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_reset_perf);
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_bad_transfer_perf);
+}
+
+int
+ICP10100::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
+ICP10100::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);
+
+				const hrt_abstime nowx = hrt_absolute_time();
+				float temperature = _temperature_C;
+				float pressure = _pressure_Pa; // to Pascal
+				pressure = pressure / 100.0f; // to mbar
+
+				_px4_baro.set_error_count(perf_event_count(_bad_transfer_perf));
+				_px4_baro.set_temperature(temperature);
+				_px4_baro.update(nowx, pressure);
+
+				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
+ICP10100::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
+ICP10100::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
+ICP10100::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)
+{
+	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
+ICP10100::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
+ICP10100::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);
+}
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   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_ICP10100_registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/device/i2c.h>
+#include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+
+using namespace Inven_Sense_ICP10100;
+
+class ICP10100 : public device::I2C, public I2CSPIDriver<ICP10100>
+{
+public:
+	ICP10100(const I2CSPIDriverConfig &config);
+	~ICP10100() 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);
+
+	PX4Barometer _px4_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};
+};
@@ -30,49 +30,33 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
-/**
- * @author Mengxiao Li <mengxiao@cuav.net>
- */

+/**
+ * @file icp10100_registers.hpp
+ *
+ * icp10100 registers.
+ *
+ */

 #pragma once

-#include "sensor_bridge.hpp"
-#include <uORB/topics/battery_status.h>
-#include <cuav/equipment/power/CBAT.hpp>
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/module_params.h>
-#include <mathlib/mathlib.h>
+#include <cstdint>

-class UavcanCBATBridge : public UavcanSensorBridgeBase, public ModuleParams
+namespace Inven_Sense_ICP10100
 {
-public:
-	static const char *const NAME;
+static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface
+static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x63;

-	UavcanCBATBridge(uavcan::INode &node);
+static constexpr uint8_t Product_ID = 0x08;

-	const char *get_name() const override { return NAME; }
-
-	int init() override;
-
-private:
-
-	void battery_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg);
-	void determineWarning(float remaining);
-
-	typedef uavcan::MethodBinder < UavcanCBATBridge *,
-		void (UavcanCBATBridge::*)
-		(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &) >
-		CBATCbBinder;
-
-	uavcan::Subscriber<cuav::equipment::power::CBAT, CBATCbBinder> _sub_battery;
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
-		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
-		(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr
-	)
-
-	uint8_t _warning;
-	float _max_cell_voltage_delta = 0.f;
+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_ICP10100
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+#include "ICP10100.hpp"
+
+void
+ICP10100::print_usage()
+{
+	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);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x63);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int icp10100_main(int argc, char *argv[])
+{
+	using ThisDriver = ICP10100;
+	BusCLIArguments cli{true, false};
+	cli.i2c_address = I2C_ADDRESS_DEFAULT;
+	cli.default_i2c_frequency = I2C_SPEED;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP10100);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+
+	} else if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+
+	} else if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__invensense__icp10111
+	MAIN icp10111
+	COMPILE_FLAGS
+	SRCS
+		Inven_Sense_ICP10111_registers.hpp
+		ICP10111.cpp
+		ICP10111.hpp
+		icp10111_main.cpp
+	DEPENDS
+		drivers_barometer
+		px4_work_queue
+	)
@@ -0,0 +1,375 @@
+/****************************************************************************
+ *
+ *   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),
+	_px4_baro(get_device_id())
+{
+}
+
+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);
+
+				const hrt_abstime nowx = hrt_absolute_time();
+				float temperature = _temperature_C;
+				float pressure = _pressure_Pa; // to Pascal
+				pressure = pressure / 100.0f; // to mbar
+
+				_px4_baro.set_error_count(perf_event_count(_bad_transfer_perf));
+				_px4_baro.set_temperature(temperature);
+				_px4_baro.update(nowx, pressure);
+
+				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);
+}
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   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 <lib/drivers/barometer/PX4Barometer.hpp>
+#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);
+
+	PX4Barometer _px4_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};
+};
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file icp10111_registers.hpp
+ *
+ * icp10111 registers.
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+
+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 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_ICP10111
--- a/Show More
+++ b/Show More