boards: cubepilot_cubeorange only start ADSB mavlink if console not present

- cubepilot_cubeorange_test has the console enabled (used for test rack and bench debugging)
Jenkins: hardware remove boards that aren't present
2026-05-26 23:40:06 +08:00 · 2021-09-06 11:57:42 -04:00 · 2021-09-06 10:28:52 -04:00 · 2021-09-06 10:28:17 -04:00 · 2021-09-06 10:26:49 -04:00 · 2021-09-06 10:24:43 -04:00
68 changed files with 847 additions and 650 deletions
@@ -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") {
@@ -51,6 +51,16 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: px4_fmu-v5x_default
+    px4_fmu-v6x_default:
+      short: px4_fmu-v6x
+      buildType: MinSizeRel
+      settings:
+        CONFIG: px4_fmu-v6x_default
+    px4_fmu-v6x_bootloader:
+      short: px4_fmu-v6x_bootloader
+      buildType: MinSizeRel
+      settings:
+        CONFIG: px4_fmu-v6x_bootloader
    airmind_mindpx-v2_default:
      short: airmind_mindpx-v2
      buildType: MinSizeRel
@@ -226,6 +226,8 @@ then
 	fi
 fi

+param set PWM_AUX_OUT ${PWM_AUX_OUT}
+
 if [ $MIXER_AUX != none -a $AUX_MODE = none -a -e $OUTPUT_AUX_DEV ]
 then
 	#
@@ -268,6 +270,8 @@ then
 	fi
 fi

+param set PWM_MAIN_OUT ${PWM_OUT}
+
 if [ $EXTRA_MIXER_MODE != none ]
 then

@@ -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
@@ -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);

@@ -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);

@@ -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
@@ -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
@@ -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  \
@@ -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);

@@ -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  \
@@ -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
@@ -65,11 +65,11 @@ using namespace time_literals;
 #define NCP5623_LED_OFF     0x00  /**< off */


-class RGBLED_NPC5623C : public device::I2C
+class RGBLED_NCP5623C : public device::I2C
 {
 public:
-	RGBLED_NPC5623C(const int bus, int bus_frequency, const int address);
-	virtual ~RGBLED_NPC5623C() = default;
+	RGBLED_NCP5623C(const int bus, int bus_frequency, const int address);
+	virtual ~RGBLED_NCP5623C() = default;

 	int   init() override;
 	int   probe() override;
@@ -84,13 +84,13 @@ private:
 	int     write(uint8_t reg, uint8_t data);
 };

-RGBLED_NPC5623C::RGBLED_NPC5623C(const int bus, int bus_frequency, const int address) :
+RGBLED_NCP5623C::RGBLED_NCP5623C(const int bus, int bus_frequency, const int address) :
 	I2C(DRV_LED_DEVTYPE_RGBLED_NCP5623C, MODULE_NAME, bus, address, bus_frequency)
 {
 }

 int
-RGBLED_NPC5623C::write(uint8_t reg, uint8_t data)
+RGBLED_NCP5623C::write(uint8_t reg, uint8_t data)
 {
 	uint8_t msg[1] = { 0x00 };
 	msg[0] = ((reg & 0xe0) | (data & 0x1f));
@@ -101,7 +101,7 @@ RGBLED_NPC5623C::write(uint8_t reg, uint8_t data)
 }

 int
-RGBLED_NPC5623C::init()
+RGBLED_NCP5623C::init()
 {
 	int ret = I2C::init();

@@ -113,7 +113,7 @@ RGBLED_NPC5623C::init()
 }

 int
-RGBLED_NPC5623C::probe()
+RGBLED_NCP5623C::probe()
 {
 	_retries = 4;

@@ -125,7 +125,7 @@ RGBLED_NPC5623C::probe()
 * Send RGB PWM settings to LED driver according to current color and brightness
 */
 int
-RGBLED_NPC5623C::send_led_rgb(uint8_t red, uint8_t green, uint8_t blue)
+RGBLED_NCP5623C::send_led_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {

 	uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80};
@@ -139,7 +139,7 @@ RGBLED_NPC5623C::send_led_rgb(uint8_t red, uint8_t green, uint8_t blue)
 	return transfer(&msg[0], 7, nullptr, 0);
 }

-static RGBLED_NPC5623C instance(1, 100000, ADDR);
+static RGBLED_NCP5623C instance(1, 100000, ADDR);

 #define TMR_BASE        STM32_TIM1_BASE
 #define TMR_FREQUENCY   STM32_APB2_TIM1_CLKIN
@@ -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 */
@@ -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);

@@ -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);

@@ -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);

@@ -16,4 +16,4 @@ bmi088 -A -R 8 -s start
 bmi088 -G -R 8 -s start

 # Internal SPI bus ICM-42688
-#icm42688p -R 12 -s start
+icm42688p -R 12 -s start
@@ -6,6 +6,7 @@
 # modifications.
 #
 # CONFIG_DISABLE_OS_API is not set
+# CONFIG_KINETIS_EDMA_HOE is not set
 # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
 # CONFIG_MMCSD_MMCSUPPORT is not set
 # CONFIG_MMCSD_SPI is not set
@@ -65,7 +66,11 @@ CONFIG_IOB_THROTTLE=0
 CONFIG_KINETIS_ADC0=y
 CONFIG_KINETIS_ADC1=y
 CONFIG_KINETIS_CRC=y
-CONFIG_KINETIS_DMA=y
+CONFIG_KINETIS_EDMA=y
+CONFIG_KINETIS_EDMA_ELINK=y
+CONFIG_KINETIS_EDMA_ERCA=y
+CONFIG_KINETIS_EDMA_ERGA=y
+CONFIG_KINETIS_EDMA_NTCD=16
 CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y
 CONFIG_KINETIS_ENET=y
 CONFIG_KINETIS_FLEXCAN0=y
@@ -88,7 +93,9 @@ CONFIG_KINETIS_SDHC=y
 CONFIG_KINETIS_SERIALBRK_BSDCOMPAT=y
 CONFIG_KINETIS_SPI0=y
 CONFIG_KINETIS_SPI1=y
+CONFIG_KINETIS_SPI1_DMA=y
 CONFIG_KINETIS_SPI2=y
+CONFIG_KINETIS_SPI_DMA=y
 CONFIG_KINETIS_UART0=y
 CONFIG_KINETIS_UART0_RXDMA=y
 CONFIG_KINETIS_UART1=y
@@ -6,6 +6,7 @@
 # modifications.
 #
 # CONFIG_DISABLE_OS_API is not set
+# CONFIG_KINETIS_EDMA_HOE is not set
 # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
 # CONFIG_MMCSD_MMCSUPPORT is not set
 # CONFIG_MMCSD_SPI is not set
@@ -66,7 +67,11 @@ CONFIG_IOB_NCHAINS=18
 CONFIG_KINETIS_ADC0=y
 CONFIG_KINETIS_ADC1=y
 CONFIG_KINETIS_CRC=y
-CONFIG_KINETIS_DMA=y
+CONFIG_KINETIS_EDMA=y
+CONFIG_KINETIS_EDMA_ELINK=y
+CONFIG_KINETIS_EDMA_ERCA=y
+CONFIG_KINETIS_EDMA_ERGA=y
+CONFIG_KINETIS_EDMA_NTCD=16
 CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y
 CONFIG_KINETIS_ENET=y
 CONFIG_KINETIS_FLEXCAN0=y
@@ -89,7 +94,9 @@ CONFIG_KINETIS_SDHC=y
 CONFIG_KINETIS_SERIALBRK_BSDCOMPAT=y
 CONFIG_KINETIS_SPI0=y
 CONFIG_KINETIS_SPI1=y
