Updated default parameter value

boards: ARK Flow fix typo
drivers: broadcom AFBR update to API 1.5.6
2026-05-28 09:10:05 +08:00 · 2024-03-11 16:24:06 +01:00 · 2024-03-09 16:40:59 -05:00 · 2024-03-09 16:40:59 -05:00 · 2024-03-08 16:28:24 -05:00 · 2024-03-08 14:50:53 -05:00
67 changed files with 1014 additions and 661 deletions
@@ -30,4 +30,5 @@ exec find boards msg src platforms test \
    -path src/lib/cdrstream/cyclonedds -prune -o \
    -path src/lib/cdrstream/rosidl -prune -o \
    -path src/modules/zenoh/zenoh-pico -prune -o \
+    -path src/modules/muorb/apps/libfc-sensor-api -prune -o \
    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
@@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>

+@{
+queue_length = 1
+for constant in spec.constants:
+	if constant.name == 'ORB_QUEUE_LENGTH':
+		queue_length = constant.val
+}@
+
@[for topic in topics]@
 static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
-ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
+ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic), @queue_length);
@[end for]

 void print_message(const orb_metadata *meta, const @uorb_struct& message)
@@ -4,7 +4,7 @@
    "description": "Firmware for the ARK flow board",
    "image": "",
    "build_time": 0,
-    "summary": "ARFFLOW",
+    "summary": "ARKFLOW",
    "version": "0.1",
    "image_size": 0,
    "image_maxsize": 2080768,
@@ -1,9 +1,11 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
-CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
-CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
-CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
+CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
@@ -25,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -96,4 +99,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y
@@ -6,4 +6,4 @@
 param set-default BAT1_V_DIV 10.1
 param set-default BAT1_A_PER_V 17

-safety_button start
+param set-default TEL_FRSKY_CONFIG 103
@@ -16,3 +16,5 @@ icm20948 -s -b 1 -R 8 -M start

 # Interal DPS310 (barometer)
 dps310 -s -b 2 start
+
+safety_button start
@@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
-CONFIG_ARCH_CHIP_STM32H743II=y
+CONFIG_ARCH_CHIP_STM32H743ZI=y
 CONFIG_ARCH_CHIP_STM32H7=y
 CONFIG_ARCH_INTERRUPTSTACK=768
 CONFIG_ARMV7M_BASEPRI_WAR=y
@@ -222,6 +222,9 @@


 /* UART/USART */
+#define GPIO_USART1_TX	 GPIO_USART1_TX_3      /* PB6  */
+#define GPIO_USART1_RX	 GPIO_USART1_RX_3      /* PB7  */
+
 #define GPIO_USART2_TX   GPIO_USART2_TX_2      /* PD5  */
 #define GPIO_USART2_RX   GPIO_USART2_RX_2      /* PD6  */
 #define GPIO_USART2_CTS  GPIO_USART2_CTS_NSS_2 /* PD3  */
@@ -235,9 +238,6 @@
 #define GPIO_UART4_TX    GPIO_UART4_TX_2       /* PA0  */
 #define GPIO_UART4_RX    GPIO_UART4_RX_2       /* PA1  */

-#define GPIO_USART6_TX   0                     /* USART6 is RX-only */
-#define GPIO_USART6_RX   GPIO_USART6_RX_1      /* PC7  */
-
 #define GPIO_UART7_TX    GPIO_UART7_TX_3       /* PE8  */
 #define GPIO_UART7_RX    GPIO_UART7_RX_3       /* PE7  */

@@ -268,13 +268,14 @@
 #define GPIO_SPI5_MISO   GPIO_SPI5_MISO_1               /* PF8  */
 #define GPIO_SPI5_MOSI   GPIO_SPI5_MOSI_2               /* PF9  */

+#define GPIO_SPI6_SCK    ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13  */
+#define GPIO_SPI6_MISO   GPIO_SPI6_MISO_1               /* PG12  */
+#define GPIO_SPI6_MOSI   GPIO_SPI6_MOSI_1               /* PG14  */
+

 /* I2C */
 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2       /* PB9  */

-#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2       /* PH7 */
-#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2       /* PH8 */
-
-#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4       /* PB6 */
-#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4       /* PB7 */
+#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2       /* PF14 */
+#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2       /* PF15 */
@@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
-CONFIG_ARCH_CHIP_STM32H743II=y
+CONFIG_ARCH_CHIP_STM32H743ZI=y
 CONFIG_ARCH_CHIP_STM32H7=y
 CONFIG_ARCH_INTERRUPTSTACK=768
 CONFIG_ARCH_STACKDUMP=y
@@ -192,7 +192,6 @@ CONFIG_STM32H7_DMA2=y
 CONFIG_STM32H7_DMACAPABLE=y
 CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
 CONFIG_STM32H7_I2C1=y
-CONFIG_STM32H7_I2C3=y
 CONFIG_STM32H7_I2C4=y
 CONFIG_STM32H7_I2C_DYNTIMEO=y
 CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
@@ -212,17 +211,20 @@ CONFIG_STM32H7_SPI2=y
 CONFIG_STM32H7_SPI5=y
 CONFIG_STM32H7_SPI5_DMA=y
 CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI6=y
+CONFIG_STM32H7_TIM15=y
+CONFIG_STM32H7_TIM16=y
 CONFIG_STM32H7_TIM1=y
 CONFIG_STM32H7_TIM2=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
 CONFIG_STM32H7_TIM8=y
+CONFIG_STM32H7_USART1=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
 CONFIG_STM32H7_UART4=y
 CONFIG_STM32H7_UART7=y
 CONFIG_STM32H7_UART8=y
-CONFIG_STM32H7_USART2=y
-CONFIG_STM32H7_USART3=y
-CONFIG_STM32H7_USART6=y
 CONFIG_STM32H7_USART_BREAKS=y
 CONFIG_STM32H7_USART_INVERT=y
 CONFIG_STM32H7_USART_SINGLEWIRE=y
@@ -252,9 +254,6 @@ CONFIG_USART3_IFLOWCONTROL=y
 CONFIG_USART3_OFLOWCONTROL=y
 CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_TXBUFSIZE=3000
-CONFIG_USART6_BAUD=57600
-CONFIG_USART6_RXBUFSIZE=600
-CONFIG_USART6_TXBUFSIZE=1500
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -45,95 +45,111 @@
 #include <stm32_gpio.h>

 /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
-#define GPIO_nLED_RED   /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
-#define GPIO_nLED_GREEN /* PB1  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_nLED_RED   /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_nLED_GREEN /* PB5  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
 #define GPIO_nLED_BLUE  /* PB3  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)

+#define GPIO_LED_SAFETY /* PE6  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
+
 #define BOARD_HAS_CONTROL_STATUS_LEDS      1
 #define BOARD_OVERLOAD_LED     LED_RED
 #define BOARD_ARMED_STATE_LED  LED_BLUE

 /* ADC channels */
 #define PX4_ADC_GPIO  \
-	/* PA2 */  GPIO_ADC12_INP14,  \
+	/* PC4 */  GPIO_ADC12_INP4,   \
 	/* PA3 */  GPIO_ADC12_INP15,  \
 	/* PA4 */  GPIO_ADC12_INP18,  \
-	/* PC1 */  GPIO_ADC123_INP11
+	/* PC0 */  GPIO_ADC123_INP10, \
+	/* PC5 */  GPIO_ADC12_INP8,   \
+	/* PB0 */  GPIO_ADC12_INP9,   \
+	/* PB1 */  GPIO_ADC12_INP5

 /* Define Channel numbers must match above GPIO pins */
-#define ADC_BATTERY_VOLTAGE_CHANNEL       14 /* PA2 BATT_VOLT_SENS */
+#define ADC_BATTERY_VOLTAGE_CHANNEL       4 /* PC4 BATT_VOLT_SENS */
 #define ADC_BATTERY_CURRENT_CHANNEL       15 /* PA3 BATT_CURRENT_SENS */
 #define ADC_SCALED_V5_CHANNEL             18 /* PA4 VDD_5V_SENS */
-#define ADC_RC_RSSI_CHANNEL               11 /* PC1 */
+#define ADC_RC_RSSI_CHANNEL               10 /* PC0 */
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL       8 /* PC5 */
+#define ADC_AUX1_VOLTAGE_CHANNEL       	   9 /* PB0 */
+#define ADC_AUX2_VOLTAGE_CHANNEL       	   5 /* PB1 */

 #define ADC_CHANNELS \
 	((1 << ADC_BATTERY_VOLTAGE_CHANNEL)       | \
 	 (1 << ADC_BATTERY_CURRENT_CHANNEL)       | \
 	 (1 << ADC_SCALED_V5_CHANNEL)             | \
-	 (1 << ADC_RC_RSSI_CHANNEL))
+	 (1 << ADC_RC_RSSI_CHANNEL)		  | \
+	 (1 << ADC_AIRSPEED_VOLTAGE_CHANNEL)	  | \
+	 (1 << ADC_AUX1_VOLTAGE_CHANNEL)	  | \
+	 (1 << ADC_AUX2_VOLTAGE_CHANNEL))

 /* HW has to large of R termination on ADC todo:change when HW value is chosen */
 #define BOARD_ADC_OPEN_CIRCUIT_V     (5.6f)

 /* CAN Silence: Silent mode control */
-#define GPIO_CAN1_SILENT_S0  /* PF5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
-#define GPIO_CAN2_SILENT_S0  /* PF5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
+#define GPIO_CAN1_SILENT_S0  /* PF11  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11)
+#define GPIO_CAN2_SILENT_S0  /* PF12  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)

 /* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS  12

 /* Power supply control and monitoring GPIOs */
-#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_nPOWER_IN_A                /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)

 #define GPIO_VDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
 #define BOARD_NUMBER_BRICKS             1

 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)

+#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+
 /* Define True logic Power Control in arch agnostic form */
 #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
 #define READ_VDD_3V3_SPEKTRUM_POWER_EN()   (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)

 /* Tone alarm output */
-#define TONE_ALARM_TIMER        2  /* timer 2 */
-#define TONE_ALARM_CHANNEL      1  /* PA15 TIM2_CH1 */
+#define TONE_ALARM_TIMER        16  /* timer 16 */
+#define TONE_ALARM_CHANNEL      1  /* PF6 TIM16_CH1 */

-#define GPIO_BUZZER_1           /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_BUZZER_1           /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6)

 #define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
-#define GPIO_TONE_ALARM         GPIO_TIM2_CH1OUT_2
+#define GPIO_TONE_ALARM         GPIO_TIM16_CH1OUT_2

 /* USB OTG FS */
 #define GPIO_OTGFS_VBUS         /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)

 /* High-resolution timer */
 #define HRT_TIMER               3  /* use timer3 for the HRT */
-#define HRT_TIMER_CHANNEL       1 /* use capture/compare channel 1 */
+#define HRT_TIMER_CHANNEL       2 /* use capture/compare channel 1 */

-#define HRT_PPM_CHANNEL         /* T3C3 */  3  /* use capture/compare channel 3 */
-#define GPIO_PPM_IN             /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
+#define HRT_PPM_CHANNEL         /* T3C2 */  2  /* use capture/compare channel 2 */
+#define GPIO_PPM_IN             /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3

 /* RC Serial port */
-#define RC_SERIAL_PORT          "/dev/ttyS3"
+#define RC_SERIAL_PORT          "/dev/ttyS5"

-#define GPIO_RSSI_IN            /* PC1  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
+#define GPIO_RSSI_IN            /* PC0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0)

 /* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
-#define GPIO_SAFETY_SWITCH_IN              /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
-/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
+#define GPIO_SAFETY_SWITCH_IN              /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
+/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
 #define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */

+/* No PX4IO processor present */
+#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0
+
 /* Power switch controls ******************************************************/
 #define SPEKTRUM_POWER(_on_true)           VDD_3V3_SPEKTRUM_POWER_EN(_on_true)

 /*
 * Board has a separate RC_IN
 *
- * GPIO PPM_IN on PB0 T3CH3
+ * GPIO PPM_IN on PC7 T3CH2
 * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
 *   Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
 */
-#define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
 #define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
@@ -156,7 +172,7 @@
 #define BOARD_HAS_STATIC_MANIFEST 1


-#define BOARD_NUM_IO_TIMERS 5
+#define BOARD_NUM_IO_TIMERS 6


 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -171,9 +187,12 @@
 		GPIO_CAN2_SILENT_S0,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
+		GPIO_LEVEL_SHIFTER_OE, 		  \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_SAFETY_SWITCH_IN,            \
 		GPIO_OTGFS_VBUS,                  \
+		GPIO_BTN_SAFETY,                  \
+		GPIO_LED_SAFETY,                  \
 	}

 __BEGIN_DECLS
@@ -35,6 +35,5 @@

 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(1),
-	initI2CBusExternal(3),
 	initI2CBusExternal(4),
 };
@@ -37,7 +37,7 @@

 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}),
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
 	}, {GPIO::PortE, GPIO::Pin3}),
 	initSPIBus(SPI::Bus::SPI2, {
@@ -46,7 +46,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	}),
 	initSPIBus(SPI::Bus::SPI5, {
 		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
-		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
+		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
+	}),
+	initSPIBusExternal(SPI::Bus::SPI6, {
+		initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}),
 	}),
 };

@@ -33,6 +33,28 @@

 #include <px4_arch/io_timer_hw_description.h>

+/* Timer allocation
+ *
+ * TIM1_CH4	T FMU_CH1
+ * TIM1_CH3	T FMU_CH2
+ * TIM1_CH2	T FMU_CH3
+ * TIM1_CH1	T FMU_CH4
+ *
+ * TIM4_CH2	T FMU_CH5
+ * TIM4_CH3	T FMU_CH6
+ * TIM2_CH3	T FMU_CH7
+ * TIM2_CH1	T FMU_CH8
+ *
+ * TIM2_CH4	T FMU_CH9
+ * TIM15_CH1	T FMU_CH10
+ *
+ * TIM8_CH1	T FMU_CH11
+ *
+ * TIM4_CH4	T FMU_CH12
+ *
+ * TIM16_CH1	T BUZZER	- Driven by other driver
+ */
+
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 	initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
 	initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
