WIP: ctrl zero h7 pixracer pro sync

.ci/Jenkinsfile-hardware

@@ -673,6 +673,153 @@ pipeline {
           }
         }
 
+        stage("modalai_fc-v1_test") {
+          stages {
+            stage("build modalai_fc-v1_test") {
+              agent {
+                docker {
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
+                }
+              }
+              steps {
+                checkout scm
+                sh 'export'
+                sh 'make distclean'
+                sh 'ccache -s'
+                sh 'git fetch --tags'
+                sh 'make modalai_fc-v1_test'
+                sh 'make sizes'
+                sh 'ccache -s'
+                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'modalai_fc-v1_test'
+              }
+              post {
+                always {
+                    sh 'make distclean'
+                }
+              }
+            } // stage build
+            stage("test") {
+              agent {
+                label 'modalai_fc-v1'
+              }
+              stages {
+                stage("flash") {
+                  steps {
+                    sh 'export'
+                    sh 'find /dev/serial'
+                    unstash 'modalai_fc-v1_test'
+                    // flash board and watch bootup
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_test/modalai_fc-v1_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
+                  }
+                }
+                stage("configure") {
+                  steps {
+                    // configure
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"'    // disable buzzer
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"'    // generic quadcopter
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"'       // update bootloader
+                    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 "param status"'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true'                  // reboot to apply
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show"'
+                  }
+                }
+                stage("status") {
+                  steps {
+                    statusFTDI()
+                  }
+                }
+                stage("tests") {
+                  steps {
+                    // run tests
+                    sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`'
+                  }
+                }
+                stage("reset") {
+                  steps {
+                    cleanupFTDI()
+                  }
+                }
+              }
+            } // stage test
+          }
+        }
+
+        stage("holybro_durandal-v1_test") {
+          stages {
+            stage("build holybro_durandal-v1_test") {
+              agent {
+                docker {
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
+                }
+              }
+              steps {
+                checkout scm
+                sh 'export'
+                sh 'make distclean'
+                sh 'ccache -s'
+                sh 'git fetch --tags'
+                sh 'make holybro_durandal-v1_test'
+                sh 'make sizes'
+                sh 'ccache -s'
+                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'holybro_durandal-v1_test'
+              }
+              post {
+                always {
+                    sh 'make distclean'
+                }
+              }
+            } // stage build
+            stage("test") {
+              agent {
+                label 'holybro_durandal-v1'
+              }
+              stages {
+                stage("flash") {
+                  steps {
+                    sh 'export'
+                    sh 'find /dev/serial'
+                    unstash 'holybro_durandal-v1_test'
+                    // flash board and watch bootup
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/holybro_durandal-v1_test/holybro_durandal-v1_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
+                  }
+                }
+                stage("configure") {
+                  steps {
+                    // configure
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"'    // disable buzzer
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"'    // generic quadcopter
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"'       // update bootloader
+                    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 "param status"'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true'                  // reboot to apply
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show"'
+                  }
+                }
+                stage("status") {
+                  steps {
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dshot status"'
+                    statusFTDI()
+                  }
+                }
+                stage("tests") {
+                  steps {
+                    // run tests
+                    sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`'
+                  }
+                }
+                stage("reset") {
+                  steps {
+                    cleanupFTDI()
+                  }
+                }
+              }
+            } // stage test
+          }
+        }
+
         stage("nxp_fmuk66-v3_test") {
           stages {
             stage("build nxp_fmuk66-v3_test") {

ROMFS/px4fmu_common/init.d/rcS

@@ -128,7 +128,7 @@ else
 	then
 		param reset_all
 	fi
-	if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X
+	if ver hwtypecmp V5X00 V5X90 V5Xa0
 	then
 		netman update -i eth0
 	fi

boards/cuav/nora/src/init.c

@@ -60,8 +60,6 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
-#include <mpu.h>
-
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -129,16 +127,6 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/cuav/x7pro/src/init.c

@@ -60,8 +60,6 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
-#include <mpu.h>
-
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -129,16 +127,6 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/cubepilot/cubeorange/default.cmake

@@ -15,7 +15,7 @@ px4_add_board(
 		TEL2:/dev/ttyS1
 		GPS1:/dev/ttyS2
 		# PX4IO:/dev/ttyS3
-		# TEL3:/dev/ttyS4  # connected to ADS-B receiver
+		TEL3:/dev/ttyS4
 		GPS2:/dev/ttyS5
 	DRIVERS
 		adc/ads1115

boards/cubepilot/cubeorange/init/rc.board_mavlink

@@ -5,9 +5,3 @@
 
 # Start MAVLink on the USB port
 mavlink start -d /dev/ttyACM0
-
-# Start ADS-B receiver mavlink connection if console not present
-if [ ! -e /dev/console ]
-then
-	mavlink start -d /dev/ttyS4 -b 57600 -m minimal
-fi

boards/cubepilot/cubeorange/src/board_config.h

@@ -62,7 +62,7 @@
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
 
 #define BOARD_HAS_CONTROL_STATUS_LEDS      1
-#define BOARD_ARMED_LED  LED_AMBER
+#define BOARD_ARMED_STATE_LED  LED_AMBER
 
 /* ADC channels */
 #define PX4_ADC_GPIO  \

boards/cubepilot/cubeorange/src/init.c

@@ -60,8 +60,6 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
-#include <mpu.h>
-
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -120,16 +118,6 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/cubepilot/cubeyellow/src/board_config.h

@@ -62,7 +62,7 @@
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
 
 #define BOARD_HAS_CONTROL_STATUS_LEDS      1
-#define BOARD_ARMED_LED  LED_AMBER
+#define BOARD_ARMED_STATE_LED  LED_AMBER
 
 /* ADC channels */
 #define PX4_ADC_GPIO  \

boards/cubepilot/io-v2/nuttx-config/nsh/defconfig

@@ -27,7 +27,7 @@ CONFIG_FDCLONE_DISABLE=y
 CONFIG_FDCLONE_STDIO=y
 CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
-CONFIG_IDLETHREAD_STACKSIZE=316
+CONFIG_IDLETHREAD_STACKSIZE=280
 CONFIG_MAX_TASKS=2
 CONFIG_MM_FILL_ALLOCATIONS=y
 CONFIG_MM_SMALL=y

boards/holybro/durandal-v1/src/init.c

@@ -75,8 +75,6 @@
 #include <px4_platform/board_determine_hw_info.h>
 #include <px4_platform/board_dma_alloc.h>
 
-#include <mpu.h>
-
 /****************************************************************************
  * Pre-Processor Definitions
  ****************************************************************************/
@@ -162,16 +160,6 @@ __EXPORT void board_on_reset(int status)
 __EXPORT void
 stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	board_on_reset(-1); /* Reset PWM first thing */
 
 	/* configure LEDs */

boards/mro/ctrl-zero-h7-oem/src/init.c

@@ -60,8 +60,6 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
-#include <mpu.h>
-
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -123,16 +121,6 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/mro/ctrl-zero-h7/nuttx-config/include/board.h

@@ -221,32 +221,32 @@
 
 
 /* UART/USART */
-#define GPIO_USART2_RX   GPIO_USART2_RX_2       /* PD6 */
-#define GPIO_USART2_TX   GPIO_USART2_TX_2       /* PD5 */
-#define GPIO_USART2_RTS  GPIO_USART2_RTS_2      /* PD4 */
-#define GPIO_USART2_CTS  GPIO_USART2_CTS_NSS_2  /* PD3 */
+#define GPIO_USART2_TX   GPIO_USART2_TX_2      /* PD5  */
+#define GPIO_USART2_RX   GPIO_USART2_RX_2      /* PD6  */
+#define GPIO_USART2_CTS  GPIO_USART2_CTS_NSS_2 /* PD3  */
+#define GPIO_USART2_RTS  GPIO_USART2_RTS_2     /* PD4  */
 
-#define GPIO_USART3_RX   GPIO_USART3_RX_3       /* PD9 */
-#define GPIO_USART3_TX   GPIO_USART3_TX_3       /* PD8 */
-#define GPIO_USART3_RTS  GPIO_USART3_RTS_2      /* PD12 */
-#define GPIO_USART3_CTS  GPIO_USART3_CTS_NSS_2  /* PD11 */
+#define GPIO_USART3_TX   GPIO_USART3_TX_3      /* PD8  */
+#define GPIO_USART3_RX   GPIO_USART3_RX_3      /* PD9  */
+#define GPIO_USART3_CTS  GPIO_USART3_CTS_NSS_2 /* PD11 */
+#define GPIO_USART3_RTS  GPIO_USART3_RTS_2     /* PD12 */
 
-#define GPIO_UART4_RX    GPIO_UART4_RX_1        /* PA1 */
-#define GPIO_UART4_TX    GPIO_UART4_TX_1        /* PA0 */
+#define GPIO_UART4_TX    GPIO_UART4_TX_2       /* PA0  */
+#define GPIO_UART4_RX    GPIO_UART4_RX_2       /* PA1  */
 
-#define GPIO_USART6_RX   GPIO_USART6_RX_1       /* PC7 */
-#define GPIO_USART6_TX   GPIO_USART6_TX_2       /* PG14 */
+#define GPIO_USART6_TX   0                     /* USART6 is RX-only */
+#define GPIO_USART6_RX   GPIO_USART6_RX_1      /* PC7  */
 
-#define GPIO_UART7_RX    GPIO_UART7_RX_1        /* PE7 */
-#define GPIO_UART7_TX    GPIO_UART7_TX_1        /* PE8 */
+#define GPIO_UART7_TX    GPIO_UART7_TX_3       /* PE8  */
+#define GPIO_UART7_RX    GPIO_UART7_RX_3       /* PE7  */
 
-#define GPIO_UART8_RX    GPIO_UART8_RX_1        /* PE0 */
-#define GPIO_UART8_TX    GPIO_UART8_TX_1        /* PE1 */
+#define GPIO_UART8_TX    GPIO_UART8_TX_1       /* PE1  */
+#define GPIO_UART8_RX    GPIO_UART8_RX_1       /* PE0  */
 
 
 /* CAN */
-#define GPIO_CAN1_RX     GPIO_CAN1_RX_3     /* PD0  */
-#define GPIO_CAN1_TX     GPIO_CAN1_TX_3     /* PD1 */
+#define GPIO_CAN1_RX     GPIO_CAN1_RX_3        /* PD0  */
+#define GPIO_CAN1_TX     GPIO_CAN1_TX_3        /* PD1  */
 
 
 /* SPI */
@@ -262,7 +262,7 @@
 
 #define GPIO_SPI5_SCK    ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7  */
 #define GPIO_SPI5_MISO   GPIO_SPI5_MISO_1               /* PF8  */
-#define GPIO_SPI5_MOSI   GPIO_SPI5_MOSI_1               /* PF9  */
+#define GPIO_SPI5_MOSI   GPIO_SPI5_MOSI_2               /* PF9  */
 
 
 /* I2C */

boards/mro/ctrl-zero-h7/nuttx-config/include/board_dma_map.h

@@ -37,8 +37,9 @@
 #define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */
 #define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */
 
-#define DMAMAP_USART6_RX   DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */
-#define DMAMAP_USART6_TX   DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */
+#define DMAMAP_SPI2_RX    DMAMAP_DMA12_SPI2RX_0 /* DMA1:39 */
+#define DMAMAP_SPI2_TX    DMAMAP_DMA12_SPI2TX_0 /* DMA1:40 */
+
+#define DMAMAP_SPI5_RX    DMAMAP_DMA12_SPI5RX_0 /* DMA1:85 */
+#define DMAMAP_SPI5_TX    DMAMAP_DMA12_SPI5TX_0 /* DMA1:86 */
 
-#define DMAMAP_SPI5_RX    DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */
-#define DMAMAP_SPI5_TX    DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */

boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig

@@ -53,13 +53,11 @@ CONFIG_CDCACM_VENDORSTR="mRo"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
-CONFIG_DEBUG_MEMFAULT=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_EXPERIMENTAL=y
 CONFIG_FAT_DMAMEMORY=y
 CONFIG_FAT_LCNAMES=y
@@ -188,10 +186,8 @@ CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
 CONFIG_STM32H7_SPI_DMA=y
 CONFIG_STM32H7_SPI_DMATHRESHOLD=8
 CONFIG_STM32H7_TIM1=y
-CONFIG_STM32H7_TIM2=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
-CONFIG_STM32H7_TIM8=y
 CONFIG_STM32H7_UART4=y
 CONFIG_STM32H7_UART7=y
 CONFIG_STM32H7_UART8=y

boards/mro/ctrl-zero-h7/nuttx-config/scripts/script.ld

@@ -206,7 +206,6 @@ SECTIONS
 		_sram4_heap_start = ABSOLUTE(.);
 	} > SRAM4
 
-
 	/* Stabs debugging sections. */
 	.stab 0 : { *(.stab) }
 	.stabstr 0 : { *(.stabstr) }
@@ -220,13 +219,4 @@ SECTIONS
 	.debug_line 0 : { *(.debug_line) }
 	.debug_pubnames 0 : { *(.debug_pubnames) }
 	.debug_aranges 0 : { *(.debug_aranges) }
-
-	.ramfunc : {
-		_sramfuncs = .;
-		*(.ramfunc  .ramfunc.*)
-		. = ALIGN(4);
-		_eramfuncs = .;
-	} > ITCM_RAM AT > FLASH
-
-	_framfuncs = LOADADDR(.ramfunc);
 }

boards/mro/ctrl-zero-h7/src/init.c

@@ -60,8 +60,6 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
-#include <mpu.h>
-
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -123,16 +121,6 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/mro/pixracerpro/nuttx-config/scripts/script.ld

@@ -198,7 +198,6 @@ SECTIONS
 	} > AXI_SRAM
 
 	/* Emit the the D3 power domain section for locating BDMA data */
-
 	.sram4_reserve (NOLOAD) :
 	{
 		*(.sram4)

boards/mro/pixracerpro/src/init.c

@@ -60,8 +60,6 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
-#include <mpu.h>
-
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -123,16 +121,6 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/nxp/fmuk66-e/src/init.c

@@ -235,25 +235,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "DMA alloc FAILED\n");
 	}
 
