px4_platform: fix linking for sitl

For sitl, px4_platform links directly to drivers_board Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
print_load: Remove reference to CONFIG_MAX_TASKS
2026-06-02 19:30:06 +08:00 · 2021-09-23 13:57:48 +03:00 · 2021-09-23 13:57:48 +03:00 · 2021-09-23 13:57:48 +03:00 · 2021-09-23 13:57:48 +03:00 · 2021-09-23 13:57:48 +03:00
105 changed files with 4516 additions and 265 deletions
@@ -899,6 +899,9 @@ void resetParameters() {
 void runTests() {
  resetParameters()

+  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_CAL_EN" --value "0" || true' // disable during testing
+  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing
+
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply

@@ -1047,25 +1050,25 @@ void resetBoard() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply

  // check SD card
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage" || true'
  // format
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman stop"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "umount /fs/microsd"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman stop" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "umount /fs/microsd" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mkfatfs -F 32 /dev/mmcsd0" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once" || true'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount -t vfat /dev/mmcsd0 /fs/microsd"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount -t vfat /dev/mmcsd0 /fs/microsd" || true'
  // check SD card
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage" || true'
 }
@@ -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)

 include(px4_config)
 include(px4_add_board)
@@ -70,7 +70,7 @@ def do_nsh_cmd(port, baudrate, cmd):
    success_cmd = "cmd succeeded!"

    # wait for command echo
-    serial_cmd = '{0}; echo "{1}"\r\n'.format(cmd, success_cmd)
+    serial_cmd = '{0}; echo "{1}"; echo "{2}";\r\n'.format(cmd, success_cmd, success_cmd)
    ser.write(serial_cmd.encode("ascii"))
    ser.flush()
    while True:
@@ -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
@@ -73,6 +73,7 @@ else()
 			nuttx_drivers # sdio
 			drivers__led # drv_led_start
 			px4_layer
+			px4_platform # I2CBusIterator
 			arch_io_pins
 		)
 endif()
@@ -0,0 +1,253 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32f7"
+CONFIG_ARCH_CHIP_STM32F765II=y
+CONFIG_ARCH_CHIP_STM32F7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_USE_MPU=y
+CONFIG_ARM_MPU=y
+CONFIG_ARM_MPU_NREGIONS=8
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL=y
+CONFIG_BOARDCTL_IOCTL=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=22114
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILD_PROTECTED=y
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_PRODUCTID=0x0032
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_LIB_USRWORK=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_KERNEL_HEAP=y
+CONFIG_MM_KERNEL_HEAPSIZE=131072
+CONFIG_MM_REGIONS=3
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_NUTTX_USERSPACE=0x08100000
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAMTRON_WRITEWAIT=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_CPULOAD=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_MODE=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32F7_ADC1=y
+CONFIG_STM32F7_BBSRAM=y
+CONFIG_STM32F7_BBSRAM_FILES=5
+CONFIG_STM32F7_BKPSRAM=y
+CONFIG_STM32F7_DMA1=y
+CONFIG_STM32F7_DMA2=y
+CONFIG_STM32F7_DMACAPABLE=y
+CONFIG_STM32F7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32F7_I2C1=y
+CONFIG_STM32F7_I2C2=y
+CONFIG_STM32F7_I2C3=y
+CONFIG_STM32F7_I2C4=y
+CONFIG_STM32F7_I2C_DYNTIMEO=y
+CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32F7_OTGFS=y
+CONFIG_STM32F7_PROGMEM=y
+CONFIG_STM32F7_PWR=y
+CONFIG_STM32F7_RTC=y
+CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y
+CONFIG_STM32F7_RTC_MAGIC_REG=1
+CONFIG_STM32F7_SAVE_CRASHDUMP=y
+CONFIG_STM32F7_SDMMC1=y
+CONFIG_STM32F7_SDMMC_DMA=y
+CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32F7_SPI1=y
+CONFIG_STM32F7_SPI1_DMA=y
+CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32F7_SPI2=y
+CONFIG_STM32F7_SPI4=y
+CONFIG_STM32F7_SPI5=y
+CONFIG_STM32F7_SPI6=y
+CONFIG_STM32F7_SPI_DMA=y
+CONFIG_STM32F7_SPI_DMATHRESHOLD=8
+CONFIG_STM32F7_TIM10=y
+CONFIG_STM32F7_TIM11=y
+CONFIG_STM32F7_UART4=y
+CONFIG_STM32F7_UART7=y
+CONFIG_STM32F7_UART8=y
+CONFIG_STM32F7_USART1=y
+CONFIG_STM32F7_USART2=y
+CONFIG_STM32F7_USART3=y
+CONFIG_STM32F7_USART6=y
+CONFIG_STM32F7_USART_BREAKS=y
+CONFIG_STM32F7_USART_INVERT=y
+CONFIG_STM32F7_USART_SINGLEWIRE=y
+CONFIG_STM32F7_USART_SWAP=y
+CONFIG_STM32F7_WWDG=y
+CONFIG_SYS_RESERVED=9
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_RXDMA=y
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_RXDMA=y
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_RXDMA=y
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART3_TXDMA=y
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_RXDMA=y
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="px4_entry"
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * kernel-space.ld
+ *
+ *   Copyright (C) 2020 Technology Innovation Institute. All rights reserved.
+ *   Author: Jukka Laitinen <jukka.laitinen@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 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)
+
+SECTIONS
+{
+    .text : {
+        _stext = ABSOLUTE(.);
+        *(.vectors)
+	. = ALIGN(32);
+	/*
+	This signature provides the bootloader with a way to delay booting
+	*/
+	_bootdelay_signature = ABSOLUTE(.);
+	FILL(0xffecc2925d7d05c5)
+	. += 8;
+        *(.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
+
+    /*
+     * Construction data for parameters.
+     */
+     __param ALIGN(4): {
+     __param_start = ABSOLUTE(.);
+     KEEP(*(__param*))
+     __param_end = 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
+        . = ALIGN(4);
+        _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,123 @@
+/****************************************************************************
+ * boards/arm/stm32f7/stm32f746g-disco/scripts/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_section : {
+        _sinit = ABSOLUTE(.);
+        KEEP(*(.init_array .init_array.*))
+        _einit = ABSOLUTE(.);
+    } > uflash
+
+    /*
+     * Construction data for parameters.
+     */
+     __param ALIGN(4): {
+     __param_start = ABSOLUTE(.);
+     KEEP(*(__param*))
+     __param_end = 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
+        . = ALIGN(4);
+        _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) }
+}
@@ -0,0 +1,156 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m7
+	ROMFSROOT px4fmu_common
+	IO px4_io-v2_default
+	UAVCAN_INTERFACES 2
+	UAVCAN_TIMER_OVERRIDE 6
+	SERIAL_PORTS
+		GPS1:/dev/ttyS0
+		TEL1:/dev/ttyS1
+		TEL2:/dev/ttyS2
+		TEL4:/dev/ttyS3
+
+	# Drivers in kernel space
+	KERNEL_DRIVERS
+		adc/ads1115
+		adc/board_adc
+#		barometer # all available barometer drivers
+		barometer/ms5611
+#		batt_smbus
+#		camera_capture
+#		camera_trigger
+#		differential_pressure # all available differential pressure drivers
+#		distance_sensor # all available distance sensor drivers
+#		dshot
+#		heater
+		#imu # all available imu drivers
+#		imu/analog_devices/adis16448
+		imu/bosch/bmi055
+#		imu/invensense/icm20602
+		imu/invensense/icm20689
+#		imu/invensense/icm20948 # required for ak09916 mag
+#		irlock
+
+#		lights # all available light drivers
+#		lights/rgbled_pwm
+#		magnetometer # all available magnetometer drivers
+		magnetometer/isentek/ist8310
+#		optical_flow # all available optical flow drivers
+#		osd
+#		pca9685
+#		pca9685_pwm_out
+#		power_monitor/ina226
+		#protocol_splitter
+#		pwm_input
+#		pwm_out_sim
+		pwm_out
+		px4io
+		rc_input
+#		roboclaw
+#		smart_battery/batmon
+#		rpm
+		safety_button
+#		telemetry # all available telemetry drivers
+		tone_alarm
+#		uavcan
+
+	# Drivers in user space
+	DRIVERS
+		gps
+
+	# Modules in kernel space
+	KERNEL_MODULES
+		battery_status
+		sensors
+		ekf2
+		mc_att_control
+		mc_rate_control
+
+	# Modules in user space
+	MODULES
+#		airspeed_selector
+#		attitude_estimator
+#		camera_feedback
+		commander
+		dataman
+#		esc_battery
+		events
+		flight_mode_manager
+		fw_att_control
+		fw_pos_control_l1
+		gyro_calibration
+		gyro_fft
+		land_detector
+		landing_target_estimator
+#		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_hover_thrust_estimator
+		mc_pos_control
+		#micrortps_bridge
+		navigator
+		rc_update
+#		rover_pos_control
+
+#		sih
+		temperature_compensation
+		uuv_att_control
+#		uuv_pos_control
+		vmount
+                #		vtol_att_control
+
+	# systemcmds in kernel space
+	KERNEL_SYSTEMCMDS
+                kuorb
+                kparam
+		pwm
+		top
+
+	# systemcmds in user space
+	SYSTEMCMDS
+		px4_insmod
+#		bl_update
+#		dmesg
+#		dumpfile
+#		esc_calib
+#		gpio
+#		hardfault_log
+#		i2cdetect
+#		led_control
+#		mft
+		mixer
+#		motor_ramp
+#		motor_test
+#		mtd
+		nshterm
+		param
+		perf
+		reboot
+#		reflect
+#		sd_bench
+#		serial_test
+#		system_time
+		topic_listener
+		tune_control
+		uorb
+#		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		#fake_gps
+		#fake_imu
+		#fake_magnetometer
+		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		#hello
+		#hwtest # Hardware test
+		#matlab_csv_serial
+		#px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		#px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		#rover_steering_control # Rover example app
+		#uuv_example_app
+		#work_item
+	)
@@ -52,5 +52,18 @@ target_link_libraries(drivers_board
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
 		nuttx_drivers # sdio
-		px4_layer
 )
+
+if (NOT DEFINED CONFIG_BUILD_FLAT)
+	target_link_libraries(drivers_board PRIVATE px4_kernel_layer)
+else()
+	target_link_libraries(drivers_board PRIVATE px4_layer)
+endif()
+
+add_library(drivers_userspace
+	stm32_userspace.c
+)
+
+target_link_libraries(drivers_userspace PRIVATE px4_userspace_init)
+
+add_dependencies(drivers_userspace arch_board_hw_info)
@@ -235,7 +235,13 @@
 #define GPIO_nARMED_INIT     /* PI0 */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTI|GPIO_PIN0)
 #define GPIO_nARMED          /* PI0 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0)

+#ifdef CONFIG_BUILD_FLAT
 #define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled)  px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
+#else
+/* TODO: indicate armed state in protected & kernel build;
+ * this will need some kernel side driver and user-space interface
+ */
+#endif

 /* PWM
 */
@@ -0,0 +1,147 @@
+/****************************************************************************
+ * boards/arm/stm32f7/stm32f746g-disco/kernel/stm32_userspace.c
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdlib.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/mm/mm.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/userspace.h>
+#include <sys/boardctl.h>
+
+#if !defined(CONFIG_BUILD_FLAT) && !defined(__KERNEL__)
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+#ifndef CONFIG_NUTTX_USERSPACE
+#  error "CONFIG_NUTTX_USERSPACE not defined"
+#endif
+
+#if CONFIG_NUTTX_USERSPACE != 0x08100000
+#  error "CONFIG_NUTTX_USERSPACE must be 0x08100000 to match memory.ld"
+#endif
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* These 'addresses' of these values are setup by the linker script. They are
+ * not actual uint32_t storage locations! They are only used meaningfully in
+ * the following way:
+ *
+ *  - The linker script defines, for example, the symbol_sdata.
+ *  - The declaration extern uint32_t _sdata; makes C happy.  C will believe
+ *    that the value _sdata is the address of a uint32_t variable _data (it
+ *    is not!).
+ *  - We can recover the linker value then by simply taking the address of
+ *    of _data.  like:  uint32_t *pdata = &_sdata;
+ */
+
+extern uint32_t _stext;           /* Start of .text */
+extern uint32_t _etext;           /* End_1 of .text + .rodata */
+extern const uint32_t _eronly;    /* End+1 of read only section (.text + .rodata) */
+extern uint32_t _sdata;           /* Start of .data */
+extern uint32_t _edata;           /* End+1 of .data */
+extern uint32_t _sbss;            /* Start of .bss */
+extern uint32_t _ebss;            /* End+1 of .bss */
+
+/* This is the user space entry point */
+
+int CONFIG_USER_ENTRYPOINT(int argc, char *argv[]);
+int nsh_main(int argc, char *argv[]);
+
+const struct userspace_s userspace __attribute__((section(".userspace"))) = {
+	/* General memory map */
+
+	.us_entrypoint    = (main_t)CONFIG_USER_ENTRYPOINT,
+	.us_textstart     = (uintptr_t) &_stext,
+	.us_textend       = (uintptr_t) &_etext,
+	.us_datasource    = (uintptr_t) &_eronly,
+	.us_datastart     = (uintptr_t) &_sdata,
+	.us_dataend       = (uintptr_t) &_edata,
+	.us_bssstart      = (uintptr_t) &_sbss,
+	.us_bssend        = (uintptr_t) &_ebss,
+
+	/* Memory manager heap structure */
+
+	.us_heap          = &g_mmheap,
+
+	/* Task/thread startup routines */
+
+	.task_startup     = nxtask_startup,
+
+	/* Signal handler trampoline */
+
+	.signal_handler   = up_signal_handler,
+
+	/* User-space work queue support (declared in include/nuttx/wqueue.h) */
+
+#ifdef CONFIG_LIB_USRWORK
+	.work_usrstart    = work_usrstart,
+#endif
+};
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+void px4_userspace_init(void);
+
+int CONFIG_USER_ENTRYPOINT(int argc, char *argv[])
+{
+
+#ifdef CONFIG_NSH_ARCHINIT
+#error CONFIG_NSH_ARCHINIT must not be defined!
+#endif
+
+	boardctl(BOARDIOC_INIT, 0);
+
+	px4_userspace_init();
+
+	return nsh_main(argc, argv);
+}
+
+
+#endif /* !CONFIG_BUILD_FLAT && !__KERNEL__ */
@@ -41,6 +41,7 @@ px4_add_board(
 		pca9685_pwm_out
 		power_monitor/ina226
 		power_monitor/ina228
+		power_monitor/ina238
 		#protocol_splitter
 		pwm_input
 		pwm_out_sim
@@ -3,6 +3,7 @@
 # board specific defaults
 #------------------------------------------------------------------------------

+param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

@@ -19,6 +19,13 @@ then
 	ina228 -X -b 2 -t 2 -k start
 fi

+if param compare SENS_EN_INA238 1
+then
+	# Start Digital power monitors
+	ina238 -X -b 1 -t 1 -k start
+	ina238 -X -b 2 -t 2 -k start
+fi
+
 if ver hwtypecmp V5X90 V5X91 V5X92 V5Xa0 V5Xa1 V5Xa2
 then
 	#SKYNODE base fmu board orientation
@@ -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
@@ -42,6 +42,7 @@ px4_add_board(
 		pca9685_pwm_out
 		power_monitor/ina226
 		power_monitor/ina228
+		power_monitor/ina238
 		#protocol_splitter
 		pwm_out_sim
 		pwm_out
@@ -3,7 +3,8 @@
 # board specific defaults
 #------------------------------------------------------------------------------

+param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

-safety_button start
+safety_button start
@@ -18,6 +18,13 @@ then
 	ina228 -X -b 2 -t 2 -k start
 fi

+if param compare SENS_EN_INA238 1
+then
+	# Start Digital power monitors
+	ina238 -X -b 1 -t 1 -k start
+	ina238 -X -b 2 -t 2 -k start
+fi
+
 # Internal SPI BMI088
 bmi088 -A -R 4 -s start
 bmi088 -G -R 4 -s start
@@ -147,6 +147,9 @@ function(px4_add_board)
 			CRYPTO
 			KEYSTORE
 		MULTI_VALUE
+			KERNEL_DRIVERS
+			KERNEL_MODULES
+			KERNEL_SYSTEMCMDS
 			DRIVERS
 			MODULES
 			SYSTEMCMDS
@@ -296,6 +299,28 @@ function(px4_add_board)
 	###########################################################################
 	# Modules (includes drivers, examples, modules, systemcmds)
 	set(config_module_list)
+        set(config_kernel_list)
+
+	if(KERNEL_DRIVERS)
+		foreach(driver ${KERNEL_DRIVERS})
+			list(APPEND config_module_list drivers/${driver})
+			list(APPEND config_kernel_list drivers/${driver})
+		endforeach()
+	endif()
+
+	if(KERNEL_MODULES)
+		foreach(module ${KERNEL_MODULES})
+			list(APPEND config_module_list modules/${module})
+			list(APPEND config_kernel_list modules/${module})
+		endforeach()
+	endif()
+
+	if(KERNEL_SYSTEMCMDS)
+		foreach(systemcmd ${KERNEL_SYSTEMCMDS})
+			list(APPEND config_module_list systemcmds/${systemcmd})
+			list(APPEND config_kernel_list systemcmds/${systemcmd})
+		endforeach()
+	endif()

 	if(DRIVERS)
 		foreach(driver ${DRIVERS})
@@ -326,5 +351,6 @@ function(px4_add_board)
 	list(APPEND config_module_list ${board_support_src_rel}/src)

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

 endfunction()
@@ -43,14 +43,10 @@ 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 uorb_msgs)
+	add_dependencies(${target} uorb_headers parameters)

-	# TODO: move to platform layer
-	if ("${PX4_PLATFORM}" MATCHES "nuttx")
-		target_link_libraries(${target} PRIVATE m nuttx_c)
-	endif()
+	# all PX4 libraries have access to uORB
+	target_link_libraries(${target} PRIVATE prebuild_targets uorb_msgs)

 	set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
 	px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${ARGN})
@@ -153,9 +153,26 @@ 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})
@@ -195,6 +212,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()
@@ -53,7 +53,7 @@ set(msg_files
 	collision_report.msg
 	commander_state.msg
 	control_allocator_status.msg
-	cpuload.msg
+	vehicle_cpuload.msg
 	differential_pressure.msg
 	distance_sensor.msg
 	ekf2_timestamps.msg
@@ -1,4 +1,5 @@
 # Fused local position in NED.
+# The coordinate system origin is the vehicle position at the time when the EKF2-module was started.

 uint64 timestamp			# time since system start (microseconds)
 uint64 timestamp_sample                 # the timestamp of the raw data (microseconds)
@@ -52,11 +52,14 @@ add_library(px4_platform
 	spi.cpp
 	${SRCS}
 )
-add_dependencies(px4_platform prebuild_targets)
+target_link_libraries(px4_platform prebuild_targets px4_work_queue)
+
+if("${PX4_BOARD}" MATCHES "sitl")
+	target_link_libraries(px4_platform drivers_board)
+endif()

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

 add_subdirectory(px4_work_queue)
@@ -44,4 +44,4 @@ if(PX4_TESTING)
 endif()

 target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_link_libraries(px4_work_queue PRIVATE px4_platform)
+add_dependencies(px4_work_queue px4_platform)
@@ -218,10 +218,11 @@ const wq_config_t &ins_instance_to_wq(uint8_t instance)
 	return wq_configurations::INS0;
 }

