boards: CUAV-x7pro: reoder brick to fixed cuav hvpm cannot detect voltage

- cubepilot_cubeorange_test has the console enabled (used for test rack and bench debugging)

.ci/Jenkinsfile-hardware

@@ -673,153 +673,6 @@ 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 hwtypecmp V5X00 V5X90 V5Xa0
+	if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X
 	then
 		netman update -i eth0
 	fi

boards/cuav/nora/src/init.c

@@ -60,6 +60,8 @@
 #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);
@@ -127,6 +129,16 @@ __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/board_config.h

@@ -117,8 +117,8 @@
 #define GPIO_nPOWER_IN_CAN              /* PG2  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2)
 #define GPIO_nPOWER_IN_C                /* PG0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0)
 
-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */
-#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen  */
+#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen  */
 #define GPIO_nVDD_USB_VALID             GPIO_nPOWER_IN_C   /* USB     is Chosen */
 
 #define GPIO_VDD_5V_HIPOWER_EN          /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)

boards/cuav/x7pro/src/init.c

@@ -60,6 +60,8 @@
 #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);
@@ -127,6 +129,16 @@ __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
+		# TEL3:/dev/ttyS4  # connected to ADS-B receiver
 		GPS2:/dev/ttyS5
 	DRIVERS
 		adc/ads1115

boards/cubepilot/cubeorange/init/rc.board_mavlink

@@ -5,3 +5,9 @@
 
 # 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_STATE_LED  LED_AMBER
+#define BOARD_ARMED_LED  LED_AMBER
 
 /* ADC channels */
 #define PX4_ADC_GPIO  \

boards/cubepilot/cubeorange/src/init.c

@@ -60,6 +60,8 @@
 #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);
@@ -118,6 +120,16 @@ __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_STATE_LED  LED_AMBER
+#define BOARD_ARMED_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=280
+CONFIG_IDLETHREAD_STACKSIZE=316
 CONFIG_MAX_TASKS=2
 CONFIG_MM_FILL_ALLOCATIONS=y
 CONFIG_MM_SMALL=y

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

@@ -75,6 +75,8 @@
 #include <px4_platform/board_determine_hw_info.h>
 #include <px4_platform/board_dma_alloc.h>
 
+#include <mpu.h>
+
 /****************************************************************************
  * Pre-Processor Definitions
  ****************************************************************************/
@@ -160,6 +162,16 @@ __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,6 +60,8 @@
 #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);
@@ -121,6 +123,16 @@ __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/src/init.c

@@ -60,6 +60,8 @@
 #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);
@@ -121,6 +123,16 @@ __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/src/init.c

@@ -60,6 +60,8 @@
 #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);
@@ -121,6 +123,16 @@ __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,6 +235,25 @@ __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,6 +235,25 @@ __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,7 +23,8 @@ px4_add_board(
 	DRIVERS
 		adc/ads1115
 		adc/board_adc
-		barometer # all available barometer drivers
+		#barometer # all available barometer drivers
+		barometer/ms5611
 		batt_smbus
 		camera_capture
 		camera_trigger
@@ -116,7 +117,7 @@ px4_add_board(
 		perf
 		pwm
 		reboot
-		reflect
+		#reflect
 		sd_bench
 		#serial_test
 		system_time

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

@@ -55,17 +55,20 @@ 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 V5X91 V5Xa1
+	if ver hwtypecmp V5X00 V5X90 V5Xa0
 	then
-		bmp388 -X -b 2 start
-	else
 		bmp388 -I start
+	else
+		bmp388 -X -b 2 start
 	fi
 
 fi

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

@@ -21,6 +21,9 @@ 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=280
+CONFIG_IDLETHREAD_STACKSIZE=316
 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, _i2c_address);
+		PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, get_i2c_address());
 
 	} else {
 		PX4_INFO("Running on SPI Bus %i", _bus);

platforms/common/include/px4_platform_common/i2c_spi_buses.h

@@ -62,11 +62,13 @@ 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,6 +96,7 @@ 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,11 +59,20 @@ 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();
 
@@ -83,11 +92,14 @@ bool WorkQueue::Attach(WorkItem *item)
 	}
 
 	work_unlock();