-	/* set up the serial DMA polling */
-#ifdef SERIAL_HAVE_DMA
-	static struct hrt_call serial_dma_call;
-	struct timespec ts;
-
-	/*
-	 * Poll at 1ms intervals for received bytes that have not triggered
-	 * a DMA event.
-	 */
-	ts.tv_sec = 0;
-	ts.tv_nsec = 1000000;
-
-	hrt_call_every(&serial_dma_call,
-		       ts_to_abstime(&ts),
-		       ts_to_abstime(&ts),
-		       (hrt_callout)kinetis_serial_dma_poll,
-		       NULL);
-#endif
-
 	/* initial LED state */
 	drv_led_start();
 	led_off(LED_RED);

boards/nxp/fmuk66-v3/src/init.c

@@ -235,25 +235,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "DMA alloc FAILED\n");
 	}
 
-	/* set up the serial DMA polling */
-#ifdef SERIAL_HAVE_DMA
-	static struct hrt_call serial_dma_call;
-	struct timespec ts;
-
-	/*
-	 * Poll at 1ms intervals for received bytes that have not triggered
-	 * a DMA event.
-	 */
-	ts.tv_sec = 0;
-	ts.tv_nsec = 1000000;
-
-	hrt_call_every(&serial_dma_call,
-		       ts_to_abstime(&ts),
-		       ts_to_abstime(&ts),
-		       (hrt_callout)kinetis_serial_dma_poll,
-		       NULL);
-#endif
-
 	/* initial LED state */
 	drv_led_start();
 	led_off(LED_RED);