-static void *
-WorkQueueRunner(void *context)
+static int
+WorkQueueRunner(int argc, char *argv[])
 {
-	wq_config_t *config = static_cast<wq_config_t *>(context);
+	// The argv list differs on posix and nuttx, use the last arg
+	wq_config_t *config = (wq_config_t *)strtoul(argv[argc - 1], nullptr, 16);
 	WorkQueue wq(*config);

 	// add to work queue list
@@ -232,7 +233,7 @@ WorkQueueRunner(void *context)
 	// remove from work queue list
 	_wq_manager_wqs_list->remove(&wq);

-	return nullptr;
+	return 0;
 }

 static int
@@ -248,20 +249,6 @@ WorkQueueManagerRun(int, char **)
 		if (wq != nullptr) {
 			// create new work queue

-			pthread_attr_t attr;
-			int ret_attr_init = pthread_attr_init(&attr);
-
-			if (ret_attr_init != 0) {
-				PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init);
-			}
-
-			sched_param param;
-			int ret_getschedparam = pthread_attr_getschedparam(&attr, &param);
-
-			if (ret_getschedparam != 0) {
-				PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam);
-			}
-
 			// stack size
 #if defined(__PX4_QURT)
 			const size_t stacksize = math::max(8 * 1024, PX4_STACK_ADJUSTED(wq->stacksize));
@@ -274,47 +261,27 @@ WorkQueueManagerRun(int, char **)
 			const size_t stacksize_adj = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize));
 			const size_t stacksize = (stacksize_adj + page_size - (stacksize_adj % page_size));
 #endif
-			int ret_setstacksize = pthread_attr_setstacksize(&attr, stacksize);
-
-			if (ret_setstacksize != 0) {
-				PX4_ERR("setting stack size for %s failed (%i)", wq->name, ret_setstacksize);
-			}
-
-#ifndef __PX4_QURT
-
-			// schedule policy FIFO
-			int ret_setschedpolicy = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
-
-			if (ret_setschedpolicy != 0) {
-				PX4_ERR("failed to set sched policy SCHED_FIFO (%i)", ret_setschedpolicy);
-			}
-
-#endif // ! QuRT

 			// priority
-			param.sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority;
-			int ret_setschedparam = pthread_attr_setschedparam(&attr, &param);
-
-			if (ret_setschedparam != 0) {
-				PX4_ERR("setting sched params for %s failed (%i)", wq->name, ret_setschedparam);
-			}
+			int sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority;

 			// create thread
-			pthread_t thread;
-			int ret_create = pthread_create(&thread, &attr, WorkQueueRunner, (void *)wq);
+			char arg1[sizeof(void *) * 3];
+			sprintf(arg1, "%lx", (long unsigned)wq);
+			const char *arg[2] = {arg1, nullptr};

-			if (ret_create == 0) {
-				PX4_DEBUG("starting: %s, priority: %d, stack: %zu bytes", wq->name, param.sched_priority, stacksize);
+			int pid = px4_task_spawn_cmd(wq->name,
+						     SCHED_FIFO,
+						     sched_priority,
+						     stacksize,
+						     WorkQueueRunner,
+						     (char *const *)arg);
+
+			if (pid > 0) {
+				PX4_DEBUG("starting: %s, priority: %d, stack: %zu bytes", wq->name, sched_priority, stacksize);

 			} else {
-				PX4_ERR("failed to create thread for %s (%i): %s", wq->name, ret_create, strerror(ret_create));
-			}
-
-			// destroy thread attributes
-			int ret_destroy = pthread_attr_destroy(&attr);
-
-			if (ret_destroy != 0) {
-				PX4_ERR("failed to destroy thread attributes for %s (%i)", wq->name, ret_create);
+				PX4_ERR("failed to create thread for %s (%i): %s", wq->name, pid, strerror(pid));
 			}
 		}
 	}
@@ -56,6 +56,7 @@

 #ifdef __PX4_NUTTX
 #include <nuttx/board.h>
+#include <sys/boardctl.h>
 #endif

 using namespace time_literals;
@@ -99,7 +100,7 @@ int px4_shutdown_unlock()
 	return ret;
 }

-#if defined(CONFIG_SCHED_WORKQUEUE)
+#if defined(CONFIG_SCHED_WORKQUEUE) || (!defined(CONFIG_BUILD_FLAT) && defined(CONFIG_LIB_USRWORK))

 static struct work_s shutdown_work = {};
 static uint16_t shutdown_counter = 0; ///< count how many times the shutdown worker was executed
@@ -174,7 +175,7 @@ static void shutdown_worker(void *arg)
 		if (shutdown_args & SHUTDOWN_ARG_REBOOT) {
 #if defined(CONFIG_BOARDCTL_RESET)
 			PX4_INFO_RAW("Reboot NOW.");
-			board_reset((shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0);
+			boardctl(BOARDIOC_RESET, (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0);
 #else
 			PX4_PANIC("board reset not available");
 #endif
@@ -34,7 +34,9 @@
 # this includes the generated topics directory
 include_directories(${CMAKE_CURRENT_BINARY_DIR})

-px4_add_library(uORB
+set(SRCS)
+
+set(SRCS_COMMON
 	ORBSet.hpp
 	Publication.hpp
 	PublicationMulti.hpp
@@ -47,15 +49,48 @@ px4_add_library(uORB
 	uORB.h
 	uORBCommon.hpp
 	uORBCommunicator.hpp
-	uORBDeviceMaster.cpp
-	uORBDeviceMaster.hpp
-	uORBDeviceNode.cpp
-	uORBDeviceNode.hpp
-	uORBManager.cpp
 	uORBManager.hpp
 	uORBUtils.cpp
 	uORBUtils.hpp
-)
+	uORBDeviceMaster.hpp
+	uORBDeviceNode.hpp
+	)
+
+set(SRCS_KERNEL
+	uORBDeviceMaster.cpp
+	uORBDeviceNode.cpp
+	uORBManager.cpp
+	)
+
+set(SRCS_USER
+	uORBManagerUsr.cpp
+	)
+
+if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
+	# This is the kernel side library in nuttx kernel/protected build
+	px4_add_library(uORB_kernel
+			${SRCS_COMMON}
+			${SRCS_KERNEL}
+		)
+
+	# Sources for the user side library in nuttx kernel/protected build
+	list(APPEND SRCS
+		${SRCS_COMMON}
+		${SRCS_USER}
+		)
+	target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__)
+
+else()
+	# Sources for all other targets (flat build, posix...)
+	list(APPEND SRCS
+		${SRCS_COMMON}
+		${SRCS_KERNEL}
+		)
+endif()
+
+px4_add_library(uORB
+		${SRCS}
+	)

 target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(uORB PRIVATE cdev uorb_msgs)
@@ -42,7 +42,7 @@
 #include <systemlib/err.h>

 #include <uORB/uORB.h>
-#include "uORBDeviceNode.hpp"
+#include "uORBManager.hpp"
 #include <uORB/topics/uORBTopics.hpp>

 namespace uORB
@@ -72,7 +72,7 @@ public:

 	bool advertised() const { return _handle != nullptr; }

-	bool unadvertise() { return (DeviceNode::unadvertise(_handle) == PX4_OK); }
+	bool unadvertise() { return (Manager::orb_unadvertise(_handle) == PX4_OK); }

 	orb_id_t get_topic() const { return get_orb_meta(_orb_id); }

@@ -84,7 +84,7 @@ protected:
 	{
 		if (_handle != nullptr) {
 			// don't automatically unadvertise queued publications (eg vehicle_command)
-			if (static_cast<DeviceNode *>(_handle)->get_queue_size() == 1) {
+			if (Manager::orb_get_queue_size(_handle) == 1) {
 				unadvertise();
 			}
 		}
@@ -129,7 +129,7 @@ public:
 			advertise();
 		}

-		return (DeviceNode::publish(get_topic(), _handle, &data) == PX4_OK);
+		return (Manager::orb_publish(get_topic(), _handle, &data) == PX4_OK);
 	}
 };

@@ -96,7 +96,7 @@ public:
 	{
 		// advertise if not already advertised
 		if (advertise()) {
-			return static_cast<uORB::DeviceNode *>(_handle)->get_instance();
+			return Manager::orb_get_instance(_handle);
 		}

 		return -1;
@@ -49,25 +49,14 @@ bool Subscription::subscribe()
 		return true;
 	}

-	if ((_orb_id != ORB_ID::INVALID) && uORB::Manager::get_instance()) {
-		DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
+	if (_orb_id != ORB_ID::INVALID && uORB::Manager::get_instance()) {
+		unsigned initial_generation;
+		void *node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &initial_generation);

-		if (device_master != nullptr) {
-
-			if (!device_master->deviceNodeExists(_orb_id, _instance)) {
-				return false;
-			}
-
-			uORB::DeviceNode *node = device_master->getDeviceNode(get_topic(), _instance);
-
-			if (node != nullptr) {
-				_node = node;
-				_node->add_internal_subscriber();
-
-				_last_generation = _node->get_initial_generation();
-
-				return true;
-			}
+		if (node) {
+			_node = node;
+			_last_generation = initial_generation;
+			return true;
 		}
 	}

@@ -77,7 +66,7 @@ bool Subscription::subscribe()
 void Subscription::unsubscribe()
 {
 	if (_node != nullptr) {
-		_node->remove_internal_subscriber();
+		uORB::Manager::orb_remove_internal_subscriber(_node);
 	}

 	_node = nullptr;
@@ -87,13 +76,7 @@ void Subscription::unsubscribe()
 bool Subscription::ChangeInstance(uint8_t instance)
 {
 	if (instance != _instance) {
-		DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
-
-		if (device_master != nullptr) {
-			if (!device_master->deviceNodeExists(_orb_id, instance)) {
-				return false;
-			}
-
+		if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) {
 			// if desired new instance exists, unsubscribe from current
 			unsubscribe();
 			_instance = instance;
@@ -44,7 +44,6 @@
 #include <px4_platform_common/defines.h>
 #include <lib/mathlib/mathlib.h>

-#include "uORBDeviceNode.hpp"
 #include "uORBManager.hpp"
 #include "uORBUtils.hpp"

@@ -120,14 +119,14 @@ public:
 	bool advertised()
 	{
 		if (valid()) {
-			return _node->is_advertised();
+			return Manager::is_advertised(_node);
 		}

 		// try to initialize
 		if (subscribe()) {
 			// check again if valid
 			if (valid()) {
-				return _node->is_advertised();
+				return Manager::is_advertised(_node);
 			}
 		}

@@ -137,19 +136,19 @@ public:
 	/**
 	 * Check if there is a new update.
 	 */
-	bool updated() { return advertised() && _node->updates_available(_last_generation); }
+	bool updated() { return advertised() && Manager::updates_available(_node, _last_generation); }

 	/**
 	 * Update the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool update(void *dst) { return updated() && _node->copy(dst, _last_generation); }
+	bool update(void *dst) { return updated() && Manager::orb_data_copy(_node, dst, _last_generation); }

 	/**
 	 * Copy the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool copy(void *dst) { return advertised() && _node->copy(dst, _last_generation); }
+	bool copy(void *dst) { return advertised() && Manager::orb_data_copy(_node, dst, _last_generation); }

 	/**
 	 * Change subscription instance
@@ -166,9 +165,9 @@ protected:
 	friend class SubscriptionCallback;
 	friend class SubscriptionCallbackWorkItem;

-	DeviceNode *get_node() { return _node; }
+	void *get_node() { return _node; }

-	DeviceNode *_node{nullptr};
+	void *_node{nullptr};

 	unsigned _last_generation{0}; /**< last generation the subscriber has seen */

@@ -69,7 +69,7 @@ public:
 	bool registerCallback()
 	{
 		if (!_registered) {
-			if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
+			if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) {
 				// registered
 				_registered = true;

@@ -79,7 +79,7 @@ public:

 				// try to register callback again
 				if (_subscription.subscribe()) {
-					if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
+					if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) {
 						_registered = true;
 					}
 				}
@@ -94,7 +94,7 @@ public:
 	void unregisterCallback()
 	{
 		if (_subscription.get_node()) {
-			_subscription.get_node()->unregister_callback(this);
+			Manager::unregister_callback(_subscription.get_node(), this);
 		}

 		_registered = false;
@@ -164,7 +164,7 @@ public:
 	{
 		// schedule immediately if updated (queue depth or subscription interval)
 		if ((_required_updates == 0)
-		    || (_subscription.get_node()->updates_available(_subscription.get_last_generation()) >= _required_updates)) {
+		    || (Manager::updates_available(_subscription.get_node(), _subscription.get_last_generation()) >= _required_updates)) {
 			if (updated()) {
 				_work_item->ScheduleNow();
 			}
@@ -41,6 +41,10 @@
 #include "uORBManager.hpp"
 #include "uORBCommon.hpp"

+#ifdef __PX4_NUTTX
+#include <sys/boardctl.h>
+#endif
+
 static uORB::DeviceMaster *g_dev = nullptr;

 int uorb_start(void)
@@ -56,6 +60,7 @@ int uorb_start(void)
 		return -ENOMEM;
 	}

+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__)
 	/* create the driver */
 	g_dev = uORB::Manager::get_instance()->get_device_master();

@@ -63,11 +68,15 @@ int uorb_start(void)
 		return -errno;
 	}

+#endif
+
 	return OK;
 }

 int uorb_status(void)
 {
+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__)
+
 	if (g_dev != nullptr) {
 		g_dev->printStatistics();

@@ -75,11 +84,16 @@ int uorb_status(void)
 		PX4_INFO("uorb is not running");
 	}

+#else
+	boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_STATUS);
+#endif
 	return OK;
 }

 int uorb_top(char **topic_filter, int num_filters)
 {
+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__)
+
 	if (g_dev != nullptr) {
 		g_dev->showTop(topic_filter, num_filters);

@@ -87,10 +101,12 @@ int uorb_top(char **topic_filter, int num_filters)
 		PX4_INFO("uorb is not running");
 	}

+#else
+	boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_TOP);
+#endif
 	return OK;
 }

-
 orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
 {
 	return uORB::Manager::get_instance()->orb_advertise(meta, data);
@@ -117,42 +133,42 @@ int orb_unadvertise(orb_advert_t handle)
 	return uORB::Manager::get_instance()->orb_unadvertise(handle);
 }

-int  orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
+int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
 {
 	return uORB::Manager::get_instance()->orb_publish(meta, handle, data);
 }

-int  orb_subscribe(const struct orb_metadata *meta)
+int orb_subscribe(const struct orb_metadata *meta)
 {
 	return uORB::Manager::get_instance()->orb_subscribe(meta);
 }

-int  orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
+int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
 {
 	return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance);
 }

-int  orb_unsubscribe(int handle)
+int orb_unsubscribe(int handle)
 {
 	return uORB::Manager::get_instance()->orb_unsubscribe(handle);
 }

-int  orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
+int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
 {
 	return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer);
 }

-int  orb_check(int handle, bool *updated)
+int orb_check(int handle, bool *updated)
 {
 	return uORB::Manager::get_instance()->orb_check(handle, updated);
 }

-int  orb_exists(const struct orb_metadata *meta, int instance)
+int orb_exists(const struct orb_metadata *meta, int instance)
 {
 	return uORB::Manager::get_instance()->orb_exists(meta, instance);
 }

-int  orb_group_count(const struct orb_metadata *meta)
+int orb_group_count(const struct orb_metadata *meta)
 {
 	unsigned instance = 0;

@@ -49,6 +49,10 @@
 #include <poll.h>
 #endif // PX4_QURT

+#if defined(__PX4_NUTTX)
+#include <nuttx/mm/mm.h>
+#endif
+
 uORB::DeviceMaster::DeviceMaster()
 {
 	px4_sem_init(&_lock, 0, 1);
@@ -111,7 +115,11 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver

 		/* if we didn't get a device, that's bad, free the path too */
 		if (node == nullptr) {
+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+			kmm_free((void *)devpath);
+#else
 			free((void *)devpath);
+#endif
 			return -ENOMEM;
 		}

@@ -323,7 +323,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
 	}

 	/* check if the orb meta data matches the publication */
-	if (devnode->_meta != meta) {
+	if (devnode->_meta->o_id != meta->o_id) {
 		errno = EINVAL;
 		return PX4_ERROR;
 	}
@@ -233,6 +233,19 @@ public:
 	// remove item from list of work items
 	void unregister_callback(SubscriptionCallback *callback_sub);

+	// Allocate this always from user heap, also in NuttX
+	// protected build. This allows direct access to some member
+	// variables from the whole system; sort of a shared memmory
+	// TODO: For nuttx kernel build, in shared memory
+	void *operator new (size_t nbytes)
+	{
+		return malloc(nbytes);
+	}
+	void operator delete (void *p)
+	{
+		free(p);
+	}
+
 protected:

 	px4_pollevent_t poll_state(cdev::file_t *filp) override;
@@ -40,6 +40,10 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
+#include <px4_platform/board_ctrl.h>
+#endif
+
 #include "uORBDeviceNode.hpp"
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"
@@ -52,6 +56,9 @@ bool uORB::Manager::initialize()
 		_Instance = new uORB::Manager();
 	}

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
+	px4_register_boardct_ioctl(_ORBIOCBASE, orb_ioctl);
+#endif
 	return _Instance != nullptr;
 }

@@ -103,6 +110,112 @@ uORB::DeviceMaster *uORB::Manager::get_device_master()
 	return _device_master;
 }

+#if defined (__PX4_NUTTX) && !defined (CONFIG_BUILD_FLAT) && defined(__KERNEL__)
+int	uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg)
+{
+	int ret = PX4_OK;
+
+	switch (cmd) {
+	case ORBIOCDEVEXISTS: {
+			orbiocdevexists_t *data = (orbiocdevexists_t *)arg;
+
+			if (data->check_advertised) {
+				data->ret = uORB::Manager::orb_exists(get_orb_meta(data->orb_id), data->instance);
+
+			} else {
+				data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR;
+			}
+		}
+		break;
+
+	case ORBIOCDEVADVERTISE: {
+			orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg;
+			uORB::DeviceMaster *dev =  uORB::Manager::get_instance()->get_device_master();
+
+			if (dev) {
+				data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance);
+
+			} else {
+				data->ret = PX4_ERROR;
+			}
+		}
+		break;
+
+	case ORBIOCDEVUNADVERTISE: {
+			orbiocdevunadvertise_t *data = (orbiocdevunadvertise_t *)arg;
+			data->ret = uORB::Manager::orb_unadvertise(data->handle);
+		}
+		break;
+
+	case ORBIOCDEVPUBLISH: {
+			orbiocdevpublish_t *data = (orbiocdevpublish_t *)arg;
+			data->ret = uORB::Manager::orb_publish(data->meta, data->handle, data->data);
+		}
+		break;
+
+	case ORBIOCDEVADDSUBSCRIBER: {
+			orbiocdevaddsubscriber_t *data = (orbiocdevaddsubscriber_t *)arg;
+			data->handle = uORB::Manager::orb_add_internal_subscriber(data->orb_id, data->instance, data->initial_generation);
+		}
+		break;
+
+	case ORBIOCDEVREMSUBSCRIBER: {
+			uORB::Manager::orb_remove_internal_subscriber(reinterpret_cast<void *>(arg));
+		}
+		break;
+
+	case ORBIOCDEVQUEUESIZE: {
+			orbiocdevqueuesize_t *data = (orbiocdevqueuesize_t *)arg;
+			data->size = uORB::Manager::orb_get_queue_size(data->handle);
+		}
+		break;
+
+	case ORBIOCDEVDATACOPY: {
+			orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg;
+			data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation);
+		}
+		break;
+
+	case ORBIOCDEVREGCALLBACK: {
+			orbiocdevregcallback_t *data = (orbiocdevregcallback_t *)arg;
+			data->registered = uORB::Manager::register_callback(data->handle, data->callback_sub);
+		}
+		break;
+
+	case ORBIOCDEVUNREGCALLBACK: {
+			orbiocdevunregcallback_t *data = (orbiocdevunregcallback_t *)arg;
+			uORB::Manager::unregister_callback(data->handle, data->callback_sub);
+		}
+		break;
+
+	case ORBIOCDEVGETINSTANCE: {
+			orbiocdevgetinstance_t *data = (orbiocdevgetinstance_t *)arg;
+			data->instance = uORB::Manager::orb_get_instance(data->handle);
+		}
+		break;
+
+	case ORBIOCDEVMASTERCMD: {
+			uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master();
+
+			if (dev) {
+				if (arg == ORB_DEVMASTER_TOP) {
+					dev->showTop(nullptr, 0);
+
+				} else {
+					dev->printStatistics();
+				}
+			}
+		}
+		break;
+
+	default:
+		ret = -ENOTTY;
+	}
+
+	return ret;
+}
+#endif
+
 int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 {
 	int ret = PX4_ERROR;
@@ -112,8 +225,10 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		return ret;
 	}