+
 	return false;
 }
 
 void WorkQueue::Detach(WorkItem *item)
 {
+	bool exiting = false;
+
 	work_lock();
 
 	_work_items.remove(item);
@@ -96,11 +108,21 @@ 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/stm/stm32f7/px4io_serial/px4io_serial.cpp

@@ -328,6 +328,8 @@ 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,6 +373,8 @@ 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,7 +34,8 @@
 /**
  * @file rgbled_ncp5623c.cpp
  *
- * Driver for the onboard RGB LED controller (NCP5623C) connected via I2C.
+ * Driver for the onboard RGB LED controller (NCP5623B or NCP5623C)
+ * connected via I2C.
  *
  * @author CUAVcaijie <caijie@cuav.net>
  */
@@ -52,7 +53,8 @@
 
 using namespace time_literals;
 
-#define ADDR			0x39	/**< I2C adress of NCP5623C */
+#define NCP5623B_ADDR       0x38  /**< I2C address of NCP5623B */
+#define NCP5623C_ADDR       0x39  /**< I2C address of NCP5623C */
 
 #define NCP5623_LED_CURRENT 0x20  /**< Current register */
 #define NCP5623_LED_PWM0    0x40  /**< pwm0 register */
@@ -66,7 +68,7 @@ using namespace time_literals;
 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);
+	RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address, const int order);
 	virtual ~RGBLED_NCP5623C() = default;
 
 	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
@@ -77,8 +79,13 @@ 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};
@@ -94,16 +101,40 @@ private:
 
 	LedController		_led_controller;
 
-	int			send_led_rgb();
+	uint8_t		_red{NCP5623_LED_PWM0};
-	void			update_params();
+	uint8_t		_green{NCP5623_LED_PWM1};
-
+	uint8_t		_blue{NCP5623_LED_PWM2};
-	int			write(uint8_t reg, uint8_t data);
 };
 
-RGBLED_NCP5623C::RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) :
+RGBLED_NCP5623C::RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address,
+				 const int order) :
 	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
@@ -138,9 +169,15 @@ RGBLED_NCP5623C::init()
 int
 RGBLED_NCP5623C::probe()
 {
-	_retries = 4;
+	_retries = 2;
+	int status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF);
 
-	return write(NCP5623_LED_CURRENT, 0x00);
+	if (status == PX4_ERROR) {
+		set_device_address(NCP5623B_ADDR);
+		status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF);
+	}
+
+	return status;
 }
 
 void
@@ -212,14 +249,13 @@ 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] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f);
+	msg[2] = _red | (uint8_t(_r * _brightness) & 0x1f);
-	msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f);
+	msg[4] = _green | (uint8_t(_g * _brightness) & 0x1f);
-	msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f);
+	msg[6] = _blue | (uint8_t(_b * _brightness) & 0x1f);
 
 	return transfer(&msg[0], 7, nullptr, 0);
 }
@@ -246,6 +282,7 @@ 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();
 }
 
@@ -253,7 +290,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.i2c_address, cli.custom1);
 
 	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
@@ -273,9 +310,19 @@ 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 = ADDR;
+	cli.i2c_address = NCP5623C_ADDR;
+	cli.custom1 = 123;
+	int ch;
 
-	const char *verb = cli.parseDefaultArguments(argc, argv);
+	while ((ch = cli.getOpt(argc, argv, "o:")) != EOF) {
+		switch (ch) {
+		case 'o':
+			cli.custom1 = atoi(cli.optArg());
+			break;
+		}
+	}
+
+	const char *verb = cli.optArg();
 
 	if (!verb) {
 		ThisDriver::print_usage();

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

@@ -70,6 +70,8 @@ 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/lib/systemlib/system_params.c

@@ -191,6 +191,20 @@ 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 = _vehicle_imu_sub.get_last_generation();
+		const unsigned last_generation = _sensor_combined_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,6 +74,18 @@ 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,10 +52,17 @@ 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,10 +911,12 @@ 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 matrix::Vector3f velocity_setpoint{R * velocity_body_sp};
+
-				setpoint.vx = velocity_setpoint(0);
+				const float yaw = matrix::Eulerf{R}(2);
-				setpoint.vy = velocity_setpoint(1);
+
-				setpoint.vz = velocity_setpoint(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);
 
 			} else {
 				setpoint.vx = NAN;
@@ -2521,28 +2523,25 @@ 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 = hrt_absolute_time();
+	const uint64_t timestamp_sample = 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);
 	}
 