boards/px4/fmu-v2/default.cmake

@@ -76,7 +76,7 @@ px4_add_board(
 		#gyro_fft
 		land_detector
 		#landing_target_estimator
-		#load_mon
+		load_mon
 		#local_position_estimator
 		logger
 		mavlink
@@ -118,7 +118,7 @@ px4_add_board(
 		#sd_bench
 		#serial_test
 		#system_time
-		#top
+		top
 		#topic_listener
 		tune_control
 		#uorb

boards/px4/fmu-v2/multicopter.cmake

@@ -48,7 +48,7 @@ px4_add_board(
 		#events
 		land_detector
 		landing_target_estimator
-		#load_mon
+		load_mon
 		#local_position_estimator
 		logger
 		mavlink

boards/px4/fmu-v5/optimized.cmake

@@ -23,8 +23,7 @@ px4_add_board(
 	DRIVERS
 		adc/ads1115
 		adc/board_adc
-		#barometer # all available barometer drivers
-		barometer/ms5611
+		barometer # all available barometer drivers
 		batt_smbus
 		camera_capture
 		camera_trigger
@@ -117,7 +116,7 @@ px4_add_board(
 		perf
 		pwm
 		reboot
-		#reflect
+		reflect
 		sd_bench
 		#serial_test
 		system_time

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

@@ -55,20 +55,17 @@ else
 
 fi
 
-# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
-ist8310 -X -b 1 -R 10 start
-
 # Possible internal Baro
 
 # Disable startup of internal baros if param is set to false
 if param compare SENS_INT_BARO_EN 1
 then
 	bmp388 -I -a 0x77 start
-	if ver hwtypecmp V5X00 V5X90 V5Xa0
+	if ver hwtypecmp V5X91 V5Xa1
 	then
-		bmp388 -I start
-	else
 		bmp388 -X -b 2 start
+	else
+		bmp388 -I start
 	fi
 
 fi

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

@@ -21,9 +21,6 @@ icm20649 -R 14 -s start
 # Internal magnetometer on I2c
 bmm150 -I start
 
-# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
-ist8310 -X -b 1 -R 10 start
-
 # Possible internal Baro
 bmp388 -I -a 0x77 start
 

boards/px4/io-v2/nuttx-config/nsh/defconfig

@@ -27,7 +27,7 @@ CONFIG_FDCLONE_DISABLE=y
 CONFIG_FDCLONE_STDIO=y
 CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
-CONFIG_IDLETHREAD_STACKSIZE=316
+CONFIG_IDLETHREAD_STACKSIZE=280
 CONFIG_MAX_TASKS=2
 CONFIG_MM_FILL_ALLOCATIONS=y
 CONFIG_MM_SMALL=y

platforms/common/i2c_spi_buses.cpp

@@ -642,7 +642,7 @@ void I2CSPIDriverBase::print_status()
 	bool is_i2c_bus = _bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal;
 
 	if (is_i2c_bus) {
-		PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, get_i2c_address());
+		PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, _i2c_address);
 
 	} else {
 		PX4_INFO("Running on SPI Bus %i", _bus);

platforms/common/include/px4_platform_common/i2c_spi_buses.h

@@ -62,13 +62,11 @@ class I2CSPIInstance : public ListNode<I2CSPIInstance *>
 {
 public:
 	virtual ~I2CSPIInstance() = default;
-	virtual int8_t  get_i2c_address() {return _i2c_address;}
 
 private:
 	I2CSPIInstance(const char *module_name, I2CSPIBusOption bus_option, int bus, uint8_t i2c_address, uint16_t type)
 		: _module_name(module_name), _bus_option(bus_option), _bus(bus), _type(type), _i2c_address(i2c_address) {}
 
-
 	friend class BusInstanceIterator;
 	friend class I2CSPIDriverBase;
 

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp

@@ -96,7 +96,6 @@ private:
 
 	IntrusiveQueue<WorkItem *>	_q;
 	px4_sem_t			_process_lock;
-	px4_sem_t			_exit_lock;
 	const wq_config_t		&_config;
 	BlockingList<WorkItem *>	_work_items;
 	px4::atomic_bool		_should_exit{false};

platforms/common/px4_work_queue/WorkQueue.cpp

@@ -59,20 +59,11 @@ WorkQueue::WorkQueue(const wq_config_t &config) :
 
 	px4_sem_init(&_process_lock, 0, 0);
 	px4_sem_setprotocol(&_process_lock, SEM_PRIO_NONE);
-
-	px4_sem_init(&_exit_lock, 0, 1);
-	px4_sem_setprotocol(&_exit_lock, SEM_PRIO_NONE);
 }
 
 WorkQueue::~WorkQueue()
 {
-
 	work_lock();
-
-	// Synchronize with ::Detach
-	px4_sem_wait(&_exit_lock);
-	px4_sem_destroy(&_exit_lock);
-
 	px4_sem_destroy(&_process_lock);
 	work_unlock();
 
@@ -92,14 +83,11 @@ bool WorkQueue::Attach(WorkItem *item)
 	}
 
 	work_unlock();
-
 	return false;
 }
 
 void WorkQueue::Detach(WorkItem *item)
 {
-	bool exiting = false;
-
 	work_lock();
 
 	_work_items.remove(item);
@@ -108,21 +96,11 @@ void WorkQueue::Detach(WorkItem *item)
 		// shutdown, no active WorkItems
 		PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name);
 
-		// Deletion of this work queue might happen right after request_stop or
-		// SignalWorkerThread. Use a separate lock to prevent premature deletion
-		px4_sem_wait(&_exit_lock);
-		exiting = true;
 		request_stop();
 		SignalWorkerThread();
 	}
 
 	work_unlock();
-
-	// In case someone is deleting this wq already, signal
-	// that it is now allowed
-	if (exiting) {
-		px4_sem_post(&_exit_lock);
-	}
 }
 
 void WorkQueue::Add(WorkItem *item)

platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c

@@ -567,9 +567,9 @@ FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev,
 		priv->mtd.ioctl  = at24c_ioctl;
 		priv->dev        = dev;
 
-		priv->perf_transfers = perf_alloc(PC_ELAPSED, "[at24c] eeprom transfer");
-		priv->perf_resets_retries = perf_alloc(PC_COUNT, "[at24c] eeprom reset");
-		priv->perf_errors = perf_alloc(PC_COUNT, "[at24c] eeprom errors");
+		priv->perf_transfers = perf_alloc(PC_ELAPSED, "eeprom_trans");
+		priv->perf_resets_retries = perf_alloc(PC_COUNT, "eeprom_rst");
+		priv->perf_errors = perf_alloc(PC_COUNT, "eeprom_errs");
 	}
 
 	/* attempt to read to validate device is present */