-	if (get_device_master()) {
-		uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance);
+	uORB::DeviceMaster *dev =  uORB::Manager::get_instance()->get_device_master();
+
+	if (dev) {
+		uORB::DeviceNode *node = dev->getDeviceNode(meta, instance);

 		if (node != nullptr) {
 			if (node->is_advertised()) {
@@ -319,6 +434,65 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
 	return ret;
 }

+
+bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance)
+{
+	DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
+
+	return device_master != nullptr &&
+	       device_master->deviceNodeExists(orb_id, instance);
+}
+
+void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation)
+{
+	uORB::DeviceNode *node = nullptr;
+	DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
+
+	if (device_master != nullptr) {
+		node = device_master->getDeviceNode(get_orb_meta(orb_id), instance);
+
+		if (node) {
+			node->add_internal_subscriber();
+			*initial_generation = node->get_initial_generation();
+		}
+	}
+
+	return node;
+}
+
+void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)
+{
+	static_cast<DeviceNode *>(node_handle)->remove_internal_subscriber();
+}
+
+uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->get_queue_size(); }
+
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
+{
+	return static_cast<DeviceNode *>(node_handle)->copy(dst, generation);
+}
+
+// add item to list of work items to schedule on node update
+bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub)
+{
+	return static_cast<DeviceNode *>(node_handle)->register_callback(callback_sub);
+}
+
+// remove item from list of work items
+void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub)
+{
+	static_cast<DeviceNode *>(node_handle)->unregister_callback(callback_sub);
+}
+
+uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
+{
+	if (node_handle) {
+		return static_cast<const uORB::DeviceNode *>(node_handle)->get_instance();
+	}
+
+	return -1;
+}
+
 int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
 {
 	char path[orb_maxpath];
@@ -34,9 +34,11 @@
 #ifndef _uORBManager_hpp_
 #define _uORBManager_hpp_

+#include "uORBDeviceNode.hpp"
 #include "uORBCommon.hpp"
 #include "uORBDeviceMaster.hpp"
-
+#include <uORB/topics/uORBTopics.hpp> // For ORB_ID enum
+#include <lib/cdev/CDev.hpp>
 #include <stdint.h>

 #ifdef __PX4_NUTTX
@@ -54,8 +56,103 @@
 namespace uORB
 {
 class Manager;
+class SubscriptionCallback;
 }

+
+/*
+ * IOCTLs for manager to access device nodes using
+ * a handle
+ *
+ * This is WIP; handle is just a pointer to kernel side object, so this is a
+ * security hole.
+ *
+ * The handle in the user side should just be a file descriptor, and IOCTL go
+ * directly to the device nodes.
+ * But for now, publisher doesn't even keep the descriptors open.
+ * This needs to be addressed later!
+ */
+
+#define ORBIOCDEVEXISTS	_ORBIOC(30)
+typedef struct orbiocdevexists {
+	const ORB_ID orb_id;
+	const uint8_t instance;
+	const bool check_advertised;
+	int ret;
+} orbiocdevexists_t;
+
+#define ORBIOCDEVADVERTISE	_ORBIOC(31)
+typedef struct orbiocadvertise {
+	const struct orb_metadata *meta;
+	bool is_advertiser;
+	int *instance;
+	int ret;
+} orbiocdevadvertise_t;
+
+#define ORBIOCDEVUNADVERTISE	_ORBIOC(32)
+typedef struct orbiocunadvertise {
+	void *handle;
+	int ret;
+} orbiocdevunadvertise_t;
+
+#define ORBIOCDEVPUBLISH	_ORBIOC(33)
+typedef struct orbiocpublish {
+	const struct orb_metadata *meta;
+	orb_advert_t handle;
+	const void *data;
+	int ret;
+} orbiocdevpublish_t;
+
+#define ORBIOCDEVADDSUBSCRIBER	_ORBIOC(34)
+typedef struct {
+	const ORB_ID orb_id;
+	const uint8_t instance;
+	unsigned *initial_generation;
+	void *handle;
+} orbiocdevaddsubscriber_t;
+
+#define ORBIOCDEVREMSUBSCRIBER	_ORBIOC(35)
+
+#define ORBIOCDEVQUEUESIZE	_ORBIOC(36)
+typedef struct {
+	const void *handle;
+	uint8_t size;
+} orbiocdevqueuesize_t;
+
+#define ORBIOCDEVDATACOPY	_ORBIOC(37)
+typedef struct {
+	void *handle;
+	void *dst;
+	unsigned generation;
+	bool ret;
+} orbiocdevdatacopy_t;
+
+#define ORBIOCDEVREGCALLBACK	_ORBIOC(38)
+typedef struct {
+	void *handle;
+	class uORB::SubscriptionCallback *callback_sub;
+	bool registered;
+} orbiocdevregcallback_t;
+
+#define ORBIOCDEVUNREGCALLBACK	_ORBIOC(39)
+typedef struct {
+	void *handle;
+	class uORB::SubscriptionCallback *callback_sub;
+} orbiocdevunregcallback_t;
+
+#define ORBIOCDEVGETINSTANCE	_ORBIOC(40)
+typedef struct {
+	const void *handle;
+	uint8_t instance;
+} orbiocdevgetinstance_t;
+
+typedef enum {
+	ORB_DEVMASTER_STATUS = 0,
+	ORB_DEVMASTER_TOP = 1
+} orbiocdevmastercmd_t;
+#define ORBIOCDEVMASTERCMD	_ORBIOC(45)
+
+
 /**
 * This is implemented as a singleton.  This class manages creating the
 * uORB nodes for each uORB topics and also implements the behavor of the
@@ -96,6 +193,10 @@ public:
 	 */
 	uORB::DeviceMaster *get_device_master();

+#if defined (__PX4_NUTTX) && !defined (CONFIG_BUILD_FLAT) && defined(__KERNEL__)
+	static int orb_ioctl(unsigned int cmd, unsigned long arg);
+#endif
+
 	// ==== uORB interface methods ====
 	/**
 	 * Advertise as the publisher of a topic.
@@ -165,7 +266,7 @@ public:
 	 * @param handle  handle returned by orb_advertise or orb_advertise_multi.
 	 * @return 0 on success
 	 */
-	int orb_unadvertise(orb_advert_t handle);
+	static int orb_unadvertise(orb_advert_t handle);

 	/**
 	 * Publish new data to a topic.
@@ -180,7 +281,7 @@ public:
 	 * @param data    A pointer to the data to be published.
 	 * @return    OK on success, PX4_ERROR otherwise with errno set accordingly.
 	 */
-	int  orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data);
+	static int  orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data);

 	/**
 	 * Subscribe to a topic.
@@ -301,7 +402,7 @@ public:
 	 * @param instance  ORB instance
 	 * @return    OK if the topic exists, PX4_ERROR otherwise.
 	 */
-	int  orb_exists(const struct orb_metadata *meta, int instance);
+	static int  orb_exists(const struct orb_metadata *meta, int instance);

 	/**
 	 * Set the minimum interval between which updates are seen for a subscription.
@@ -335,6 +436,28 @@ public:
 	 */
 	int	orb_get_interval(int handle, unsigned *interval);