+CONFIG_KINETIS_SPI1_DMA=y
 CONFIG_KINETIS_SPI2=y
+CONFIG_KINETIS_SPI_DMA=y
 CONFIG_KINETIS_UART0=y
 CONFIG_KINETIS_UART0_RXDMA=y
 CONFIG_KINETIS_UART1=y
@@ -5,6 +5,7 @@
 # You can then do "make savedefconfig" to generate a new defconfig file that includes your
 # modifications.
 #
+# CONFIG_KINETIS_EDMA_HOE is not set
 # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
 # CONFIG_MMCSD_MMCSUPPORT is not set
 # CONFIG_MMCSD_SPI is not set
@@ -66,7 +67,11 @@ CONFIG_IOB_THROTTLE=0
 CONFIG_KINETIS_ADC0=y
 CONFIG_KINETIS_ADC1=y
 CONFIG_KINETIS_CRC=y
-CONFIG_KINETIS_DMA=y
+CONFIG_KINETIS_EDMA=y
+CONFIG_KINETIS_EDMA_ELINK=y
+CONFIG_KINETIS_EDMA_ERCA=y
+CONFIG_KINETIS_EDMA_ERGA=y
+CONFIG_KINETIS_EDMA_NTCD=16
 CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y
 CONFIG_KINETIS_ENET=y
 CONFIG_KINETIS_FLEXCAN0=y
@@ -5,6 +5,7 @@
 # You can then do "make savedefconfig" to generate a new defconfig file that includes your
 # modifications.
 #
+# CONFIG_KINETIS_EDMA_HOE is not set
 # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
 # CONFIG_MMCSD_MMCSUPPORT is not set
 # CONFIG_MMCSD_SPI is not set
@@ -67,7 +68,11 @@ CONFIG_IOB_NCHAINS=18
 CONFIG_KINETIS_ADC0=y
 CONFIG_KINETIS_ADC1=y
 CONFIG_KINETIS_CRC=y
-CONFIG_KINETIS_DMA=y
+CONFIG_KINETIS_EDMA=y
+CONFIG_KINETIS_EDMA_ELINK=y
+CONFIG_KINETIS_EDMA_ERCA=y
+CONFIG_KINETIS_EDMA_ERGA=y
+CONFIG_KINETIS_EDMA_NTCD=16
 CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y
 CONFIG_KINETIS_ENET=y
 CONFIG_KINETIS_FLEXCAN0=y
@@ -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
@@ -38,7 +38,7 @@ px4_add_board(
 		pwm_out
 		px4io
 		#telemetry # all available telemetry drivers
-		telemetry/iridiumsbd
+		#telemetry/iridiumsbd
 		tone_alarm
 		#uavcan
 	MODULES
@@ -48,7 +48,7 @@ px4_add_board(
 		#events
 		land_detector
 		landing_target_estimator
-		load_mon
+		#load_mon
 		#local_position_estimator
 		logger
 		mavlink
@@ -40,7 +40,6 @@ px4_add_board(
 		imu/lsm303d
 		imu/invensense/mpu6000
 		#imu/invensense/mpu9250
-		#iridiumsbd
 		#irlock
 		lights/rgbled
 		#magnetometer # all available magnetometer drivers
@@ -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
@@ -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
@@ -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

@@ -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
@@ -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);
@@ -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;

@@ -362,17 +362,16 @@ protected:
 	 * @brief Waits until _object is initialized, (from the new thread). This can be called from task_spawn().
 	 * @return Returns 0 iff successful, -1 on timeout or otherwise.
 	 */
-	static int wait_until_running()
+	static int wait_until_running(int timeout_ms = 1000)
 	{
 		int i = 0;

 		do {
-			/* Wait up to 1s. */
-			px4_usleep(2500);
+			px4_usleep(2000);

-		} while (!_object.load() && ++i < 400);
+		} while (!_object.load() && ++i < timeout_ms / 2);

-		if (i == 400) {
+		if (i >= timeout_ms / 2) {
 			PX4_ERR("Timed out while waiting for thread to start");
 			return -1;
 		}
@@ -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};
@@ -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)
@@ -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, "eeprom_trans");
-		priv->perf_resets_retries = perf_alloc(PC_COUNT, "eeprom_rst");
-		priv->perf_errors = perf_alloc(PC_COUNT, "eeprom_errs");
+		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");
 	}

 	/* attempt to read to validate device is present */
@@ -600,6 +600,14 @@ 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;
 	}

@@ -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;
@@ -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;
@@ -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,22 +53,23 @@

 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 */
-#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_NPC5623C : public device::I2C, public I2CSPIDriver<RGBLED_NPC5623C>
+class RGBLED_NCP5623C : public device::I2C, public I2CSPIDriver<RGBLED_NCP5623C>
 {
 public:
-	RGBLED_NPC5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address);
-	virtual ~RGBLED_NPC5623C() = default;
+	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,
 					     int runtime_instance);
@@ -77,15 +79,20 @@ 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};
@@ -94,20 +101,44 @@ private:

 	LedController		_led_controller;

-	int			send_led_rgb();
-	void			update_params();
-
-	int			write(uint8_t reg, uint8_t data);
+	uint8_t		_red{NCP5623_LED_PWM0};
+	uint8_t		_green{NCP5623_LED_PWM1};
+	uint8_t		_blue{NCP5623_LED_PWM2};
 };

-RGBLED_NPC5623C::RGBLED_NPC5623C(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
-RGBLED_NPC5623C::write(uint8_t reg, uint8_t data)
+RGBLED_NCP5623C::write(uint8_t reg, uint8_t data)
 {
 	uint8_t msg[1] = { 0x00 };
 	msg[0] = ((reg & 0xe0) | (data & 0x1f));
@@ -118,7 +149,7 @@ RGBLED_NPC5623C::write(uint8_t reg, uint8_t data)
 }

 int
-RGBLED_NPC5623C::init()
+RGBLED_NCP5623C::init()
 {
 	int ret = I2C::init();

@@ -136,15 +167,21 @@ RGBLED_NPC5623C::init()
 }

 int
-RGBLED_NPC5623C::probe()
+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
-RGBLED_NPC5623C::RunImpl()
+RGBLED_NCP5623C::RunImpl()
 {
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
@@ -210,22 +247,21 @@ RGBLED_NPC5623C::RunImpl()
 * Send RGB PWM settings to LED driver according to current color and brightness
 */
 int
-RGBLED_NPC5623C::send_led_rgb()
+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[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f);
-	msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _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);

 	return transfer(&msg[0], 7, nullptr, 0);
 }

 void
-RGBLED_NPC5623C::update_params()
+RGBLED_NCP5623C::update_params()
 {
 	int32_t maxbrt = 31;
 	param_get(param_find("LED_RGB1_MAXBRT"), &maxbrt);
@@ -240,20 +276,21 @@ RGBLED_NPC5623C::update_params()
 }

 void
-RGBLED_NPC5623C::print_usage()
+RGBLED_NCP5623C::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("rgbled", "driver");
 	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();
 }

-I2CSPIDriverBase *RGBLED_NPC5623C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+I2CSPIDriverBase *RGBLED_NCP5623C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 		int runtime_instance)
 {
-	RGBLED_NPC5623C *instance = new RGBLED_NPC5623C(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency,
-			cli.i2c_address);
+	RGBLED_NCP5623C *instance = new RGBLED_NCP5623C(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency,
+			cli.i2c_address, cli.custom1);

 	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
@@ -270,12 +307,22 @@ I2CSPIDriverBase *RGBLED_NPC5623C::instantiate(const BusCLIArguments &cli, const

 extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[])
 {
-	using ThisDriver = RGBLED_NPC5623C;
+	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();
@@ -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) {
@@ -663,6 +663,7 @@ void PWMOut::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;

@@ -673,6 +674,7 @@ void PWMOut::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);

 	} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
 		prefix = "PWM_AUX";