@@ -2550,13 +2549,13 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	{
 		vehicle_global_position_s hil_global_pos{};
 
-		hil_global_pos.timestamp = timestamp;
+		hil_global_pos.timestamp_sample = timestamp_sample;
 		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.0f;
+		hil_global_pos.eph = 2.f;
-		hil_global_pos.epv = 4.0f;
+		hil_global_pos.epv = 4.f;
-
+		hil_global_pos.timestamp = hrt_absolute_time();
 		_global_pos_pub.publish(hil_global_pos);
 	}
 
@@ -2565,32 +2564,31 @@ 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;
 
-		map_projection_reference_s global_local_proj_ref;
+		if (!map_projection_initialized(&_global_local_proj_ref) || !PX4_ISFINITE(_global_local_alt0)) {
-		map_projection_init(&global_local_proj_ref, lat, lon);
+			map_projection_init(&_global_local_proj_ref, lat, lon);
+			_global_local_alt0 = hil_state.alt / 1000.f;
+		}
 
-		float global_local_alt0 = hil_state.alt / 1000.f;
+		float x = 0.f;
-
+		float y = 0.f;
-		float x = 0.0f;
+		map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
-		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 = timestamp;
+		hil_local_pos.timestamp_sample = timestamp_sample;
-
+		hil_local_pos.ref_timestamp = _global_local_proj_ref.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_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_lon = math::degrees(global_local_proj_ref.lon_rad);
+		hil_local_pos.ref_alt = _global_local_alt0;
-		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.0f;
+		hil_local_pos.z = _global_local_alt0 - hil_state.alt / 1000.f;
-		hil_local_pos.vx = hil_state.vx / 100.0f;
+		hil_local_pos.vx = hil_state.vx / 100.f;
-		hil_local_pos.vy = hil_state.vy / 100.0f;
+		hil_local_pos.vy = hil_state.vy / 100.f;
-		hil_local_pos.vz = hil_state.vz / 100.0f;
+		hil_local_pos.vz = hil_state.vz / 100.f;
 
 		matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)};
 		hil_local_pos.heading = euler.psi();
@@ -2600,7 +2598,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);
 	}
 
@@ -2618,7 +2616,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, hil_state.xacc, hil_state.yacc, hil_state.zacc);
+			_px4_accel->update(timestamp_sample, hil_state.xacc, hil_state.yacc, hil_state.zacc);
 		}
 	}
 
@@ -2634,20 +2632,18 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		}
 
 		if (_px4_gyro != nullptr) {
-			_px4_gyro->update(timestamp, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
+			_px4_gyro->update(timestamp_sample, 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,6 +342,9 @@ 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 in_mixer = false;
+static volatile bool mix_failsafe = false;
 
 static bool new_fmu_data = false;
 static uint64_t last_fmu_update = 0;
@@ -94,31 +94,22 @@ 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_mix_threadsafe(float *outputs, volatile uint16_t *limits);
+static int mixer_handle_text_create_mixer();
+static void mixer_mix_failsafe();
 
 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;
@@ -314,7 +305,13 @@ mixer_tick()
 		}
 
 		/* mix */
-		mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
+		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;
+		}
 
 		/* 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,
@@ -480,12 +477,7 @@ mixer_callback(uintptr_t handle,
 	}
 
 	/* limit output */
-	if (control > 1.0f) {
+	control = math::constrain(control, -1.f, 1.f);
-		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)) {
@@ -520,25 +512,26 @@ 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 bool mixer_update_pending = false;
+static volatile bool mixer_update_pending = false;
+static volatile bool mixer_reset_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;
 	}
 