+	static bool orb_device_node_exists(ORB_ID orb_id, uint8_t instance);
+
+	static void *orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation);
+
+	static void orb_remove_internal_subscriber(void *node_handle);
+
+	static uint8_t orb_get_queue_size(const void *node_handle);
+
+	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation);
+
+	static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub);
+
+	static void unregister_callback(void *node_handle, SubscriptionCallback *callback_sub);
+
+	static uint8_t orb_get_instance(const void *node_handle);
+
+	static void device_master_cmd(orbiocdevmastercmd_t cmd);
+
+	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation); }
+
+	static bool is_advertised(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->is_advertised(); }
+
 #ifdef ORB_COMMUNICATOR
 	/**
 	 * Method to set the uORBCommunicator::IChannel instance.
@@ -0,0 +1,340 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 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 <sys/types.h>
+#include <sys/stat.h>
+#include <stdarg.h>
+#include <fcntl.h>
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+
+#include <sys/boardctl.h>
+
+#include "uORBDeviceNode.hpp"
+#include "uORBUtils.hpp"
+#include "uORBManager.hpp"
+
+uORB::Manager *uORB::Manager::_Instance = nullptr;
+
+bool uORB::Manager::initialize()
+{
+	if (_Instance == nullptr) {
+		_Instance = new uORB::Manager();
+	}
+
+	return _Instance != nullptr;
+}
+
+bool uORB::Manager::terminate()
+{
+	if (_Instance != nullptr) {
+		delete _Instance;
+		_Instance = nullptr;
+		return true;
+	}
+
+	return false;
+}
+
+uORB::Manager::Manager()
+{
+}
+
+uORB::Manager::~Manager()
+{
+}
+
+int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
+{
+	// instance valid range: [0, ORB_MULTI_MAX_INSTANCES)
+	if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) {
+		return PX4_ERROR;
+	}
+
+	orbiocdevexists_t data = {static_cast<ORB_ID>(meta->o_id), static_cast<uint8_t>(instance), true, PX4_ERROR};
+	boardctl(ORBIOCDEVEXISTS, reinterpret_cast<unsigned long>(&data));
+
+	return data.ret;
+}
+
+orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
+		unsigned int queue_size)
+{
+	/* open the node as an advertiser */
+	int fd = node_open(meta, true, instance);
+
+	if (fd == PX4_ERROR) {
+		PX4_ERR("%s advertise failed (%i)", meta->o_name, errno);
+		return nullptr;
+	}
+
+	/* Set the queue size. This must be done before the first publication; thus it fails if
+	 * this is not the first advertiser.
+	 */
+	int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
+
+	if (result < 0 && queue_size > 1) {
+		PX4_WARN("orb_advertise_multi: failed to set queue size");
+	}
+
+	/* get the advertiser handle and close the node */
+	orb_advert_t advertiser;
+
+	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	px4_close(fd);
+
+	if (result == PX4_ERROR) {
+		PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
+		return nullptr;
+	}
+
+	/* the advertiser may perform an initial publish to initialise the object */
+	if (data != nullptr) {
+		result = orb_publish(meta, advertiser, data);
+
+		if (result == PX4_ERROR) {
+			PX4_ERR("orb_publish failed %s", meta->o_name);
+			return nullptr;
+		}
+	}
+
+	return advertiser;
+}
+
+int uORB::Manager::orb_unadvertise(orb_advert_t handle)
+{
+	orbiocdevunadvertise_t data = {handle, PX4_ERROR};
+	boardctl(ORBIOCDEVUNADVERTISE, reinterpret_cast<unsigned long>(&data));
+
+	return data.ret;
+}
+
+int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
+{
+	return node_open(meta, false);
+}
+
+int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
+{
+	int inst = instance;
+	return node_open(meta, false, &inst);
+}
+
+int uORB::Manager::orb_unsubscribe(int fd)
+{
+	return px4_close(fd);
+}
+
+int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
+{
+	orbiocdevpublish_t d = {meta, handle, data, PX4_ERROR};
+	boardctl(ORBIOCDEVPUBLISH, reinterpret_cast<unsigned long>(&d));
+
+	return d.ret;
+}
+
+int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
+{
+	int ret;
+
+	ret = px4_read(handle, buffer, meta->o_size);
+
+	if (ret < 0) {
+		return PX4_ERROR;
+	}
+
+	if (ret != (int)meta->o_size) {
+		errno = EIO;
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int uORB::Manager::orb_check(int handle, bool *updated)
+{
+	/* Set to false here so that if `px4_ioctl` fails to false. */
+	*updated = false;
+	return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
+}
+
+int uORB::Manager::orb_set_interval(int handle, unsigned interval)
+{
+	return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
+}
+
+int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
+{
+	int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval);
+	*interval /= 1000;
+	return ret;
+}
+
+int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
+{
+	char path[orb_maxpath];
+	int fd = -1;
+	int ret = PX4_ERROR;
+
+	/*
+	 * If meta is null, the object was not defined, i.e. it is not
+	 * known to the system.  We can't advertise/subscribe such a thing.
+	 */
+	if (nullptr == meta) {
+		errno = ENOENT;
+		return PX4_ERROR;
+	}
+
+	/* if we have an instance and are an advertiser, we will generate a new node and set the instance,
+	 * so we do not need to open here */
+	if (!instance || !advertiser) {
+		/*
+		 * Generate the path to the node and try to open it.
+		 */
+		ret = uORB::Utils::node_mkpath(path, meta, instance);
+
+		if (ret != OK) {
+			errno = -ret;
+			return PX4_ERROR;
+		}
+
+		/* open the path as either the advertiser or the subscriber */
+		fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY);
+
+	} else {
+		*instance = 0;
+	}
+
+	/* we may need to advertise the node... */
+	if (fd < 0) {
+		ret = PX4_ERROR;
+
+		orbiocdevadvertise_t data = {meta, advertiser, instance, PX4_ERROR};
+		boardctl(ORBIOCDEVADVERTISE, (unsigned long)&data);
+		ret = data.ret;
+
+		/* it's OK if it already exists */
+		if ((ret != PX4_OK) && (EEXIST == errno)) {
+			ret = PX4_OK;
+		}
+
+		if (ret == PX4_OK) {
+			/* update the path, as it might have been updated during the node advertise call */
+			ret = uORB::Utils::node_mkpath(path, meta, instance);
+
+			/* on success, try to open again */
+			if (ret == PX4_OK) {
+				fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
+
+			} else {
+				errno = -ret;
+				return PX4_ERROR;
+			}
+		}
+	}
+
+	if (fd < 0) {
+		errno = EIO;
+		return PX4_ERROR;
+	}
+
+	/* everything has been OK, we can return the handle now */
+	return fd;
+}
+
+bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance)
+{
+	orbiocdevexists_t data = {orb_id, instance, false, 0};
+	boardctl(ORBIOCDEVEXISTS, reinterpret_cast<unsigned long>(&data));
+
+	return data.ret == PX4_OK ? true : false;
+}
+
+void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation)
+{
+	orbiocdevaddsubscriber_t data = {orb_id, instance, initial_generation, nullptr};
+	boardctl(ORBIOCDEVADDSUBSCRIBER, reinterpret_cast<unsigned long>(&data));
+
+	return data.handle;
+}
+
+void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)
+{
+	boardctl(ORBIOCDEVREMSUBSCRIBER, reinterpret_cast<unsigned long>(node_handle));
+}
+
+uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle)
+{
+	orbiocdevqueuesize_t data = {node_handle, 0};
+	boardctl(ORBIOCDEVQUEUESIZE, reinterpret_cast<unsigned long>(&data));
+
+	return data.size;
+}
+
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
+{
+	orbiocdevdatacopy_t data = {node_handle, dst, generation, false};
+	boardctl(ORBIOCDEVDATACOPY, reinterpret_cast<unsigned long>(&data));
+	generation = data.generation;
+
+	return data.ret;
+}
+
+bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub)
+{
+	orbiocdevregcallback_t data = {node_handle, callback_sub, false};
+	boardctl(ORBIOCDEVREGCALLBACK, reinterpret_cast<unsigned long>(&data));
+
+	return data.registered;
+}
+
+void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub)
+{
+	orbiocdevunregcallback_t data = {node_handle, callback_sub};
+	boardctl(ORBIOCDEVUNREGCALLBACK, reinterpret_cast<unsigned long>(&data));
+}
+
+uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
+{
+	orbiocdevgetinstance_t data = {node_handle, 0};
+	boardctl(ORBIOCDEVGETINSTANCE, reinterpret_cast<unsigned long>(&data));
+
+	return data.instance;
+}
+
+void uORB::Manager::device_master_cmd(orbiocdevmastercmd_t cmd)
+{
+	boardctl(ORBIOCDEVMASTERCMD, cmd);
+}
@@ -47,6 +47,15 @@ add_dependencies(px4 git_nuttx nuttx_build)

 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 nuttx_build)
+get_property(kernel_module_libraries GLOBAL PROPERTY PX4_KERNEL_MODULE_LIBRARIES)
+endif()
+
+
 # build NuttX
 add_subdirectory(NuttX ${PX4_BINARY_DIR}/NuttX)

@@ -84,21 +93,34 @@ 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_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)
 	target_link_libraries(nuttx_fs INTERFACE nuttx_net)
+	target_link_libraries(nuttx_net INTERFACE nuttx_mm)
 endif()

 file(RELATIVE_PATH PX4_BINARY_DIR_REL ${CMAKE_CURRENT_BINARY_DIR} ${PX4_BINARY_DIR})
@@ -107,13 +129,132 @@ 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(PX4_BINARY_DIR PX4_BINARY_DIR_CYG)

+if((DEFINED ENV{SIGNING_TOOL}) AND (NOT NUTTX_DIR MATCHES "external"))
+	set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR}/${PX4_BOARD}_unsigned.bin)
+
+	add_custom_command(OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_BOARD}.bin
+		COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_BOARD}.bin
+		DEPENDS ${PX4_BINARY_OUTPUT}
+		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+)
+else()
+set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_BOARD}.bin)
+endif()
+
+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=${PX4_BINARY_DIR_CYG}/NuttX/nuttx-config/scripts/${SCRIPT_PREFIX}memory.ld,--script=${PX4_BINARY_DIR_CYG}/NuttX/nuttx-config/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=${PX4_BINARY_DIR_CYG}/NuttX/nuttx-config/scripts/${SCRIPT_PREFIX}memory.ld,--script=${PX4_BINARY_DIR_CYG}/NuttX/nuttx-config/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(drivers_board
+	PRIVATE
+		nuttx_c
+		nuttx_mm
+		nuttx_fs
+)
+
+# nxsched_get_streams
+target_link_libraries(nuttx_c INTERFACE nuttx_sched)
+
 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_c INTERFACE nuttx_drivers nuttx_fs)
 target_link_libraries(nuttx_xx INTERFACE nuttx_c)
 target_link_libraries(nuttx_fs INTERFACE nuttx_c)

@@ -150,23 +291,13 @@ if (config_romfs_root)
 	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_BOARD}_unsigned.bin)
-
-	add_custom_command(OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_BOARD}.bin
-		COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_BOARD}.bin
-		DEPENDS ${PX4_BINARY_OUTPUT}
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-	)
-else()
-	set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_BOARD}.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
 )

+endif()
+
 # create .px4 with parameter and airframe metadata
 if (TARGET parameters_xml AND TARGET airframes_xml)

@@ -269,6 +269,72 @@ endif()

 add_custom_target(nuttx_builtin_list_target DEPENDS ${nuttx_builtin_list})