@@ -681,6 +683,7 @@ void PWMOut::update_params()
 		param_get(param_find("PWM_AUX_MAX"), &pwm_max_default);
 		param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default);
 		param_get(param_find("PWM_AUX_RATE"), &pwm_rate_default);
+		param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels);

 	} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
 		prefix = "PWM_EXTRA";
@@ -695,6 +698,14 @@ void PWMOut::update_params()
 		return;
 	}

+	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;
+	}
+
 	// update the counter
 	// this is needed to decide if disarmed PWM output should be turned on or not
 	int num_disarmed_set = 0;
@@ -716,7 +727,7 @@ void PWMOut::update_params()
 						param_set(param_find(str), &pwm_min_new);
 					}

-				} else {
+				} else if (pwm_default_channel_mask & 1 << i) {
 					_mixing_output.minValue(i) = pwm_min_default;
 				}
 			}
@@ -736,7 +747,7 @@ void PWMOut::update_params()
 						param_set(param_find(str), &pwm_max_new);
 					}

-				} else {
+				} else if (pwm_default_channel_mask & 1 << i) {
 					_mixing_output.maxValue(i) = pwm_max_default;
 				}
 			}
@@ -773,7 +784,7 @@ void PWMOut::update_params()
 						param_set(param_find(str), &pwm_dis_new);
 					}

-				} else {
+				} else if (pwm_default_channel_mask & 1 << i) {
 					_mixing_output.disarmedValue(i) = pwm_disarmed_default;
 				}
 			}
@@ -853,6 +853,9 @@ PX4IO::init()
 		_primary_pwm_device = true;
 	}

+	/* ensure PWM limits are applied before any other module starts */
+	update_params();
+
 	/* start the IO interface task */
 	_task = px4_task_spawn_cmd("px4io",
 				   SCHED_DEFAULT,
@@ -1209,6 +1212,7 @@ 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";

@@ -1216,10 +1220,18 @@ 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{};
@@ -1238,7 +1250,7 @@ void PX4IO::update_params()
 						param_set(param_find(str), &pwm_min_new);
 					}

-				} else {
+				} else if (pwm_default_channel_mask & 1 << i) {
 					pwm.values[i] = pwm_min_default;
 				}
 			}
@@ -1267,7 +1279,7 @@ void PX4IO::update_params()
 						param_set(param_find(str), &pwm_max_new);
 					}

-				} else {
+				} else if (pwm_default_channel_mask & 1 << i) {
 					pwm.values[i] = pwm_max_default;
 				}
 			}
@@ -1322,7 +1334,7 @@ void PX4IO::update_params()
 						param_set(param_find(str), &pwm_dis_new);
 					}

-				} else {
+				} else if (pwm_default_channel_mask & 1 << i) {
 					pwm.values[i] = pwm_disarmed_default;
 				}
 			}
@@ -34,4 +34,4 @@
 add_subdirectory(bst)
 add_subdirectory(frsky_telemetry)
 add_subdirectory(hott)