@@ -99,6 +99,7 @@ set(msg_files
 	FollowTargetStatus.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
+	GeofenceStatus.msg
 	GimbalControls.msg
 	GimbalDeviceAttitudeStatus.msg
 	GimbalDeviceInformation.msg
@@ -0,0 +1,7 @@
+uint64 timestamp                        # time since system start (microseconds)
+
+uint32 geofence_id 			# loaded geofence id
+uint8 status 				# Current geofence status
+
+uint8 GF_STATUS_LOADING = 0
+uint8 GF_STATUS_READY = 1
@@ -1,8 +1,8 @@
 uint64 timestamp		# time since system start (microseconds)

-uint16 mission_id   		# Id for the mission for which the result was generated
-uint16 geofence_id  		# Id for the corresponding geofence for which the result was generated (used for mission feasibility)
-uint64 home_position_counter  	# Counter of the home position for which the result was generated (used for mission feasibility)
+uint32 mission_id   		# Id for the mission for which the result was generated
+uint32 geofence_id  		# Id for the corresponding geofence for which the result was generated (used for mission feasibility)
+uint32 home_position_counter  	# Counter of the home position for which the result was generated (used for mission feasibility)

 int32 seq_reached		# Sequence of the mission item which has been reached, default -1
 uint16 seq_current		# Sequence of the current mission item
@@ -4,4 +4,6 @@ int32 val

 uint8[64] junk

+uint8 ORB_QUEUE_LENGTH = 16
+
 # TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll
@@ -47,4 +47,4 @@ uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18
 uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19
 uint16 ADSB_EMITTER_TYPE_ENUM_END=20

-uint8 ORB_QUEUE_LENGTH = 8
+uint8 ORB_QUEUE_LENGTH = 16
@@ -74,7 +74,7 @@ void px4_log_initialize(void)
 	log_message.severity = 6; // info
 	strcpy((char *)log_message.text, "initialized uORB logging");
 	log_message.timestamp = hrt_absolute_time();
-	orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, log_message_s::ORB_QUEUE_LENGTH);
+	orb_log_message_pub = orb_advertise(ORB_ID(log_message), &log_message);
 }

 __EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...)
@@ -48,24 +48,6 @@
 namespace uORB
 {

-template <typename U> class DefaultQueueSize
-{
-private:
-	template <typename T>
-	static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *)
-	{
-		return T::ORB_QUEUE_LENGTH;
-	}
-
-	template <typename T> static constexpr uint8_t get_queue_size(...)
-	{
-		return 1;
-	}
-
-public:
-	static constexpr unsigned value = get_queue_size<U>(nullptr);
-};
-
 class PublicationBase
 {
 public:
@@ -97,7 +79,7 @@ protected:
 /**
 * uORB publication wrapper class
 */
-template<typename T, uint8_t ORB_QSIZE = DefaultQueueSize<T>::value>
+template<typename T>
 class Publication : public PublicationBase
 {
 public:
@@ -113,7 +95,7 @@ public:
 	bool advertise()
 	{
 		if (!advertised()) {
-			_handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE);
+			_handle = orb_advertise(get_topic(), nullptr);
 		}

 		return advertised();
@@ -51,7 +51,7 @@ namespace uORB
 /**
 * Base publication multi wrapper class
 */
-template<typename T, uint8_t QSIZE = DefaultQueueSize<T>::value>
+template<typename T>
 class PublicationMulti : public PublicationBase
 {
 public:
@@ -73,7 +73,7 @@ public:
 	{
 		if (!advertised()) {
 			int instance = 0;
-			_handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE);
+			_handle = orb_advertise_multi(get_topic(), nullptr, &instance);
 		}

 		return advertised();
@@ -118,22 +118,11 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
 	return uORB::Manager::get_instance()->orb_advertise(meta, data);
 }

-orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
-{
-	return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
-}
-
 orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance);
 }

-orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
-				       unsigned int queue_size)
-{
-	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size);
-}
-
 int orb_unadvertise(orb_advert_t handle)
 {
 	return uORB::Manager::get_instance()->orb_unadvertise(handle);
@@ -227,6 +216,14 @@ const char *orb_get_c_type(unsigned char short_type)
 	return nullptr;
 }

+uint8_t orb_get_queue_size(const struct orb_metadata *meta)
+{
+	if (meta) {
+		return meta->o_queue;
+	}
+
+	return 0;
+}

 void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name)
 {
@@ -53,6 +53,8 @@ struct orb_metadata {
 	const uint16_t o_size_no_padding;   /**< object size w/o padding at the end (for logger) */
 	uint32_t message_hash;	/**< Hash over all fields for message compatibility checks */
 	orb_id_size_t  o_id;                /**< ORB_ID enum */
+	uint8_t o_queue;					/**< queue size */
+
 };

 typedef const struct orb_metadata *orb_id_t;
@@ -102,14 +104,16 @@ typedef const struct orb_metadata *orb_id_t;
 * @param _size_no_padding	Struct size w/o padding at the end
 * @param _message_hash	32 bit message hash over all fields
 * @param _orb_id_enum	ORB ID enum e.g.: ORB_ID::vehicle_status
+ * @param _queue_size Queue size from topic definition
 */
-#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum)		\
-	const struct orb_metadata __orb_##_name = {	\
-		#_name,					\
-		sizeof(_struct),		\
-		_size_no_padding,			\
-		_message_hash,				\
-		_orb_id_enum				\
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum, _queue_size)               \
+	const struct orb_metadata __orb_##_name = {     \
+		#_name,                                 \
+		sizeof(_struct),                \
+		_size_no_padding,                       \
+		_message_hash,                          \
+		_orb_id_enum,                           \
+		_queue_size                             \
 	}; struct hack

 __BEGIN_DECLS
@@ -135,23 +139,11 @@ typedef void 	*orb_advert_t;
 */
 extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;

-/**
- * @see uORB::Manager::orb_advertise()
- */
-extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data,
-					unsigned int queue_size) __EXPORT;
-
 /**
 * @see uORB::Manager::orb_advertise_multi()
 */
 extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT;

-/**
- * @see uORB::Manager::orb_advertise_multi()
- */
-extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size) __EXPORT;
-
 /**
 * @see uORB::Manager::orb_unadvertise()
 */
@@ -160,7 +152,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT;
 /**
 * @see uORB::Manager::orb_publish()
 */
-extern int	orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
+extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;

 /**
 * Advertise as the publisher of a topic.
@@ -241,6 +233,12 @@ extern int	orb_get_interval(int handle, unsigned *interval) __EXPORT;
 */
 const char *orb_get_c_type(unsigned char short_type);

+/**
+ * Returns the queue size of a topic
+ * @param meta orb topic metadata
+ */
+extern uint8_t orb_get_queue_size(const struct orb_metadata *meta);
+
 /**
 * Print a topic to console. Do not call this directly, use print_message() instead.
 * @param meta orb topic metadata
@@ -48,37 +48,10 @@

 static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }

-// round up to nearest power of two
-// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
-// Note: When the input value > 128, the output is always 128
-static inline uint8_t round_pow_of_two_8(uint8_t n)
-{
-	if (n == 0) {
-		return 1;
-	}
-
-	// Avoid is already a power of 2
-	uint8_t value = n - 1;
-
-	// Fill 1
-	value |= value >> 1U;
-	value |= value >> 2U;
-	value |= value >> 4U;
-
-	// Unable to round-up, take the value of round-down
-	if (value == UINT8_MAX) {
-		value >>= 1U;
-	}
-
-	return value + 1;
-}
-
-uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path,
-			     uint8_t queue_size) :
+uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) :
 	CDev(strdup(path)), // success is checked in CDev::init
 	_meta(meta),
-	_instance(instance),
-	_queue_size(round_pow_of_two_8(queue_size))
+	_instance(instance)
 {
 }

@@ -186,7 +159,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)

 			/* re-check size */
 			if (nullptr == _data) {
-				const size_t data_size = _meta->o_size * _queue_size;
+				const size_t data_size = _meta->o_size * _meta->o_queue;
 				_data = (uint8_t *) px4_cache_aligned_alloc(data_size);

 				if (_data) {
@@ -217,7 +190,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
 	unsigned generation = _generation.fetch_add(1);

-	memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size);
+	memcpy(_data + (_meta->o_size * (generation % _meta->o_queue)), buffer, _meta->o_size);

 	// callbacks
 	for (auto item : _callbacks) {
@@ -254,13 +227,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 		*(uintptr_t *)arg = (uintptr_t)this;
 		return PX4_OK;

-	case ORBIOCSETQUEUESIZE: {
-			lock();
-			int ret = update_queue_size(arg);
-			unlock();
-			return ret;
-		}
-
 	case ORBIOCGETINTERVAL:
 		*(unsigned *)arg = filp_to_subscription(filp)->get_interval_us();
 		return PX4_OK;
@@ -389,12 +355,11 @@ uORB::DeviceNode::print_statistics(int max_topic_length)

 	const uint8_t instance = get_instance();
 	const int8_t sub_count = subscriber_count();
-	const uint8_t queue_size = get_queue_size();

 	unlock();

 	PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
-		     queue_size, get_meta()->o_size, get_devname());
+		     get_meta()->o_queue, get_meta()->o_size, get_devname());

 	return true;
 }
@@ -483,21 +448,6 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data
 }
 #endif /* CONFIG_ORB_COMMUNICATOR */

-int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
-{
-	if (_queue_size == queue_size) {
-		return PX4_OK;
-	}
-
-	//queue size is limited to 255 for the single reason that we use uint8 to store it
-	if (_data || _queue_size > queue_size || queue_size > 255) {
-		return PX4_ERROR;
-	}
-
-	_queue_size = round_pow_of_two_8(queue_size);
-	return PX4_OK;
-}
-
 unsigned uORB::DeviceNode::get_initial_generation()
 {
 	ATOMIC_ENTER;
@@ -62,7 +62,7 @@ class UnitTest;
 class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode<uORB::DeviceNode *>
 {
 public:
-	DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1);
+	DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path);
 	virtual ~DeviceNode();

 	// no copy, assignment, move, move assignment
@@ -179,15 +179,6 @@ public:

 	void mark_as_advertised() { _advertised = true; }

-	/**
-	 * Try to change the size of the queue. This can only be done as long as nobody published yet.
-	 * This is the case, for example when orb_subscribe was called before an orb_advertise.
-	 * The queue size can only be increased.
-	 * @param queue_size new size of the queue
-	 * @return PX4_OK if queue size successfully set
-	 */
-	int update_queue_size(unsigned int queue_size);
-
 	/**
 	 * Print statistics
 	 * @param max_topic_length max topic name length for printing
@@ -195,7 +186,7 @@ public:
 	 */
 	bool print_statistics(int max_topic_length);

-	uint8_t get_queue_size() const { return _queue_size; }
+	uint8_t get_queue_size() const { return _meta->o_queue; }

 	int8_t subscriber_count() const { return _subscriber_count; }

@@ -234,7 +225,7 @@ public:
 	bool copy(void *dst, unsigned &generation)
 	{
 		if ((dst != nullptr) && (_data != nullptr)) {
-			if (_queue_size == 1) {
+			if (_meta->o_queue == 1) {
 				ATOMIC_ENTER;
 				memcpy(dst, _data, _meta->o_size);
 				generation = _generation.load();
@@ -253,12 +244,12 @@ public:
 				}

 				// Compatible with normal and overflow conditions
-				if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
+				if (!is_in_range(current_generation - _meta->o_queue, generation, current_generation - 1)) {
 					// Reader is too far behind: some messages are lost
-					generation = current_generation - _queue_size;
+					generation = current_generation - _meta->o_queue;
 				}

-				memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
+				memcpy(dst, _data + (_meta->o_size * (generation % _meta->o_queue)), _meta->o_size);
 				ATOMIC_LEAVE;

 				++generation;
@@ -295,7 +286,7 @@ private:

 	const uint8_t _instance; /**< orb multi instance identifier */
 	bool _advertised{false};  /**< has ever been advertised (not necessarily published data yet) */
-	uint8_t _queue_size; /**< maximum number of elements in the queue */
+
 	int8_t _subscriber_count{0};


@@ -265,8 +265,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 	return ret;
 }

-orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size)
+orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 #ifdef ORB_USE_PUBLISHER_RULES

@@ -300,19 +299,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}

-	/* Set the queue size. This must be done before the first publication; thus it fails if
-	 * this is not the first advertiser.
-	 */
-	int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
-
-	if (result < 0 && queue_size > 1) {
-		PX4_WARN("orb_advertise_multi: failed to set queue size");
-	}
-
 	/* get the advertiser handle and close the node */
 	orb_advert_t advertiser;

-	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
 	px4_close(fd);

 	if (result == PX4_ERROR) {
@@ -602,6 +592,22 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 {
 	PX4_DEBUG("entering process_remote_topic: name: %s", topic_name);

+	// First make sure this is a valid topic
+	const struct orb_metadata *const *topic_list = orb_get_topics();
+	orb_id_t topic_ptr = nullptr;
+
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
+			topic_ptr = topic_list[i];
+			break;
+		}
+	}
+
+	if (! topic_ptr) {
+		PX4_ERR("process_remote_topic meta not found for %s\n", topic_name);
+		return -1;
+	}
+
 	// Look to see if we already have a node for this topic
 	char nodepath[orb_maxpath];
 	int ret = uORB::Utils::node_mkpath(nodepath, topic_name);
@@ -613,7 +619,7 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 			uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);

 			if (node) {
-				PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
+				PX4_DEBUG("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
 				node->mark_as_advertised();
 				_remote_topics.insert(topic_name);
 				return 0;
@@ -622,27 +628,9 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 	}

 	// We didn't find a node so we need to create it via an advertisement
-	const struct orb_metadata *const *topic_list = orb_get_topics();
-	orb_id_t topic_ptr = nullptr;
-
-	for (size_t i = 0; i < orb_topics_count(); i++) {
-		if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
-			topic_ptr = topic_list[i];
-			break;
-		}
-	}
-
-	if (topic_ptr) {
-		PX4_INFO("Advertising remote topic %s", topic_name);
-		_remote_topics.insert(topic_name);
-		// Add some queue depth when advertising remote topics. These
-		// topics may get aggregated and thus delivered in a batch that
-		// requires some buffering in a queue.
-		orb_advertise(topic_ptr, nullptr, 5);
-
-	} else {
-		PX4_INFO("process_remote_topic meta not found for %s\n", topic_name);
-	}
+	PX4_DEBUG("Advertising remote topic %s", topic_name);
+	_remote_topics.insert(topic_name);
+	orb_advertise(topic_ptr, nullptr, topic_ptr->o_queue);

 	return 0;
 }
@@ -663,8 +651,11 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName)
 			PX4_DEBUG("DeviceNode(%s) not created yet", messageName);

 		} else {
-			// node is present.
-			node->process_add_subscription();
+			// node is present. But don't send any data to it if it
+			// is a node advertised by the remote side
+			if (_remote_topics.find(messageName) == false) {
+				node->process_add_subscription();
+			}
 		}

 	} else {
@@ -215,17 +215,15 @@ public:
 	 * @param data    A pointer to the initial data to be published.
 	 *      For topics updated by interrupt handlers, the advertisement
 	 *      must be performed from non-interrupt context.
-	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
-	 *      used.
 	 * @return    nullptr on error, otherwise returns an object pointer
 	 *      that can be used to publish to the topic.
 	 *      If the topic in question is not known (due to an
 	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
 	 *      this function will return nullptr and set errno to ENOENT.
 	 */
-	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
+	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data = nullptr)
 	{
-		return orb_advertise_multi(meta, data, nullptr, queue_size);
+		return orb_advertise_multi(meta, data, nullptr);
 	}

 	/**
@@ -250,16 +248,13 @@ public:
 	 * @param instance  Pointer to an integer which will yield the instance ID (0-based)
 	 *      of the publication. This is an output parameter and will be set to the newly
 	 *      created instance, ie. 0 for the first advertiser, 1 for the next and so on.
-	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
-	 *      used.
 	 * @return    nullptr on error, otherwise returns a handle
 	 *      that can be used to publish to the topic.
 	 *      If the topic in question is not known (due to an
 	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
 	 *      this function will return nullptr and set errno to ENOENT.
 	 */
-	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-					 unsigned int queue_size = 1);
+	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance);

 	/**
 	 * Unadvertise a topic.
@@ -89,8 +89,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 	return data.ret;
 }

-orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size)
+orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 	/* open the node as an advertiser */
 	int fd = node_open(meta, true, instance);