+if (NOT CONFIG_BUILD_FLAT)
+
+set(nuttx_kernel_builtin_list)
+
+set(KERNEL_BUILTIN_DIR ${PX4_BINARY_DIR}/NuttX/kernel_builtin)
+
+# force builtins regeneration and apps rebuild if nuttx or px4 configuration have changed
+add_custom_command(OUTPUT ${KERNEL_BUILTIN_DIR}/kernel_builtins_clean.stamp
+	WORKING_DIRECTORY ${KERNEL_BUILTIN_DIR}
+	COMMAND find . -name px4_\*.bdat -delete
+	COMMAND find . -name px4_\*.pdat -delete
+	COMMAND rm -f kernel_builtin_list.h
+	COMMAND ${CMAKE_COMMAND} -E touch kernel_builtins_clean.stamp
+	DEPENDS
+		nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h
+		px4_config_file_target ${PX4_CONFIG_FILE}
+	)
+
+add_custom_target(kernel_builtins_clean_target DEPENDS ${KERNEL_BUILTIN_DIR}/kernel_builtins_clean.stamp)
+
+foreach(module ${kernel_module_libraries})
+	get_target_property(MAIN ${module} MAIN)
+	get_target_property(STACK_MAIN ${module} STACK_MAIN)
+	get_target_property(PRIORITY ${module} PRIORITY)
+
+	if(MAIN)
+		add_custom_command(OUTPUT ${KERNEL_BUILTIN_DIR}/registry/px4_${MAIN}_kernel_main.bdat
+			WORKING_DIRECTORY  ${KERNEL_BUILTIN_DIR}
+			COMMAND echo "{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, ${MAIN}_main }," > registry/px4_${MAIN}_kernel_main.bdat
+			COMMAND echo "{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, launch_kmod_main }," > ${APPS_DIR}/builtin/registry/px4_${MAIN}_main.bdat
+			COMMAND ${CMAKE_COMMAND} -E touch registry/.kernel_updated
+			COMMAND ${CMAKE_COMMAND} -E touch ${APPS_DIR}/builtin/registry/.updated
+			DEPENDS
+				kernel_builtins_clean_target ${KERNEL_BUILTIN_DIR}/kernel_builtins_clean.stamp
+				builtins_clean_target ${PX4_BINARY_DIR}/NuttX/builtins_clean.stamp
+				nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h
+			VERBATIM
+			)
+		list(APPEND nuttx_kernel_builtin_list ${KERNEL_BUILTIN_DIR}/registry/px4_${MAIN}_kernel_main.bdat)
+
+		add_custom_command(OUTPUT ${KERNEL_BUILTIN_DIR}/registry/px4_${MAIN}_kernel_main.pdat
+			WORKING_DIRECTORY  ${KERNEL_BUILTIN_DIR}
+			COMMAND echo "int ${MAIN}_main(int argc, char *argv[]);" > registry/px4_${MAIN}_kernel_main.pdat
+			COMMAND ${CMAKE_COMMAND} -E touch registry/.kernel_updated
+			DEPENDS
+				kernel_builtins_clean_target
+			nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h
+			VERBATIM
+			)
+		list(APPEND nuttx_kernel_builtin_list ${KERNEL_BUILTIN_DIR}/registry/px4_${MAIN}_kernel_main.pdat)
+
+	endif()
+endforeach()
+
+add_custom_command(OUTPUT ${KERNEL_BUILTIN_DIR}/kernel_builtins.h ${KERNEL_BUILTIN_DIR}/kernel_builtin_prototypes.h
+	WORKING_DIRECTORY  ${KERNEL_BUILTIN_DIR}
+	COMMAND cat registry/*.bdat > kernel_builtins.h
+	COMMAND cat registry/*.pdat > kernel_builtin_prototypes.h
+	DEPENDS ${nuttx_kernel_builtin_list}
+)
+
+add_custom_target(nuttx_kernel_builtin_list_target DEPENDS ${KERNEL_BUILTIN_DIR}/kernel_builtins.h)
+
+add_dependencies(nuttx_builtin_list_target kernel_builtins_clean_target)
+endif() # NOT CONFIG_BUILD_FLAT
+
 # APPS

 # libapps.a
@@ -288,14 +354,21 @@ add_dependencies(nuttx_build nuttx_apps_build)
 target_link_libraries(nuttx_build INTERFACE nuttx_apps)

 # 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}/*)

 	add_custom_command(OUTPUT ${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} ${nuttx_build_options} --no-print-directory all TOPDIR=${NUTTX_DIR} KERNEL=${kernel} EXTRAFLAGS=${extra}
+		COMMAND make -C ${nuttx_lib_dir} ${nuttx_build_options} --no-print-directory ${nuttx_lib_target} TOPDIR=${NUTTX_DIR} KERNEL=${kernel} EXTRAFLAGS=${extra}
+
 		DEPENDS
 			${nuttx_lib_files}
 			nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h
@@ -309,19 +382,36 @@ function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra)
 	target_link_libraries(nuttx_build INTERFACE nuttx_${nuttx_lib})
 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(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)
+
+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()

 ###############################################################################
@@ -167,7 +167,9 @@ 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_build uorb_headers)

 endfunction()
@@ -39,4 +39,5 @@ px4_add_library(arch_bootloader
 target_link_libraries(arch_bootloader
 	PRIVATE
 		bootloader_lib
+		nuttx_arch
 )
@@ -34,7 +34,7 @@
 # skip for px4_layer support on an IO board
 if(NOT PX4_BOARD MATCHES "io-v2")

-	add_library(px4_layer
+	set(SRCS
 		board_crashdump.c
 		board_dma_alloc.c
 		board_fat_dma_alloc.c
@@ -49,19 +49,98 @@ if(NOT PX4_BOARD MATCHES "io-v2")
 		px4_mtd.cpp
 		px4_24xxxx_mtd.c
 	)
+
+	set(LIBS
+		arch_board_reset
+		arch_board_critmon
+		arch_hrt
+		arch_version
+		nuttx_xx
+		nuttx_sched
+	)
+
+if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
+
+	list(APPEND LIBS
+		nuttx_kc
+		nuttx_karch
+		nuttx_kmm
+	)
+
+	set(SRCS_USER
+		tasks.cpp
+		console_buffer_usr.cpp
+		px4_nuttx_impl.cpp
+		usr_mcu_version.cpp
+		usr_hrt.cpp
+		${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp
+		${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp
+		usr_reset.cpp
+	)
+
+	set(LIBS_USER
+		m
+		nuttx_c
+		nuttx_xx
+		nuttx_mm
+	)
+
+	add_library(px4_layer
+		${SRCS_USER}
+	)
+
 	target_link_libraries(px4_layer
 		PRIVATE
-			arch_board_reset
-			arch_board_critmon
-			arch_version
-			nuttx_apps
-			nuttx_sched
-			px4_work_queue
-			uORB
-		)
+			${LIBS_USER}
+	)
+
+	add_library(px4_kernel_layer
+		${SRCS}
+	)
+
+	target_link_libraries(px4_kernel_layer
+		PRIVATE
+			${LIBS}
+	)
+
+	target_compile_options(px4_kernel_layer PRIVATE -D__KERNEL__)
+
+	add_library(px4_board_ctrl
+		board_ctrl.c
+		kernel_modules.c
+	)
+
+	add_dependencies(px4_board_ctrl nuttx_context nuttx_kernel_builtin_list_target)
+
+	add_library(px4_userspace_init
+		px4_userspace_init.cpp
+	)
+
+	add_dependencies(px4_userspace_init nuttx_context)
+
+	add_dependencies(px4_kernel_layer prebuild_targets)
 else()
+	list(APPEND LIBS
+		nuttx_c
+		nuttx_arch
+		nuttx_mm
+	)
+
+	add_library(px4_layer
+		${SRCS}
+	)
+
+	target_link_libraries(px4_layer
+		PRIVATE
+			${LIBS}
+	)
+endif()
+
+else()
+
 	add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/common/empty.c)
 endif()
+
 add_dependencies(px4_layer prebuild_targets)

 add_subdirectory(gpio)
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_ctrl.c
+ *
+ * Provide a kernel-userspace boardctl_ioctl interface
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/sem.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform/board_ctrl.h>
+#include "board_config.h"
+
+#include <stdint.h>
+#include <errno.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_orb_dev.h>
+
+struct {
+	unsigned base;
+	ioctl_ptr_t ioctl_func;
+} ioctl_ptrs[] = {
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL}
+};
+#define MAX_IOCTL_PTRS (sizeof(ioctl_ptrs)/sizeof(ioctl_ptrs[0]))
+
+int launch_builtin_module(int argc, char *argv[]);
+
+/* internal functions */
+
+int px4_register_boardct_ioctl(unsigned base, ioctl_ptr_t func)
+{
+	unsigned i;
+	int ret = PX4_ERROR;
+
+	for (i = 0; i < MAX_IOCTL_PTRS; i++) {
+		if (ioctl_ptrs[i].base == 0) {
+			ioctl_ptrs[i].base = base;
+			ioctl_ptrs[i].ioctl_func = func;
+			ret = PX4_OK;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/************************************************************************************
+ * Name: board_ioctl
+ *
+ * Description:
+ *   px4 platform layer kernel-userspace interfaces
+ *
+ ************************************************************************************/
+
+int
+board_ioctl(unsigned int cmd, uintptr_t arg)
+{
+	px4_boardctl_t *kcmd = (px4_boardctl_t *)arg;
+	unsigned i;
+
+	if (cmd == PX4_KERNEL_CMD) {
+		/* Launch module on kernel side */
+		kcmd->ret = launch_builtin_module(kcmd->argc, kcmd->argv);
+
+	} else {
+		/* Run some other registered ioctl */
+		for (i = 0; i < MAX_IOCTL_PTRS; i++) {
+			if ((cmd & 0xFF00) == ioctl_ptrs[i].base) {
+				return ioctl_ptrs[i].ioctl_func(cmd, arg);
+			}
+		}
+
+		return -EINVAL;
+	}
+
+	return OK;
+}
@@ -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 */
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 Technology Innovation Institute. All rights reserved.
+ *                 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.
+ *
+ ****************************************************************************/
+
+#ifndef PX4_BOARD_CTRL_H_
+#define PX4_BOARD_CTRL_H_
+
+#define _PLATFORMIOCBASE		(0x4000)
+#define _PLATFORMIOC(_n)		(_PX4_IOC(_PLATFORMIOCBASE, _n))
+#define PX4_KERNEL_CMD			_PLATFORMIOC(1)
+
+typedef struct px4_boardctl {
+	int argc;
+	char **argv;
+	int ret;
+} px4_boardctl_t;
+
+typedef int (*ioctl_ptr_t)(unsigned int, unsigned long);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Function to register a px4 boardctl handler */
+int px4_register_boardct_ioctl(unsigned base, ioctl_ptr_t func);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+#pragma once
+
+#if defined(CONFIG_BUILD_FLAT)
+
+static inline int board_reset_request(int status)
+{
+	return board_reset(status);
+}
+
+static inline int board_poweroff_request(int status)
+{
+	return board_power_off(status);
+}
+
+#else
+
+__EXPORT int  board_reset_request(int status);
+__EXPORT int  board_poweroff_request(int status);
+
+#endif
+
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file kernel_modules.c
+ *
+ * Provide the kernel side module loading interface
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/sem.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/tasks.h>
+#include "board_config.h"
+
+#include <stdint.h>
+#include <errno.h>
+
+#include <nuttx/lib/builtin.h>
+
+#include <NuttX/kernel_builtin/kernel_builtin_prototypes.h>
+
+FAR const struct builtin_s g_kernel_builtins[] = {
+#include <NuttX/kernel_builtin/kernel_builtins.h>
+};
+
+const int g_n_kernel_builtins = sizeof(g_kernel_builtins) / sizeof(struct builtin_s);
+
+
+/* internal functions */
+
+__EXPORT int launch_builtin_module(int argc, char *argv[])
+{
+	int i;
+	FAR const struct builtin_s *builtin = NULL;
+
+	for (i = 0; i < g_n_kernel_builtins; i++) {
+		if (!strcmp(g_kernel_builtins[i].name, argv[0])) {
+			builtin = &g_kernel_builtins[i];
+			break;
+		}
+	}
+
+	if (builtin) {
+		/* This is running in the userspace thread, created by nsh, and
+		   called via boardctl. Just call the main directly */
+		builtin->main(argc, argv);
+		return OK;
+	}
+
+	return ENOENT;
+}
@@ -52,8 +52,45 @@
 # include <nuttx/i2c/i2c_master.h>
 #endif // CONFIG_I2C

+#if !defined(CONFIG_BUILD_FLAT)
+typedef CODE void (*initializer_t)(void);
+extern initializer_t _sinit;
+extern initializer_t _einit;
+extern uint32_t _stext;
+extern uint32_t _etext;
+
+static void cxx_initialize(void)
+{
+	initializer_t *initp;
+
+	/* Visit each entry in the initialization table */
+
+	for (initp = &_sinit; initp != &_einit; initp++) {
+		initializer_t initializer = *initp;
+
+		/* Make sure that the address is non-NULL and lies in the text
+		* region defined by the linker script.  Some toolchains may put
+		* NULL values or counts in the initialization table.
+		*/
+
+		if ((FAR void *)initializer >= (FAR void *)&_stext &&
+		    (FAR void *)initializer < (FAR void *)&_etext) {
+			initializer();
+		}
+	}
+}
+#endif
+
 int px4_platform_init()
 {
+	/* In protected/kernel build this is called from user space thread via
+	 *  BOARDCTL.
+	 *  In PX4 there are static C++ objects created also in kernel side;
+	 *  initialize them here
+	 */
+#if !defined(CONFIG_BUILD_FLAT)
+	cxx_initialize();
+#endif

 	int ret = px4_console_buffer_init();

@@ -0,0 +1,16 @@
+#include <drivers/drv_hrt.h>
+#include <lib/parameters/param.h>
+#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
+#include <uORB/uORB.h>
+#include <sys/boardctl.h>
+
+extern "C" void px4_userspace_init(void)
+{
+	hrt_init();
+
+	param_init();
+
+	px4::WorkQueueManagerStart();
+
+	uorb_start();
+}
@@ -42,6 +42,7 @@
 #include <px4_platform_common/tasks.h>

 #include <nuttx/board.h>
+#include <nuttx/kthread.h>

 #include <sys/wait.h>
 #include <stdbool.h>
@@ -67,8 +68,12 @@ int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_
 	clearenv();
 #endif

+#if !defined(__KERNEL__)
 	/* create the task */
 	int pid = task_create(name, priority, stack_size, entry, argv);
+#else
+	int pid = kthread_create(name, priority, stack_size, entry, argv);
+#endif

 	if (pid > 0) {
 		/* configure the scheduler */
@@ -88,7 +93,7 @@ int px4_task_delete(int pid)

 const char *px4_get_taskname(void)
 {
-#if CONFIG_TASK_NAME_SIZE > 0
+#if CONFIG_TASK_NAME_SIZE > 0 && (defined(__KERNEL__) || defined(CONFIG_BUILD_FLAT))
 	FAR struct tcb_s	*thisproc = nxsched_self();

 	return thisproc->name;
@@ -0,0 +1,255 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 usr_hrt.c
+ *
+ * Userspace High-resolution timer callouts and timekeeping.
+ *
+ * This can be used with nuttx userspace
+ *
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <sched.h>
+#include <errno.h>
+
+#include <assert.h>
+#include <debug.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <sys/boardctl.h>
+
+#ifdef CONFIG_DEBUG_HRT
+#  define hrtinfo _info
+#else
+#  define hrtinfo(x...)
+#endif
+
+
+/**
+ * Fetch a never-wrapping absolute time value in microseconds from
+ * some arbitrary epoch shortly after system start.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	hrt_abstime abstime = 0;
+	boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime);
+	return abstime;
+}
+
+/**
+ * Convert a timespec to absolute time
+ */
+hrt_abstime
+ts_to_abstime(const struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/**
+ * Convert absolute time to a timespec.
+ */
+void
+abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}
+
+/**
+ * Compare a time value with the current time as atomic operation
+ */
+hrt_abstime
+hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
+{
+	irqstate_t flags = px4_enter_critical_section();
+
+	hrt_abstime delta = hrt_absolute_time() - *then;
+
+	px4_leave_critical_section(flags);
+
+	return delta;
+}
+
+/**
+ * Store the absolute time in an interrupt-safe fashion
+ */
+void
+hrt_store_absolute_time(volatile hrt_abstime *t)
+{
+	irqstate_t flags = px4_enter_critical_section();
+	*t = hrt_absolute_time();
+	px4_leave_critical_section(flags);
+}
+
+/**
+ * Event dispatcher thread
+ */
+int
+event_thread(int argc, char *argv[])
+{
+	struct hrt_call *entry = NULL;
+
+	while (1) {
+		/* Wait for hrt tick */
+		boardctl(HRT_WAITEVENT, (uintptr_t)&entry);
+
+		/* HRT event received, dispatch */
+		if (entry) {
+			entry->usr_callout(entry->usr_arg);
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Initialise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1000, event_thread, NULL);
+}
+
+/**
+ * Call callout(arg) after interval has elapsed.
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = delay;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_AFTER, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Call callout(arg) at calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = calltime;
+	ioc_parm.interval = 0;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_AT, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Call callout(arg) every period.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = delay;
+	ioc_parm.interval = interval;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_EVERY, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	boardctl(HRT_CANCEL, (uintptr_t)entry);
+}
+
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+/**
+ * If this returns true, the call has been invoked and removed from the callout list.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+latency_info_t
+get_latency(uint16_t bucket_idx, uint16_t counter_idx)
+{
+	latency_boardctl_t latency_ioc;
+	latency_ioc.bucket_idx = bucket_idx;
+	latency_ioc.counter_idx = counter_idx;
+	latency_ioc.latency = {0, 0};
+	boardctl(HRT_GET_LATENCY, (uintptr_t)&latency_ioc);
+	return latency_ioc.latency;
+}
+
+void reset_latency_counters()
+{
+	boardctl(HRT_RESET_LATENCY, NULL);
+}
@@ -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;
+}
+
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   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_reset.cpp
+ * Implementation of nuttx userspace reset api
+ */
+
+#include <sys/boardctl.h>
+
+__EXPORT int board_reset_request(int status)
+{
+#if defined(CONFIG_BOARDCTL_RESET)
+	boardctl(BOARDIOC_RESET, status);
+#endif
+
+	/* Was not able to reset */
+	return -1;
+}
+
+
+__EXPORT int board_poweroff_request(int status)
+{
+#if defined(CONFIG_BOARDCTL_POWEROFF)
+	boardctl(BOARDIOC_POWEROFF, status);
+#endif
+
+	/* Was not able to power off */
+	return -1;
+}
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -62,7 +62,6 @@
 #include <board_config.h>
 #include <drivers/drv_hrt.h>

-
 #include "stm32_gpio.h"
 #include "stm32_tim.h"

@@ -72,6 +71,23 @@
 #  define hrtinfo(x...)
 #endif

+#ifdef __PX4_NUTTX
+#include <px4_platform_common/defines.h>
+#include <px4_platform/board_ctrl.h>
+#include <px4_platform_common/sem.h>
+
+static px4_sem_t g_wait_sem;
+static struct hrt_call *next_hrt_entry;
+
+void hrt_usr_call(void *arg)
+{
+	// These calls will be made one by one by hrt, there is no race in here
+	next_hrt_entry = (struct hrt_call *)arg;
+	px4_sem_post(&g_wait_sem);
+}
+
+#endif
+
 #ifdef HRT_TIMER

 /* HRT configuration */
@@ -275,6 +291,8 @@ static void		hrt_call_enter(struct hrt_call *entry);
 static void		hrt_call_reschedule(void);
 static void		hrt_call_invoke(void);

+
+int hrt_ioctl(unsigned int cmd, unsigned long arg);
 /*
 * Specific registers and bits used by PPM sub-functions
 */
@@ -758,6 +776,16 @@ hrt_init(void)
 	/* configure the PPM input pin */
 	px4_arch_configgpio(GPIO_PPM_IN);
 #endif
+
+#if !defined(CONFIG_BUILD_FLAT)
+	/* Create a semaphore for handling hrt driver callbacks */
+	px4_sem_init(&g_wait_sem, 0, 0);
+	/* this is a signalling semaphore */
+	px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE);
+
+	/* register ioctl callbacks */
+	px4_register_boardct_ioctl(_HRTIOCBASE, hrt_ioctl);
+#endif
 }

 /**
@@ -1003,4 +1031,79 @@ hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
 	entry->deadline = hrt_absolute_time() + delay;
 }

+#if !defined(CONFIG_BUILD_FLAT)
+/* These functions are inlined in all but NuttX protected/kernel builds */
+
+latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx)
+{
+	latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]};
+	return ret;
+}
+
+void reset_latency_counters(void)
+{
+	for (int i = 0; i <= get_latency_bucket_count(); i++) {
+		latency_counters[i] = 0;
+	}
+}
+
+/* board_ioctl interface for user-space hrt driver */
+int
+hrt_ioctl(unsigned int cmd, unsigned long arg)
+{
+	hrt_boardctl_t *h = (hrt_boardctl_t *)arg;
+
+	switch (cmd) {
+	case HRT_WAITEVENT:
+		/* The user side thread calling this is at highest priority, there
+		 * is no race in here
+		 */
+		px4_sem_wait(&g_wait_sem);
+		*(struct hrt_call **)arg = next_hrt_entry;
+		break;
+
+	case HRT_ABSOLUTE_TIME:
+		*(hrt_abstime *)arg = hrt_absolute_time();
+		break;
+
+	case HRT_CALL_AFTER:
+		hrt_call_after(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CALL_AT:
+		hrt_call_at(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CALL_EVERY:
+		hrt_call_every(h->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CANCEL:
+		if (h && h->entry) {
+			hrt_cancel(h->entry);
+
+		} else {
+			PX4_ERR("HRT_CANCEL called with NULL entry");
+		}
+
+		break;
+
+	case HRT_GET_LATENCY: {
+			latency_boardctl_t *latency = (latency_boardctl_t *)arg;
+			latency->latency = get_latency(latency->bucket_idx, latency->counter_idx);
+		}
+		break;
+
+	case HRT_RESET_LATENCY:
+		reset_latency_counters();
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return OK;
+}
+#endif
+
 #endif /* HRT_TIMER */
@@ -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
@@ -72,7 +72,7 @@ void init_print_load(struct print_load_s *s)
 	s->new_time = hrt_absolute_time();
 	s->interval_start_time = s->new_time;

-	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+	for (size_t i = 0; i < sizeof(s->last_times) / sizeof(s->last_times[0]); i++) {
 		s->last_times[i] = 0;
 	}

@@ -81,20 +81,11 @@ void init_print_load(struct print_load_s *s)

 void print_load(int fd, struct print_load_s *print_state)
 {
-	char clear_line[] = CL;
-
-	/* print system information */
-	if (fd == 1) {
-		dprintf(fd, "\033[H"); /* move cursor home and clear screen */
-
-	} else {
-		memset(clear_line, 0, sizeof(clear_line));
-	}
-
 #if defined(__PX4_LINUX) || defined(__PX4_CYGWIN) || defined(__PX4_QURT)
+	char clear_line[] = CL;
 	dprintf(fd, "%sTOP NOT IMPLEMENTED ON LINUX, QURT, WINDOWS (ONLY ON NUTTX, APPLE)\n", clear_line);
-
 #elif defined(__PX4_DARWIN)
+	char clear_line[] = CL;
 	pid_t pid = getpid();   //-- this is the process id you need info for
 	task_t task_handle;
 	task_for_pid(mach_task_self(), pid, &task_handle);
@@ -118,6 +109,14 @@ void print_load(int fd, struct print_load_s *print_state)
 	thread_basic_info_t basic_info_th;
 	uint32_t stat_thread = 0;

+	/* print system information */
+	if (fd == 1) {
+		dprintf(fd, "\033[H"); /* move cursor home and clear screen */
+
+	} else {
+		memset(clear_line, 0, sizeof(clear_line));
+	}
+
 	// get all threads of the PX4 main task
 	kr = task_threads(task_handle, &thread_list, &th_cnt);

@@ -271,7 +271,10 @@ CameraTrigger::CameraTrigger() :
 	param_get(_p_distance, &_distance);
 	param_get(_p_mode, (int32_t *)&_trigger_mode);
 	param_get(_p_interface, (int32_t *)&_camera_interface_mode);
-	param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
+
+	if (_p_cam_cap_fback != PARAM_INVALID) {
+		param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
+	}

 	switch (_camera_interface_mode) {
 #ifdef __PX4_NUTTX
@@ -76,6 +76,10 @@ typedef struct hrt_call {
 	hrt_abstime		period;
 	hrt_callout		callout;
 	void			*arg;
+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+	hrt_callout		usr_callout;
+	void			*usr_arg;
+#endif
 } *hrt_call_t;


@@ -84,6 +88,38 @@ extern const uint16_t latency_bucket_count;
 extern const uint16_t latency_buckets[LATENCY_BUCKET_COUNT];
 extern uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];

+
+typedef struct hrt_boardctl {
+	hrt_call_t		entry;
+	hrt_abstime		time; /* delay or calltime */
+	hrt_abstime		interval;
+	hrt_callout		callout;
+	void			*arg;
+} hrt_boardctl_t;
+
+typedef struct latency_info {
+	uint16_t		bucket;
+	uint32_t		counter;
+} latency_info_t;
+
+typedef struct latency_boardctl {
+	uint16_t		bucket_idx;
+	uint16_t		counter_idx;
+	latency_info_t	latency;
+} latency_boardctl_t;
+
+#define _HRTIOCBASE		(0x1000)
+#define _HRTIOC(_n)		(_PX4_IOC(_HRTIOCBASE, _n))
+
+#define HRT_WAITEVENT		_HRTIOC(1)
+#define HRT_ABSOLUTE_TIME	_HRTIOC(2)
+#define HRT_CALL_AFTER		_HRTIOC(3)
+#define HRT_CALL_AT		_HRTIOC(4)
+#define HRT_CALL_EVERY		_HRTIOC(5)
+#define HRT_CANCEL		_HRTIOC(6)
+#define HRT_GET_LATENCY		_HRTIOC(7)
+#define HRT_RESET_LATENCY	_HRTIOC(8)
+
 /**
 * Get absolute time in [us] (does not wrap).
 */
@@ -208,8 +244,36 @@ static inline void px4_lockstep_progress(int component) { }
 static inline void px4_lockstep_wait_for_components(void) { }
 #endif /* defined(ENABLE_LOCKSTEP_SCHEDULER) */

-__END_DECLS

+/* Latency counter functions */
+
+static inline uint16_t get_latency_bucket_count(void) { return LATENCY_BUCKET_COUNT; }
+
+#if defined(CONFIG_BUILD_FLAT) || !defined(__PX4_NUTTX)
+
+static inline latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx)
+{
+	latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]};
+	return ret;
+}
+
+static inline void reset_latency_counters(void)
+{
+	for (int i = 0; i <= get_latency_bucket_count(); i++) {
+		latency_counters[i] = 0;
+	}
+}
+
+#else
+
+/* NuttX protected/kernel build interface functions */
+
+latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx);
+void reset_latency_counters(void);
+
+#endif
+
+__END_DECLS


 #ifdef	__cplusplus
@@ -182,6 +182,7 @@
 #define DRV_DIST_DEVTYPE_GY_US42       0x9C

 #define DRV_BAT_DEVTYPE_BATMON_SMBUS   0x9d
+#define DRV_POWER_DEVTYPE_INA238       0x9E
 #define DRV_GPIO_DEVTYPE_MCP23009      0x9F

 #define DRV_GPS_DEVTYPE_ASHTECH 0xA0
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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__ina238
+	MAIN ina238
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		ina238_main.cpp
+		ina238.cpp
+	DEPENDS
+		battery
+		px4_work_queue
+	)
@@ -0,0 +1,296 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Driver for the I2C attached INA238
+ */
+
+#include "ina238.h"
+
+
+INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
+	I2C(config),
+	ModuleParams(nullptr),
+	I2CSPIDriver(config),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")),
+	_collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")),
+	_battery(battery_index, this, INA238_SAMPLE_INTERVAL_US)
+{
+	float fvalue = DEFAULT_MAX_CURRENT;
+	_max_current = fvalue;
+	param_t ph = param_find("INA238_CURRENT");
+
+	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
+		_max_current = fvalue;
+	}
+
+	_range = _max_current > (DEFAULT_MAX_CURRENT - 1.0f) ? INA238_ADCRANGE_HIGH : INA238_ADCRANGE_LOW;
+
+	fvalue = DEFAULT_SHUNT;
+	_rshunt = fvalue;
+	ph = param_find("INA238_SHUNT");
+
+	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
+		_rshunt = fvalue;
+	}
+
+	_current_lsb = _max_current / INA238_DN_MAX;
+
+	// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
+	_battery.updateBatteryStatus(
+		hrt_absolute_time(),
+		0.0,
+		0.0,
+		false,
+		battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+		0,
+		0.0
+	);
+}
+
+INA238::~INA238()
+{
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_collection_errors);
+}
+
+int INA238::read(uint8_t address, uint16_t &data)
+{
+	// read desired little-endian value via I2C
+	uint16_t received_bytes;
+	const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
+
+	if (ret == PX4_OK) {
+		data = swap16(received_bytes);
+
+	} else {
+		perf_count(_comms_errors);
+		PX4_DEBUG("i2c::transfer returned %d", ret);
+	}
+
+	return ret;
+}
+
+int INA238::write(uint8_t address, uint16_t value)
+{
+	uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)};
+	return transfer(data, sizeof(data), nullptr, 0);
+}
+
+int INA238::init()
+{
+	int ret = PX4_ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != PX4_OK) {
+		return ret;
+	}
+
+	write(INA238_REG_CONFIG, (uint16_t)(INA238_RST_RESET | _range));
+
+	uint16_t shunt_calibration = static_cast<uint16_t>(INA238_CONST * _current_lsb * _rshunt);
+
+	if (_range == INA238_ADCRANGE_LOW) {
+		shunt_calibration *= 4;
+	}
+
+	if (write(INA238_REG_SHUNTCAL, shunt_calibration) < 0) {
+		return -3;
+	}
+
+	// Set the CONFIG for max I
+	write(INA238_REG_CONFIG, (uint16_t) _range);
+
+	// Start ADC continous mode here
+	ret = write(INA238_REG_ADCCONFIG, (uint16_t)INA238_ADCCONFIG);
+
+	start();
+	_sensor_ok = true;
+
+	_initialized = ret == PX4_OK;
+	return ret;
+}
+
+int INA238::force_init()
+{
+	int ret = init();
+
+	start();
+
+	return ret;
+}
+
+int INA238::probe()
+{
+	uint16_t value{0};
+
+	if (read(INA238_MANUFACTURER_ID, value) != PX4_OK || value != INA238_MFG_ID_TI) {
+		PX4_DEBUG("probe mfgid %d", value);
+		return -1;
+	}
+
+	if (read(INA238_DEVICE_ID, value) != PX4_OK || (
+		    INA238_DEVICEID(value) != INA238_MFG_DIE
+	    )) {
+		PX4_DEBUG("probe die id %d", value);
+		return -1;
+	}
+
+	return PX4_OK;
+}
+
+
+int INA238::collect()
+{
+	perf_begin(_sample_perf);
+
+	if (_parameter_update_sub.updated()) {
+		// Read from topic to clear updated flag
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
+
+		updateParams();
+	}
+
+	// read from the sensor
+	// Note: If the power module is connected backwards, then the values of _current will be negative but otherwise valid.
+	bool success{true};
+	int16_t bus_voltage{0};
+	int16_t current{0};
+
+	success = success && (read(INA238_REG_VSBUS, bus_voltage) == PX4_OK);
+	success = success && (read(INA238_REG_CURRENT, current) == PX4_OK);
+
+	if (!success) {
+		PX4_DEBUG("error reading from sensor");
+		bus_voltage = current = 0;
+	}
+
+	_actuators_sub.copy(&_actuator_controls);
+
+	_battery.updateBatteryStatus(
+		hrt_absolute_time(),
+		(float) bus_voltage * INA238_VSCALE,
+		(float) current * _current_lsb,
+		success,
+		battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+		0,
+		_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]
+	);
+
+	perf_end(_sample_perf);
+
+	if (success) {
+		return PX4_OK;
+
+	} else {
+		return PX4_ERROR;
+	}
+}
+
+void INA238::start()
+{
+	ScheduleClear();
+
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+
+	_measure_interval = INA238_CONVERSION_INTERVAL;
+
+	/* schedule a cycle to start things */
+	ScheduleDelayed(5);
+}
+
+void INA238::RunImpl()
+{
+	if (_initialized) {
+		if (_collect_phase) {
+			/* perform collection */
+			if (collect() != PX4_OK) {
+				perf_count(_collection_errors);
+				/* if error restart the measurement state machine */
+				start();
+				return;
+			}
+
+			/* next phase is measurement */
+			_collect_phase = true;
+
+			if (_measure_interval > INA238_CONVERSION_INTERVAL) {
+				/* schedule a fresh cycle call when we are ready to measure again */
+				ScheduleDelayed(_measure_interval - INA238_CONVERSION_INTERVAL);
+				return;
+			}
+		}
+
+		/* next phase is collection */
+		_collect_phase = true;
+
+		/* schedule a fresh cycle call when the measurement is done */
+		ScheduleDelayed(INA238_CONVERSION_INTERVAL);
+
+	} else {
+		_battery.updateBatteryStatus(
+			hrt_absolute_time(),
+			0.0f,
+			0.0f,
+			false,
+			battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+			0,
+			0.0f
+		);
+
+		if (init() != PX4_OK) {
+			ScheduleDelayed(INA238_INIT_RETRY_INTERVAL_US);
+		}
+	}
+}
+
+void INA238::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	if (_initialized) {
+		perf_print_counter(_sample_perf);
+		perf_print_counter(_comms_errors);
+
+		printf("poll interval:  %u \n", _measure_interval);
+
+	} else {
+		PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
+			 INA238_INIT_RETRY_INTERVAL_US / 1000);
+	}
+}
@@ -0,0 +1,370 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <drivers/device/i2c.h>
+#include <lib/perf/perf_counter.h>
+#include <battery/battery.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+
+using namespace time_literals;
+
+/* Configuration Constants */
+#define INA238_BASEADDR 	            0x45 /* 7-bit address. 8-bit address is 0x45 */
+// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to
+// connect to the INA238 every this many microseconds
+#define INA238_INIT_RETRY_INTERVAL_US       500000
+
+/* INA238 Registers addresses */
+#define INA238_REG_CONFIG                   (0x00)
+#define INA238_REG_ADCCONFIG                (0x01)
+#define INA238_REG_SHUNTCAL                 (0x02)
+#define INA238_REG_SHUNTTEMPCO              (0x03)
+#define INA238_REG_VSHUNT                   (0x04)
+#define INA238_REG_VSBUS                    (0x05)
+#define INA238_REG_DIETEMP                  (0x06)
+#define INA238_REG_CURRENT                  (0x07)
+#define INA238_REG_POWER                    (0x08)
+#define INA238_REG_ENERGY                   (0x09)
+#define INA238_REG_CHARGE                   (0x0a)
+#define INA238_REG_DIAG_ALRT                (0x0b)
+#define INA238_REG_SOVL                     (0x0c)
+#define INA238_REG_SUVL                     (0x0d)
+#define INA238_REG_BOVL                     (0x0e)
+#define INA238_REG_BUVL                     (0x0f)
+#define INA238_REG_TEMP_LIMIT               (0x10)
+#define INA238_REG_TPWR_LIMIT               (0x11)
+#define INA238_MANUFACTURER_ID              (0x3e)
+#define INA238_DEVICE_ID                    (0x3f)
+
+#define INA238_MFG_ID_TI                    (0x5449) // TI
+#define INA238_MFG_DIE                      (0x238) // INA237, INA238
+
+/* INA238 Configuration (CONFIG) 16-bit Register (Address = 0h) [reset = 0h] */
+#define INA238_ADCRANGE_SHIFTS              (4)
+#define INA238_ADCRANGE_MASK                (1 << INA238_ADCRANGE_SHIFTS)
+#define INA238_ADCRANGE_LOW                 (1 << INA238_ADCRANGE_SHIFTS) // ± 40.96 mV
+#define INA238_ADCRANGE_HIGH                (0 << INA238_ADCRANGE_SHIFTS) // ±163.84 mV
+#define INA238_TEMPCOMP_SHIFTS              (5)
+#define INA238_TEMPCOMP_MASK                (1 << INA238_TEMPCOMP_SHIFTS)
+#define INA238_TEMPCOMP_ENABLE              (1 << INA238_TEMPCOMP_SHIFTS)
+#define INA238_TEMPCOMP_DISABLE             (0 << INA238_TEMPCOMP_SHIFTS)
+
+#define INA238_CONVDLY_SHIFTS               (6)
+#define INA238_CONVDLY_MASK                 (0xff << INA238_CONVDLY_SHIFTS)
+#define INA238_CONVDLY2MS(n)                ((n)  << INA238_CONVDLY_SHIFTS)
+
+#define INA238_RSTACC_SHIFTS                (14)
+#define INA238_RSTACC_MASK                  (1 << INA238_RSTACC_SHIFTS)
+#define INA238_RSTACC_CLEAR                 (1 << INA238_RSTACC_SHIFTS)
+#define INA238_RSTACC_NORMAL                (0 << INA238_RSTACC_SHIFTS)
+
+#define INA238_RST_SHIFTS                   (15)
+#define INA238_RST_MASK                     (1 << INA238_RST_SHIFTS)
+#define INA238_RST_RESET                    (1 << INA238_RST_SHIFTS)
+#define INA238_RST_NORMAL                   (0 << INA238_RST_SHIFTS)
+
+/* INA238 ADC Configuration (ADC_CONFIG) 16-bit  Register (Address = 1h) [reset = FB68h] */
+#define INA238_MODE_SHIFTS                  (12)
+#define INA238_MODE_MASK                    (0xf << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUTDOWN_TRIG           (0 << INA238_MODE_SHIFTS)
+#define INA238_MODE_BUS_TRIG                (1 << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUNT_TRIG              (2 << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUNT_BUS_TRIG          (3 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_TRIG               (4 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_BUS_TRIG           (5 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_SHUNT_TRIG         (6 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_SHUNT_BUS_TRIG     (7 << INA238_MODE_SHIFTS)
+
+#define INA238_MODE_SHUTDOWN_CONT           (8 << INA238_MODE_SHIFTS)
+#define INA238_MODE_BUS_CONT                (9 << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUNT_CONT              (10 << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUNT_BUS_CONT          (11 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_CONT               (12 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_BUS_CONT           (13 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_SHUNT_CONT         (14 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_SHUNT_BUS_CONT     (15 << INA238_MODE_SHIFTS)
+
+#define INA238_VBUSCT_SHIFTS                (9)
+#define INA238_VBUSCT_MASK                  (7 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_50US                  (0 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_84US                  (1 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_150US                 (2 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_280US                 (3 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_540US                 (4 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_1052US                (5 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_2074US                (6 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_4170US                (7 << INA238_VBUSCT_SHIFTS)
+
+#define INA238_VSHCT_SHIFTS                 (6)
+#define INA238_VSHCT_MASK                   (7 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_50US                   (0 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_84US                   (1 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_150US                  (2 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_280US                  (3 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_540US                  (4 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_1052US                 (5 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_2074US                 (6 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_4170US                 (7 << INA238_VSHCT_SHIFTS)
+
+#define INA238_VTCT_SHIFTS                  (3)
+#define INA238_VTCT_MASK                    (7 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_50US                    (0 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_84US                    (1 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_150US                   (2 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_280US                   (3 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_540US                   (4 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_1052US                  (5 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_2074US                  (6 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_4170US                  (7 << INA238_VTCT_SHIFTS)
+
+#define INA238_AVERAGES_SHIFTS              (0)
+#define INA238_AVERAGES_MASK                (7 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_1                   (0 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_4                   (1 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_16                  (2 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_64                  (3 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_128                 (4 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_256                 (5 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_512                 (6 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_1024                (7 << INA238_AVERAGES_SHIFTS)
+
+#define INA238_ADCCONFIG (INA238_MODE_TEMP_SHUNT_BUS_CONT | INA238_VBUSCT_540US | INA238_VSHCT_540US | INA238_VTCT_540US |INA238_AVERAGES_64)
+
+/* INA238 Shunt Calibration (SHUNT_CAL) 16-bit Register (Address = 2h) [reset = 1000h] */
+
+#define INA238_CURRLSB_SHIFTS               (0)
+#define INA238_CURRLSB_MASK                 (0x7fff << INA238_CURRLSB_SHIFTS)
+
+/* INA238 Shunt Temperature Coefficient (SHUNT_TEMPCO) 16-bit Register (Address = 3h) [reset = 0h] */
+
+#define INA238_TEMPCO_SHIFTS                (0)
+#define INA238_TEMPCO_MASK                  (0x1fff << INA238_TEMPCO_SHIFTS)
+
+/* INA238 Shunt Voltage Measurement (VSHUNT) 24-bit Register (Address = 4h) [reset = 0h] */
+
+#define INA238_VSHUNT_SHIFTS                (4)
+#define INA238_VSHUNT_MASK                  (UINT32_C(0xffffff) << INA238_VSHUNT_SHIFTS)
+
+/* INA238 Bus Voltage Measurement (VBUS) 24-bit Register (Address = 5h) [reset = 0h] */
+
+#define INA238_VBUS_SHIFTS                  (4)
+#define INA238_VBUS_MASK                    (UINT32_C(0xffffff) << INA238_VBUS_SHIFTS)
+
+/* INA238 Temperature Measurement (DIETEMP) 16-bit Register (Address = 6h) [reset = 0h] */
+
+#define INA238_DIETEMP_SHIFTS               (0)
+#define INA238_DIETEMP_MASK                 (0xffff << INA238_DIETEMP_SHIFTS)
+
+/* INA238 Current Result (CURRENT) 24-bit Register (Address = 7h) [reset = 0h] */
+
+#define INA238_CURRENT_SHIFTS               (4)
+#define INA238_CURRENT_MASK                 (UINT32_C(0xffffff) << INA238_CURRENT_SHIFTS)
+
+/* INA238 Power Result (POWER) 24-bit Register (Address = 8h) [reset = 0h] */
+
+#define INA238_POWER_SHIFTS                 (0)
+#define INA238_POWER_MASK                   (UINT32_C(0xffffff) << INA238_POWER_SHIFTS)
+
+/* INA238 Energy Result (ENERGY) 40-bit Register (Address = 9h) [reset = 0h] */
+
+#define INA238_ENERGY_SHIFTS                 (0)
+#define INA238_ENERGY_MASK                   (UINT64_C(0xffffffffff) << INA238_ENERGY_SHIFTS)
+
+/* INA238 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */
+
+#define INA238_CHARGE_SHIFTS                (0)
+#define INA238_CHARGE_MASK                  (UINT64_C(0xffffffffff) << INA238_CHARGE_SHIFTS)
+
+
+/* INA238 Diagnostic Flags and Alert (DIAG_ALRT) 16-bit Register (Address = Bh) [reset = 0001h] */
+
+#define INA238_MEMSTAT                      (1 << 0)  // This bit is set to 0 if a checksum error is detected in the device trim memory space
+#define INA238_CNVRF                        (1 << 1)  // This bit is set to 1 if the conversion is completed. When ALATCH =1 this bit is cleared by reading the register or starting a new triggered conversion.
+#define INA238_POL                          (1 << 2)  // This bit is set to 1 if the power measurement exceeds the threshold limit in the power limit register.
+#define INA238_BUSUL                        (1 << 3)  // This bit is set to 1 if the bus voltage measurement falls below the threshold limit in the bus under-limit register.
+#define INA238_BUSOL                        (1 << 4)  // This bit is set to 1 if the bus voltage measurement exceeds the threshold limit in the bus over-limit register.
+#define INA238_SHNTUL                       (1 << 5)  // This bit is set to 1 if the shunt voltage measurement falls below the threshold limit in the shunt under-limit register
+#define INA238_SHNTOL                       (1 << 6)  // This bit is set to 1 if the shunt voltage measurement exceeds the threshold limit in the shunt over-limit register.
+#define INA238_TMPOL                        (1 << 7)  // This bit is set to 1 if the temperature measurement exceeds the threshold limit in the temperature over-limit register.
+#define INA238_MATHOF                       (1 << 9)  // This bit is set to 1 if an arithmetic operation resulted in an overflow error.
+#define INA238_CHARGEOF                     (1 << 10) // This bit indicates the health of the CHARGE register. If the 40 bit CHARGE register has overflowed this bit is set to 1.
+#define INA238_ENERGYOF                     (1 << 11) // This bit indicates the health of the ENERGY register. If the 40 bit ENERGY register has overflowed this bit is set to 1.
+#define INA238_APOL                         (1 << 12) // Alert Polarity bit sets the Alert pin polarity.
+#define INA238_SLOWALER                     (1 << 13) // ALERT function is asserted on the completed averaged value.  This gives the flexibility to delay the ALERT after the averaged value.
+#define INA238_CNVR                         (1 << 14) // Setting this bit high configures the Alert pin to be asserted when the Conversion Ready Flag (bit 1) is asserted, indicating that a conversion cycle has completed
+#define INA238_ALATCH                       (1 << 15) // When the Alert Latch Enable bit is set to Transparent mode, the Alert pin and Flag bit reset to the idle state when the fault has been
+// cleared. When the Alert Latch Enable bit is set to Latch mode, the Alert pin and Alert Flag bit remain active following a fault until
+// the DIAG_ALRT Register has been read.
+
+/* Shunt Overvoltage Threshold (SOVL) 16-bit Register (Address = Ch) [reset = 7FFFh] */
+
+#define INA238_SOVL_SHIFTS                   (0)
+#define INA238_SOVL_MASK                     (0xffff << INA238_SOVL_SHIFTS)
+
+/* Shunt Undervoltage Threshold (SUVL) 16-bit Register (Address = Dh) [reset = 8000h] */
+
+#define INA238_SUVL_SHIFTS                   (0)
+#define INA238_SUVL_MASK                     (0xffff << INA238_SUVL_SHIFTS)
+
+/* Bus Overvoltage Threshold (BOVL) 16-bit Register (Address = Eh) [reset = 7FFFh] */
+
+#define INA238_BOVL_SHIFTS                   (0)
+#define INA238_BOVL_MASK                     (0xffff << INA238_BOVL_SHIFTS)
+
+/* Bus Undervoltage Threshold (BUVL) 16-bit Register (Address = Fh) [reset = 0h] */
+
+#define INA238_BUVL_SHIFTS                   (0)
+#define INA238_BUVL_MASK                     (0xffff << INA238_BUVL_SHIFTS)
+
+/* Temperature Over-Limit Threshold (TEMP_LIMIT) 16-bit Register (Address = 10h) [reset = 7FFFh */
+
+#define INA238_TEMP_LIMIT_SHIFTS             (0)
+#define INA238_TEMP_LIMIT_MASK               (0xffff << INA238_TEMP_LIMIT_SHIFTS)
+
+/* Power Over-Limit Threshold (PWR_LIMIT) 16-bit Register (Address = 11h) [reset = FFFFh] */
+
+#define INA238_POWER_LIMIT_SHIFTS            (0)
+#define INA238_POWER_LIMIT_MASK              (0xffff << INA238_POWER_LIMIT_SHIFTS)
+
+/* Manufacturer ID (MANUFACTURER_ID) 16-bit Register (Address = 3Eh) [reset = 5449h] */
+
+/* Device ID (DEVICE_ID) 16-bit Register (Address = 3Fh) [reset = 2380h] */
+
+#define INA238_DEVICE_REV_ID_SHIFTS          (0)
+#define INA238_DEVICE_REV_ID_MASK            (0xf << INA238_DEVICE_REV_ID_SHIFTS)
+#define INA238_DEVICEREV_ID(v)               (((v) & INA238_DEVICE_REV_ID_MASK) >> INA238_DEVICE_REV_ID_SHIFTS)
+#define INA238_DEVICE_ID_SHIFTS              (4)
+#define INA238_DEVICE_ID_MASK                (0xfff << INA238_DEVICE_ID_SHIFTS)
+#define INA238_DEVICEID(v)                   (((v) & INA238_DEVICE_ID_MASK) >> INA238_DEVICE_ID_SHIFTS)
+
+#define INA238_SAMPLE_FREQUENCY_HZ           10
+#define INA238_SAMPLE_INTERVAL_US            (1_s / INA238_SAMPLE_FREQUENCY_HZ)
+#define INA238_CONVERSION_INTERVAL           (INA238_SAMPLE_INTERVAL_US - 7)
+#define INA238_DN_MAX                        32768.0f   /* 2^15 */
+#define INA238_CONST                         819.2e6f  /* is an internal fixed value used to ensure scaling is maintained properly  */
+#define INA238_VSCALE                        3.125e-03f  /* LSB of voltage is 3.1255 mV/LSB */
+
+
+#define DEFAULT_MAX_CURRENT                  327.68f    /* Amps */
+#define DEFAULT_SHUNT                        0.0003f   /* Shunt is 300 uOhm */
+
+#define swap16(w)                            __builtin_bswap16((w))
+#define swap32(d)                            __builtin_bswap32((d))
+#define swap64(q)                            __builtin_bswap64((q))
+
+class INA238 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA238>
+{
+public:
+	INA238(const I2CSPIDriverConfig &config, int battery_index);
+	virtual ~INA238();
+
+	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+
+	/**
+	 * Tries to call the init() function. If it fails, then it will schedule to retry again in
+	 * INA238_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds.
+	 *
+	 * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise.
+	 */
+	int force_init();
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void print_status() override;
+
+protected:
+	int probe() override;
+
+private:
+	bool _sensor_ok{false};
+	unsigned int _measure_interval{0};
+	bool _collect_phase{false};
+	bool _initialized{false};
+
+	perf_counter_t _sample_perf;
+	perf_counter_t _comms_errors;
+	perf_counter_t _collection_errors;
+
+	// Configuration state, computed from params
+	float _max_current;
+	float _rshunt;
+	float _current_lsb;
+	int16_t _range;
+
+	actuator_controls_s _actuator_controls{};
+
+	Battery _battery;
+	uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	int read(uint8_t address, uint16_t &data);
+	int write(uint8_t address, uint16_t data);
+
+	int read(uint8_t address, int16_t &data)
+	{
+		return read(address, (uint16_t &)data);
+	}
+
+	int write(uint8_t address, int16_t data)
+	{
+		return write(address, (uint16_t)data);
+	}
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void start();
+
+	int collect();
+
+};
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   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/getopt.h>
+#include <px4_platform_common/module.h>
+
+#include "ina238.h"
+
+I2CSPIDriverBase *INA238::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
+{
+	INA238 *instance = new INA238(config, config.custom1);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (config.keep_running) {
+		if (instance->force_init() != PX4_OK) {
+			PX4_INFO("Failed to init INA238 on bus %d, but will try again periodically.", config.bus);
+		}
+
+	} else if (instance->init() != PX4_OK) {
+		delete instance;
+		return nullptr;
+	}
+
+	return instance;
+}
+
+void
+INA238::print_usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Driver for the INA238 power monitor.
+
+Multiple instances of this driver can run simultaneously, if each instance has a separate bus OR I2C address.
+
+For example, one instance can run on Bus 2, address 0x45, and one can run on Bus 2, address 0x45.
+
+If the INA238 module is not powered, then by default, initialization of the driver will fail. To change this, use
+the -f flag. If this flag is set, then if initialization fails, the driver will keep trying to initialize again
+every 0.5 seconds. With this flag set, you can plug in a battery after the driver starts, and it will work. Without
+this flag set, the battery must be plugged in before starting the driver.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("ina238", "driver");
+
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x45);
+	PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG();
+	PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "battery index for calibration values (1 or 2)", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int
+ina238_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = INA238;
+	BusCLIArguments cli{true, false};
+	cli.i2c_address = INA238_BASEADDR;
+	cli.default_i2c_frequency = 100000;
+	cli.support_keep_running = true;
+	cli.custom1 = 1;
+
+	while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) {
+		switch (ch) {
+		case 't': // battery index
+			cli.custom1 = (int)strtol(cli.optArg(), NULL, 0);
+			break;
+		}
+	}
+
+	const char *verb = cli.optArg();
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA238);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable INA238 Power Monitor
+ *
+ * For systems a INA238 Power Monitor, this should be set to true
+ *
+ * @group Sensors
+ * @boolean
+ * @reboot_required true
+*/
+PARAM_DEFINE_INT32(SENS_EN_INA238, 0);
+
+/**
+ * INA238 Power Monitor Max Current
+ *
+ * @group Sensors
+ * @min 0.1
+ * @max 327.68
+ * @decimal 2
+ * @increment 0.1
+ */
+PARAM_DEFINE_FLOAT(INA238_CURRENT, 327.68f);
+
+/**
+ * INA238 Power Monitor Shunt
+ *
+ * @group Sensors
+ * @min 0.000000001
+ * @max 0.1
+ * @decimal 10
+ * @increment .000000001
+ */
+PARAM_DEFINE_FLOAT(INA238_SHUNT, 0.0003f);
@@ -43,6 +43,10 @@

 #include <px4_platform_common/posix.h>