-add_subdirectory(iridiumsbd)
+#add_subdirectory(iridiumsbd)
@@ -53,135 +53,198 @@ static constexpr const char *satcom_state_string[4] = {"STANDBY", "SIGNAL CHECK"

 #define IRIDIUMSBD_DEVICE_PATH	"/dev/iridium"

-IridiumSBD *IridiumSBD::instance;
-int IridiumSBD::task_handle;
-
 IridiumSBD::IridiumSBD()
 	: CDev(IRIDIUMSBD_DEVICE_PATH)
 {
 }

-///////////////////////////////////////////////////////////////////////
-// public functions                                                  //
-///////////////////////////////////////////////////////////////////////
-
-int IridiumSBD::start(int argc, char *argv[])
+IridiumSBD::~IridiumSBD()
 {
-	PX4_INFO("starting");
+	::close(_uart_fd);
+}

-	if (IridiumSBD::instance != nullptr) {
-		PX4_WARN("already started");
-		return PX4_ERROR;
+int IridiumSBD::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("iridiumsbd",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_SLOW_DRIVER,
+				      1350,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
 	}

-	IridiumSBD::instance = new IridiumSBD();
-
-	IridiumSBD::task_handle = px4_task_spawn_cmd("iridiumsbd", SCHED_DEFAULT,
-				  SCHED_PRIORITY_SLOW_DRIVER, 1350, (main_t)&IridiumSBD::main_loop_helper, argv);
-
-	int counter = 0;
-	IridiumSBD::instance->_start_completed = false;
-	IridiumSBD::instance->_task_should_exit = false;
-
-	// give the driver 6 seconds to start up
-	while (!IridiumSBD::instance->_start_completed && (IridiumSBD::task_handle != -1) && counter < 60) {
-		counter++;
-		usleep(100000);
+	// wait until task is up & running (max 6 seconds)
+	if (wait_until_running(6000) < 0) {
+		_task_id = -1;
+		return -1;
 	}

-	if (IridiumSBD::instance->_start_completed && (IridiumSBD::task_handle != -1)) {
-		return PX4_OK;
+	return 0;
+}

-	} else {
-		// the driver failed to start so make sure it is shut down before exiting
-		IridiumSBD::instance->_task_should_exit = true;
+IridiumSBD *IridiumSBD::instantiate(int argc, char *argv[])
+{
+	IridiumSBD *instance = new IridiumSBD();

-		for (int i = 0; (i < 10 + 1) && (IridiumSBD::task_handle != -1); i++) {
-			sleep(1);
+	if (instance) {
+		int ret = instance->init(argc, argv);
+
+		if (ret != PX4_OK) {
+			delete instance;
+			return nullptr;
+		}
+	}
+
+	return instance;
+}
+
+int IridiumSBD::init(int argc, char *argv[])
+{
+	int ret = CDev::init();
+
+	if (ret != PX4_OK) {
+		return ret;
+	}
+
+	pthread_mutex_init(&_tx_buf_mutex, NULL);
+	pthread_mutex_init(&_rx_buf_mutex, NULL);
+
+	int arg_i = 1;
+	int arg_uart_name = 0;
+
+	while (arg_i < argc) {
+		if (!strcmp(argv[arg_i], "-d")) {
+			arg_i++;
+			arg_uart_name = arg_i;
+
+		} else if (!strcmp(argv[arg_i], "-v")) {
+			PX4_INFO("verbose mode ON");
+			_verbose = true;
 		}

-		return PX4_ERROR;
+		arg_i++;
 	}
+
+	if (arg_uart_name == 0) {
+		PX4_ERR("no Iridium SBD modem UART port provided!");
+		return -EINVAL;
+	}
+
+
+	bool command_executed = false;
+
+	for (int counter = 0; (counter < 20) && !command_executed; counter++) {
+		if (open_uart(argv[arg_uart_name]) == SATCOM_UART_OK) {
+			command_executed = true;
+
+		} else {
+			usleep(100000);
+		}
+	}
+
+	if (!command_executed) {
+		PX4_ERR("failed to open UART port!");
+		return -EIO;
+	}
+
+	// disable flow control
+	command_executed = false;
+
+	for (int counter = 0; (counter < 20) && !command_executed; counter++) {
+		write_at("AT&K0");
+
+		if (read_at_command() != SATCOM_RESULT_OK) {
+			usleep(100000);
+
+		} else {
+			command_executed = true;
+		}
+	}
+
+	if (!command_executed) {
+		PX4_ERR("modem not responding");
+		return -EIO;
+	}
+
+	// disable command echo
+	command_executed = false;
+
+	for (int counter = 0; (counter < 10) && !command_executed; counter++) {
+		write_at("ATE0");
+
+		if (read_at_command() != SATCOM_RESULT_OK) {
+			usleep(100000);
+
+		} else {
+			command_executed = true;
+		}
+	}
+
+	if (!command_executed) {
+		PX4_ERR("modem not responding");
+		return -EIO;
+	}
+
+	param_t param_pointer;
+
+	param_pointer = param_find("ISBD_READ_INT");
+	param_get(param_pointer, &_param_read_interval_s);
+
+	param_pointer = param_find("ISBD_SBD_TIMEOUT");
+	param_get(param_pointer, &_param_session_timeout_s);
+
+	if (_param_session_timeout_s < 0) {
+		_param_session_timeout_s = 60;
+	}
+
+	param_pointer = param_find("ISBD_STACK_TIME");
+	param_get(param_pointer, &_param_stacking_time_ms);
+
+	if (_param_stacking_time_ms < 0) {
+		_param_stacking_time_ms = 0;
+	}
+
+	VERBOSE_INFO("read interval: %" PRId32 " s", _param_read_interval_s);
+	VERBOSE_INFO("SBD session timeout: %" PRId32 " s", _param_session_timeout_s);
+	VERBOSE_INFO("SBD stack time: %" PRId32 " ms", _param_stacking_time_ms);
+	return PX4_OK;
 }

-int IridiumSBD::stop()
+int IridiumSBD::print_status()
 {
-	if (IridiumSBD::instance == nullptr) {
-		PX4_WARN("not started");
-		return PX4_ERROR;
-	}
-
-	if (IridiumSBD::instance->_cdev_used) {
-		PX4_WARN("device is used. Stop all users (MavLink)");
-		return PX4_ERROR;
-	}
-
-	PX4_WARN("stopping...");
-
-	IridiumSBD::instance->_task_should_exit = true;
-
-	// give it enough time to stop
-	//param_timeout_s = 10;
-
-	// TODO
-	for (int i = 0; (i < 10 + 1) && (IridiumSBD::task_handle != -1); i++) {
-		sleep(1);
-	}
-
-	// well, kill it anyway, though this may crash
-	if (IridiumSBD::task_handle != -1) {
-		PX4_WARN("killing task forcefully");
-
-		::close(IridiumSBD::instance->uart_fd);
-		task_delete(IridiumSBD::task_handle);
-		IridiumSBD::task_handle = -1;
-		delete IridiumSBD::instance;
-		IridiumSBD::instance = nullptr;
-	}
-
-	return OK;
-}
-
-void IridiumSBD::status()
-{
-	if (IridiumSBD::instance == nullptr) {
-		PX4_WARN("not started");
-		return;
-	}
-
 	PX4_INFO("started");
-	PX4_INFO("state: %s", satcom_state_string[instance->_state]);
+	PX4_INFO("state: %s", satcom_state_string[_state]);

-	PX4_INFO("TX buf written:               %d", instance->_tx_buf_write_idx);
-	PX4_INFO("Signal quality:               %" PRId8, instance->_signal_quality);
-	PX4_INFO("TX session pending:           %d", instance->_tx_session_pending);
-	PX4_INFO("RX session pending:           %d", instance->_rx_session_pending);
-	PX4_INFO("RX read pending:              %d", instance->_rx_read_pending);
-	PX4_INFO("Time since last signal check: %" PRId64, hrt_absolute_time() - instance->_last_signal_check);
-	PX4_INFO("Last heartbeat:               %" PRId64, instance->_last_heartbeat);
+	PX4_INFO("TX buf written:               %d", _tx_buf_write_idx);
+	PX4_INFO("Signal quality:               %" PRId8, _signal_quality);
+	PX4_INFO("TX session pending:           %d", _tx_session_pending);
+	PX4_INFO("RX session pending:           %d", _rx_session_pending);
+	PX4_INFO("RX read pending:              %d", _rx_read_pending);
+	PX4_INFO("Time since last signal check: %" PRId64, hrt_absolute_time() - _last_signal_check);
+	PX4_INFO("Last heartbeat:               %" PRId64, _last_heartbeat);
+	return 0;
 }

 void IridiumSBD::test(int argc, char *argv[])
 {
-	if (instance == nullptr) {
-		PX4_WARN("not started");
-		return;
-	}
-
-	if (instance->_state != SATCOM_STATE_STANDBY || instance->_test_pending) {
+	if (_state != SATCOM_STATE_STANDBY || _test_pending.load()) {
 		PX4_WARN("MODEM BUSY!");
 		return;
 	}

-	if (argc > 2) {
-		strncpy(instance->_test_command, argv[2], sizeof(instance->_test_command));
-		instance->_test_command[sizeof(instance->_test_command) - 1] = 0;
+	if (argc > 1) {
+		strncpy(_test_command, argv[1], sizeof(_test_command));
+		_test_command[sizeof(_test_command) - 1] = 0;

 	} else {
-		instance->_test_command[0] = 0;
+		_test_command[0] = 0;
 	}

-	instance->schedule_test();
+	schedule_test();
 }

 int IridiumSBD::ioctl(struct file *filp, int cmd, unsigned long arg)
@@ -209,138 +272,9 @@ int IridiumSBD::ioctl(struct file *filp, int cmd, unsigned long arg)
 	}
 }

-///////////////////////////////////////////////////////////////////////
-// private functions                                                 //
-///////////////////////////////////////////////////////////////////////
-
-int IridiumSBD::main_loop_helper(int argc, char *argv[])
+void IridiumSBD::run()
 {
-	// start the main loop and stay in it
-	IridiumSBD::instance->main_loop(argc, argv);
-
-	// tear down everything after the main loop exits
-	::close(IridiumSBD::instance->uart_fd);
-	IridiumSBD::task_handle = -1;
-	delete IridiumSBD::instance;
-	IridiumSBD::instance = nullptr;
-
-	PX4_WARN("stopped");
-	return 0;
-}
-
-void IridiumSBD::main_loop(int argc, char *argv[])
-{
-	CDev::init();
-
-	pthread_mutex_init(&_tx_buf_mutex, NULL);
-	pthread_mutex_init(&_rx_buf_mutex, NULL);
-
-	int arg_i = 3;
-	int arg_uart_name = 0;
-
-	while (arg_i < argc) {
-		if (!strcmp(argv[arg_i], "-d")) {
-			arg_i++;
-			arg_uart_name = arg_i;
-
-		} else if (!strcmp(argv[arg_i], "-v")) {
-			PX4_WARN("verbose mode ON");
-			_verbose = true;
-		}
-
-		arg_i++;
-	}
-
-	if (arg_uart_name == 0) {
-		PX4_ERR("no Iridium SBD modem UART port provided!");
-		_task_should_exit = true;
-		return;
-	}
-
-
-	bool command_executed = false;
-
-	for (int counter = 0; (counter < 20) && !command_executed; counter++) {
-		if (open_uart(argv[arg_uart_name]) == SATCOM_UART_OK) {
-			command_executed = true;
-
-		} else {
-			usleep(100000);
-		}
-	}
-
-	if (!command_executed) {
-		PX4_ERR("failed to open UART port!");
-		_task_should_exit = true;
-		return;
-	}
-
-	// disable flow control
-	command_executed = false;
-
-	for (int counter = 0; (counter < 20) && !command_executed; counter++) {
-		write_at("AT&K0");
-
-		if (read_at_command() != SATCOM_RESULT_OK) {
-			usleep(100000);
-
-		} else {
-			command_executed = true;
-		}
-	}
-
-	if (!command_executed) {
-		PX4_ERR("modem not responding");
-		_task_should_exit = true;
-		return;
-	}
-
-	// disable command echo
-	command_executed = false;
-
-	for (int counter = 0; (counter < 10) && !command_executed; counter++) {
-		write_at("ATE0");
-
-		if (read_at_command() != SATCOM_RESULT_OK) {
-			usleep(100000);
-
-		} else {
-			command_executed = true;
-		}
-	}
-
-	if (!command_executed) {
-		PX4_ERR("modem not responding");
-		_task_should_exit = true;
-		return;
-	}
-
-	param_t param_pointer;
-
-	param_pointer = param_find("ISBD_READ_INT");
-	param_get(param_pointer, &_param_read_interval_s);
-
-	param_pointer = param_find("ISBD_SBD_TIMEOUT");
-	param_get(param_pointer, &_param_session_timeout_s);
-
-	if (_param_session_timeout_s < 0) {
-		_param_session_timeout_s = 60;
-	}
-
-	param_pointer = param_find("ISBD_STACK_TIME");
-	param_get(param_pointer, &_param_stacking_time_ms);
-
-	if (_param_stacking_time_ms < 0) {
-		_param_stacking_time_ms = 0;
-	}
-
-	VERBOSE_INFO("read interval: %" PRId32 " s", _param_read_interval_s);
-	VERBOSE_INFO("SBD session timeout: %" PRId32 " s", _param_session_timeout_s);
-	VERBOSE_INFO("SBD stack time: %" PRId32 " ms", _param_stacking_time_ms);
-
-	_start_completed = true;
-
-	while (!_task_should_exit) {
+	while (!should_exit()) {
 		switch (_state) {
 		case SATCOM_STATE_STANDBY:
 			standby_loop();
@@ -373,8 +307,8 @@ void IridiumSBD::main_loop(int argc, char *argv[])

 void IridiumSBD::standby_loop(void)
 {
-	if (_test_pending) {
-		_test_pending = false;
+	if (_test_pending.load()) {
+		_test_pending.store(false);

 		if (!strcmp(_test_command, "s")) {
 			write(0, "kreczmer", 8);
@@ -796,7 +730,7 @@ void IridiumSBD::write_tx_buf()
 	int written = 0;

 	while (written != _tx_buf_write_idx) {
-		written += ::write(uart_fd, _tx_buf + written, _tx_buf_write_idx - written);
+		written += ::write(_uart_fd, _tx_buf + written, _tx_buf_write_idx - written);
 	}

 	for (int i = 0; i < _tx_buf_write_idx; i++) {
@@ -804,7 +738,7 @@ void IridiumSBD::write_tx_buf()
 	}

 	uint8_t checksum[2] = {(uint8_t)(sum / 256), (uint8_t)(sum & 255)};
-	::write(uart_fd, checksum, 2);
+	::write(_uart_fd, checksum, 2);


 	VERBOSE_INFO("SEND SBD: CHECKSUM %" PRId8 " %" PRId8, checksum[0], checksum[1]);
@@ -887,8 +821,8 @@ void IridiumSBD::write_at(const char *command)
 {
 	VERBOSE_INFO("WRITING AT COMMAND: %s", command);

-	::write(uart_fd, command, strlen(command));
-	::write(uart_fd, "\r", 1);
+	::write(_uart_fd, command, strlen(command));
+	::write(_uart_fd, "\r", 1);
 }

 satcom_result_code IridiumSBD::read_at_command(int16_t timeout)
@@ -904,7 +838,7 @@ satcom_result_code IridiumSBD::read_at_msg(int16_t timeout)
 satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len, int16_t timeout)
 {
 	struct pollfd fds[1];
-	fds[0].fd = uart_fd;
+	fds[0].fd = _uart_fd;
 	fds[0].events = POLLIN;

 	uint8_t buf = 0;
@@ -914,7 +848,7 @@ satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len, int16_t tim

 	while (1) {
 		if (::poll(&fds[0], 1, timeout) > 0) {
-			if (::read(uart_fd, &buf, 1) > 0) {
+			if (::read(_uart_fd, &buf, 1) > 0) {
 				if (rx_buf_pos == 0 && (buf == '\r' || buf == '\n')) {
 					// ignore the leading \r\n
 					continue;
@@ -968,7 +902,7 @@ satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len, int16_t tim

 void IridiumSBD::schedule_test(void)
 {
-	_test_pending = true;
+	_test_pending.store(true);
 }

 bool IridiumSBD::clear_mo_buffer()
@@ -987,18 +921,18 @@ satcom_uart_status IridiumSBD::open_uart(char *uart_name)
 {
 	VERBOSE_INFO("opening Iridium SBD modem UART: %s", uart_name);

-	uart_fd = ::open(uart_name, O_RDWR | O_BINARY);
+	_uart_fd = ::open(uart_name, O_RDWR | O_BINARY);

-	if (uart_fd < 0) {
+	if (_uart_fd < 0) {
 		VERBOSE_INFO("UART open failed!");
 		return SATCOM_UART_OPEN_FAIL;
 	}

 	// set the UART speed to 115200
 	struct termios uart_config;
-	tcgetattr(uart_fd, &uart_config);
+	tcgetattr(_uart_fd, &uart_config);
 	cfsetspeed(&uart_config, 115200);
-	tcsetattr(uart_fd, TCSANOW, &uart_config);
+	tcsetattr(_uart_fd, TCSANOW, &uart_config);

 	VERBOSE_INFO("UART opened");

@@ -1116,39 +1050,64 @@ void IridiumSBD::publish_iridium_status()

 int IridiumSBD::open_first(struct file *filep)
 {
-	_cdev_used = true;
+	_cdev_used.store(true);
 	return CDev::open_first(filep);
 }

 int IridiumSBD::close_last(struct file *filep)
 {
-	_cdev_used = false;
+	_cdev_used.store(false);
 	return CDev::close_last(filep);
 }

+int IridiumSBD::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_INFO("%s", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+IridiumSBD driver.
+
+Creates a virtual serial port that another module can use for communication (e.g. mavlink).
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("iridiumsbd", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Serial device", false);
+	PRINT_MODULE_USAGE_PARAM_FLAG('v', "Enable verbose output", true);
+	PRINT_MODULE_USAGE_COMMAND("test");
+	PRINT_MODULE_USAGE_ARG("s|read|AT <cmd>", "Test command", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 1;
+}
+
+int IridiumSBD::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		print_usage("not running");
+		return 1;
+	}
+
+	if (!strcmp(argv[0], "test")) {
+		get_instance()->test(argc, argv);
+		return 0;
+	}
+
+	return print_usage("unknown command");
+}
+
 int iridiumsbd_main(int argc, char *argv[])
 {
-	if (argc < 2) {
-		goto out_error;
+	if (argc >= 2 && !strcmp(argv[1], "stop") && IridiumSBD::is_running()) {
+		if (!IridiumSBD::can_stop()) {
+			PX4_ERR("Device is used. Stop all users first (mavlink stop-all)");
+			return -1;
+		}
 	}

-	if (!strcmp(argv[1], "start")) {
-		return IridiumSBD::start(argc, argv);
-
-	} else if (!strcmp(argv[1], "stop")) {
-		return IridiumSBD::stop();
-
-	} else if (!strcmp(argv[1], "status")) {
-		IridiumSBD::status();
-		return OK;
-
-	} else if (!strcmp(argv[1], "test")) {
-		IridiumSBD::test(argc, argv);
-		return OK;
-	}
-
-out_error:
-	PX4_INFO("usage: iridiumsbd {start|stop|status|test} [-d uart_device]");
-
-	return PX4_ERROR;
+	return IridiumSBD::main(argc, argv);
 }
@@ -42,6 +42,9 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/iridiumsbd_status.h>

+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/module.h>
+
 typedef enum {
 	SATCOM_OK = 0,
 	SATCOM_NO_MSG = -1,
@@ -97,28 +100,29 @@ extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[]);
 * 	- Improve TX buffer handling:
 * 		- Do not reset the full TX buffer but delete the oldest HIGH_LATENCY2 message if one is in the buffer or delete the oldest message in general
 */
-class IridiumSBD : public cdev::CDev
+class IridiumSBD : public cdev::CDev, public ModuleBase<IridiumSBD>
 {
 public:
-	/*
-	 * Constructor
-	 */
 	IridiumSBD();
+	~IridiumSBD();

-	/*
-	 * Start the driver
-	 */
-	static int start(int argc, char *argv[]);
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);

-	/*
-	 * Stop the driver
-	 */
-	static int stop();
+	/** @see ModuleBase */
+	static IridiumSBD *instantiate(int argc, char *argv[]);

-	/*
-	 * Display driver status
-	 */
-	static void status();
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;

 	/*
 	 * Run a driver test based on the input
@@ -126,23 +130,17 @@ public:
 	 *  - `read`: Start a sbd read session
 	 *  - else: Is assumed to be a valid AT command and written to the modem
 	 */
-	static void test(int argc, char *argv[]);
+	void test(int argc, char *argv[]);

 	/*
 	 * Passes everything to CDev
 	 */
 	int ioctl(struct file *filp, int cmd, unsigned long arg);

-private:
-	/*
-	 * Entry point of the task, has to be a static function
-	 */
-	static int main_loop_helper(int argc, char *argv[]);
+	static bool can_stop() { return !get_instance()->_cdev_used.load(); }

-	/*
-	 * Main driver loop
-	 */
-	void main_loop(int argc, char *argv[]);
+private:
+	int init(int argc, char *argv[]);

 	/*
 	 * Loop executed while in SATCOM_STATE_STANDBY
@@ -282,11 +280,7 @@ private:
 	 */
 	virtual int	close_last(struct file *filep) override;

-	static IridiumSBD *instance;
-	static int task_handle;
-	bool _task_should_exit = false;
-	bool _start_completed = false;
-	int uart_fd = -1;
+	int _uart_fd = -1;

 	int32_t _param_read_interval_s = -1;
 	int32_t _param_session_timeout_s = -1;
@@ -303,7 +297,7 @@ private:

 	uORB::Publication<iridiumsbd_status_s> _iridiumsbd_status_pub{ORB_ID(iridiumsbd_status)};

-	bool _test_pending = false;
+	px4::atomic_bool _test_pending{false};
 	char _test_command[32];
 	hrt_abstime _test_timer = 0;

@@ -323,7 +317,7 @@ private:
 	bool _rx_read_pending = false;
 	bool _tx_session_pending = false;

-	bool _cdev_used = false;
+	px4::atomic_bool _cdev_used{false};

 	hrt_abstime _last_write_time = 0;
 	hrt_abstime _last_read_time = 0;
@@ -107,10 +107,11 @@ public:
 private:


-	const UavcanParamBinder _uavcan_params[11] {
+	const UavcanParamBinder _uavcan_params[12] {
 		{"uavcan.pub.esc.0.id",                "UCAN1_ESC_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.servo.0.id",              "UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.gps.0.id",                "UCAN1_GPS_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.actuator_outputs.0.id",   "UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.esc.0.id",                "UCAN1_ESC0_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.gps.0.id",                "UCAN1_GPS0_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.gps.1.id",                "UCAN1_GPS1_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
@@ -80,7 +80,7 @@ public:
 		// Set _port_id from _uavcan_param
 		uavcan_register_Value_1_0 value;
 		_param_manager.GetParamByName(uavcan_param, value);
-		int32_t new_id = value.integer32.value.elements[0];
+		uint16_t new_id = value.natural16.value.elements[0];

 		// Allow, for example, a default PX4 param value of '-1' to disable publication
 		if (!isValidPortId(new_id)) {
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_outputs.hpp
+ *
+* Defines uORB over UAVCANv1 actuator_outputs publisher
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <uORB/topics/actuator_outputs.h>
+
+#include "../Publisher.hpp"
+
+class UORB_over_UAVCAN_actuator_outputs_Publisher : public UavcanPublisher
+{
+public:
+	UORB_over_UAVCAN_actuator_outputs_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanPublisher(ins, pmgr, "actuator_outputs", instance)
+	{};
+
+	// Update the uORB Subscription and broadcast a UAVCAN message
+	// FIXME think about update and limiting
+	virtual void update() override
+	{
+		// Not sure if actuator_armed is a good indication of readiness but seems close to it
+		if (_actuator_outputs_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) {
+			actuator_outputs_s actuator_msg {};
+			_actuator_outputs_sub.update(&actuator_msg);
+
+			CanardTransfer transfer = {
+				.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id, // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,
+				.transfer_id    = _transfer_id,
+				.payload_size   = actuator_payload_size(&actuator_msg),
+				.payload        = &actuator_msg,
+			};
+
+			// set the data ready in the buffer and chop if needed
+			++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			canardTxPush(&_canard_instance, &transfer);
+		}
+	};
+
+private:
+
+	// Remove unvalid output & padding from payload_size to save bandwidth
+	size_t actuator_payload_size(actuator_outputs_s *msg)
+	{
+		return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) -
+		       ((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0]));
+	}
+
+	uORB::Subscription _actuator_outputs_sub{ORB_ID(actuator_outputs)};
+};
@@ -59,6 +59,7 @@

 #include "Publishers/Publisher.hpp"
 #include "Publishers/Gnss.hpp"
+#include "Publishers/uORB/actuator_outputs.hpp"

 #include "NodeManager.hpp"

@@ -183,11 +184,13 @@ private:

 	UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};

+	UORB_over_UAVCAN_actuator_outputs_Publisher _actuator_pub {_canard_instance, _param_manager}; // uORB
+
 	UavcanEscController _esc_controller {_canard_instance, _param_manager};

 	// Publication objects: Any object used to bridge a uORB message to a UAVCAN message
 	/// TODO: For some service implementations, it makes sense to have them be both Publishers and Subscribers
-	UavcanPublisher *_publishers[2] {&_gps_pub, &_esc_controller};
+	UavcanPublisher *_publishers[3] {&_gps_pub, &_esc_controller, &_actuator_pub};

 	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};

@@ -169,3 +169,12 @@ PARAM_DEFINE_INT32(UCAN1_GPS_PUB, -1);
 * @group UAVCAN v1
 */
 PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1);
+
+/**
+ * actuator_outputs uORB over UAVCAN v1 port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_ACTR_PUB, -1);
@@ -6,6 +6,17 @@ parameters:
    - group: PWM Outputs
      definitions:

+        PWM_AUX_OUT:
+            description:
+                short: PWM channels used as ESC outputs
+                long: |
+                    Number representing the channels e.g. 134 - Channel 1, 3 and 4.
+                    Global e.g. PWM_AUX_MIN/MAX/DISARM limits only apply to these channels.
+            type: int32
+            min: 0
+            max: 123456789
+            default: 0
+
        PWM_AUX_RATE:
            description:
                short: PWM aux output frequency
@@ -6,6 +6,17 @@ parameters:
    - group: PWM Outputs
      definitions:

+        PWM_MAIN_OUT:
+            description:
+                short: PWM channels used as ESC outputs
+                long: |
+                    Number representing the channels e.g. 134 - Channel 1, 3 and 4.
+                    Global e.g. PWM_MAIN_MIN/MAX/DISARM limits only apply to these channels.
+            type: int32
+            min: 0
+            max: 123456789
+            default: 0
+
        PWM_MAIN_RATE:
            description:
                short: PWM main output frequency
@@ -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
 *
@@ -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);

@@ -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;
 }

@@ -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 */
 };
@@ -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);
-				setpoint.vy = velocity_setpoint(1);
-				setpoint.vz = velocity_setpoint(2);
+
+				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);

 			} 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 = timestamp;
-
+		hil_attitude.timestamp_sample = timestamp_sample;
 		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.epv = 4.0f;
-
+		hil_global_pos.eph = 2.f;
+		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;
-		map_projection_init(&global_local_proj_ref, lat, lon);
+		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;
+		}

-		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);
+		float x = 0.f;
+		float y = 0.f;
+		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.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_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.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.vx = hil_state.vx / 100.0f;
-		hil_local_pos.vy = hil_state.vy / 100.0f;
-		hil_local_pos.vz = hil_state.vz / 100.0f;
+		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;

 		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);
 	}
 }
@@ -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.
@@ -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 = 1.0f;
-
-	} else if (control < -1.0f) {
-		control = -1.0f;
-	}
+	control = math::constrain(control, -1.f, 1.f);

 	/* 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 (in_mixer) {
-		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;
 	}

 	/* 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
-mixer_handle_text(const void *buffer, size_t length)
+int interrupt_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,16 +572,7 @@ 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;
+	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;
 	}
-
 }
@@ -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	mixer_handle_text(const void *buffer, size_t length);
+extern int	interrupt_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.
@@ -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;
@@ -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,12 +535,10 @@ 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();
+			_vehicle_air_data = new VehicleAirData();

-				if (_vehicle_air_data) {
-					_vehicle_air_data->Start();
-				}
+			if (_vehicle_air_data) {
+				_vehicle_air_data->Start();
 			}
 		}
 	}
@@ -547,8 +546,8 @@ void Sensors::InitializeVehicleAirData()

 void Sensors::InitializeVehicleGPSPosition()
 {
-	if (_vehicle_gps_position == nullptr) {
-		if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
+	if (_param_sys_has_gps.get()) {
+		if (_vehicle_gps_position == nullptr) {
 			_vehicle_gps_position = new VehicleGPSPosition();

 			if (_vehicle_gps_position) {
@@ -602,12 +601,10 @@ 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();
+			_vehicle_magnetometer = new VehicleMagnetometer();

-				if (_vehicle_magnetometer) {
-					_vehicle_magnetometer->Start();
-				}
+			if (_vehicle_magnetometer) {
+				_vehicle_magnetometer->Start();
 			}
 		}
 	}
@@ -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");
 	}
@@ -54,6 +54,7 @@
 * Roll proportional gain
 *
 * @group UUV Attitude Control
+ * @decimal 2
 */
 PARAM_DEFINE_FLOAT(UUV_ROLL_P, 4.0f);

@@ -61,6 +62,7 @@ PARAM_DEFINE_FLOAT(UUV_ROLL_P, 4.0f);
 * Roll differential gain
 *
 * @group UUV Attitude Control
+ * @decimal 2
 */
 PARAM_DEFINE_FLOAT(UUV_ROLL_D, 1.5f);

@@ -70,6 +72,7 @@ PARAM_DEFINE_FLOAT(UUV_ROLL_D, 1.5f);
 * Pitch proportional gain
 *
 * @group UUV Attitude Control
+ * @decimal 2
 */
 PARAM_DEFINE_FLOAT(UUV_PITCH_P, 4.0f);

@@ -77,6 +80,7 @@ PARAM_DEFINE_FLOAT(UUV_PITCH_P, 4.0f);
 * Pitch differential gain
 *
 * @group UUV Attitude Control
+ * @decimal 2
 */
 PARAM_DEFINE_FLOAT(UUV_PITCH_D, 2.0f);

@@ -86,6 +90,7 @@ PARAM_DEFINE_FLOAT(UUV_PITCH_D, 2.0f);
 * Yawh proportional gain
 *
 * @group UUV Attitude Control
+ * @decimal 2
 */
 PARAM_DEFINE_FLOAT(UUV_YAW_P, 4.0f);

@@ -93,6 +98,7 @@ PARAM_DEFINE_FLOAT(UUV_YAW_P, 4.0f);
 * Yaw differential gain
 *
 * @group UUV Attitude Control
+ * @decimal 2
 */
 PARAM_DEFINE_FLOAT(UUV_YAW_D, 2.0f);

@@ -77,8 +77,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	for (auto &pwm_disarmed : _disarmed_pwm_values.values) {
 		pwm_disarmed = PWM_MOTOR_OFF;
 	}
-
-	_current_max_pwm_values = _max_mc_pwm_values;
 }

 bool VtolType::init()
@@ -93,7 +91,7 @@ bool VtolType::init()
 	}

 	int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values);
-
+	_current_max_pwm_values = _max_mc_pwm_values;

 	if (ret != PX4_OK) {
 		PX4_ERR("failed getting max values");
Author	SHA1	Message	Date
Daniel Agar	2e8918da66	boards: cubepilot_cubeorange only start ADSB mavlink if console not present - cubepilot_cubeorange_test has the console enabled (used for test rack and bench debugging)	2021-09-06 11:57:42 -04:00
Daniel Agar	5def17b1af	Jenkins: hardware remove boards that aren't present	2021-09-06 10:28:52 -04:00
Daniel Agar	1daf0654ff	boards: px4_fmu-v5_opitmized disable unused modules to save flash	2021-09-06 10:28:17 -04:00
Daniel Agar	6e3a2314cb	boards: px4_fmu-v2_default disable systemcmds/top to save flash	2021-09-06 10:26:49 -04:00
Sander Swart	6121b287f1	Removed TEL3 from cube orange default.cmake as it is used for the built in ADS-B receiver	2021-09-06 10:24:43 -04:00
Sander Swart	c0980bd273	Added new line at the end of the file as per code style	2021-09-06 10:24:38 -04:00
Sander Swart	42d14e6072	Enable Cube Orange built in ADS-B receiver by default	2021-09-06 10:24:34 -04:00
Daniel Agar	ba0b512f0d	boards: Holybro H7 boards clear MPU early in board init - needed if the bootloader configures the memory protection unit (MPU)	2021-08-31 18:01:06 -04:00
Daniel Agar	7476a2953e	boards: CUAV H7 boards clear MPU early in board init - needed if the bootloader configures the memory protection unit (MPU)	2021-08-31 17:59:34 -04:00
Lorenz Meier	15e6ca9e1e	MRO: Set up MPU after boot to app This is needed in case the bootloader configured the MPU, which could interfere with the app as the bootloader memory map / configuration is different.	2021-08-31 17:57:23 -04:00
Daniel Agar	b9e7063237	boards: cube orange fully clear any existing MPU configuration as soon as possible	2021-08-31 17:56:23 -04:00
Daniel Agar	0a6045367f	px4io_serial: ensure TX DMA is stopped if exiting early on stream error - otherwise the next retry can happen quickly enough that dma setup hangs waiting for the stream	2021-08-31 17:56:00 -04:00
Daniel Agar	3f88a6d0bf	boards: cubeorange/cubeyellow use amber LED for armed state	2021-08-29 21:31:21 -04:00
Jukka Laitinen	d1c09ec358	Fix memory corruption when work queue is being deleted When the last WorkItem is deleted, it is removed from a work queue and the queue is being stopped. But, the queue itself might get deleted in the middle, in a higher priority thread than where the WorkItem deletion was performed from If the WorkQueue::Detach accesses the member variables after this, there is memory corruption This happens in particular when launching i2c or spi devices in I2CSPIDriverBase::module_start: - The "initializer" is deleted when the instance is not found and the iterator while loop continues. - The workqueue is deleted in the middle of "initializer" deletion when the WorkQueueRunner returns. This prevents deletion of the WorkQueue before the Detach has been finished, in the specific case that the ::Detach triggers the deletion Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-08-29 21:31:12 -04:00
Daniel Agar	ca744626cd	boards: io-v2 increase idle thread stack 280 -> 316 bytes	2021-08-26 23:57:10 -04:00
Daniel Agar	ea577f15b9	px4iofirmware: never directly touch mixer from isr	2021-08-26 23:57:10 -04:00
Daniel Agar	d9f3c820ab	sensors: always start baro/GPS/mag aggregators if SYS_HAS_* set - add new SYS_HAS_GPS parameter	2021-08-23 14:14:27 -04:00
David Sidrane	8e57a62c9d	rgbled_ncp5623c:Add comand line mapping of PWM to color	2021-08-21 11:52:15 -04:00
David Sidrane	b961f6a1e1	rgbled_ncp5623c:Needs 2 retries	2021-08-21 11:52:15 -04:00
David Sidrane	09140c01bf	rgbled_ncp5623c:Can change address, return actual	2021-08-21 11:52:15 -04:00
David Sidrane	822ae46235	rgbled_ncp5623c:Document support for ncp5623[c\|b] parts	2021-08-21 11:52:15 -04:00
David Sidrane	f70381dfdd	i2c_spi_buses:Support devices that may change address	2021-08-21 11:52:15 -04:00
David Sidrane	84066f574d	vcm1193l:Bug Fix set default address	2021-08-21 11:52:15 -04:00
David Sidrane	c8426da50d	px4_fmu-v5x:Newer HW Start BARRO on bus 2	2021-08-21 11:52:15 -04:00
David Sidrane	6bab917a3d	px4_fmu-v6x:Use HB 10pin GPS rotation	2021-08-21 11:52:15 -04:00
David Sidrane	531301e176	px4_fmu-v5x:Use HB 10pin GPS rotation	2021-08-21 11:52:15 -04:00
David Sidrane	04f4157828	rcS:Scope netman to all 5X and 6X	2021-08-21 11:52:15 -04:00
Daniel Agar	c5f82ed838	ekf2: fix sensor_combined last_generation	2021-08-12 10:03:25 -04:00
Daniel Agar	99501b4c38	flight_mode_manager: ManualAcceleration support weathervane yaw handler - fixes https://github.com/PX4/PX4-Autopilot/issues/17911	2021-08-12 10:02:13 -04:00
Daniel Agar	9e4a04f58a	mavlink: receiver fix HIL_STATE_QUATERNION map projection init - fixes https://github.com/PX4/PX4-Autopilot/issues/17977	2021-08-02 09:59:41 -04:00
Daniel Agar	1682fd5671	boards: px4_fmu-v2 disable load_mon module to save flash	2021-08-01 13:51:11 -04:00
bresch	a8572f0fdd	ECL: update to include latest bugfixes - fix yaw spin on ground after large mag field change - change covariance prediction for better stability - fix yaw alignment in air for mag-less mode - improve tilt alignment for non-static applications - add momentum drag model	2021-08-01 12:54:15 -04:00
Daniel Agar	a299a3bbd0	sensors: populate sensors_status_imu healthy flags even in multi-EKF mode	2021-08-01 12:54:08 -04:00
Julian Oes	349dd63072	mavlink: fix offboard velocity input This reverts the behavior for offboard velocity setpoint. Back in v1.11, the velocity input in NED_BODY was assumed to be in the world frame but rotated by yaw to the vehicle frame. With the current state the frame is completely fixed to the body. While this might be technically correct, it doesn't seem much intuitive for multicopters and breaks the MAVSDK offboard velocity API. So as an example, with a velocity setpoint of 5 m/s forward, the drone would in - v1.11: fly forward with 5 m/s - v1.12: start to fly forward by pitching down but then descend rapidly as the forward velocity would translate to a setpoint in Z into the ground as it is pitched down. This commit restores the behavior to what we had previously.	2021-07-22 22:09:56 -04:00
David Sidrane	6b51c6390a	Revert "nxp_fmuk66-v3:DMA Poll not needed" This reverts commit `962f02220a`.	2021-07-22 22:08:18 -04:00
David Sidrane	4f7909ee8e	Revert "nxp_fmuk66-e:DMA Poll not needed" This reverts commit `39d684958d`.	2021-07-22 22:08:11 -04:00
David Sidrane	cbb48f9af3	NuttX with Kinetis SerialPoll back in	2021-07-22 22:08:04 -04:00
Daniel Agar	9524e8ec03	drivers/px4io: mirror PWM_MAIN_OUT support	2021-07-05 21:23:04 -04:00
Daniel Agar	6dd0a58302	[at24c] free perf counter if px4_at24c_initialize fails - add driver prefix to perf counter naming	2021-07-05 21:22:08 -04:00
murata	c9b5e488f5	NCP5623C: Change the class name to the device name	2021-07-04 14:22:20 -04:00
David Sidrane	9dee09b81b	NuttX and Apps updated w/ALL backports	2021-07-02 09:08:10 -07:00
David Sidrane	962f02220a	nxp_fmuk66-v3:DMA Poll not needed	2021-07-02 09:08:10 -07:00
David Sidrane	39d684958d	nxp_fmuk66-e:DMA Poll not needed	2021-07-02 09:08:10 -07:00
David Sidrane	2a7c95d7ac	nxp_fmuk66-v3:Use eDMA	2021-07-02 09:08:10 -07:00
David Sidrane	cd8182ba3c	nxp_fmuk66-e:Use SPI DMA	2021-07-02 09:08:10 -07:00
David Sidrane	5ea4b7dc9e	NuttX with Kinetis eDMA (SPI, Serial) Backports	2021-07-02 09:08:10 -07:00
Matthias Grob	acf848ba99	PWM: Draft implementation to respect PWM_OUT when loading defaults	2021-07-02 12:48:04 +02:00
Beat Küng	50b0f0e392	iridiumsbd: disable module until everything is fixed There seem to be more issues in combination with MAVLink.	2021-07-02 12:45:18 +02:00
Beat Küng	fcf3bb5af9	fix iridiumsbd: use ModuleBase fixes hardfaults, e.g. when device not connected	2021-07-02 12:45:18 +02:00
Beat Küng	9b7170551c	ModuleBase: allow configurable timeout for wait_until_running()	2021-07-02 12:45:18 +02:00
Peter van der Perk	cf524cd2c9	[UAVCANv1] Added uORB actuator_outputs publisher	2021-07-02 00:58:38 -04:00
Beat Küng	6a44fc7cac	fix vtol_att_control: set _current_max_pwm_values to current values on init	2021-07-02 00:53:42 -04:00
Beat Küng	894ecac8da	px4io: ensure pwm params are loaded before any other module starts The vtol module will read them later on.	2021-07-02 00:53:42 -04:00
alexklimaj	962c2cc960	Add fmuv6x to vscode cmake variants	2021-07-01 20:14:05 -04:00
David Sidrane	3e00450052	NuttX with SDMMC BACKPORT	2021-07-01 11:51:14 -07:00
Thies Lennart Alff	cacab75b42	define decimals for uuv_att_control gains decimal for the uuv_att_control gain parameters was not defined. So QGroundControl displays them as integers what is rather unhandy.	2021-07-01 11:08:16 -04:00