@@ -600,14 +600,6 @@ FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev,
 	perf_end(priv->perf_transfers);
 
 	if (ret < 0) {
-		perf_free(priv->perf_transfers);
-		perf_free(priv->perf_resets_retries);
-		perf_free(priv->perf_errors);
-
-		priv->perf_transfers = NULL;
-		priv->perf_resets_retries = NULL;
-		priv->perf_errors = NULL;
-
 		return NULL;
 	}
 

platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp

@@ -328,8 +328,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		if (ret == OK) {
 			/* check for DMA errors */
 			if (_rx_dma_status & DMA_STATUS_TEIF) {
-				// stream transfer error, ensure TX DMA is also stopped before exiting early
-				stm32_dmastop(_tx_dma);
 				perf_count(_pc_dmaerrs);
 				ret = -EIO;
 				break;

platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp

@@ -373,8 +373,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		if (ret == OK) {
 			/* check for DMA errors */
 			if (_rx_dma_status & DMA_STATUS_TEIF) {
-				// stream transfer error, ensure TX DMA is also stopped before exiting early
-				stm32_dmastop(_tx_dma);
 				perf_count(_pc_dmaerrs);
 				ret = -EIO;
 				break;

src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp

@@ -34,8 +34,7 @@
 /**
  * @file rgbled_ncp5623c.cpp
  *
- * Driver for the onboard RGB LED controller (NCP5623B or NCP5623C)
- * connected via I2C.
+ * Driver for the onboard RGB LED controller (NCP5623C) connected via I2C.
  *
  * @author CUAVcaijie <caijie@cuav.net>
  */
@@ -53,22 +52,21 @@
 
 using namespace time_literals;
 
-#define NCP5623B_ADDR       0x38  /**< I2C address of NCP5623B */
-#define NCP5623C_ADDR       0x39  /**< I2C address of NCP5623C */
+#define ADDR			0x39	/**< I2C adress of NCP5623C */
 
-#define NCP5623_LED_CURRENT 0x20  /**< Current register */
-#define NCP5623_LED_PWM0    0x40  /**< pwm0 register */
-#define NCP5623_LED_PWM1    0x60  /**< pwm1 register */
-#define NCP5623_LED_PWM2    0x80  /**< pwm2 register */
+#define NCP5623_LED_CURRENT	0x20	/**< Current register */
+#define NCP5623_LED_PWM0	0x40	/**< pwm0 register */
+#define NCP5623_LED_PWM1	0x60	/**< pwm1 register */
+#define NCP5623_LED_PWM2	0x80	/**< pwm2 register */
 
-#define NCP5623_LED_BRIGHT  0x1f  /**< full brightness */
-#define NCP5623_LED_OFF     0x00  /**< off */
+#define NCP5623_LED_BRIGHT	0x1f	/**< full brightness */
+#define NCP5623_LED_OFF		0x00	/**< off */
 
 
 class RGBLED_NCP5623C : public device::I2C, public I2CSPIDriver<RGBLED_NCP5623C>
 {
 public:
-	RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address, const int order);
+	RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address);
 	virtual ~RGBLED_NCP5623C() = default;
 
 	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
@@ -79,20 +77,15 @@ public:
 	int		probe() override;
 
 	void			RunImpl();
-	virtual int8_t  get_i2c_address() {return get_device_address();}
 
 private:
-	int			send_led_rgb();
-	void			update_params();
-
-	int			write(uint8_t reg, uint8_t data);
 
 	float			_brightness{1.0f};
 	float			_max_brightness{1.0f};
 
-	uint8_t		_r{0};
-	uint8_t		_g{0};
-	uint8_t		_b{0};
+	uint8_t			_r{0};
+	uint8_t			_g{0};
+	uint8_t			_b{0};
 	volatile bool		_running{false};
 	volatile bool		_should_run{true};
 	bool			_leds_enabled{true};
@@ -101,40 +94,16 @@ private:
 
 	LedController		_led_controller;
 
-	uint8_t		_red{NCP5623_LED_PWM0};
-	uint8_t		_green{NCP5623_LED_PWM1};
-	uint8_t		_blue{NCP5623_LED_PWM2};
+	int			send_led_rgb();
+	void			update_params();
+
+	int			write(uint8_t reg, uint8_t data);
 };
 
-RGBLED_NCP5623C::RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address,
-				 const int order) :
+RGBLED_NCP5623C::RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) :
 	I2C(DRV_LED_DEVTYPE_RGBLED_NCP5623C, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
 {
-	int ordering = order;
-	// ordering is RGB: Hundreds is Red, Tens is green and ones is Blue
-	// 123 would drive the
-	//      R LED from = NCP5623_LED_PWM0
-	//      G LED from = NCP5623_LED_PWM1
-	//      B LED from = NCP5623_LED_PWM2
-	// 321 would drive the
-	//      R LED from = NCP5623_LED_PWM2
-	//      G LED from = NCP5623_LED_PWM1
-	//      B LED from = NCP5623_LED_PWM0
-	const uint8_t sig[] = {NCP5623_LED_PWM0, NCP5623_LED_PWM1, NCP5623_LED_PWM2};
-	// Process ordering in lsd to msd order.(BGR)
-	uint8_t *color[] = {&_blue, &_green, &_red };
-	unsigned int s = 0;
-
-	for (unsigned int i = 0; i < arraySize(color); i++) {
-		s = (ordering % 10) - 1;
-
-		if (s < arraySize(sig)) {
-			*color[i] = sig[s];
-		}
-
-		ordering /= 10;
-	}
 }
 
 int
@@ -169,15 +138,9 @@ RGBLED_NCP5623C::init()
 int
 RGBLED_NCP5623C::probe()
 {
-	_retries = 2;
-	int status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF);
+	_retries = 4;
 
-	if (status == PX4_ERROR) {
-		set_device_address(NCP5623B_ADDR);
-		status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF);
-	}
-
-	return status;
+	return write(NCP5623_LED_CURRENT, 0x00);
 }
 
 void