@@ -100,19 +99,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}

-	/* Set the queue size. This must be done before the first publication; thus it fails if
-	 * this is not the first advertiser.
-	 */
-	int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
-
-	if (result < 0 && queue_size > 1) {
-		PX4_WARN("orb_advertise_multi: failed to set queue size");
-	}
-
 	/* get the advertiser handle and close the node */
 	orb_advert_t advertiser;

-	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
 	px4_close(fd);

 	if (result == PX4_ERROR) {
@@ -574,8 +574,8 @@ int uORBTest::UnitTest::test_wrap_around()
 	bool updated{false};

 	// Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation
-	const int queue_size = 16;
-	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_wrap_around), nullptr, queue_size);
+	const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_wrap_around));
+	ptopic = orb_advertise(ORB_ID(orb_test_medium_wrap_around), nullptr);

 	if (ptopic == nullptr) {
 		return test_fail("advertise failed: %d", errno);
@@ -828,9 +828,9 @@ int uORBTest::UnitTest::test_queue()
 		return test_fail("subscribe failed: %d", errno);
 	}

-	const int queue_size = 16;
+	const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue));
 	orb_test_medium_s t{};
-	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
+	ptopic = orb_advertise(ORB_ID(orb_test_medium_queue), &t);

 	if (ptopic == nullptr) {
 		return test_fail("advertise failed: %d", errno);
@@ -935,9 +935,9 @@ int uORBTest::UnitTest::pub_test_queue_main()
 {
 	orb_test_medium_s t{};
 	orb_advert_t ptopic{nullptr};
-	const int queue_size = 50;
+	const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue_poll));

-	if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
+	if ((ptopic = orb_advertise(ORB_ID(orb_test_medium_queue_poll), &t)) == nullptr) {
 		_thread_should_exit = true;
 		return test_fail("advertise failed: %d", errno);
 	}
@@ -45,28 +45,10 @@
 namespace uORB
 {

-template <typename U> class DefaultQueueSize
-{
-private:
-	template <typename T>
-	static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *)
-	{
-		return T::ORB_QUEUE_LENGTH;
-	}
-
-	template <typename T> static constexpr uint8_t get_queue_size(...)
-	{
-		return 1;
-	}
-
-public:
-	static constexpr unsigned value = get_queue_size<U>(nullptr);
-};
-
 /**
 * uORB publication wrapper class
 */
-template<typename T, uint8_t ORB_QSIZE = DefaultQueueSize<T>::value>
+template<typename T>
 class Publication
 {
 public:
@@ -314,7 +314,7 @@ CameraTrigger::CameraTrigger() :
 	// Advertise critical publishers here, because we cannot advertise in interrupt context
 	camera_trigger_s trigger{};

-	_trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH);
+	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
 }

 CameraTrigger::~CameraTrigger()
@@ -160,6 +160,11 @@ status_t Argus_InitMode(argus_hnd_t *hnd, s2pi_slave_t spi_slave, argus_mode_t m
 *          Also refer to #Argus_ReinitMode, which uses a specified measurement
 *          mode instead of the currently active measurement mode.
 *
+ * @note    If a full re-initialization is not desired, refer to the
+ *          #Argus_RestoreDeviceState function that will only re-write the
+ *          register map to the device to restore its state after an power
+ *          cycle.
+ *
 * @param   hnd The API handle; contains all internal states and data.
 *
 * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
@@ -182,6 +187,11 @@ status_t Argus_Reinit(argus_hnd_t *hnd);
 *          Also refer to #Argus_Reinit, which re-uses the currently active
 *          measurement mode instead of an user specified measurement mode.
 *
+ * @note    If a full re-initialization is not desired, refer to the
+ *          #Argus_RestoreDeviceState function that will only re-write the
+ *          register map to the device to restore its state after an power
+ *          cycle.
+ *
 * @param   hnd The API handle; contains all internal states and data.
 *
 * @param   mode The specified measurement mode to be initialized.
@@ -274,6 +284,69 @@ argus_hnd_t *Argus_CreateHandle(void);
 *****************************************************************************/
 status_t Argus_DestroyHandle(argus_hnd_t *hnd);

+/*!***************************************************************************
+ * @brief   Restores the device state with a re-write of all register values.
+ *
+ * @details The function invalidates and restores the device state by executing
+ *          a re-write of the full register map.
+ *
+ *          The purpose of this function is to recover from known external
+ *          events like power cycles, for example due to sleep / wake-up
+ *          functionality. This can be implemented by cutting off the external
+ *          power supply of the device (e.g. via a MOSFET switch controlled by
+ *          a GPIB pin). By calling this function, the expected state of the
+ *          API is written to the device without the need to fully re-initialize
+ *          the device. Thus, the API can resume where it has stopped as if
+ *          there has never been a power cycle.
+ *
+ *          The internal state machines like the dynamic configuration adaption
+ *          (DCA) algorithm will not be reseted. The API/sensor will immediately
+ *          resume at the last state that was optimized for the given
+ *          environmental conditions.
+ *
+ *          The use case of sleep / wake-up can be implemented as follows:
+ *
+ *          1. In case of ongoing measurements, stop the measurements via
+ *             the #Argus_StopMeasurementTimer function (if started by the
+ *             #Argus_StartMeasurementTimer function).
+ *
+ *          2. Shut down the device by removing the 5V power supply, e.g.
+ *             via a GPIO pin that switches a MOSFET circuit.
+ *
+ *          3. After the desired sleep period, power the device by switching
+ *             the 5V power supply on again. Wait until the power-on-reset
+ *             (POR) is finished (approx. 1 ms) or just repeat step 4 until
+ *             it succeeds.
+ *
+ *          4. Call the #Argus_RestoreDeviceState function to trigger the
+ *             restoration of the device state in the API. Note that the
+ *             function will return an error code if it fails. One can repeat
+ *             the execution of that function a few times until it succeeds.
+ *
+ *          6. Continue with measurements via #Argus_StartMeasurementTimer
+ *             of #Argus_TriggerMeasurement functions as desired.
+ *
+ * @note    If a complete re-initialization (= soft-reset) is desired, see
+ *          the #Argus_Reinit functionality.
+ *
+ * @note    Changing a configuration or calibration parameter will always
+ *          invalidate the device state as well as the state machine of the
+ *          dynamic configuration adaption (DCA) algorithm. In that case, the
+ *          device/API needs a few measurements to adopt to the present
+ *          environmental conditions before the first valid measurement result
+ *          can be obtained. This is almost similar to re-initializing the
+ *          device (see #Argus_Reinit) which would also re-read the EEPROM.
+ *          On the other hand, the #Argus_RestoreDeviceState does not reset
+ *          or re-initialize anything. It just makes sure that the device
+ *          register map (which has changed to its reset values after the
+ *          power cycle) is what the API expects upon the next measurement.
+ *
+ * @param   hnd The device handle object to be invalidated.
+ *
+ * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ *****************************************************************************/
+status_t Argus_RestoreDeviceState(argus_hnd_t *hnd);
+
 /*!**************************************************************************
 * Generic API
 ****************************************************************************/
@@ -726,7 +799,7 @@ status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd);
 *          After calibration has finished successfully, the obtained data is
 *          applied immediately and can be read from the API using the
 *          #Argus_GetCalibrationPixelRangeOffsets or
- *          #Argus_GetCalibrationGlobalRangeOffset function.
+ *          #Argus_GetCalibrationGlobalRangeOffsets function.
 *
 * @param   hnd The API handle; contains all internal states and data.
 * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
@@ -775,7 +848,7 @@ status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd);
 *          After calibration has finished successfully, the obtained data is
 *          applied immediately and can be read from the API using the
 *          #Argus_GetCalibrationPixelRangeOffsets or
- *          #Argus_GetCalibrationGlobalRangeOffset function.
+ *          #Argus_GetCalibrationGlobalRangeOffsets function.
 *
 * @param   hnd The API handle; contains all internal states and data.
 * @param   targetRange The absolute range between the reference plane and the
@@ -1043,28 +1116,40 @@ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd,
 ****************************************************************************/

 /*!***************************************************************************
- * @brief   Sets the global range offset value to a specified device.
+ * @brief   Sets the global range offset values to a specified device.
 *
- * @details The global range offset is subtracted from the raw range values.
+ * @details The global range offsets are subtracted from the raw range values.
+ *          There are two distinct values that are applied in low or high
+ *          power stage setting respectively.
 *
 * @param   hnd The API handle; contains all internal states and data.
- * @param   value The new global range offset in meter and Q0.15 format.
+ * @param   offset_low The new global range offset for the low power stage in
+ *                     meter and Q0.15 format.
+ * @param   offset_high The new global range offset for the high power stage in
+ *                      meter and Q0.15 format.
 * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
 *****************************************************************************/
-status_t Argus_SetCalibrationGlobalRangeOffset(argus_hnd_t *hnd,
-		q0_15_t value);
+status_t Argus_SetCalibrationGlobalRangeOffsets(argus_hnd_t *hnd,
+		q0_15_t offset_low,
+		q0_15_t offset_high);

 /*!***************************************************************************
- * @brief   Gets the global range offset value from a specified device.
+ * @brief   Gets the global range offset values from a specified device.
 *
- * @details The global range offset is subtracted from the raw range values.
+ * @details The global range offsets are subtracted from the raw range values.
+ *          There are two distinct values that are applied in low or high
+ *          power stage setting respectively.
 *
 * @param   hnd The API handle; contains all internal states and data.
- * @param   value The current global range offset in meter and Q0.15 format.
+ * @param   offset_low The current range offset for the low power stage in
+ *                     meter and Q0.15 format.
+ * @param   offset_high The current global range offset for the high power stage
+ *                      in meter and Q0.15 format.
 * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
 *****************************************************************************/