-	/* abort if we're in the mixer - it will be tried again in the next iteration */
+	if (mixer_reset_pending) {
-	if (in_mixer) {
+		mixer_group.reset();
-		return 1;
+		mixer_reset_pending = false;
+	}
+
+	/* only run on update */
+	if (!mixer_update_pending || (mixer_text_length == 0)) {
+		return 0;
 	}
 
 	/* process the text buffer, adding new mixers as their descriptions can be parsed */
@@ -562,11 +555,13 @@ mixer_handle_text_create_mixer()
 
 	mixer_update_pending = false;
 
+	update_trims = true;
+	update_mc_thrust_param = true;
+
 	return 0;
 }
 
-int
+int interrupt_mixer_handle_text(const void *buffer, size_t length)
-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) &&
@@ -577,15 +572,6 @@ 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);
 
-	/* 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);
@@ -601,7 +587,7 @@ mixer_handle_text(const void *buffer, size_t length)
 		isr_debug(2, "reset");
 
 		/* THEN actually delete it */
-		mixer_group.reset();
+		mixer_reset_pending = true;
 		mixer_text_length = 0;
 
 	/* FALLTHROUGH */
@@ -634,14 +620,18 @@ mixer_handle_text(const void *buffer, size_t length)
 	return 0;
 }
 
+void interrupt_mixer_set_failsafe()
+{
+	mix_failsafe = true;
+}
+
 void
-mixer_set_failsafe()
+mixer_mix_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;
@@ -670,19 +660,22 @@ mixer_set_failsafe()
 	}
 
 	/* mix */
-	mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
+	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;
+	}
 
 	/* 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,10 +193,9 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
  * Mixer
  */
 extern void	mixer_tick(void);
-extern int	mixer_handle_text_create_mixer(void);
+extern int	interrupt_mixer_handle_text(const void *buffer, size_t length);
-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	mixer_set_failsafe(void);
+extern void	interrupt_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 mixer_handle_text(values, num_values * sizeof(*values));
+		return interrupt_mixer_handle_text(values, num_values * sizeof(*values));
 
 	default:
 
@@ -514,9 +514,8 @@ 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 */
-				mixer_set_failsafe();
+				interrupt_mixer_set_failsafe();
 			}
 
 			break;

src/modules/sensors/sensors.cpp

@@ -219,6 +219,7 @@ 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
 	)
@@ -534,7 +535,6 @@ void Sensors::InitializeVehicleAirData()
 {
 	if (_param_sys_has_baro.get()) {
 		if (_vehicle_air_data == nullptr) {
-			if (orb_exists(ORB_ID(sensor_baro), 0) == PX4_OK) {
 			_vehicle_air_data = new VehicleAirData();
 
 			if (_vehicle_air_data) {
@@ -542,13 +542,12 @@ void Sensors::InitializeVehicleAirData()
 			}
 		}
 	}
-	}
 }
 
 void Sensors::InitializeVehicleGPSPosition()
 {
+	if (_param_sys_has_gps.get()) {
 		if (_vehicle_gps_position == nullptr) {
-		if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
 			_vehicle_gps_position = new VehicleGPSPosition();
 
 			if (_vehicle_gps_position) {
@@ -602,7 +601,6 @@ void Sensors::InitializeVehicleMagnetometer()
 {
 	if (_param_sys_has_mag.get()) {
 		if (_vehicle_magnetometer == nullptr) {
-			if (orb_exists(ORB_ID(sensor_mag), 0) == PX4_OK) {
 			_vehicle_magnetometer = new VehicleMagnetometer();
 
 			if (_vehicle_magnetometer) {
@@ -610,7 +608,6 @@ void Sensors::InitializeVehicleMagnetometer()
 			}
 		}
 	}
-	}
 }
 
 void Sensors::Run()

src/modules/sensors/voted_sensors_update.cpp

@@ -187,6 +187,8 @@ 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
@@ -213,9 +215,6 @@ 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");
 	}