@@ -249,13 +212,14 @@ RGBLED_NCP5623C::RunImpl()
 int
 RGBLED_NCP5623C::send_led_rgb()
 {
+
 	uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80};
 	uint8_t brightness = 0x1f * _max_brightness;
 
 	msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f);
-	msg[2] = _red | (uint8_t(_r * _brightness) & 0x1f);
-	msg[4] = _green | (uint8_t(_g * _brightness) & 0x1f);
-	msg[6] = _blue | (uint8_t(_b * _brightness) & 0x1f);
+	msg[2] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f);
+	msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f);
+	msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f);
 
 	return transfer(&msg[0], 7, nullptr, 0);
 }
@@ -282,7 +246,6 @@ RGBLED_NCP5623C::print_usage()
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
 	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x39);
-	PRINT_MODULE_USAGE_PARAM_INT('o', 123, 123, 321, "RGB PWM Assignment", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
@@ -290,7 +253,7 @@ I2CSPIDriverBase *RGBLED_NCP5623C::instantiate(const BusCLIArguments &cli, const
 		int runtime_instance)
 {
 	RGBLED_NCP5623C *instance = new RGBLED_NCP5623C(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency,
-			cli.i2c_address, cli.custom1);
+			cli.i2c_address);
 
 	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
@@ -310,19 +273,9 @@ extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[])
 	using ThisDriver = RGBLED_NCP5623C;
 	BusCLIArguments cli{true, false};
 	cli.default_i2c_frequency = 100000;
-	cli.i2c_address = NCP5623C_ADDR;
-	cli.custom1 = 123;
-	int ch;
+	cli.i2c_address = ADDR;
 
-	while ((ch = cli.getOpt(argc, argv, "o:")) != EOF) {
-		switch (ch) {
-		case 'o':
-			cli.custom1 = atoi(cli.optArg());
-			break;
-		}
-	}
-
-	const char *verb = cli.optArg();
+	const char *verb = cli.parseDefaultArguments(argc, argv);
 
 	if (!verb) {
 		ThisDriver::print_usage();

src/drivers/magnetometer/vtrantech/vcm1193l/vcm1193l_main.cpp

@@ -70,8 +70,6 @@ extern "C" int vcm1193l_main(int argc, char *argv[])
 	using ThisDriver = VCM1193L;
 	BusCLIArguments cli{true, false};
 	cli.default_i2c_frequency = I2C_SPEED;
-	cli.i2c_address = I2C_ADDRESS_DEFAULT;
-
 
 	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
 		switch (ch) {

src/drivers/px4io/px4io.cpp

@@ -1212,7 +1212,6 @@ void PX4IO::update_params()
 	int32_t pwm_max_default = PWM_DEFAULT_MAX;
 	int32_t pwm_disarmed_default = 0;
 	int32_t pwm_rate_default = 50;
-	int32_t pwm_default_channels = 0;
 
 	const char *prefix = "PWM_MAIN";
 
@@ -1220,18 +1219,10 @@ void PX4IO::update_params()
 	param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
 	param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
 	param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default);
-	param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);
-
-	uint32_t single_ch = 0;
-	uint32_t pwm_default_channel_mask = 0;
-
-	while ((single_ch = pwm_default_channels % 10)) {
-		pwm_default_channel_mask |= 1 << (single_ch - 1);
-		pwm_default_channels /= 10;
-	}
 
 	char str[17];
 