-status_t Argus_GetCalibrationGlobalRangeOffset(argus_hnd_t *hnd,
-		q0_15_t *value);
+status_t Argus_GetCalibrationGlobalRangeOffsets(argus_hnd_t *hnd,
+		q0_15_t *offset_low,
+		q0_15_t *offset_high);

 /*!***************************************************************************
 * @brief   Sets the relative pixel offset table to a specified device.
@@ -210,9 +210,13 @@ typedef enum argus_dca_gain_t {
 *          - [9]: #ARGUS_STATE_LASER_ERROR
 *          - [10]: #ARGUS_STATE_HAS_DATA
 *          - [11]: #ARGUS_STATE_HAS_AUX_DATA
- *          - [12]: #ARGUS_STATE_DCA_MAX
+ *          - [12]: #ARGUS_STATE_SATURATED_PIXELS
 *          - [13]: DCA Power Stage
 *          - [14-15]: DCA Gain Stages
+ *          - [16]: #ARGUS_STATE_DCA_MIN
+ *          - [17]: #ARGUS_STATE_DCA_MAX
+ *          - [18]: #ARGUS_STATE_DCA_RESET
+ *          - [18-31]: not used
 *          .
 *****************************************************************************/
 typedef enum argus_state_t {
@@ -229,36 +233,35 @@ typedef enum argus_state_t {
 	 *          - 1: Enabled: measurement with detuned frequency. */
 	ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U,

-	/*! 0x0004: Measurement Frequency for Dual Frequency Mode
+	/*! 0x0004: Measurement Frequency for Dual Frequency Mode \n
 	 *          (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set).
 	 *          - 0: A-Frame w/ detuned frequency,
 	 *          - 1: B-Frame w/ detuned frequency */
 	ARGUS_STATE_MEASUREMENT_FREQ = 1U << 2U,

-	/*! 0x0008: Debug Mode. If set, the range value of erroneous pixels
+	/*! 0x0008: Debug Mode. \n
+	 *          If set, the range value of erroneous pixels
 	 *          are not cleared or reset.
 	 *          - 0: Disabled (default).
 	 *          - 1: Enabled. */
 	ARGUS_STATE_DEBUG_MODE = 1U << 3U,

-	/*! 0x0010: Weak Signal Flag.
+	/*! 0x0010: Weak Signal Flag. \n
 	 *          Set whenever the Pixel Binning Algorithm is detecting a
 	 *          weak signal, i.e. if the amplitude dies not reach its
-	 *          (absolute) threshold. If the Golden Pixel is enabled,
-	 *          this also indicates that the Pixel Binning Algorithm
-	 *          falls back to the Golden Pixel.
+	 *          (absolute) threshold.
 	 *          - 0: Normal Signal.
-	 *          - 1: Weak Signal or Golden Pixel Mode. */
+	 *          - 1: Weak Signal. */
 	ARGUS_STATE_WEAK_SIGNAL = 1U << 4U,

-	/*! 0x0020: Background Light Warning Flag.
+	/*! 0x0020: Background Light Warning Flag. \n
 	 *          Set whenever the background light is very high and the
 	 *          measurement data might be unreliable.
 	 *          - 0: No Warning: Background Light is within valid range.
 	 *          - 1: Warning: Background Light is very high. */
 	ARGUS_STATE_BGL_WARNING = 1U << 5U,

-	/*! 0x0040: Background Light Error Flag.
+	/*! 0x0040: Background Light Error Flag. \n
 	 *          Set whenever the background light is too high and the
 	 *          measurement data is unreliable or invalid.
 	 *          - 0: No Error: Background Light is within valid range.
@@ -270,7 +273,7 @@ typedef enum argus_state_t {
 	 *          - 1: PLL locked at start of integration. */
 	ARGUS_STATE_PLL_LOCKED = 1U << 7U,

-	/*! 0x0100: Laser Failure Warning Flag.
+	/*! 0x0100: Laser Failure Warning Flag. \n
 	 *          Set whenever the an invalid system condition is detected.
 	 *          (i.e. DCA at max state but no amplitude on any (incl. reference)
 	 *          pixel, not amplitude but any saturated pixel).
@@ -279,7 +282,7 @@ typedef enum argus_state_t {
 	 *               condition stays, a laser malfunction error is raised. */
 	ARGUS_STATE_LASER_WARNING = 1U << 8U,

-	/*! 0x0200: Laser Failure Error Flag.
+	/*! 0x0200: Laser Failure Error Flag. \n
 	 *          Set whenever a laser malfunction error is raised and the
 	 *          system is put into a safe state.
 	 *          - 0: No Error: Laser is operating properly.
@@ -297,13 +300,12 @@ typedef enum argus_state_t {
 	 *          - 1: Auxiliary data is available and correctly evaluated. */
 	ARGUS_STATE_HAS_AUX_DATA = 1U << 11U,

-	/*! 0x1000: DCA Maximum State Flag.
-	 *          Set whenever the DCA has extended all its parameters to their
-	 *          maximum values and can not increase the integration energy any
-	 *          further.
-	 *          - 0: DCA has not yet reached its maximum state.
-	 *          - 1: DCA has reached its maximum state and can not increase any further. */
-	ARGUS_STATE_DCA_MAX = 1U << 12U,
+	/*! 0x0100: Pixel Saturation Flag. \n
+	 *          Set whenever any pixel is saturated, i.e. its pixel state is
+	 *          #PIXEL_SAT
+	 *          - 0: No saturated pixels.
+	 *          - 1: Any saturated pixels. */
+	ARGUS_STATE_SATURATED_PIXELS = 1U << 12U,

 	/*! 0x2000: DCA is in high Optical Output Power stage. */
 	ARGUS_STATE_DCA_POWER_HIGH = DCA_POWER_HIGH << ARGUS_STATE_DCA_POWER_SHIFT,
@@ -320,6 +322,31 @@ typedef enum argus_state_t {
 	/*! 0xC000: DCA is in high Pixel Input Gain stage. */
 	ARGUS_STATE_DCA_GAIN_HIGH = DCA_GAIN_HIGH << ARGUS_STATE_DCA_GAIN_SHIFT,

+	/*! 0x10000: DCA Minimum State Flag. \n
+	 *           Set whenever the DCA has reduced all its parameters to their
+	 *           minimum values and it can not decrease the integration energy
+	 *           any further.
+	 *           - 0: DCA has not yet reached its minimum state.
+	 *           - 1: DCA has reached its minimum state and can not decrease
+	 *                its parameters any further. */
+	ARGUS_STATE_DCA_MIN = 1U << 16U,
+
+	/*! 0x20000: DCA Maximum State Flag. \n
+	 *           Set whenever the DCA has extended all its parameters to their
+	 *           maximum values and it can not increase the integration energy
+	 *           any further.
+	 *           - 0: DCA has not yet reached its maximum state.
+	 *           - 1: DCA has reached its maximum state and can not increase
+	 *                its parameters any further. */
+	ARGUS_STATE_DCA_MAX = 1U << 17U,
+
+	/*! 0x20000: DCA Reset State Flag. \n
+	 *           Set whenever the DCA is resetting all its parameters to their
+	 *           minimum values because it has detected too many saturated pixels.
+	 *           - 0: DCA is operating in normal mode.
+	 *           - 1: DCA is performing a reset. */
+	ARGUS_STATE_DCA_RESET = 1U << 18U,
+
 } argus_state_t;

 /*!***************************************************************************
@@ -58,6 +58,7 @@ extern "C" {
 *****************************************************************************/

 #include "utility/int_math.h"
+#include <stdbool.h>
 #include <assert.h>


@@ -138,6 +139,13 @@ extern "C" {
 #define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U))


+/*!*****************************************************************************
+ * @brief   Macro to create a pixel mask given by the pixels n-index.
+ * @param   n n-index of the pixel.
+ * @return  The pixel mask with only n-index pixel set.
+ ******************************************************************************/
+#define PIXELN_MASK(n) (0x01U << (n))
+
 /*!*****************************************************************************
 * @brief   Macro to determine if a pixel given by the n-index is enabled in a pixel mask.
 * @param   msk 32-bit pixel mask
@@ -151,16 +159,23 @@ extern "C" {
 * @param   msk 32-bit pixel mask
 * @param   n n-index of the pixel to enable.
 ******************************************************************************/
-#define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (n)))
+#define PIXELN_ENABLE(msk, n) ((msk) |= (PIXELN_MASK(n)))

 /*!*****************************************************************************
 * @brief   Macro disable a pixel given by the n-index in a pixel mask.
 * @param   msk 32-bit pixel mask
 * @param   n n-index of the pixel to disable.
 ******************************************************************************/
-#define PIXELN_DISABLE(msk, n) ((msk) &= (~(0x01U << (n))))
+#define PIXELN_DISABLE(msk, n) ((msk) &= (~PIXELN_MASK(n)))


+/*!*****************************************************************************
+ * @brief   Macro to create a pixel mask given by the pixels ADC channel number.
+ * @param   c The ADC channel number of the pixel.
+ * @return  The 32-bit pixel mask with only pixel ADC channel set.
+ ******************************************************************************/
+#define PIXELCH_MASK(c) (0x01U << (PIXEL_CH2N(c)))
+
 /*!*****************************************************************************
 * @brief   Macro to determine if an ADC pixel channel is enabled from a pixel mask.
 * @param   msk The 32-bit pixel mask
@@ -184,6 +199,14 @@ extern "C" {
 #define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c)))


+/*!*****************************************************************************
+ * @brief   Macro to create a pixel mask given by the pixel x-y-indices.
+ * @param   x x-index of the pixel.
+ * @param   y y-index of the pixel.
+ * @return  The 32-bit pixel mask with only pixel ADC channel set.
+ ******************************************************************************/
+#define PIXELXY_MASK(x, y) (0x01U << (PIXEL_XY2N(x, y)))
+
 /*!*****************************************************************************
 * @brief   Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask.
 * @param   msk 32-bit pixel mask
@@ -337,10 +360,10 @@ static inline uint32_t ShiftSelectedPixels(const uint32_t pixel_mask,

 	uint32_t shifted_mask = 0;

-	for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
-		for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
-			int8_t x_src = x - dx;
-			int8_t y_src = y - dy;
+	for (int8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
+		for (int8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
+			int8_t x_src = (int8_t)(x - dx);
+			int8_t y_src = (int8_t)(y - dy);

 			if (dy & 0x1) {
 				/* Compensate for hexagonal pixel shape. */
@@ -409,8 +432,8 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,
 		int8_t min_y = -1;

 		/* Find nearest not selected pixel. */
-		for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
-			for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
+		for (int8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
+			for (int8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
 				if (!PIXELXY_ISENABLED(pixel_mask, x, y)) {
 					int32_t distx = (x - center_x) << 1;

@@ -423,8 +446,8 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,

 					if (dist < min_dist) {
 						min_dist = dist;
-						min_x = x;
-						min_y = y;
+						min_x = (int8_t)x;
+						min_y = (int8_t)y;
 					}
 				}
 			}
@@ -438,6 +461,64 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,

 	return pixel_mask;
 }
+
+/*!*****************************************************************************
+ * @brief   Fills a pixel mask with the direct neighboring pixels around a pixel.
+ *
+ * @details The pixel mask is iteratively filled with the direct neighbors of the
+ *          specified center pixel.
+ *
+ *          Note that the function is able to handle corner and edge pixels and
+ *          also to handle odd/even lines (which have different layouts)
+ *
+ * @param   x The selected pixel x-index.
+ * @param   y The selected pixel y-index.
+ * @return  The filled pixel mask with all direct neighbors of the selected pixel.
+ ******************************************************************************/
+static inline uint32_t GetAdjacentPixelsMask(const uint_fast8_t x,
+		const uint_fast8_t y)
+{
+	assert(x < ARGUS_PIXELS_X);
+	assert(y < ARGUS_PIXELS_Y);
+
+	uint32_t mask = 0u;
+
+	bool isXEdgeLow = (x == 0);
+	bool isXEdgeHigh = (x == (ARGUS_PIXELS_X - 1));
+	bool isYEdgeLow = (y == 0);
+	bool isYEdgeHigh = (y == (ARGUS_PIXELS_Y - 1));
+
+	if (y % 2 == 0) {
+		if (!isYEdgeLow) { PIXELXY_ENABLE(mask, x,     y - 1); }
+
+		if ((!isXEdgeHigh) && (!isYEdgeLow)) { PIXELXY_ENABLE(mask, x + 1, y - 1); }
+
+		if (!isXEdgeHigh) { PIXELXY_ENABLE(mask, x + 1, y); }
+
+		if ((!isXEdgeHigh) && (!isYEdgeHigh)) { PIXELXY_ENABLE(mask, x + 1, y + 1); }
+
+		if (!isYEdgeHigh) { PIXELXY_ENABLE(mask, x,     y + 1); }
+
+		if (!isXEdgeLow) { PIXELXY_ENABLE(mask, x - 1, y); }
+
+	} else {
+		if ((!isXEdgeLow) && (!isYEdgeLow)) { PIXELXY_ENABLE(mask, x - 1, y - 1); }
+
+		if (!isYEdgeLow) { PIXELXY_ENABLE(mask, x,     y - 1); }
+
+		if (!isXEdgeHigh) { PIXELXY_ENABLE(mask, x + 1, y); }
+
+		if (!isYEdgeHigh) { PIXELXY_ENABLE(mask, x,     y + 1); }
+
+		if ((!isXEdgeLow) && (!isYEdgeHigh)) { PIXELXY_ENABLE(mask, x - 1, y + 1); }
+
+		if (!isXEdgeLow) { PIXELXY_ENABLE(mask, x - 1, y); }
+	}
+
+	return mask;
+}
+
+
 /*! @} */
 #ifdef __cplusplus
 } // extern "C"
@@ -1,170 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief    	This file is part of the AFBR-S50 API.
- * @details		Defines macros to work with pixel and ADC channel masks.
- *
- * @copyright
- *
- * Copyright (c) 2021, Broadcom Inc
- * 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 of the copyright holder 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 HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *****************************************************************************/
-
-#ifndef ARGUS_MSK_H
-#define ARGUS_MSK_H
-
-/*!***************************************************************************
- * @defgroup	argusmap ADC Channel Mapping
- * @ingroup		argusres
- *
- * @brief		Pixel ADC Channel (n) to x-y-Index Mapping
- *
- * @details		The ADC Channels of each pixel or auxiliary channel on the device
- * 				is numbered in a way that is convenient on the chip. The macros
- * 				in this module are defined in order to obtain the x-y-indices of
- * 				each channel and vice versa.
- *
- * @addtogroup 	argusmap
- * @{
- *****************************************************************************/
-
-#include "api/argus_def.h"
-#include "utility/int_math.h"
-
-/*!*****************************************************************************
- * @brief	Macro to determine the channel number of an specified Pixel.
- * @param	x The x index of the pixel.
- * @param	y The y index of the pixel.
- * @return	The channel number n of the pixel.
- ******************************************************************************/
-#define PIXEL_XY2N(x, y) ((((x) ^ 7) << 1) | ((y) & 2) << 3 | ((y) & 1))
-
-/*!*****************************************************************************
- * @brief	Macro to determine the x index of an specified Pixel channel.
- * @param	n The channel number of the pixel.
- * @return	The x index number of the pixel.
- ******************************************************************************/
-#define PIXEL_N2X(n) ((((n) >> 1U) & 7) ^ 7)
-
-/*!*****************************************************************************
- * @brief	Macro to determine the y index of an specified Pixel channel.
- * @param	n The channel number of the pixel.
- * @return	The y index number of the pixel.
- ******************************************************************************/
-#define PIXEL_N2Y(n) (((n) & 1U) | (((n) >> 3) & 2U))
-
-/*!*****************************************************************************
- * @brief	Macro to determine if a ADC Pixel channel was enabled from a pixel mask.
- * @param	msk The 32-bit pixel mask
- * @param	ch The channel number of the pixel.
- * @return 	True if the pixel channel n was enabled, false elsewise.
- ******************************************************************************/
-#define PIXELN_ISENABLED(msk, ch) (((msk) >> (ch)) & 0x01U)
-
-/*!*****************************************************************************
- * @brief	Macro enables an ADC Pixel channel in a pixel mask.
- * @param	msk The 32-bit pixel mask
- * @param	ch The channel number of the pixel.
- ******************************************************************************/
-#define PIXELN_ENABLE(msk, ch) ((msk) |= (0x01U << (ch)))
-
-/*!*****************************************************************************
- * @brief	Macro disables an ADC Pixel channel in a pixel mask.
- * @param	msk The 32-bit pixel mask
- * @param	ch The channel number of the pixel.
- ******************************************************************************/
-#define PIXELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << (ch))))
-
-/*!*****************************************************************************
- * @brief	Macro to determine if an ADC Pixel channel was enabled from a pixel mask.
- * @param	msk 32-bit pixel mask
- * @param	x x index of the pixel.
- * @param	y y index of the pixel.
- * @return 	True if the pixel (x,y) was enabled, false elsewise.
- ******************************************************************************/
-#define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y)))
-
-/*!*****************************************************************************
- * @brief	Macro enables an ADC Pixel channel in a pixel mask.
- * @param	msk 32-bit pixel mask
- * @param	x x index of the pixel.
- * @param	y y index of the pixel.
- ******************************************************************************/
-#define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y)))
-
-/*!*****************************************************************************
- * @brief	Macro disables an ADC Pixel channel in a pixel mask.
- * @param	msk 32-bit pixel mask
- * @param	x x index of the pixel.
- * @param	y y index of the pixel.
- ******************************************************************************/
-#define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y)))
-
-/*!*****************************************************************************
- * @brief	Macro to determine if a ADC channel was enabled from a channel mask.
- * @param	msk 32-bit channel mask
- * @param	ch channel number of the ADC channel.
- * @return 	True if the ADC channel n was enabled, false elsewise.
- ******************************************************************************/
-#define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U)
-
-/*!*****************************************************************************
- * @brief	Macro to determine if a ADC channel was enabled from a channel mask.
- * @param	msk 32-bit channel mask
- * @param	ch channel number of the ADC channel.
- * @return 	True if the ADC channel n was enabled, false elsewise.
- ******************************************************************************/
-#define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U)))
-
-/*!*****************************************************************************
- * @brief	Macro to determine if a ADC channel was enabled from a channel mask.
- * @param	msk 32-bit channel mask
- * @param	ch channel number of the ADC channel.
- * @return 	True if the ADC channel n was enabled, false elsewise.
- ******************************************************************************/
-#define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U))))
-
-
-/*!*****************************************************************************
- * @brief	Macro to determine the number of enabled pixel channels via a popcount
- * 			algorithm.
- * @param	pxmsk 32-bit pixel mask
- * @return 	The count of enabled pixel channels.
- ******************************************************************************/
-#define PIXEL_COUNT(pxmsk) popcount(pxmsk)
-
-/*!*****************************************************************************
- * @brief	Macro to determine the number of enabled channels via a popcount
- * 			algorithm.
- * @param	pxmsk 32-bit pixel mask
- * @param	chmsk 32-bit channel mask
- * @return 	The count of enabled ADC channels.
- ******************************************************************************/
-#define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk))
-
-/*! @} */
-#endif /* ARGUS_MSK_H */
@@ -36,6 +36,9 @@

 #ifndef ARGUS_OFFSET_H
 #define ARGUS_OFFSET_H
+#ifdef __cplusplus
+extern "C" {
+#endif

 /*!***************************************************************************
 * @addtogroup  argus_cal
@@ -48,12 +51,26 @@
 * @brief   Pixel Range Offset Table.
 * @details Contains pixel range offset values for all 32 active pixels.
 *****************************************************************************/
-typedef struct argus_cal_offset_table_t {
-	/*! The offset values per pixel in meter and Q0.15 format. */
-	q0_15_t Table[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+typedef union argus_cal_offset_table_t {
+	struct {
+		/*! The offset values table for Low Power Stage of all 32 pixels.
+		 *  Unit: meter; Format: Q0.15 */
+		q0_15_t LowPower[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+
+		/*! The offset values table for High Power Stage of all 32 pixels.
+		 *  Unit: meter; Format: Q0.15 */
+		q0_15_t HighPower[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+	};
+
+	/*! The offset values table for Low/High Power Stages of all 32 pixels.
+	 *  Unit: meter; Format: Q0.15 */
+	q0_15_t Table[ARGUS_DCA_POWER_STAGE_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];

 } argus_cal_offset_table_t;


 /*! @} */
+#ifdef __cplusplus
+} // extern "C"
+#endif
 #endif /* ARGUS_OFFSET_T */
@@ -55,11 +55,11 @@ extern "C" {
 *              information from the filtered pixels by averaging them in a
 *              specified way.
 *
- *              The Pixel Binning Algorithm is a three-stage filter with a
- *              fallback value:
+ *              Basically, the Pixel Binning Algorithm is a multi-stage filter:
 *
 *              -# A fixed pre-filter mask is applied to statically disable
 *                  specified pixels.
+ *
 *              -# A relative and absolute amplitude filter is applied in the
 *                  second stage. The relative filter is determined by a ratio
 *                  of the maximum amplitude off all available (i.e. not filtered
@@ -75,12 +75,28 @@ extern "C" {
 *                  selected and considered for the final 1D distance. The
 *                  absolute threshold is used to dismiss pixels that are below
 *                  the noise level. The latter would be considered for the 1D
- *                  result if the maximum amplitude is already very low.
+ *                  result if the maximum amplitude is already very low.\n
+ *                  Those threshold are implemented using a hysteresis behavior.
+ *                  For its configuration, see the following parameters:
+ *                  - #argus_cfg_pba_t::RelativeAmplitudeInclusion
+ *                  - #argus_cfg_pba_t::RelativeAmplitudeExclusion
+ *                  - #argus_cfg_pba_t::AbsoluteAmplitudeInclusion
+ *                  - #argus_cfg_pba_t::AbsoluteAmplitudeExclusion
+ *                  .
+ *
 *              -# An absolute minimum distance filter is applied in addition
 *                  to the amplitude filter. This removes all pixel that have
 *                  a lower distance than the specified threshold. This is used
 *                  to remove invalid pixels that can be detected by a physically
- *                  not correct negative distance.
+ *                  not correct negative distance.\n
+ *                  For its configuration, see the following parameters:
+ *                  - #PBA_ENABLE_MIN_DIST_SCOPE
+ *                  - #argus_cfg_pba_t::AbsoluteDistanceScopeInclusion
+ *                  - #argus_cfg_pba_t::AbsoluteDistanceScopeExclusion
+ *                  - #argus_cfg_pba_t::RelativeDistanceScopeInclusion
+ *                  - #argus_cfg_pba_t::RelativeDistanceScopeExclusion
+ *                  .
+ *
 *              -# A distance filter is used to distinguish pixels that target
 *                  the actual object from pixels that see the brighter background,
 *                  e.g. white walls. Thus, the pixel with the minimum distance
@@ -90,11 +106,31 @@ extern "C" {
 *                  determined by an relative (to the current minimum distance)
 *                  and an absolute value. The larger scope value is the
 *                  relevant one, i.e. the relative distance scope can be used
- *                  to heed the increasing noise at larger distances.
+ *                  to heed the increasing noise at larger distances.\n
+ *                  For its configuration, see the following parameters:
+ *                  - #argus_cfg_pba_t::AbsoluteMinimumDistanceThreshold
+ *                  .
+ *
 *              -# If all of the above filters fail to determine a single valid
 *                  pixel, the Golden Pixel is used as a fallback value. The
 *                  Golden Pixel is the pixel that sits right at the focus point
- *                  of the optics at large distances.
+ *                  of the optics at large distances. Thus, it is expected to
+ *                  have the best signal at large distances.\n
+ *                  For its configuration, see the following parameters:
+ *                  - #PBA_ENABLE_GOLDPX_FALLBACK_MODE
+ *                  .
+ *
+ *              -# In order to avoid unwanted effects from "out-of-focus" pixels
+ *                  in application that require a smaller focus, the Golden Pixel
+ *                  Priority Mode prioritizes a valid signal on the central
+ *                  Golden Pixel over other pixels. That is, while the Golden
+ *                  Pixel has a reasonable signal strength, it is the only pixel
+ *                  considered for the 1D result.\n
+ *                  For its configuration, see the following parameters:
+ *                  - #PBA_ENABLE_GOLDPX_FALLBACK_MODE
+ *                  - #argus_cfg_pba_t::GoldenPixelPriorityAmplitudeInclusion
+ *                  - #argus_cfg_pba_t::GoldenPixelPriorityAmplitudeExclusion
+ *                  .
 *              .
 *
 *              After filtering is done, there may be more than a single pixel
@@ -113,14 +149,17 @@ extern "C" {
 * @brief   Enable flags for the pixel binning algorithm.
 *
 * @details Determines the pixel binning algorithm feature enable status.
+ *
 *          - [0]: #PBA_ENABLE: Enables the pixel binning feature.
 *          - [1]: reserved
 *          - [2]: reserved
 *          - [3]: reserved
- *          - [4]: reserved
- *          - [5]: #PBA_ENABLE_GOLDPX: Enables the Golden Pixel feature.
- *          - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope
- *                   feature.
+ *          - [4]: #PBA_ENABLE_GOLDPX_PRIORITY_MODE: Enables the Golden Pixel
+ *                                                   priority mode feature.
+ *          - [5]: #PBA_ENABLE_GOLDPX_FALLBACK_MODE: Enables the Golden Pixel
+ *                                                   fallback mode feature.
+ *          - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance
+ *                                             scope feature.
 *          - [7]: reserved
 *          .
 *****************************************************************************/
@@ -128,8 +167,17 @@ typedef enum argus_pba_flags_t {
 	/*! Enables the pixel binning feature. */
 	PBA_ENABLE = 1U << 0U,

-	/*! Enables the Golden Pixel. */
-	PBA_ENABLE_GOLDPX = 1U << 5U,
+	/*! Enables the Golden Pixel Priority Mode.
+	 *  If enabled, the Golden Pixel is prioritized over other Pixels as long
+	 *  as it has a good signal (determined by # */
+	PBA_ENABLE_GOLDPX_PRIORITY_MODE = 1U << 4U,
+
+	/*! Enables the Golden Pixel Fallback Mode.
+	 *  If enabled, the Golden Pixel is used as a last fallback pixel to obtain
+	 *  a valid signal from. This is recommended for all non-multi pixel
+	 *  devices whose TX field-of-view is aligned to target the Golden Pixel in
+	 *  factory calibration. */
+	PBA_ENABLE_GOLDPX_FALLBACK_MODE = 1U << 5U,

 	/*! Enables the minimum distance scope filter. */
 	PBA_ENABLE_MIN_DIST_SCOPE = 1U << 6U,
@@ -168,65 +216,297 @@ typedef struct {
 	 *  about the individual evaluation modes. */
 	argus_pba_averaging_mode_t AveragingMode;

-	/*! The Relative amplitude threshold value (in %) of the max. amplitude.
+	/*! The relative amplitude inclusion threshold (in %) of the max. amplitude.
 	 *
-	 *  Pixels with amplitude below this threshold value are dismissed.
+	 *  Pixels, whose amplitudes raise above this inclusion threshold, are
+	 *  added to the pixel binning. The amplitude must fall below the
+	 *  exclusion (#RelativeAmplitudeExclusion) threshold to be removed from
+	 *  the pixel binning again.
 	 *
 	 *  All available values from the 8-bit representation are valid.
 	 *  The actual percentage value is determined by 100%/256*x.
 	 *
-	 *  Use 0 to disable the relative amplitude threshold. */
-	uq0_8_t RelAmplThreshold;
-
-	/*! The relative minimum distance scope value in %.
+	 *  Note: in addition to the relative criteria, there is also the absolute
+	 *  criteria (#AbsoluteAmplitudeInclusion, #AbsoluteAmplitudeExclusion).
+	 *  The pixels are added to the pixel binning if their respective amplitude
+	 *  is larger than the absolute AND relative inclusion values. On the other
+	 *  hand, they are removed if their amplitude falls below the absolute OR
+	 *  relative exclusion threshold.
 	 *
-	 *  Pixels that have a range value within [x0, x0 + dx] are considered
-	 *  for the pixel binning, where x0 is the minimum distance of all
-	 *  amplitude picked pixels and dx is the minimum distance scope value.
-	 *  The minimum distance scope value will be the maximum of relative
-	 *  and absolute value.
+	 *  Must be greater than or equal to the #RelativeAmplitudeExclusion.
+	 *
+	 *  Use #RelativeAmplitudeExclusion == #RelativeAmplitudeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only.
+	 *
+	 *  Use 0 (for both, #RelativeAmplitudeExclusion and
+	 *  #RelativeAmplitudeInclusion) to disable the relative amplitude
+	 *  hysteresis. */
+	uq0_8_t RelativeAmplitudeInclusion;
+
+	/*! The relative amplitude exclusion threshold (in %) of the max. amplitude.
+	 *
+	 *  Pixels, whose amplitudes fall below this exclusion threshold, are
+	 *  removed from the pixel binning. The amplitude must raise above the
+	 *  inclusion (#RelativeAmplitudeInclusion) threshold to be added back
+	 *  to be pixel binning again.
 	 *
 	 *  All available values from the 8-bit representation are valid.
 	 *  The actual percentage value is determined by 100%/256*x.
 	 *
-	 *  Special values:
-	 *  - 0: Use 0 for absolute value only or to choose the pixel with the
-	 *       minimum distance only (of also the absolute value is 0)! */
-	uq0_8_t RelMinDistanceScope;
+	 *  Note: in addition to the relative criteria, there is also the absolute
+	 *  criteria (#AbsoluteAmplitudeInclusion, #AbsoluteAmplitudeExclusion).
+	 *  The pixels are added to the pixel binning if their respective amplitude
+	 *  is larger than the absolute AND relative inclusion values. On the other
+	 *  hand, they are removed if their amplitude falls below the absolute OR
+	 *  relative exclusion threshold.
+	 *
+	 *  Must be less than or equal to #RelativeAmplitudeInclusion.
+	 *
+	 *  Use #RelativeAmplitudeExclusion == #RelativeAmplitudeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only.
+	 *
+	 *  Use 0 (for both, #RelativeAmplitudeExclusion and
+	 *  #RelativeAmplitudeInclusion) to disable the relative amplitude
+	 *  hysteresis. */
+	uq0_8_t RelativeAmplitudeExclusion;

-	/*! The absolute amplitude threshold value in LSB.
+	/*! The absolute amplitude inclusion threshold in LSB.
 	 *
-	 *  Pixels with amplitude below this threshold value are dismissed.
+	 *  Pixels, whose amplitudes raise above this inclusion threshold, are
+	 *  added to the pixel binning. The amplitude must fall below the
+	 *  exclusion (#RelativeAmplitudeExclusion) threshold to be removed from
+	 *  the pixel binning again.
 	 *
-	 *  The absolute amplitude threshold is only valid if the Golden Pixel
-	 *  mode is enabled. Otherwise, the threshold is set to 0 LSB internally.
+	 *  The absolute amplitude hysteresis is only valid if the Golden Pixel
+	 *  mode is enabled. Otherwise, the thresholds are set to 0 LSB internally
+	 *  which disables the absolute criteria.
 	 *
 	 *  All available values from the 16-bit representation are valid.
 	 *  The actual LSB value is determined by x/16.
 	 *
-	 *  Use 0 to disable the absolute amplitude threshold. */
-	uq12_4_t AbsAmplThreshold;
-
-	/*! The absolute minimum distance scope value in m.
+	 *  Note: in addition to the absolute criteria, there is also the relative
+	 *  criteria (#RelativeAmplitudeInclusion, #RelativeAmplitudeExclusion).
+	 *  The pixels are added to the pixel binning if their respective amplitude
+	 *  is larger than the absolute AND relative inclusion values. On the other
+	 *  hand, they are removed if their amplitude falls below the absolute OR
+	 *  relative exclusion threshold.
 	 *
-	 *  Pixels that have a range value within [x0, x0 + dx] are considered
-	 *  for the pixel binning, where x0 is the minimum distance of all
-	 *  amplitude picked pixels and dx is the minimum distance scope value.
-	 *  The minimum distance scope value will be the maximum of relative
-	 *  and absolute value.
+	 *  Must be greater than or equal to #AbsoluteAmplitudeExclusion.
+	 *
+	 *  Use #AbsoluteAmplitudeExclusion == #AbsoluteAmplitudeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only.
+	 *
+	 *  Use 0 (for both, #AbsoluteAmplitudeExclusion and
+	 *  #AbsoluteAmplitudeInclusion) to disable the absolute amplitude
+	 *  hysteresis. */
+	uq12_4_t AbsoluteAmplitudeInclusion;
+
+	/*! The absolute amplitude exclusion threshold in LSB.
+	 *
+	 *  Pixels, whose amplitudes fall below this exclusion threshold, are
+	 *  removed from the pixel binning. The amplitude must raise above the
+	 *  inclusion (#RelativeAmplitudeInclusion) threshold to be added back
+	 *  to be pixel binning again.
+	 *
+	 *  The absolute amplitude hysteresis is only valid if the Golden Pixel
+	 *  mode is enabled. Otherwise, the thresholds are set to 0 LSB internally
+	 *  which disables the absolute criteria.
+	 *
+	 *  All available values from the 16-bit representation are valid.
+	 *  The actual LSB value is determined by x/16.
+	 *
+	 *  Note: in addition to the absolute criteria, there is also the relative
+	 *  criteria (#RelativeAmplitudeInclusion, #RelativeAmplitudeExclusion).
+	 *  The pixels are added to the pixel binning if their respective amplitude
+	 *  is larger than the absolute AND relative inclusion values. On the other
+	 *  hand, they are removed if their amplitude falls below the absolute OR
+	 *  relative exclusion threshold.
+	 *
+	 *  Must be less than or equal to #AbsoluteAmplitudeInclusion.
+	 *
+	 *  Use #AbsoluteAmplitudeExclusion == #AbsoluteAmplitudeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only.
+	 *
+	 *  Use 0 (for both, #AbsoluteAmplitudeExclusion and
+	 *  #AbsoluteAmplitudeInclusion) to disable the absolute amplitude
+	 *  hysteresis. */
+	uq12_4_t AbsoluteAmplitudeExclusion;
+
+	/*! The Golden Pixel Priority Mode inclusion threshold in LSB.
+	 *
+	 *  The Golden Pixel Priority Mode prioritizes a valid signal on the
+	 *  Golden Pixel over other pixel to avoid unwanted effects from
+	 *  "out-of-focus" pixels in application that require a smaller focus.
+	 *
+	 *  If the Golden Pixel priority mode is enabled (see
+	 *  #PBA_ENABLE_GOLDPX_PRIORITY_MODE) and the Golden Pixel has a valid signal
+	 *  with amplitude higher than this inclusion threshold, its priority state
+	 *  is enabled and the binning exits early by dismissing all other pixels
+	 *  regardless of their respective amplitude or state. The Golden Pixel
+	 *  priority state is disabled if the Golden Pixel amplitude falls below
+	 *  the exclusion threshold (#GoldenPixelPriorityAmplitudeExclusion) or its
+	 *  state becomes invalid (e.g. #PIXEL_SAT).
+	 *
+	 *  All available values from the 16-bit representation are valid.
+	 *  The actual LSB value is determined by x/16.
+	 *
+	 *  Use 0 to disable the Golden Pixel priority mode hysteresis. */
+	uq12_4_t GoldenPixelPriorityAmplitudeInclusion;
+
+	/*! The Golden Pixel Priority Mode exclusion threshold in LSB.
+	 *
+	 *  The Golden Pixel Priority Mode prioritizes a valid signal on the
+	 *  Golden Pixel over other pixel to avoid unwanted effects from
+	 *  "out-of-focus" pixels in application that require a smaller focus.
+	 *
+	 *  If the Golden Pixel priority mode is enabled (see
+	 *  #PBA_ENABLE_GOLDPX_PRIORITY_MODE) and the Golden Pixel has a valid
+	 *  signal with amplitude higher than the exclusion threshold
+	 *  (#GoldenPixelPriorityAmplitudeInclusion), its priority state is enabled
+	 *  and the binning exits early by dismissing all other pixels regardless
+	 *  of their respective amplitude or state. The Golden Pixel priority state
+	 *  is disabled if the Golden Pixel amplitude falls below this exclusion
+	 *  threshold or its state becomes invalid (e.g. #PIXEL_SAT).
+	 *
+	 *  All available values from the 16-bit representation are valid.
+	 *  The actual LSB value is determined by x/16.
+	 *
+	 *  Use 0 to disable the Golden Pixel priority mode hysteresis. */
+	uq12_4_t GoldenPixelPriorityAmplitudeExclusion;
+
+	/*! The relative minimum distance scope inclusion threshold (in %).
+	 *
+	 *  Pixels, whose range is smaller than the minimum distance inclusion
+	 *  threshold (x_min + dx_incl) are added to the pixel binning. The
+	 *  range must raise above the exclusion
+	 *  (#RelativeDistanceScopeExclusion) threshold to be removed
+	 *  from the pixel binning again. The relative value is determined
+	 *  by multiplying the percentage with the minimum distance.
+	 *
+	 *  The distance scope determines an interval within that pixels
+	 *  are considered valid, originating at the minimum distance (x_min).
+	 *  The width of the interval is specified by the relative and absolute
+	 *  minimum distance scope thresholds. The actual values it the
+	 *  maximum of both, the relative and absolute inclusion values
+	 *  (#AbsoluteDistanceScopeInclusion).
+	 *
+	 *  All available values from the 8-bit representation are valid.
+	 *  The actual percentage value is determined by 100%/256*x.
+	 *
+	 *  Must be smaller than or equal to the #RelativeDistanceScopeExclusion.
+	 *
+	 *  Use #RelativeDistanceScopeExclusion == #RelativeDistanceScopeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only. */
+	uq0_8_t RelativeDistanceScopeInclusion;
+
+	/*! The relative distance scope exclusion threshold (in %).
+	 *
+	 *  Pixels, whose range is larger than the minimum distance exclusion
+	 *  threshold (x_min + dx_excl) are removed from the pixel binning. The
+	 *  range must fall below the inclusion
+	 *  (#RelativeDistanceScopeInclusion) threshold to be added
+	 *  to the pixel binning again. The relative value is determined
+	 *  by multiplying the percentage with the minimum distance.
+	 *
+	 *  The distance scope determines an interval within that pixels
+	 *  are considered valid, originating at the minimum distance (x_min).
+	 *  The width of the interval is specified by the relative and absolute
+	 *  minimum distance scope thresholds. The actual values it the
+	 *  maximum of both, the relative and absolute exclusion values
+	 *  (#AbsoluteDistanceScopeExclusion).
+	 *
+	 *  All available values from the 8-bit representation are valid.
+	 *  The actual percentage value is determined by 100%/256*x.
+	 *
+	 *  Must be larger than or equal to the #RelativeDistanceScopeInclusion.
+	 *
+	 *  Use #RelativeDistanceScopeExclusion == #RelativeDistanceScopeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only. */
+	uq0_8_t RelativeDistanceScopeExclusion;
+
+	/*! The absolute minimum distance scope inclusion threshold (in m).
+	 *
+	 *  Pixels, whose range is smaller than the minimum distance inclusion
+	 *  threshold (x_min + dx_incl) are added to the pixel binning. The
+	 *  range must raise above the exclusion
+	 *  (#AbsoluteDistanceScopeExclusion) threshold to be added
+	 *  to the pixel binning again.
+	 *
+	 *  The distance scope determines an interval within that pixels
+	 *  are considered valid, originating at the minimum distance (x_min).
+	 *  The width of the interval is specified by the relative and absolute
+	 *  minimum distance scope thresholds. The actual values it the
+	 *  maximum of both, the relative and absolute exclusion values
+	 *  (#RelativeDistanceScopeInclusion).
 	 *
 	 *  All available values from the 16-bit representation are valid.
 	 *  The actual LSB value is determined by x/2^15.
 	 *
-	 *  Special values:
-	 *  - 0: Use 0 for relative value only or to choose the pixel with the
-	 *       minimum distance only (of also the relative value is 0)! */
-	uq1_15_t AbsMinDistanceScope;
+	 *  Must be smaller than or equal to the #AbsoluteDistanceScopeExclusion.
+	 *
+	 *  Use #AbsoluteDistanceScopeExclusion == #AbsoluteDistanceScopeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only. */
+	uq1_15_t AbsoluteDistanceScopeInclusion;
+
+	/*! The absolute minimum distance scope exclusion threshold (in m).
+	 *
+	 *  Pixels, whose range is larger than the minimum distance exclusion
+	 *  threshold (x_min + dx_excl) are removed from the pixel binning. The
+	 *  range must fall below the inclusion
+	 *  (#AbsoluteDistanceScopeInclusion) threshold to be added
+	 *  to the pixel binning again.
+	 *
+	 *  The distance scope determines an interval within that pixels
+	 *  are considered valid, originating at the minimum distance (x_min).
+	 *  The width of the interval is specified by the relative and absolute
+	 *  minimum distance scope thresholds. The actual values it the
+	 *  maximum of both, the relative and absolute exclusion values
+	 *  (#RelativeDistanceScopeExclusion).
+	 *
+	 *  All available values from the 16-bit representation are valid.
+	 *  The actual LSB value is determined by x/2^15.
+	 *
+	 *  Must be larger than or equal to the #AbsoluteDistanceScopeInclusion.
+	 *
+	 *  Use #AbsoluteDistanceScopeExclusion == #AbsoluteDistanceScopeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only. */
+	uq1_15_t AbsoluteDistanceScopeExclusion;
+
+	/*! The Golden Pixel Saturation Filter Pixel Threshold.
+	 *
+	 *  The Golden Pixel Saturation Filter will evaluate the status of the
+	 *  Golden Pixel to #PIXEL_INVALID if a certain number of active pixels,
+	 *  i.e. pixels that are not removed by the static pre-filter mask
+	 *  (#PrefilterMask), are saturated (#PIXEL_SAT).
+	 *
+	 *  The purpose of this filter is to avoid erroneous situations with highly
+	 *  reflective targets (e.g. retro-reflectors) that can invalidate the
+	 *  Golden Pixel such that it would not show the correct saturation state.
+	 *  In order to avoid using the Golden Pixel in that scenario, this filter
+	 *  mechanism can be used to remove the Golden Pixel if a specified number
+	 *  of other pixels show saturation state.
+	 *
+	 *  Use 0 to disable the Golden Pixel Saturation Filter. */
+	uint8_t GoldenPixelSaturationFilterPixelThreshold;
+
+	/*! The Golden Pixel out-of-sync age limit for the GPPM.
+	 *
+	 *  The Golden Pixel out-of-sync age is the number of consecutive frames
+	 *  where the Golden Pixel is out-of-sync. This parameters is the threshold
+	 *  to distinguish between temporary and permanent out-of-sync states.
+	 *
+	 *  Temporary out-of-sync states happen when the target rapidly changes. In
+	 *  this case, the Golden Pixel Priority Mode (GPPM) is not exited. Only if
+	 *  the out-of-sync age exceeds the specified threshold, the Golden Pixel is
+	 *  considered erroneous and the GPPM is exited.
+	 *
+	 *  Use 0 to disable the Golden Pixel out-of-sync aging (= infinity). */
+	uint8_t GoldenPixelOutOfSyncAgeThreshold;

 	/*! The absolute minimum distance threshold value in m.
 	 *
 	 *  Pixels with distance below this threshold value are dismissed. */
-	q9_22_t AbsMinDistanceThreshold;
+	q9_22_t AbsoluteMinimumDistanceThreshold;

 	/*! The pre-filter pixel mask determines the pixel channels that are
 	 *  statically excluded from the pixel binning (i.e. 1D distance) result.
@@ -55,6 +55,9 @@ extern "C" {
 * Also used as a special value to determine no object detected or infinity range. */
 #define ARGUS_RANGE_MAX (Q9_22_MAX)

+/*! Minimum range value in Q9.22 format. */
+#define ARGUS_RANGE_MIN (Q9_22_MIN)
+
 /*!***************************************************************************
 * @brief   Status flags for the evaluated pixel structure.
 *
@@ -227,12 +227,19 @@ enum Status {

 	/*! -114: AFBR-S50 Error: Register data integrity is lost (e.g. due to unexpected
 	 *  power-on-reset cycle or invalid write cycle of SPI. System tries to
-	 *  reset the values. */
+	 *  reset the values.
+	 *
+	 *  @note If this error occurs after intentionally cycling the power supply
+	 *  of the device, use the #Argus_RestoreDeviceState API function to properly
+	 *  recover the current API state into the device to avoid that issue. */
 	ERROR_ARGUS_DATA_INTEGRITY_LOST = -114,

 	/*! -115: AFBR-S50 Error: The range offsets calibration failed! */
 	ERROR_ARGUS_RANGE_OFFSET_CALIBRATION_FAILED = -115,

+	/*! -116: AFBR-S50 Error: The VSUB calibration failed! */
+	ERROR_ARGUS_VSUB_CALIBRATION_FAILED = -116,
+
 	/*! -191: AFBR-S50 Error: The device is currently busy and cannot execute the
 	 *  requested command. */
 	ERROR_ARGUS_BUSY = -191,
@@ -56,13 +56,13 @@ extern "C" {
 #define ARGUS_API_VERSION_MAJOR    1

 /*! Minor version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_MINOR    4
+#define ARGUS_API_VERSION_MINOR    5

 /*! Bugfix version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_BUGFIX   4
+#define ARGUS_API_VERSION_BUGFIX   6

 /*! Build version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_BUILD    "20230327150535"
+#define ARGUS_API_VERSION_BUILD    "20240208081753"

 /*****************************************************************************/

@@ -72,30 +72,28 @@ typedef struct xtalk_t {
 * @details Contains crosstalk vector values for all 32 active pixels,
 *          separated for A/B-Frames.
 *****************************************************************************/
-typedef struct argus_cal_xtalk_table_t {
-	union {
-		struct {
-			/*! The crosstalk vector table for A-Frames. */
-			xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+typedef union argus_cal_xtalk_table_t {
+	struct {
+		/*! The crosstalk vector table for A-Frames. */
+		xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];

-			/*! The crosstalk vector table for B-Frames. */
-			xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
-		};
-
-		/*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/
-		xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+		/*! The crosstalk vector table for B-Frames. */
+		xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
 	};

+	/*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/
+	xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+
 } argus_cal_xtalk_table_t;


 /*!***************************************************************************
- * @brief   Pixel-To-Pixel Crosstalk Compensation Parameters.
- * @details Contains calibration data that belongs to the pixel-to-pixel
- *          crosstalk compensation feature.
+ * @brief   Electrical Pixel-To-Pixel Crosstalk Compensation Parameters.
+ * @details Contains calibration data that belongs to the electrical
+ *          pixel-to-pixel crosstalk compensation feature.
 *****************************************************************************/
-typedef struct argus_cal_p2pxtalk_t {
-	/*! Pixel-To-Pixel Compensation on/off. */
+typedef struct argus_cal_electrical_p2pxtalk_t {
+	/*! Electrical Pixel-To-Pixel Compensation on/off. */
 	bool Enabled;

 	/*! The relative threshold determines when the compensation is active for
@@ -134,8 +132,39 @@ typedef struct argus_cal_p2pxtalk_t {
 	 *  Higher values determine more influence on the reference pixel signal. */
 	q3_12_t KcFactorCRefPx;

-} argus_cal_p2pxtalk_t;
+} argus_cal_electrical_p2pxtalk_t;

+/*!***************************************************************************
+ * @brief   Optical Pixel-To-Pixel Crosstalk Compensation Parameters.
+ * @details Contains calibration data that belongs to the optical
+ *          pixel-to-pixel crosstalk compensation feature.
+ *****************************************************************************/
+typedef struct argus_cal_optical_p2pxtalk_t {
+	/*! Optical Pixel-To-Pixel Compensation on/off. */
+	bool Enabled;
+
+	/*! The sine component of the coupling coefficient that determines the amount
+	 *  of a neighbour pixel signal that influences the raw signal of certain pixel.
+	 *  Higher values determine more influence on the individual pixel signal. */
+	q3_12_t CouplingCoeffS;
+
+	/*! The cosine component of the coupling coefficient that determines the amount
+	 *  of a neighbour pixel signal that influences the raw signal of a certain pixel.
+	 *  Higher values determine more influence on the individual pixel signal. */
+	q3_12_t CouplingCoeffC;
+
+} argus_cal_optical_p2pxtalk_t;
+
+/*!***************************************************************************
+ * @brief   Pixel-To-Pixel Crosstalk Compensation Parameters.
+ * @details Contains combined calibration data for electrical and
+ *          optical pixel-to-pixel crosstalk compensation feature.
+ *****************************************************************************/
+typedef struct argus_cal_p2pxtalk_t {
+	argus_cal_electrical_p2pxtalk_t Electrical;
+
+	argus_cal_optical_p2pxtalk_t Optical;
+} argus_cal_p2pxtalk_t;

 /*! @} */
 #ifdef __cplusplus
@@ -61,7 +61,7 @@ extern "C" {
 * @details Algorithm to evaluate a/b, where b is in Q15.16 format, on a 32-bit
 *          architecture with maximum precision.
 *          The result is correctly rounded and given as the input format.
- *          Division by 0 yields max. values determined by signa of numerator.
+ *          Division by 0 yields max. values determined by signs of numerator.
 *          Too high/low results are truncated to max/min values.
 *
 *          Depending on the architecture, the division is implemented with a 64-bit
@@ -89,14 +89,14 @@ inline int32_t fp_div16(int32_t a, q15_16_t b)

 		if (c > 0x80000000U) { return INT32_MIN; }

-		return -c;
+		return (int32_t) - c;

 	} else {
 		c = ((c / b) + (1 << 13U)) >> 14U;

 		if (c > (int64_t)INT32_MAX) { return INT32_MAX; }

-		return c;
+		return (int32_t)c;
 	}

 #else
@@ -159,10 +159,16 @@ inline int32_t fp_div16(int32_t a, q15_16_t b)

 	/* Figure out the sign of result */
 	if ((uint32_t)(a ^ b) & 0x80000000U) {
-		result = -result;
+		return (int32_t) - result;
+
+	} else {
+		// fix 05.10.2023; the corner case, when result == INT32_MAX + 1:
+		// Catch the wraparound (to INT32_MIN) and truncate instead.
+		if (quotient > INT32_MAX) { return INT32_MAX; }
+
+		return (int32_t)result;
 	}

-	return (int32_t)result;
 #endif
 }

@@ -118,7 +118,7 @@ inline uint32_t fp_mulu(uint32_t u, uint32_t v, uint_fast8_t shift)
 	assert(shift <= 32);
 #if USE_64BIT_MUL
 	const uint64_t w = (uint64_t)u * (uint64_t)v;
-	return (w >> shift) + ((w >> (shift - 1)) & 1U);
+	return (uint32_t)((w >> shift) + ((w >> (shift - 1)) & 1U));
 #else
 	uint32_t tmp[2] = { 0 };
 	muldwu(tmp, u, v);
@@ -158,15 +158,15 @@ inline int32_t fp_muls(int32_t u, int32_t v, uint_fast8_t shift)

 	uint32_t u2, v2;

-	if (u < 0) { u2 = -u; sign = -sign; } else { u2 = u; }
+	if (u < 0) { u2 = (uint32_t) - u; sign = -sign; } else { u2 = (uint32_t)u; }

-	if (v < 0) { v2 = -v; sign = -sign; } else { v2 = v; }
+	if (v < 0) { v2 = (uint32_t) - v; sign = -sign; } else { v2 = (uint32_t)v; }

 	const uint32_t res = fp_mulu(u2, v2, shift);

 	assert(sign > 0 ? res <= 0x7FFFFFFFU : res <= 0x80000000U);

-	return sign > 0 ? res : -res;
+	return sign > 0 ? (int32_t)res : -(int32_t)res;
 }


@@ -225,7 +225,9 @@ inline uint32_t fp_mul_u32_u16(uint32_t u, uint16_t v, uint_fast8_t shift)
 *****************************************************************************/
 inline int32_t fp_mul_s32_u16(int32_t u, uint16_t v, uint_fast8_t shift)
 {
-	return u >= 0 ? fp_mul_u32_u16(u, v, shift) : - fp_mul_u32_u16(-u, v, shift);
+	return u >= 0 ?
+	       (int32_t)fp_mul_u32_u16((uint32_t)u, v, shift) :
+	       -(int32_t)fp_mul_u32_u16((uint32_t) - u, v, shift);
 }

 /*! @} */
@@ -80,7 +80,7 @@ inline uint32_t fp_rndu(uint32_t Q, uint_fast8_t n)
 *****************************************************************************/
 inline int32_t fp_rnds(int32_t Q, uint_fast8_t n)
 {
-	return (Q < 0) ? -fp_rndu(-Q, n) : fp_rndu(Q, n);
+	return (Q < 0) ? -(int32_t)fp_rndu((uint32_t)(-Q), n) : (int32_t)fp_rndu((uint32_t)Q, n);
 }

 /*!***************************************************************************
@@ -108,7 +108,7 @@ inline uint32_t fp_truncu(uint32_t Q, uint_fast8_t n)
 *****************************************************************************/
 inline int32_t fp_truncs(int32_t Q, uint_fast8_t n)
 {
-	return (Q < 0) ? -fp_truncu(-Q, n) : fp_truncu(Q, n);
+	return (Q < 0) ? -(int32_t)fp_truncu((uint32_t)(-Q), n) : (int32_t)fp_truncu((uint32_t)Q, n);
 }

 /*! @} */
@@ -66,7 +66,7 @@ inline uint32_t log2i(uint32_t x)
 {
 	assert(x != 0);
 #if 1
-	return 31 - __builtin_clz(x);
+	return (uint32_t)(31 - __builtin_clz(x));
 #else
 #define S(k) if (x >= (1 << k)) { i += k; x >>= k; }
 	int i = 0; S(16); S(8); S(4); S(2); S(1); return i;
@@ -64,13 +64,10 @@
 /** Get the priority for the topic */
 #define ORBIOCGPRIORITY		_ORBIOC(14)

-/** Set the queue size of the topic */
-#define ORBIOCSETQUEUESIZE	_ORBIOC(15)
-
 /** Get the minimum interval at which the topic can be seen to be updated for this subscription */
-#define ORBIOCGETINTERVAL	_ORBIOC(16)
+#define ORBIOCGETINTERVAL	_ORBIOC(15)

 /** Check whether the topic is advertised, sets *(unsigned long *)arg to 1 if advertised, 0 otherwise */
-#define ORBIOCISADVERTISED	_ORBIOC(17)
+#define ORBIOCISADVERTISED	_ORBIOC(16)

 #endif /* _DRV_UORB_H */
@@ -45,8 +45,6 @@ using namespace time_literals;
 ToneAlarm::ToneAlarm() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
-	// ensure ORB_ID(tune_control) is advertised with correct queue depth
-	orb_advertise_queue(ORB_ID(tune_control), nullptr, tune_control_s::ORB_QUEUE_LENGTH);
 }

 ToneAlarm::~ToneAlarm()
@@ -151,7 +151,7 @@ private:

 	transponder_report_s tr{};

-	orb_advert_t fake_traffic_report_publisher = orb_advertise_queue(ORB_ID(transponder_report), &tr, (unsigned int)20);
+	orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr);

 	TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT};

@@ -61,7 +61,7 @@ void send(EventType &event)
 		orb_publish(ORB_ID(event), orb_event_pub, &event);

 	} else {
-		orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH);
+		orb_event_pub = orb_advertise(ORB_ID(event), &event);
 	}

 	pthread_mutex_unlock(&publish_event_mutex);
@@ -73,6 +73,6 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
 		orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg);

 	} else {
-		*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH);
+		*mavlink_log_pub = orb_advertise(ORB_ID(mavlink_log), &log_msg);
 	}
 }
@@ -55,7 +55,7 @@ public:
 	void SetUp() override
 	{
 		// ensure topic exists, otherwise we might lose first queued events
-		orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH);
+		orb_advertise(ORB_ID(event), nullptr);
 	}

 };
@@ -153,7 +153,7 @@ int buzzer_init()
 	tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW] = 800000;
 	tune_durations[tune_control_s::TUNE_ID_SINGLE_BEEP] = 300000;

-	tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH);
+	tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control);

 	return PX4_OK;
 }
@@ -330,7 +330,7 @@ int led_init()
 	led_control.mode = led_control_s::MODE_OFF;
 	led_control.priority = 0;
 	led_control.timestamp = hrt_absolute_time();
-	led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
+	led_control_pub = orb_advertise(ORB_ID(led_control), &led_control);

 	/* first open normal LEDs */
 	fd_leds = px4_open(LED0_DEVICE_PATH, O_RDWR);
@@ -52,7 +52,7 @@ parameters:
        max: 100
        increment: 0.01
        decimal: 2
-        default: 0.3
+        default: 50.0
      RDD_P_HEADING:
        description:
          short: Proportional gain for heading controller
@@ -2201,7 +2201,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 }

 void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
-		const position_setpoint_s &pos_sp_prev,
 		const position_setpoint_s &pos_sp_curr)
 {
 	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
@@ -2213,22 +2212,14 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 	_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);

-
-	if (_position_setpoint_previous_valid) {
-		Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon);
-		navigateLine(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
-
-	} else {
-
-		// if we don't have a previous waypoint for line following, then create one using the current position at the
-		// start of the transition
-		if (!_lpos_where_backtrans_started.isAllFinite()) {
-			_lpos_where_backtrans_started = curr_pos_local;
-		}
-
-		navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+	// Set the position where the backtransition started the first ime we pass through here.
+	// Will get reset if not in transition anymore.
+	if (!_lpos_where_backtrans_started.isAllFinite()) {
+		_lpos_where_backtrans_started = curr_pos_local;
 	}

+	navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+
 	_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

@@ -2452,6 +2443,7 @@ FixedwingPositionControl::Run()

 		if (_vehicle_status_sub.update(&_vehicle_status)) {
 			if (!_vehicle_status.in_transition_mode) {
+				// reset position of backtransition start if not in transition
 				_lpos_where_backtrans_started = Vector2f(NAN, NAN);
 			}
 		}
@@ -2551,8 +2543,7 @@ FixedwingPositionControl::Run()
 			}

 		case FW_POSCTRL_MODE_TRANSITON: {
-				control_backtransition(control_interval, ground_speed, _pos_sp_triplet.previous,
-						       _pos_sp_triplet.current);
+				control_backtransition(control_interval, ground_speed, _pos_sp_triplet.current);
 				break;
 			}
 		}