+#if defined(__PX4_NUTTX)
+#include <nuttx/mm/mm.h>
+#endif
+
 namespace cdev
 {

@@ -386,7 +390,11 @@ int CDev::unregister_driver_and_memory()
 	}

 	if (_devname != nullptr) {
+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+		kmm_free((void *)_devname);
+#else
 		free((void *)_devname);
+#endif
 		_devname = nullptr;

 	} else {
@@ -49,8 +49,12 @@ px4_add_library(cdev
 	CDev.hpp
 	${SRCS_PLATFORM}
 )
-target_compile_options(cdev PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(cdev PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__)

 if(PX4_TESTING)
 	add_subdirectory(test)
 endif()
+
+if(CONFIG_BUILD_FLAT) # Only defined for NuttX
+	target_link_libraries(cdev PRIVATE nuttx_fs)
+endif()
@@ -53,8 +53,13 @@ px4_add_library(drivers__device
 	${SRCS_PLATFORM}
 	)

+# px4_spibus_initialize (stm32_spibus_initialize)
 if (${PX4_PLATFORM} STREQUAL "nuttx")
-	target_link_libraries(drivers__device PRIVATE nuttx_arch)
+	if (NOT DEFINED CONFIG_BUILD_FLAT)
+	  target_link_libraries(drivers__device PRIVATE nuttx_karch)
+	else()
+	  target_link_libraries(drivers__device PRIVATE nuttx_arch)
+	endif()
 endif()

-target_link_libraries(drivers__device PRIVATE cdev)
+target_link_libraries(drivers__device PRIVATE px4_work_queue)
@@ -148,7 +148,13 @@ if (NOT "${PX4_BOARD}" MATCHES "px4_io")
 		px4_parameters.hpp
 	)

-	target_link_libraries(parameters PRIVATE perf tinybson px4_layer)
+	target_link_libraries(parameters PRIVATE perf tinybson px4_platform)
+
+	# on NuttX protected build there are separate px4 layers for userspace and kernel
+	if (NOT ${PX4_PLATFORM} STREQUAL "nuttx" OR CONFIG_BUILD_FLAT)
+		target_link_libraries(parameters PRIVATE px4_layer)
+	endif()
+
 	target_compile_definitions(parameters PRIVATE -DMODULE_NAME="parameters")
 	target_compile_options(parameters
 		PRIVATE
@@ -160,7 +166,7 @@ else()
 endif()
 add_dependencies(parameters prebuild_targets)

-if(${PX4_PLATFORM} STREQUAL "nuttx")
+if(${PX4_PLATFORM} STREQUAL "nuttx" AND CONFIG_BUILD_FLAT)
 	target_link_libraries(parameters PRIVATE flashparams tinybson)
 endif()

@@ -610,15 +610,17 @@ perf_print_all(int fd)
 void
 perf_print_latency(int fd)
 {
+	latency_info_t latency;
 	dprintf(fd, "bucket [us] : events\n");

-	for (int i = 0; i < latency_bucket_count; i++) {
-		dprintf(fd, "       %4i : %li\n", latency_buckets[i], (long int)latency_counters[i]);
+	for (int i = 0; i < get_latency_bucket_count(); i++) {
+		latency = get_latency(i, i);
+		dprintf(fd, "       %4i : %li\n", latency.bucket, (long int)latency.counter);
 	}

 	// print the overflow bucket value
-	dprintf(fd, " >%4" PRIu16 " : %" PRIu32 "\n", latency_buckets[latency_bucket_count - 1],
-		latency_counters[latency_bucket_count]);
+	latency = get_latency(get_latency_bucket_count() - 1, get_latency_bucket_count());
+	dprintf(fd, " >%4" PRIu16 " : %" PRIu32 "\n", latency.bucket, latency.counter);
 }

 void
@@ -634,7 +636,5 @@ perf_reset_all(void)

 	pthread_mutex_unlock(&perf_counters_mutex);

-	for (int i = 0; i <= latency_bucket_count; i++) {
-		latency_counters[i] = 0;
-	}
+	reset_latency_counters();
 }
@@ -45,6 +45,11 @@ target_compile_options(rc
 		#-DDEBUG_BUILD
 		-Wno-unused-result
 )
+
+if (${PX4_PLATFORM} MATCHES "nuttx")
+	target_link_libraries(rc PRIVATE nuttx_fs)
+endif()
+
 target_link_libraries(rc PRIVATE prebuild_targets)

 if(PX4_TESTING AND (${PX4_PLATFORM} MATCHES "posix"))
@@ -84,5 +84,7 @@ target_compile_definitions(version
 	)

 target_link_libraries(version PRIVATE git_ver)
-target_link_libraries(version PRIVATE drivers_board)
+if (CONFIG_BUILD_FLAT)
+	target_link_libraries(version PRIVATE drivers_board)
+endif()
 add_dependencies(version prebuild_targets)
@@ -37,7 +37,7 @@
 #include <systemlib/mavlink_log.h>
 #include <lib/parameters/param.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/cpuload.h>
+#include <uORB/topics/vehicle_cpuload.h>

 using namespace time_literals;

@@ -45,7 +45,7 @@ bool PreFlightCheck::cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool
 {
 	bool success = true;

-	uORB::SubscriptionData<cpuload_s> cpuload_sub{ORB_ID(cpuload)};
+	uORB::SubscriptionData<vehicle_cpuload_s> cpuload_sub{ORB_ID(vehicle_cpuload)};
 	cpuload_sub.update();

 	float cpuload_percent_max;
@@ -63,7 +63,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/cpuload.h>
+#include <uORB/topics/vehicle_cpuload.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/estimator_selector_status.h>
@@ -377,7 +377,7 @@ private:
 	bool		_should_set_home_on_takeoff{true};
 	bool		_system_power_usb_connected{false};

-	cpuload_s		_cpuload{};
+	vehicle_cpuload_s	_cpuload{};
 	geofence_result_s	_geofence_result{};
 	vehicle_land_detected_s	_land_detector{};
 	safety_s		_safety{};
@@ -395,7 +395,7 @@ private:
 	// Subscriptions
 	uORB::Subscription					_actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
 	uORB::Subscription					_cmd_sub {ORB_ID(vehicle_command)};
-	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription					_cpuload_sub{ORB_ID(vehicle_cpuload)};
 	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription                                      _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
@@ -1664,18 +1664,15 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	if (!_distance_sensor_selected) {
-		// get subscription index of first downward-facing range sensor
-		uORB::SubscriptionMultiArray<distance_sensor_s> distance_sensor_subs{ORB_ID::distance_sensor};

-		if (distance_sensor_subs.advertised()) {
-			for (unsigned i = 0; i < distance_sensor_subs.size(); i++) {
+		if (_distance_sensor_subs.advertised()) {
+			for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
 				distance_sensor_s distance_sensor;

-				if (distance_sensor_subs[i].copy(&distance_sensor)) {
+				if (_distance_sensor_subs[i].copy(&distance_sensor)) {
 					// only use the first instace which has the correct orientation
 					if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
 					    && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
-
 						if (_distance_sensor_sub.ChangeInstance(i)) {
 							int ndist = orb_group_count(ORB_ID(distance_sensor));

@@ -250,6 +250,8 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};

+	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
+
 	bool _callback_registered{false};

 	bool _distance_sensor_selected{false}; // because we can have several distance sensor instances with different orientations
@@ -39,5 +39,6 @@ px4_add_module(
 		EscBattery.cpp
 		EscBattery.hpp
 	DEPENDS
+		battery
 		px4_work_queue
 	)
@@ -45,7 +45,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/cpuload.h>
+#include <uORB/topics/vehicle_cpuload.h>
 #include <uORB/topics/led_control.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
@@ -80,7 +80,7 @@ protected:
 	void publish();

 	uORB::SubscriptionData<battery_status_s>       _battery_status_sub{ORB_ID(battery_status)};
-	uORB::SubscriptionData<cpuload_s>              _cpu_load_sub{ORB_ID(cpuload)};
+	uORB::SubscriptionData<vehicle_cpuload_s>      _cpu_load_sub{ORB_ID(vehicle_cpuload)};
 	uORB::SubscriptionData<vehicle_status_s>       _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::SubscriptionData<vehicle_status_flags_s> _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};

@@ -137,7 +137,7 @@ void LoadMon::cpuload()
 	// compute system load
 	const float interval = total_time_stamp - _last_total_time_stamp;
 	const float interval_spent_time = spent_time_stamp - _last_spent_time_stamp;
-#elif defined(__PX4_NUTTX)
+#elif defined(__PX4_NUTTX) && defined(CONFIG_BUILD_FLAT)

 	if (_last_idle_time == 0) {
 		// Just get the time in the first iteration */
@@ -154,9 +154,15 @@ void LoadMon::cpuload()
 	// compute system load
 	const float interval = now - _last_idle_time_sample;
 	const float interval_idletime = total_runtime - _last_idle_time;
+#elif defined(__PX4_NUTTX)
+	// TODO: NuttX protected & kernel builds
+	const hrt_abstime now = hrt_absolute_time();
+	const hrt_abstime total_runtime = 0.0f;
+	const float interval = 1.0f;
+	const float interval_idletime = 1.0f;
 #endif

-	cpuload_s cpuload{};
+	vehicle_cpuload_s cpuload{};
 #if defined(__PX4_LINUX)
 	/* following calculation is based on free(1)
 	 * https://gitlab.com/procps-ng/procps/-/blob/master/proc/sysinfo.c */
@@ -238,7 +244,7 @@ void LoadMon::cpuload()
 #endif
 }

-#if defined(__PX4_NUTTX)
+#if defined(__PX4_NUTTX) && defined(CONFIG_BUILD_FLAT)
 void LoadMon::stack_usage()
 {
 	unsigned stack_free = 0;
@@ -45,7 +45,7 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_platform/cpuload.h>
 #include <uORB/Publication.hpp>
-#include <uORB/topics/cpuload.h>
+#include <uORB/topics/vehicle_cpuload.h>
 #include <uORB/topics/task_stack_info.h>

 #if defined(__PX4_LINUX)
@@ -90,7 +90,7 @@ private:

 	uORB::Publication<task_stack_info_s> _task_stack_info_pub{ORB_ID(task_stack_info)};
 #endif
-	uORB::Publication<cpuload_s> _cpuload_pub {ORB_ID(cpuload)};
+	uORB::Publication<vehicle_cpuload_s> _cpuload_pub {ORB_ID(vehicle_cpuload)};

 #if defined(__PX4_LINUX)
 	FILE *_proc_fd = nullptr;
@@ -59,7 +59,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("camera_trigger_secondary");
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");
-	add_topic("cpuload");
+	add_topic("vehicle_cpuload");
 	add_topic("esc_status", 250);
 	add_topic("follow_target", 500);
 	add_topic("generator_status");
@@ -49,7 +49,7 @@ namespace logger
 bool watchdog_update(watchdog_data_t &watchdog_data)
 {

-#ifdef __PX4_NUTTX
+#if defined(__PX4_NUTTX) && defined(CONFIG_BUILD_FLAT)

 	if (system_load.initialized && watchdog_data.logger_main_task_index >= 0
 	    && watchdog_data.logger_writer_task_index >= 0) {
@@ -133,7 +133,7 @@ bool watchdog_update(watchdog_data_t &watchdog_data)

 void watchdog_initialize(const pid_t pid_logger_main, const pthread_t writer_thread, watchdog_data_t &watchdog_data)
 {
-#ifdef __PX4_NUTTX
+#if defined(__PX4_NUTTX) && defined(CONFIG_BUILD_FLAT)

 	// The pthread_t ID is equal to the PID on NuttX
 	const pthread_t pid_logger_writer = writer_thread;
@@ -66,10 +66,6 @@ px4_add_module(
 	DEPENDS
 		airspeed
 		component_general_json # for checksums.h
-		drivers_accelerometer
-		drivers_barometer
-		drivers_gyroscope
-		drivers_magnetometer
 		git_mavlink_v2
 		conversion
 		geo
@@ -80,3 +76,18 @@ px4_add_module(
 if(PX4_TESTING)
 	add_subdirectory(mavlink_tests)
 endif()
+
+if (NOT ${PX4_PLATFORM} STREQUAL "nuttx" OR CONFIG_BUILD_FLAT)
+	target_link_libraries(modules__mavlink
+		PRIVATE
+			drivers_accelerometer
+			drivers_barometer
+			drivers_gyroscope
+			drivers_magnetometer
+	)
+endif()
+
+if(${PX4_PLATFORM} STREQUAL "nuttx" AND CONFIG_NET)
+	# netlib_get_ipv4addr
+	target_link_libraries(modules__mavlink PRIVATE nuttx_apps)
+endif()
@@ -73,10 +73,12 @@
 MavlinkReceiver::~MavlinkReceiver()
 {
 	delete _tune_publisher;
+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT)
 	delete _px4_accel;
 	delete _px4_baro;
 	delete _px4_gyro;
 	delete _px4_mag;
+#endif
 }

 MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
@@ -2223,6 +2225,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)

 	const uint64_t timestamp = hrt_absolute_time();

+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT)
 	// temperature only updated with baro
 	float temperature = NAN;

@@ -2291,6 +2294,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		}
 	}

+#endif
+
 	// differential pressure
 	if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
 		differential_pressure_s report{};
@@ -2697,7 +2702,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.timestamp = hrt_absolute_time();
 		_local_pos_pub.publish(hil_local_pos);
 	}
-
+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT)
 	/* accelerometer */
 	{
 		if (_px4_accel == nullptr) {
@@ -2731,7 +2736,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 			_px4_gyro->update(timestamp_sample, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
 		}
 	}
-
+#endif
 	/* battery status */
 	{
 		battery_status_s hil_battery_status{};
@@ -340,10 +340,13 @@ private:
 		BARO		= 0b1101000000000,
 		DIFF_PRESS	= 0b10000000000
 	};
-	PX4Accelerometer *_px4_accel{nullptr};
+
+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT)
+	PX4Accelerometer *_px4_accel {nullptr};
 	PX4Barometer *_px4_baro{nullptr};
 	PX4Gyroscope *_px4_gyro{nullptr};
 	PX4Magnetometer *_px4_mag{nullptr};
+#endif

 	static constexpr unsigned int	MOM_SWITCH_COUNT{8};
 	uint8_t				_mom_switch_pos[MOM_SWITCH_COUNT] {};
@@ -35,7 +35,7 @@
 #define SYS_STATUS_HPP

 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/cpuload.h>
+#include <uORB/topics/vehicle_cpuload.h>
 #include <uORB/topics/vehicle_status.h>

 class MavlinkStreamSysStatus : public MavlinkStream
@@ -58,7 +58,7 @@ private:
 	explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {}

 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription _cpuload_sub{ORB_ID(vehicle_cpuload)};
 	uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};

 	bool send() override
@@ -67,7 +67,7 @@ private:
 			vehicle_status_s status{};
 			_status_sub.copy(&status);

-			cpuload_s cpuload{};
+			vehicle_cpuload_s cpuload{};
 			_cpuload_sub.copy(&cpuload);

 			battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {};
@@ -51,7 +51,6 @@ px4_add_module(
 		airspeed
 		conversion
 		data_validator
-		drivers__device
 		mathlib
 		sensor_calibration
 		vehicle_acceleration
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE systemcmds__kparam
+	MAIN kparam
+	COMPILE_FLAGS
+		-Wno-array-bounds
+	SRCS
+		../param/param.cpp
+	DEPENDS
+	)
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE systemcmds__kuorb
+	MAIN kuorb
+	COMPILE_FLAGS
+	SRCS
+		../uorb/uorb.cpp
+	DEPENDS
+	)
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Jukka Laitinen	b3bf1d8b0a	px4_platform: fix linking for sitl For sitl, px4_platform links directly to drivers_board Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	5320653aba	print_load: Remove reference to CONFIG_MAX_TASKS This is removed from nuttx, and the size of s->last_times can be just checked with sizeof() Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	e40b29a6d8	mavlink: Comment out px4 drivers in nuttx protected build These link directly to cdev, and thus cannot be linked into user-side module. There should be simple implementation for these which work purely on uORB; without fixing this the HITL won't work in protected build. Also link to nuttx_apps in nuttx net build for netlib dependency Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	7ccdaeb4f5	bootloader: link stm bootloaders to nuttx_arch for flash functions Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	ba6f3f583e	s32k1xx/led_pwm: link to arch_io_pins Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	ec2fb8d470	esc_battery: Link to battery library Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	e2ea216ab0	Fix some cmake / linker depenencies for boards Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	23a31b3dbc	Link cdev with nuttx_fs on nuttx flat build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	7c02a10d5f	Link rc against nuttx_fs in nuttx builds nuttx_fs is not part of prebuild_targets in protected/kernel builds Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	885fe62fec	cdev: Fix deallocating devname in nuttx protected/kernel builds The CDevs always live in kernel side. In kernel the devname is allocated in kernel heap. Replace "free" with kmm_free on nuttx to correct the deallocation; this automatically maps to "free" on flat build. Define __KERNEL__ flag so that the proper nuttx headers are used for building. TODO: It is fishy in the first place that the devname is allocated in inherited classes but deleted in the CDev base class. Especially when the devname is often passed as a constant string (which cannot even be freed) by many drivers. The allocation and deallocation of devname should be done within the CDev base class instead. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	c2d9797cdb	Don't link sensors module to drivers__device sensors module only works on uORB, it is not a kernel side module linking directly to the drivers. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	be68354b6a	px4_work_queue: Start worker runner depending on whether initiated from kernel or user side This uses px4_task_spawn_cmd instead of pthread, since px4_task_spawn_cmd is already made to start worker on proper side. TODO: check that we don't accidentally run user-side code in kernel context in work queue. The work items should be scheduled to work queues depending on whether the actual callback is on kerne/user side. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	597408c7aa	param: Add also a kernel-side kparam module for protected build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	ba19abd1db	uORB: Create separate systemcmds for kernel and userspace in protected build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	4b6e3f3822	Fix comparing orb_metadata in uORB::DeviceNode::publish Don't compare pointers to metadata, but the metadata contents. In protected/kernel build there are two sets of metadata, on on kernel side and another in user side. Thus the comparison of pointers would just always fail. Compare orb_id instead Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	1d79f5d134	Add a simple way to launch kernel side modules Add px4_insmod command to start/execute px4 modules in kenel space in NuttX protected build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	5a3ecb0094	Auto-generate a list of kernel-side built-in modules(drivers) Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	07ea1e3e8b	px4_task_spawn_cmd: launch kernel thread in protected/kernel build on kernel side Also task name is accessible only in kernel side for protected/kernel build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	4751869cc0	Add console buffer stub for userspace Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	04ccb26483	Initialize cxx static variables also in kernel side For protected/kernel builds the cxx static initializations needs to be done also in kernel side, since px4 creates c++ objects in kernel Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	aeeb4e47a5	Fix parameters building for nuttx protected/kernel builds - Don't link to px4_layer - Don't link to flashparams; flashparams would work only in kernel side - Add missing link to px4_platform TODO: Make flashparams to work also in protected/kernel build. This would require using some real OS interface to the flash Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae> fix: bc2f28a68479e4f41017a7d254a263e9e3db5c1e Fix parameters building for nuttx protected/kernel builds Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	01d20ba392	link drivers__device to nuttx_karch instead of nuttx_arch in protected build On protected/kernel build the library would be karch. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	f6421ac359	nuttx/stm io_pins: link to drivers_board for timer_io_channels dependency Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	c5e4e0a267	arch_board_reset: link to nuttx_arch / nuttx_karch for up_systemreset dependency Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	c63c945eb8	Remove px4_work_queue linking to px4_platform Remove linking to px4_plaform in here; this breaks linking for nuttx protected build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:48 +03:00
Jukka Laitinen	557af7e63d	Don't link px4_platform directly to uORB Since uORB is split into kernel and userspace parts, it is no longer possible to just link uORB into px4_platform, which is used in both kernel and user side. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:43 +03:00
Jukka Laitinen	93f6863bf2	Link arch_spi with drivers_board Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:43 +03:00
Jukka Laitinen	dbf7d08c23	Add support for compiling modules into kernel side Define __KERNEL__ macro during compilation and place the module in separate library Remove default library linking to m or libc on NuttX. Add these in platform layer instead, since they are different on kernel and user side Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:37 +03:00
Jukka Laitinen	2a40fcc23c	EKF2: Allocate distance sensor subscriptions as member variables Just create the subscriptions and keep them, instead of re-creating them continuously Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:37 +03:00
Jukka Laitinen	e3b024c15d	Modify uORB.cpp main file and CMakeLists.txt for NuttX protected build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:37 +03:00
Jukka Laitinen	92877e1f35	Add IOCTL interface to uORBManager for nuttx protected/kernel build split When building uORB for NuttX flat build, or for some other target, everything works as before. When building uORB for NuttX protected or kernel build, this does the following: - The kernel side uORB library reigsters a boardctl handler for calls from userspace and services the boardctl_ioctls by calling the actual uORB functions - For user mode binaries, the uORBManager acts as a proxy, making boardctl_ioctl calls to the kernel side - The uORBDeviceNodes are always allocated in user heap for protected build, so that they are accessible from both kernel and user side. This is only for optimization, a few functions such as "is_advertised" get really heavy when called frequently via ioctl Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	f54e56d6a8	Clean up interfaces towards uORB Proxy all calls to the DeviceNode through Manager; - This hides the DeviceNode from publishers and subscribers - Manager can be made an interface class between user and kernel spaces in protected build - This doesn't increase code size or harm the performance on flat build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	8d9afdab63	lib/version: Don't link to drivers_board in PROTECTED/KERNEL build The needed version specific things come from px4 layer in userspace Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	8497dbb2b5	Add a generic nuttx hrt driver userspace interface This adds a nuttx userspace interface for hrt driver, communicating with the actual hrt driver upper half via BOARDCTL IOCTLs This is be used when running PX4 in NuttX protected or kernel builds Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	8bb6cc07de	hrt: Add interface functions for latency counters Add interface functions for fetching latency buckets and counters and use those in perf_counter.cpp. This cleans up the usage of perf counters and enables implementing kernel-userspace interface for those for nuttx protected/kernel build. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	f43b9fe5d6	platforms/common/shutdown.cpp: Enable boardctl functions for shutdown and poweroff Shutdown and poweroff must go through boardctl in NuttX protected build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	340c765a14	logger/watchdog: Disable watchdog for NuttX protected/kernel builds It accesses kernel internal structures directly; this needs to be worked out with some proper userspace-kernel interface Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	fceb4eaa3f	uORB: Change cpuload topic int vehicle_cpuload This avoids clashes with NuttX cpuload_s datatype. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	fc6b2d8122	loadmon: Stub away cpuload() for nuttx protected build CPU load monitoring needs to be in kernel, and some proper interface to be used in here Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	d17fa568d0	px4 posix: print_load.cpp: Fix build error when building on nuttx This could be further developed to work also on nuttx in protected/kernel builds Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	68a4c31a76	Add nuttx userspace reset and reboot request functions Add functions for reset and reboot, which can be called from user space in nuttx kernel and protected build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	c3992634f1	Add a separate library for initializing px4 userspace in protected build The px4_userspace_init function in this library is called from userspace entrypoint before starting NSH Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	df0b740710	Split px4_layer into kernel and userspace parts for nuttx protected build Split the px4_layer into user and kernel space libraries. Add some stubs for user-space (e.g. version) where an interface to the kernel needs to be added Use posix-versions for cpuload.cpp and print_load.cpp for userspace; these link to nuttx internals. This functinality could be built on top of posix (e.g. procfs) interfaces Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	4def01178d	Fix px4_impl_os for protected build For NuttX protected or kernel build, the prebuilds can't contain libraries which are different for kernel and user-space in protected/kerenl builds Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	81f24b0a27	px4_fmuv5: Remove BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE in protected/kernel build Gpios are not directly accessible from userspace, so it should not be controlled from within Commander directly There is a need for a proper userspace interface via /dev/leds or such Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:31 +03:00
Jukka Laitinen	69fc3cbde0	NuttX Cmake changes to build combined kernel+userspace image in nuttx protected build Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-23 13:57:25 +03:00
Jukka Laitinen	08f83367ac	Add linker scripts for kernel-userspace split Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-22 13:19:47 +03:00
Jukka Laitinen	7c94bc8f02	Add kernel/userspace and nuttx_syscall libraries to build Build NuttX proxies, stubs and separate user space and kernel space libraries Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-22 13:19:47 +03:00
Jukka Laitinen	169eb616c6	Add px4_fmu-v5_protected target Add a target for nuttx protected build development Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-22 13:19:42 +03:00
Hamish Willee	f41e5985e8	vehicle_local_position: clarify origin of system	2021-09-22 10:14:23 +02:00
Thomas Debrunner	43ece74fa0	ina238: Do not expose raw adc config to user, make specific to ina238	2021-09-22 10:02:17 +02:00
Thomas Debrunner	b8d0bb44c4	ina23X: Support for ina23X power monitors	2021-09-22 10:02:17 +02:00
Daniel Agar	cf5da66e9f	Jenkins: hardware disable gyro auto cal and FFT before running tests	2021-09-21 19:13:30 -04:00
Daniel Agar	87db18c1a0	Jenkins: hardware don't allow board cleanup commands to fail build	2021-09-21 19:13:30 -04:00
Daniel Agar	a92e44c90e	Tools/HIL/run_nsh_cmd.py: echo success cmd twice to avoid missing dropouts	2021-09-21 19:13:30 -04:00
Julian Oes	faca2b17d0	camera_trigger: only get param if available	2021-09-17 12:23:30 +02:00