+
 	// PWM_MAIN_MINx
 	if (!_pwm_min_configured) {
 		pwm_output_values pwm{};
@@ -1250,7 +1241,7 @@ void PX4IO::update_params()
 						param_set(param_find(str), &pwm_min_new);
 					}
 
-				} else if (pwm_default_channel_mask & 1 << i) {
+				} else {
 					pwm.values[i] = pwm_min_default;
 				}
 			}
@@ -1279,7 +1270,7 @@ void PX4IO::update_params()
 						param_set(param_find(str), &pwm_max_new);
 					}
 
-				} else if (pwm_default_channel_mask & 1 << i) {
+				} else {
 					pwm.values[i] = pwm_max_default;
 				}
 			}
@@ -1334,7 +1325,7 @@ void PX4IO::update_params()
 						param_set(param_find(str), &pwm_dis_new);
 					}
 
-				} else if (pwm_default_channel_mask & 1 << i) {
+				} else {
 					pwm.values[i] = pwm_disarmed_default;
 				}
 			}

src/lib/systemlib/system_params.c

@@ -191,20 +191,6 @@ PARAM_DEFINE_INT32(SYS_CAL_TMIN, 5);
  */
 PARAM_DEFINE_INT32(SYS_CAL_TMAX, 10);
 
-/**
- * Control if the vehicle has a GPS
- *
- * Disable this if the system has no GPS.
- * If disabled, the sensors hub will not process sensor_gps,
- * and GPS will not be available for the rest of the system.
- *
- * @boolean
- * @reboot_required true
- *
- * @group System
- */
-PARAM_DEFINE_INT32(SYS_HAS_GPS, 1);
-
 /**
  * Control if the vehicle has a magnetometer
  *

src/modules/ekf2/EKF2.cpp

@@ -353,7 +353,7 @@ void EKF2::Run()
 		}
 
 	} else {
-		const unsigned last_generation = _sensor_combined_sub.get_last_generation();
+		const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
 		sensor_combined_s sensor_combined;
 		imu_updated = _sensor_combined_sub.update(&sensor_combined);
 

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -74,18 +74,6 @@ bool FlightTaskManualAcceleration::update()
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
 
 	_constraints.want_takeoff = _checkTakeoff();
-
-	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
-	if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) {
-		_yaw_setpoint = NAN;
-
-		// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
-		if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
-			// vehicle is steady
-			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
-		}
-	}
-
 	return ret;
 }
 

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -52,17 +52,10 @@ public:
 	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool update() override;
 
-	/**
-	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
-	 */
-	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
-
 private:
 	StickAccelerationXY _stick_acceleration_xy;
 	StickYaw _stick_yaw;
 
 	void _ekfResetHandlerPositionXY() override;
 	void _ekfResetHandlerVelocityXY() override;
-
-	WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -911,12 +911,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					(type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? 0.f : target_local_ned.vz
 				};
 
-
-				const float yaw = matrix::Eulerf{R}(2);
-
-				setpoint.vx = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1);
-				setpoint.vy = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1);
-				setpoint.vz = velocity_body_sp(2);
+				const matrix::Vector3f velocity_setpoint{R * velocity_body_sp};
+				setpoint.vx = velocity_setpoint(0);
+				setpoint.vy = velocity_setpoint(1);
+				setpoint.vz = velocity_setpoint(2);
 
 			} else {
 				setpoint.vx = NAN;
@@ -2523,25 +2521,28 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	mavlink_hil_state_quaternion_t hil_state;
 	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
-	const uint64_t timestamp_sample = hrt_absolute_time();
+	const uint64_t timestamp = hrt_absolute_time();
 
 	/* airspeed */
 	{
 		airspeed_s airspeed{};
+
+		airspeed.timestamp = timestamp;
 		airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
 		airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
-		airspeed.air_temperature_celsius = 15.f;
-		airspeed.timestamp = hrt_absolute_time();
+
 		_airspeed_pub.publish(airspeed);
 	}
 
 	/* attitude */
 	{
 		vehicle_attitude_s hil_attitude{};
-		hil_attitude.timestamp_sample = timestamp_sample;
+
+		hil_attitude.timestamp = timestamp;
+
 		matrix::Quatf q(hil_state.attitude_quaternion);
 		q.copyTo(hil_attitude.q);
-		hil_attitude.timestamp = hrt_absolute_time();
+
 		_attitude_pub.publish(hil_attitude);
 	}
 
@@ -2549,13 +2550,13 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	{
 		vehicle_global_position_s hil_global_pos{};
 
-		hil_global_pos.timestamp_sample = timestamp_sample;
+		hil_global_pos.timestamp = timestamp;
 		hil_global_pos.lat = hil_state.lat / ((double)1e7);
 		hil_global_pos.lon = hil_state.lon / ((double)1e7);
 		hil_global_pos.alt = hil_state.alt / 1000.0f;
-		hil_global_pos.eph = 2.f;
-		hil_global_pos.epv = 4.f;
-		hil_global_pos.timestamp = hrt_absolute_time();
+		hil_global_pos.eph = 2.0f;
+		hil_global_pos.epv = 4.0f;
+
 		_global_pos_pub.publish(hil_global_pos);
 	}
 
@@ -2564,31 +2565,32 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		const double lat = hil_state.lat * 1e-7;
 		const double lon = hil_state.lon * 1e-7;
 
-		if (!map_projection_initialized(&_global_local_proj_ref) || !PX4_ISFINITE(_global_local_alt0)) {
-			map_projection_init(&_global_local_proj_ref, lat, lon);
-			_global_local_alt0 = hil_state.alt / 1000.f;
-		}
+		map_projection_reference_s global_local_proj_ref;
+		map_projection_init(&global_local_proj_ref, lat, lon);
 
-		float x = 0.f;
-		float y = 0.f;
-		map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
+		float global_local_alt0 = hil_state.alt / 1000.f;
+
+		float x = 0.0f;
+		float y = 0.0f;
+		map_projection_project(&global_local_proj_ref, lat, lon, &x, &y);
 
 		vehicle_local_position_s hil_local_pos{};
-		hil_local_pos.timestamp_sample = timestamp_sample;
-		hil_local_pos.ref_timestamp = _global_local_proj_ref.timestamp;
-		hil_local_pos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
-		hil_local_pos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
-		hil_local_pos.ref_alt = _global_local_alt0;
+		hil_local_pos.timestamp = timestamp;
+
+		hil_local_pos.ref_timestamp = global_local_proj_ref.timestamp;
+		hil_local_pos.ref_lat = math::degrees(global_local_proj_ref.lat_rad);
+		hil_local_pos.ref_lon = math::degrees(global_local_proj_ref.lon_rad);
+		hil_local_pos.ref_alt = global_local_alt0;
 		hil_local_pos.xy_valid = true;
 		hil_local_pos.z_valid = true;
 		hil_local_pos.v_xy_valid = true;
 		hil_local_pos.v_z_valid = true;
 		hil_local_pos.x = x;
 		hil_local_pos.y = y;
-		hil_local_pos.z = _global_local_alt0 - hil_state.alt / 1000.f;
-		hil_local_pos.vx = hil_state.vx / 100.f;
-		hil_local_pos.vy = hil_state.vy / 100.f;
-		hil_local_pos.vz = hil_state.vz / 100.f;
+		hil_local_pos.z = global_local_alt0 - hil_state.alt / 1000.0f;
+		hil_local_pos.vx = hil_state.vx / 100.0f;
+		hil_local_pos.vy = hil_state.vy / 100.0f;
+		hil_local_pos.vz = hil_state.vz / 100.0f;
 
 		matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)};
 		hil_local_pos.heading = euler.psi();