@@ -710,11 +710,9 @@ private:
 	 *
 	 * @param control_interval Time since last position control call [s]
 	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
-	 * @param pos_sp_prev previous position setpoint
 	 * @param pos_sp_curr current position setpoint
 	 */
 	void control_backtransition(const float control_interval, const Vector2f &ground_speed,
-				    const position_setpoint_s &pos_sp_prev,
 				    const position_setpoint_s &pos_sp_curr);

 	float get_tecs_pitch();
@@ -124,13 +124,13 @@ Mavlink::Mavlink() :

 	// ensure topic exists, otherwise we might lose first queued commands
 	if (orb_exists(ORB_ID(vehicle_command), 0) == PX4_ERROR) {
-		orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH);
+		orb_advertise(ORB_ID(vehicle_command), nullptr);
 	}

 	_vehicle_command_sub.subscribe();

 	if (orb_exists(ORB_ID(event), 0) == PX4_ERROR) {
-		orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH);
+		orb_advertise(ORB_ID(event), nullptr);
 	}

 	_event_sub.subscribe();
@@ -46,13 +46,6 @@ FeasibilityChecker::FeasibilityChecker() :

 void FeasibilityChecker::reset()
 {
-
-	_is_landed = false;
-	_home_alt_msl = NAN;
-	_home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
-	_current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
-	_vehicle_type = VehicleType::RotaryWing;
-
 	_mission_validity_failed = false;
 	_takeoff_failed = false;
 	_land_pattern_validity_failed = false;
@@ -86,10 +79,16 @@ void FeasibilityChecker::updateData()

 		if (home.valid_hpos) {
 			_home_lat_lon = matrix::Vector2d(home.lat, home.lon);
+
+		} else {
+			_home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 		}

 		if (home.valid_alt) {
 			_home_alt_msl = home.alt;
+
+		} else {
+			_home_alt_msl = NAN;
 		}
 	}