@@ -2598,7 +2600,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.vz_max = INFINITY;
 		hil_local_pos.hagl_min = INFINITY;
 		hil_local_pos.hagl_max = INFINITY;
-		hil_local_pos.timestamp = hrt_absolute_time();
+
 		_local_pos_pub.publish(hil_local_pos);
 	}
 
@@ -2616,7 +2618,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		if (_px4_accel != nullptr) {
 			// accel in mG
 			_px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f);
-			_px4_accel->update(timestamp_sample, hil_state.xacc, hil_state.yacc, hil_state.zacc);
+			_px4_accel->update(timestamp, hil_state.xacc, hil_state.yacc, hil_state.zacc);
 		}
 	}
 
@@ -2632,18 +2634,20 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		}
 
 		if (_px4_gyro != nullptr) {
-			_px4_gyro->update(timestamp_sample, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
+			_px4_gyro->update(timestamp, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
 		}
 	}
 
 	/* battery status */
 	{
 		battery_status_s hil_battery_status{};
+
+		hil_battery_status.timestamp = timestamp;
 		hil_battery_status.voltage_v = 11.1f;
 		hil_battery_status.voltage_filtered_v = 11.1f;
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
-		hil_battery_status.timestamp = hrt_absolute_time();
+
 		_battery_pub.publish(hil_battery_status);
 	}
 }

src/modules/mavlink/mavlink_receiver.h

@@ -342,9 +342,6 @@ private:
 	uint8_t				_mom_switch_pos[MOM_SWITCH_COUNT] {};
 	uint16_t			_mom_switch_state{0};
 
-	map_projection_reference_s _global_local_proj_ref{};
-	float _global_local_alt0{NAN};
-
 	hrt_abstime			_last_utm_global_pos_com{0};
 
 	// Allocated if needed.

src/modules/px4iofirmware/mixer.cpp

@@ -74,7 +74,7 @@ static volatile bool mixer_servos_armed = false;
 static volatile bool should_arm = false;
 static volatile bool should_arm_nothrottle = false;
 static volatile bool should_always_enable_pwm = false;
-static volatile bool mix_failsafe = false;
+static volatile bool in_mixer = false;
 
 static bool new_fmu_data = false;
 static uint64_t last_fmu_update = 0;
@@ -94,22 +94,31 @@ enum mixer_source {
 static volatile mixer_source source;
 
 static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
-static int mixer_handle_text_create_mixer();
-static void mixer_mix_failsafe();
+static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits);
 
 static MixerGroup mixer_group;
 
+int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
+{
+	/* poor mans mutex */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) == 0) {
+		return 0;
+	}
+
+	in_mixer = true;
+	int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+	*limits = mixer_group.get_saturation_status();
+	in_mixer = false;
+
+	return mixcount;
+}
+
 void
 mixer_tick()
 {
 	/* check if the mixer got modified */
 	mixer_handle_text_create_mixer();
 
-	if (mix_failsafe) {
-		mixer_mix_failsafe();
-		mix_failsafe = false;
-	}
-
 	/* check that we are receiving fresh data from the FMU */
 	irqstate_t irq_flags = enter_critical_section();
 	const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
@@ -305,13 +314,7 @@ mixer_tick()
 		}
 
 		/* mix */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
-			mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
-			r_mixer_limits = mixer_group.get_saturation_status();
-
-		} else {
-			mixed = 0;
-		}
+		mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
 
 		/* the pwm limit call takes care of out of band errors */
 		output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
@@ -477,7 +480,12 @@ mixer_callback(uintptr_t handle,
 	}
 
 	/* limit output */
-	control = math::constrain(control, -1.f, 1.f);
+	if (control > 1.0f) {
+		control = 1.0f;
+
+	} else if (control < -1.0f) {
+		control = -1.0f;
+	}
 
 	/* motor spinup phase - lock throttle to zero */
 	if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
@@ -512,26 +520,25 @@ mixer_callback(uintptr_t handle,
 
 static char mixer_text[PX4IO_MAX_MIXER_LENGTH];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;
-static volatile bool mixer_update_pending = false;
-static volatile bool mixer_reset_pending = false;
+static bool mixer_update_pending = false;
 
 int
 mixer_handle_text_create_mixer()
 {
+	/* only run on update */
+	if (!mixer_update_pending) {
+		return 0;
+	}
+
 	/* do not allow a mixer change while safety off and FMU armed */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
 	    (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 		return 1;
 	}
 
-	if (mixer_reset_pending) {
-		mixer_group.reset();
-		mixer_reset_pending = false;
-	}
-
-	/* only run on update */
-	if (!mixer_update_pending || (mixer_text_length == 0)) {
-		return 0;
+	/* abort if we're in the mixer - it will be tried again in the next iteration */
+	if (in_mixer) {
+		return 1;
 	}
 
 	/* process the text buffer, adding new mixers as their descriptions can be parsed */
@@ -555,13 +562,11 @@ mixer_handle_text_create_mixer()
 
 	mixer_update_pending = false;
 
-	update_trims = true;
-	update_mc_thrust_param = true;
-
 	return 0;
 }
 
-int interrupt_mixer_handle_text(const void *buffer, size_t length)
+int
+mixer_handle_text(const void *buffer, size_t length)
 {
 	/* do not allow a mixer change while safety off and FMU armed */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
@@ -572,7 +577,16 @@ int interrupt_mixer_handle_text(const void *buffer, size_t length)
 	/* disable mixing, will be enabled once load is complete */
 	atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
 
-	px4io_mixdata *msg = (px4io_mixdata *)buffer;
+	/* set the update flags to dirty so we reload those values after a mixer change */
+	update_trims = true;
+	update_mc_thrust_param = true;
+
+	/* abort if we're in the mixer - the caller is expected to retry */
+	if (in_mixer) {
+		return 1;
+	}
+
+	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
 
 	isr_debug(2, "mix txt %u", length);
 
@@ -587,7 +601,7 @@ int interrupt_mixer_handle_text(const void *buffer, size_t length)
 		isr_debug(2, "reset");
 
 		/* THEN actually delete it */
-		mixer_reset_pending = true;
+		mixer_group.reset();
 		mixer_text_length = 0;
 
 	/* FALLTHROUGH */
@@ -620,18 +634,14 @@ int interrupt_mixer_handle_text(const void *buffer, size_t length)
 	return 0;
 }
 
-void interrupt_mixer_set_failsafe()
-{
-	mix_failsafe = true;
-}
-
 void
-mixer_mix_failsafe()
+mixer_set_failsafe()
 {
 	/*
 	 * Check if a custom failsafe value has been written,
 	 * or if the mixer is not ok and bail out.
 	 */
+
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
 	    !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 		return;
@@ -660,22 +670,19 @@ mixer_mix_failsafe()
 	}
 
 	/* mix */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
-		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
-		r_mixer_limits = mixer_group.get_saturation_status();
-
-	} else {
-		mixed = 0;
-	}
+	mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
 
 	/* scale to PWM and update the servo outputs as required */
 	for (unsigned i = 0; i < mixed; i++) {
+
 		/* scale to servo output */
 		r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
+
 	}
 
 	/* disable the rest of the outputs */
 	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 		r_page_servo_failsafe[i] = 0;
 	}
+
 }

src/modules/px4iofirmware/px4io.h

@@ -193,9 +193,10 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
  * Mixer
  */
 extern void	mixer_tick(void);
-extern int	interrupt_mixer_handle_text(const void *buffer, size_t length);
+extern int	mixer_handle_text_create_mixer(void);
+extern int	mixer_handle_text(const void *buffer, size_t length);
 /* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
-extern void	interrupt_mixer_set_failsafe(void);
+extern void	mixer_set_failsafe(void);
 
 /**
  * Safety switch/LED.

src/modules/px4iofirmware/registers.c

@@ -461,7 +461,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		 * this state defines an active system. This check is done in the
 		 * text handling function.
 		 */
-		return interrupt_mixer_handle_text(values, num_values * sizeof(*values));
+		return mixer_handle_text(values, num_values * sizeof(*values));
 
 	default:
 
@@ -514,8 +514,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			}
 
 			if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
+
 				/* update failsafe values, now that the mixer is set to ok */
-				interrupt_mixer_set_failsafe();
+				mixer_set_failsafe();
 			}
 
 			break;

src/modules/sensors/sensors.cpp

@@ -219,7 +219,6 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
-		(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
@@ -535,10 +534,12 @@ void Sensors::InitializeVehicleAirData()
 {
 	if (_param_sys_has_baro.get()) {
 		if (_vehicle_air_data == nullptr) {
-			_vehicle_air_data = new VehicleAirData();
+			if (orb_exists(ORB_ID(sensor_baro), 0) == PX4_OK) {
+				_vehicle_air_data = new VehicleAirData();
 
-			if (_vehicle_air_data) {
-				_vehicle_air_data->Start();
+				if (_vehicle_air_data) {
+					_vehicle_air_data->Start();
+				}
 			}
 		}
 	}
@@ -546,8 +547,8 @@ void Sensors::InitializeVehicleAirData()
 
 void Sensors::InitializeVehicleGPSPosition()
 {
-	if (_param_sys_has_gps.get()) {
-		if (_vehicle_gps_position == nullptr) {
+	if (_vehicle_gps_position == nullptr) {
+		if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
 			_vehicle_gps_position = new VehicleGPSPosition();
 
 			if (_vehicle_gps_position) {
@@ -601,10 +602,12 @@ void Sensors::InitializeVehicleMagnetometer()
 {
 	if (_param_sys_has_mag.get()) {
 		if (_vehicle_magnetometer == nullptr) {
-			_vehicle_magnetometer = new VehicleMagnetometer();
+			if (orb_exists(ORB_ID(sensor_mag), 0) == PX4_OK) {
+				_vehicle_magnetometer = new VehicleMagnetometer();
 
-			if (_vehicle_magnetometer) {
-				_vehicle_magnetometer->Start();
+				if (_vehicle_magnetometer) {
+					_vehicle_magnetometer->Start();
+				}
 			}
 		}
 	}

src/modules/sensors/voted_sensors_update.cpp

@@ -187,8 +187,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	// find the best sensor
 	int accel_best_index = -1;
 	int gyro_best_index = -1;
-	_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
-	_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
 
 	if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
 		// use sensor_selection to find best
@@ -215,6 +213,9 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 
 	} else {
 		// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
+		_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
+		_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
+
 		checkFailover(_accel, "Accel");
 		checkFailover(_gyro, "Gyro");
 	}