@@ -65,6 +65,18 @@ public:
 		orb_publish(ORB_ID(home_position), home_pub, &home);
 	}

+	void publishInvalidHome()
+	{
+		home_position_s home = {};
+		home.alt = 0.f;
+		home.valid_alt = false;
+		home.lat = 0.;
+		home.lon = 0.;
+		home.valid_hpos = false;
+		orb_advert_t home_pub = orb_advertise(ORB_ID(home_position), &home);
+		orb_publish(ORB_ID(home_position), home_pub, &home);
+	}
+
 	void publishCurrentPosition(double lat, double lon)
 	{
 		vehicle_global_position_s gpos = {};
@@ -122,6 +134,7 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity)
 	ASSERT_EQ(ret, false);

 	checker.reset();
+	checker.publishInvalidHome();
 	mission_item.nav_cmd = NAV_CMD_TAKEOFF;
 	mission_item.altitude_is_relative = true;
 	ret = checker.processNextItem(mission_item, 0, 5);
@@ -190,6 +203,7 @@ TEST_F(FeasibilityCheckerTest, check_below_home)

 	// this is done to invalidate the home position
 	checker.reset();
+	checker.publishInvalidHome();
 	checker.publishLanded(true);
 	checker.processNextItem(mission_item, 0, 1);

@@ -81,6 +81,8 @@ Geofence::Geofence(Navigator *navigator) :
 	if (_navigator != nullptr) {
 		updateFence();
 	}
+
+	_geofence_status_pub.advertise();
 }

 Geofence::~Geofence()
@@ -101,6 +103,14 @@ void Geofence::run()
 		if (_initiate_fence_updated) {
 			_initiate_fence_updated = false;
 			_dataman_state	= DatamanState::Read;
+
+			geofence_status_s status;
+			status.timestamp = hrt_absolute_time();
+			status.geofence_id = _opaque_id;
+			status.status = geofence_status_s::GF_STATUS_LOADING;
+
+			_geofence_status_pub.publish(status);
+
 		}

 		break;
@@ -147,6 +157,14 @@ void Geofence::run()

 			} else {
 				_dataman_state = DatamanState::UpdateRequestWait;
+				_fence_updated = true;
+
+				geofence_status_s status;
+				status.timestamp = hrt_absolute_time();
+				status.geofence_id = _opaque_id;
+				status.status = geofence_status_s::GF_STATUS_READY;
+
+				_geofence_status_pub.publish(status);
 			}
 		}

@@ -160,6 +178,13 @@ void Geofence::run()
 			_dataman_state = DatamanState::UpdateRequestWait;
 			_updateFence();
 			_fence_updated = true;
+
+			geofence_status_s status;
+			status.timestamp = hrt_absolute_time();
+			status.geofence_id = _opaque_id;
+			status.status = geofence_status_s::GF_STATUS_READY;
+
+			_geofence_status_pub.publish(status);
 		}

 		break;
@@ -49,6 +49,7 @@
 #include <lib/geo/geo.h>
 #include <px4_platform_common/defines.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/geofence_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/sensor_gps.h>
@@ -171,7 +172,7 @@ private:
 	mission_stats_entry_s _stats;
 	DatamanState _dataman_state{DatamanState::UpdateRequestWait};
 	DatamanState _error_state{DatamanState::UpdateRequestWait};
-	DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4};
+	DatamanCache _dataman_cache{"geofence_dm_cache_miss", 0};
 	DatamanClient	&_dataman_client = _dataman_cache.client();

 	float _altitude_min{0.0f};
@@ -185,6 +186,8 @@ private:
 	bool _fence_updated{true};  ///< flag indicating if fence are updated to dataman cache
 	bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed

+	uORB::Publication<geofence_status_s> _geofence_status_pub{ORB_ID(geofence_status)};
+
 	/**
 	 * implementation of updateFence()
 	 */
@@ -52,38 +52,22 @@ MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed
 	_dataman_cache_size_signed(dataman_cache_size_signed)
 {
 	_dataman_cache.resize(abs(dataman_cache_size_signed));
-	_is_current_planned_mission_item_valid = (initMission() == PX4_OK);

-	updateDatamanCache();
+	// Reset _mission here, and listen on changes on the uorb topic instead of initialize from dataman.
+	_mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
+	_mission.fence_dataman_id = DM_KEY_FENCE_POINTS_0;
+	_mission.safepoint_dataman_id = DM_KEY_SAFE_POINTS_0;
+	_mission.count = 0;
+	_mission.current_seq = 0;
+	_mission.land_start_index = -1;
+	_mission.land_index = -1;
+	_mission.mission_id = 0;
+	_mission.geofence_id = 0;
+	_mission.safe_points_id = 0;

 	_mission_pub.advertise();
 }

-int MissionBase::initMission()
-{
-	mission_s mission;
-	int ret_val{PX4_ERROR};
-
-	bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission),
-						sizeof(mission_s));
-
-	if (success) {
-		if (isMissionValid(mission)) {
-			_mission = mission;
-			ret_val = PX4_OK;
-
-		} else {
-			resetMission();
-		}
-
-	} else {
-		PX4_ERR("Could not initialize Mission: Dataman read failed");
-		resetMission();
-	}
-
-	return ret_val;
-}
-
 void
 MissionBase::updateDatamanCache()
 {
@@ -118,31 +102,35 @@ void MissionBase::updateMavlinkMission()
 						  static_cast<int32_t>(new_mission.count) - 1);
 		}

-		_mission = new_mission;
+		if (new_mission.geofence_id != _mission.geofence_id) {
+			// New geofence data, need to check mission again.
+			_mission_checked = false;
+		}

-		_is_current_planned_mission_item_valid = isMissionValid(_mission);
+		_mission = new_mission;

 		/* Relevant mission items updated externally*/
 		if (mission_data_changed) {

 			onMissionUpdate(mission_items_changed);
 		}
+
+		_is_current_planned_mission_item_valid = isMissionValid();
 	}
 }

 void MissionBase::onMissionUpdate(bool has_mission_items_changed)
 {
-	_is_current_planned_mission_item_valid = _mission.count > 0;
-
 	if (has_mission_items_changed) {
 		_dataman_cache.invalidate();
 		_load_mission_index = -1;

-		check_mission_valid();
+		if (canRunMissionFeasibility()) {
+			_mission_checked = true;
+			check_mission_valid();

-		// only warn if the check failed on merit
-		if ((!_navigator->get_mission_result()->valid) && _mission.count > 0U) {
-			PX4_WARN("mission check failed");
+		} else {
+			_mission_checked = false;
 		}
 	}

@@ -168,15 +156,17 @@ MissionBase::on_inactive()
 	_land_detected_sub.update();
 	_vehicle_status_sub.update();
 	_global_pos_sub.update();
+	_geofence_status_sub.update();

 	parameters_update();

 	updateMavlinkMission();

-	/* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */
-	if (_navigator->home_global_position_valid() && !_initialized_mission_checked) {
+	/* Check the mission */
+	if (!_mission_checked && canRunMissionFeasibility()) {
+		_mission_checked = true;
 		check_mission_valid();
-		_initialized_mission_checked = true;
+		_is_current_planned_mission_item_valid = isMissionValid();
 	}

 	if (_vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
@@ -217,7 +207,7 @@ MissionBase::on_activation()
 	_mission_has_been_activated = true;
 	_system_disarmed_while_inactive = false;

-	check_mission_valid();
+	check_mission_valid(true);

 	update_mission();

@@ -259,12 +249,22 @@ MissionBase::on_active()
 	_land_detected_sub.update();
 	_vehicle_status_sub.update();
 	_global_pos_sub.update();
+	_geofence_status_sub.update();

 	parameters_update();

 	updateMavlinkMission();
 	updateDatamanCache();

+	/* Check the mission */
+	if (!_mission_checked && canRunMissionFeasibility()) {
+		_mission_checked = true;
+		check_mission_valid();
+		_is_current_planned_mission_item_valid = isMissionValid();
+		update_mission();
+		set_mission_items();
+	}
+
 	// check if heading alignment is necessary, and add it to the current mission item if necessary
 	if (_align_heading_necessary && is_mission_item_reached_or_completed()) {

@@ -678,7 +678,7 @@ MissionBase::checkMissionRestart()
 	    && ((_mission.current_seq + 1) == _mission.count)) {
 		setMissionIndex(0);
 		_inactivation_index = -1; // reset
-		_is_current_planned_mission_item_valid = isMissionValid(_mission);
+		_is_current_planned_mission_item_valid = isMissionValid();
 		resetMissionJumpCounter();
 		_navigator->reset_cruising_speed();
 		_navigator->reset_vroi();
@@ -687,11 +687,13 @@ MissionBase::checkMissionRestart()
 }

 void
-MissionBase::check_mission_valid()
+MissionBase::check_mission_valid(bool forced)
 {
-	if ((_navigator->get_mission_result()->mission_id != _mission.mission_id)
-	    || (_navigator->get_mission_result()->geofence_id != _mission.geofence_id)
-	    || (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) {
+	// Allow forcing it, since we currently not rechecking if parameters have changed.
+	if (forced ||
+	    (_navigator->get_mission_result()->mission_id != _mission.mission_id) ||
+	    (_navigator->get_mission_result()->geofence_id != _mission.geofence_id) ||
+	    (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) {

 		_navigator->get_mission_result()->mission_id = _mission.mission_id;
 		_navigator->get_mission_result()->geofence_id = _mission.geofence_id;
@@ -704,6 +706,12 @@ MissionBase::check_mission_valid()
 		_navigator->get_mission_result()->failure = false;

 		set_mission_result();
+
+		// only warn if the check failed on merit
+		if ((!_navigator->get_mission_result()->valid) && _mission.count > 0U) {
+			PX4_WARN("mission check failed");
+		}
+
 	}
 }

@@ -876,16 +884,16 @@ void MissionBase::publish_navigator_mission_item()
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }

-bool MissionBase::isMissionValid(mission_s &mission) const
+bool MissionBase::isMissionValid() const
 {
 	bool ret_val{false};

-	if (((mission.current_seq < mission.count) || (mission.count == 0U && mission.current_seq <= 0)) &&
-	    (mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0
-	     || mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) &&
-	    (mission.timestamp != 0u)) {
+	if (((_mission.current_seq < _mission.count) || (_mission.count == 0U && _mission.current_seq <= 0)) &&
+	    (_mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ||
+	     _mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) &&
+	    (_mission.timestamp != 0u) &&
+	    (_navigator->get_mission_result()->valid)) {
 		ret_val = true;
-
 	}

 	return ret_val;
@@ -1141,7 +1149,7 @@ int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, floa
 void MissionBase::resetMission()
 {
 	/* we do not need to reset mission if is already.*/
-	if (_mission.count == 0u && isMissionValid(_mission)) {
+	if (_mission.count == 0u) {
 		return;
 	}

@@ -1360,3 +1368,12 @@ bool MissionBase::checkMissionDataChanged(mission_s new_mission)
 		(new_mission.mission_id != _mission.mission_id) ||
 		(new_mission.current_seq != _mission.current_seq));
 }
+
+bool MissionBase::canRunMissionFeasibility()
+{
+	return _navigator->home_global_position_valid() && // Need to have a home position checked
+	       _navigator->get_global_position()->timestamp > 0 && // Need to have a position, for first waypoint check
+	       (_geofence_status_sub.get().timestamp > 0) && // Geofence data must be loaded
+	       (_geofence_status_sub.get().geofence_id == _mission.geofence_id) &&
+	       (_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY);
+}
@@ -43,6 +43,7 @@
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
 #include <dataman_client/DatamanClient.hpp>
+#include <uORB/topics/geofence_status.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/navigator_mission_item.h>
 #include <uORB/topics/parameter_update.h>
@@ -207,13 +208,12 @@ protected:
 	int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps,
 			   bool mission_direction_backward = false);
 	/**
-	 * @brief Is Mission Parameters Valid
+	 * @brief Is Mission Valid
 	 *
-	 * @param mission Mission struct
-	 * @return true is mission parameters are valid
+	 * @return true is mission is valid
 	 * @return false otherwise
 	 */
-	bool isMissionValid(mission_s &mission) const;
+	bool isMissionValid() const;

 	/**
 	 * On mission update
@@ -309,7 +309,7 @@ protected:

 	bool _is_current_planned_mission_item_valid{false};	/**< Flag indicating if the currently loaded mission item is valid*/
 	bool _mission_has_been_activated{false};		/**< Flag indicating if the mission has been activated*/
-	bool _initialized_mission_checked{false};		/**< Flag indicating if the initialized mission has been checked by the mission validator*/
+	bool _mission_checked{false};				/**< Flag indicating if the mission has been checked by the mission validator*/
 	bool _system_disarmed_while_inactive{false};		/**< Flag indicating if the system has been disarmed while mission is inactive*/
 	mission_s _mission;					/**< Currently active mission*/
 	float _mission_init_climb_altitude_amsl{NAN}; 		/**< altitude AMSL the vehicle will climb to when mission starts */
@@ -342,9 +342,10 @@ private:
 	void updateMavlinkMission();

 	/**
-	 * Check whether a mission is ready to go
+	 * @brief Check whether a mission is ready to go
+	 * @param[in] forced flag if the check has to be run irregardles of any updates.
 	 */
-	void check_mission_valid();
+	void check_mission_valid(bool forced = false);

 	/**
 	 * Reset mission
@@ -444,6 +445,8 @@ private:
 	 */
 	bool checkMissionDataChanged(mission_s new_mission);

+	bool canRunMissionFeasibility();
+
 	int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/
 	int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/

@@ -461,4 +464,5 @@ private:
 	)

 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::SubscriptionData<geofence_status_s> _geofence_status_sub{ORB_ID(geofence_status)};
 };
@@ -168,7 +168,7 @@ bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id
 {
@[    for idx, sub in enumerate(subscriptions + subscriptions_multi)]@
 	{
-			uint16_t queue_depth = uORB::DefaultQueueSize<@(sub['simple_base_type'])_s>::value * 2; // use a bit larger queue size than internal
+			uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['simple_base_type']))) * 2; // use a bit larger queue size than internal
 			create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])", queue_depth);
 	}
@[    end for]@
@@ -39,8 +39,7 @@ VehicleCommandSrv::VehicleCommandSrv(uxrSession *session, uxrStreamId reliable_o
 				     uxrStreamId input_stream_id, uxrObjectId participant_id, const char *client_namespace, const uint8_t index) :
 	SrvBase(session, reliable_out_stream_id, input_stream_id, participant_id)
 {
-	uint16_t queue_depth = uORB::DefaultQueueSize<vehicle_command_s>::value *
-			       2; // use a bit larger queue size than internal
+	uint16_t queue_depth = orb_get_queue_size(ORB_ID(vehicle_command)) * 2; // use a bit larger queue size than internal
 	create_replier(input_stream_id, participant_id, index, client_namespace, "vehicle_command", "VehicleCommand",
 		       queue_depth);
 };
@@ -55,7 +55,7 @@ public:
 		_cdr_ops(ops)
 	{
 		int instance = 0;
-		_uorb_pub_handle = orb_advertise_multi_queue(_uorb_meta, nullptr, &instance, 1); //FIXME template magic qsize
+		_uorb_pub_handle = orb_advertise_multi(_uorb_meta, nullptr, &instance);
 	};

 	~uORB_Zenoh_Subscriber() override = default;
Author	SHA1	Message	Date
PerFrivik	461224dd4d	Updated default parameter value	2024-03-11 16:24:06 +01:00
alexklimaj	a80a5a92f4	boards: ARK Flow fix typo	2024-03-09 16:40:59 -05:00
alexklimaj	b81ad8841e	drivers: broadcom AFBR update to API 1.5.6	2024-03-09 16:40:59 -05:00
Eric Katzfey	57df7e35b2	uORB: make queue size (ORB_QUEUE_LENGTH) completely static (#22815 ) Previously uORB queue size was an awkward mix of runtime configurable (at advertise or IOCTL before allocate), but effectively static with all queue size settings (outside of test code) actually coming from the topic declaration (presently ORB_QUEUE_LENGTH in the .msg). This change finally resolves the inconsistency making the queue size fully static. Additionally there were some corner cases that the muorb and orb communicator implementation were not correctly handling. This PR provides fixes for those issues. Also correctly sets remote queue lengths now based on the topic definitions. * Made setting of uORB topic queue size in based on topic definition only * Fixes to the ModalAI muorb implementation * Removed libfc sensor from format checks * msg/TransponderReport.msg ORB_QUEUE_LENGTH 8->16 (was set to higher in AdsbConflict.h --------- Co-authored-by: Eric Katzfey <eric.katzfey@modalai.com> Co-authored-by: Daniel Agar <daniel@agar.ca>	2024-03-08 16:28:24 -05:00
Alexis Guijarro	006dcfafb7	boards/mro/ctrl-zero-classic: corrections for mRo Control Zero Classic Board (#22745 ) - Build target changed from STM32H743II to STM32H743ZI - Missing external SPI interface added - Nonexistent I2C3 interface removed - I2C4 pins changed - Red and Green LED lights remapped - Missing ADC inputs added and already present ones corrected - CAN Silent interfaces corrected - Power pins corrected and Level Shifter pin added to enable ICM20948 - Buzzer pin remapped - HRT channel and PPM pin changed - RSSI input remapped - ICM20602 and BMI088 pins corrected - Serial ports remapped	2024-03-08 14:50:53 -05:00
Silvan Fuhrer	85a882e1ce	FW Position Control: control_backtransition(): always track line from start (#22853 ) Remove option to track from previous wp to reduce complexity and fix case where prev=current point and the line following broke down. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-03-08 19:11:08 +01:00
Konrad	04099ed483	mission_base: Force mission validity check on activation	2024-03-08 17:26:04 +01:00
Konrad	1aa26a5a91	missionFeasibilityChecker: Fix tests	2024-03-08 17:26:04 +01:00
Konrad	acd750e033	mission_base: Run feasibility checker only after first global position has been published	2024-03-08 17:26:04 +01:00
Konrad	6c6142ba79	MissionFeasibiltyChecker: Do not delete uorb data on reset.	2024-03-08 17:26:04 +01:00
Konrad	7fb584adbe	MissionResult uorb: fix wrong int types	2024-03-08 17:26:04 +01:00
Konrad	fb3aab1fb0	mission_base: check mission feasibility again, if geofence has changed.	2024-03-08 17:26:04 +01:00
Konrad	1b03ac4d2b	mission_base: Only run mission feasibility if the geofence module is ready	2024-03-08 17:26:04 +01:00
Konrad	815cea2abb	geofence: publish status of loaded geofence	2024-03-08 17:26:04 +01:00
Konrad	51321c605e	mission_base: clean up mission check evaluation	2024-03-08 17:26:04 +01:00
Konrad	a0ae073d8c	mission_base: Do not initialize mission from dataman. only listen on mission topic	2024-03-08 17:26:04 +01:00