Multicopter attitude setpoint flag absolute yaw control

This allows to disable absolute yaw control while still using the attitude quaternion.
2026-06-20 05:30:35 +08:00 · 2020-10-21 08:37:19 +02:00
128 changed files with 1238 additions and 1436 deletions
@@ -68,7 +68,7 @@ fi
 # Lightware i2c lidar sensor
 if param greater -s SENS_EN_SF1XX 0
 then
-	lightware_laser_i2c start -X
+	sf1xx start -X
 fi

 # Heater driver for temperature regulated IMUs.
@@ -526,11 +526,6 @@ else
 		bst start -X
 	fi

-	if param compare IMU_GYRO_FFT_EN 1
-	then
-		gyro_fft start
-	fi
-
 	#
 	# Optional board supplied extras: rc.board_extras
 	#
@@ -32,7 +32,7 @@ if ets_airspeed start -X
 then
 fi

-if lightware_laser_i2c start
+if sf1xx start
 then
 fi

@@ -113,7 +113,7 @@ def main():

    if args.port == None:
        if sys.platform == "darwin":
-            args.port = "/dev/tty.usbmodem01"
+            args.port = "/dev/tty.usbmodem1"
        else:
            serial_list = mavutil.auto_detect_serial(preferred_list=['*FTDI*',
                "*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*", '*PX4*', '*FMU*', "*Gumstix*"])
@@ -139,7 +139,7 @@

 /* SPI selections ***********************************************************/

-#define PIN_LPSPI0_SCK   PIN_LPSPI0_SCK_3   /* PTB2 */
+#define PIN_LPSPI0_SCK   PIN_LPSPI0_SCK_2   /* PTB2 */
 #define PIN_LPSPI0_MISO  PIN_LPSPI0_SIN_2   /* PTB3 */
 #define PIN_LPSPI0_MOSI  PIN_LPSPI0_SOUT_3  /* PTB4 */
 #define PIN_LPSPI0_PCS   PIN_LPSPI0_PCS0_2  /* PTB5 */
@@ -28,7 +28,7 @@ px4_add_board(
 		differential_pressure/ms4525
 		#distance_sensor # all available distance sensor drivers
 		#distance_sensor/ll40ls
-		#distance_sensor/lightware_laser_serial
+		#distance_sensor/sf0x
 		#dshot
 		gps
 		#heater
@@ -26,7 +26,7 @@ px4_add_board(
 		differential_pressure # all available differential pressure drivers
 		#distance_sensor # all available distance sensor drivers
 		distance_sensor/ll40ls
-		distance_sensor/lightware_laser_serial
+		distance_sensor/sf0x
 		gps
 		imu/l3gd20
 		imu/lsm303d
@@ -27,7 +27,7 @@ px4_add_board(
 		differential_pressure/ms4525
 		#distance_sensor # all available distance sensor drivers
 		distance_sensor/ll40ls
-		distance_sensor/lightware_laser_serial
+		distance_sensor/sf0x
 		#dshot
 		gps
 		#heater
@@ -29,7 +29,7 @@ px4_add_board(
 		#imu/adis16497
 		#imu/bmi088
 		imu/mpu6000
-		imu/invensense/icm20602
+		#imu/invensense/icm20602
 		#imu/mpu9250
 		#irlock
 		lights/blinkm
@@ -5,10 +5,9 @@
 board_adc start

 # Internal SPI bus ICM-20602
-#mpu6000 -s -b 2 -R 11 -T 20602 start # SPI 2
+mpu6000 -s -b 2 -R 11 -T 20602 start # SPI 2
 #mpu6000 -s -b 3 -R 10 -T 20602 start # SPI 3
-icm20602 -s -b 2 -R 5 start # SPI 2
-icm20602 -s -b 3 -R 4 start # SPI 3
+#icm20602 -s -b 2 -R 5 start # SPI 2

 # Internal I2C bus
 bmp388 -I start
@@ -56,12 +56,12 @@
 /* Clocking *************************************************************************/
 /* The sp racing h7 extreme  board provides the following clock sources:
 *
- *   X1: 8 MHz crystal for HSE
+ *   X1: 25 MHz crystal for HSE
 *
 * So we have these clock source available within the STM32
 *
 *   HSI: 64 MHz RC factory-trimmed
- *   HSE:  8 MHz crystal for HSE
+ *   HSE: 25 MHz crystal for HSE
 */

 #define STM32_BOARD_XTAL        8000000ul
@@ -98,11 +98,11 @@

 /* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR
 *
- *   PLL1_VCO = (8,000,000 / 2) * 200 = 800 MHz
+ *   PLL1_VCO = (25,000,000 / 5) * 160 = 800 MHz
 *
 *   PLL1P = PLL1_VCO/2  = 800 MHz / 2   = 400 MHz
- *   PLL1Q = PLL1_VCO/4  = 800 MHz / 4  =  200 MHz
- *   PLL1R = PLL1_VCO/2  = 800 MHz / 2   = 400 MHz - locked for H750
+ *   PLL1Q = PLL1_VCO/4  = 800 MHz / 4   = 200 MHz
+ *   PLL1R = PLL1_VCO/8  = 800 MHz / 8   = 100 MHz
 */

 #define STM32_PLLCFG_PLL1CFG    (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \
@@ -33,9 +33,83 @@

 #pragma once

-#define DMAMAP_SPI2_RX    DMAMAP_DMA12_SPI2RX_0      //(SPI Gyro 1)
-#define DMAMAP_SPI2_TX    DMAMAP_DMA12_SPI2TX_0      //(SPI Gyro 1)
-#define DMAMAP_SPI3_RX    DMAMAP_DMA12_SPI3RX_0      //(SPI Gyro 2)
-#define DMAMAP_SPI3_TX    DMAMAP_DMA12_SPI3TX_0      //(SPI Gyro 2)
-#define DMAMAP_SPI4_RX    DMAMAP_DMA12_SPI4RX_0      //(SPI OSD)
-#define DMAMAP_SPI4_TX    DMAMAP_DMA12_SPI4TX_0      //(SPI OSD)
+/*
+|    DMA1    | Stream 0         | Stream 1         | Stream 2         | Stream 3         | Stream 4         | Stream 5         | Stream 6         | Stream 7         |
+|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|
+| Channel 0  | SPI3_RX_1        | SPDIFRX_DT       | SPI3_RX_2        | SPI2_RX          | SPI2_TX          | SPI3_TX_1        | SPDIFRX_CS       | SPI3_TX_2        |
+| Channel 1  | I2C1_RX          | I2C3_RX          | TIM7_UP_1        | -                | TIM7_UP_2        |  I2C1_RX_1       | I2C1_TX          | I2C1_TX_1        |
+| Channel 2  | TIM4_CH1         | -                | I2C4_RX          | TIM4_CH2         | -                | I2C4_RX          | TIM4_UP          | TIM4_CH3         |
+| Channel 3  | -                | TIM2_UP_1        | I2C3_RX_1        | -                | I2C3_TX          | TIM2_CH1         | TIM2_CH2         | TIM2_UP_2        |
+|            |                  |   TIM2_CH3       |                  |                  |                  |                  |   TIM2_CH4_1     |   TIM2_CH4_2     |
+| Channel 4  | UART5_RX         | USART3_RX        | UART4_RX         | USART3_TX_1      | UART4_TX         | USART2_RX        | USART2_TX        | UART5_TX         |
+| Channel 5  | UART8_TX         | UART7_TX         | TIM3_CH4         | UART7_RX         | TIM3_CH1         | TIM3_CH2         | UART8_RX         | TIM3_CH3         |
+|            |                  |                  |   TIM3_UP        |                  |   TIM3_TRIG      |                  |                  |                  |
+| Channel 6  | TIM5_CH3         | TIM5_CH4_1       | TIM5_CH1         | TIM5_CH4_2       | TIM5_CH2         | -                | TIM5_UP_2        | -                |
+|            |   TIM5_UP_1      |   TIM5_TRIG_1    |                  |   TIM5_TRIG_2    |                  |                  |                  |                  |
+| Channel 7  | -                | TIM6_UP          | I2C2_RX          | I2C2_RX_1        | USART3_TX_2      | DAC1             | DAC2             | I2C2_TX          |
+| Channel 8  | I2C3_TX          | I2C4_RX          | -                | -                | I2C2_TX          | -                | I2C4_TX          | -                |
+| Channel 9  | -                | SPI2_RX          | -                | -                | -                | -                | SPI2_TX          | -                |
+|            |                  |                  |                  |                  |                  |                  |                  |                  |
+| Usage      |                  |                  | TIM3_UP          |                  |                  | USART2_RX        | TIM5_UP_2        |                  |
+
+
+|    DMA2    | Stream 0         | Stream 1         | Stream 2         | Stream 3         | Stream 4         | Stream 5         | Stream 6         | Stream 7         |
+|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|
+| Channel 0  | ADC1_1           | SAI1_A           | TIM8_CH1_1       | SAI1_A_1         | ADC1_2           | SAI1_B_1         | TIM1_CH1_1       | SAI2_B_2         |
+|            |                  |                  |   TIM8_CH2_1     |                  |                  |                  |   TIM1_CH2_1     |                  |
+|            |                  |                  |   TIM8_CH3_1     |                  |                  |                  |   TIM1_CH3_1     |                  |
+| Channel 1  | -                | DCMI_1           | ADC2_1           | ADC2_2           | SAI1_B           | SPI6_TX          | SPI6_RX          | DCMI_2           |
+| Channel 2  | ADC3_1           | ADC3_2           | -                | SPI5_RX_1        | SPI5_TX_1        | CRYP_OUT         | CRYP_IN          | HASH_IN          |
+| Channel 3  | SPI1_RX_1        | -                | SPI1_RX_2        | SPI1_TX_1        | SAI2_A           | SPI1_TX_2        | SAI2_B           | QUADSPI          |
+| Channel 4  | SPI4_RX_1        | SPI4_TX_1        | USART1_RX_1      | SDMMC1_1         | -                | USART1_RX_2      | SDMMC1_2         | USART1_TX        |
+| Channel 5  | -                | USART6_RX_1      | USART6_RX_2      | SPI4_RX_2        | SPI4_TX_2        | -                | USART6_TX_1      | USART6_TX_2      |
+| Channel 6  | TIM1_TRIG_1      | TIM1_CH1_2       | TIM1_CH2_2       | TIM1_CH1         | TIM1_CH4         | TIM1_UP          | TIM1_CH3_2       | -                |
+|            |                  |                  |                  |                  |   TIM1_TRIG_2    |                  |                  |                  |
+|            |                  |                  |                  |                  |   TIM1_COM       |                  |                  |                  |
+| Channel 7  | -                | TIM8_UP          | TIM8_CH1_2       | TIM8_CH2_2       | TIM8_CH3_2       | SPI5_RX_2        | SPI5_TX_2        | TIM8_CH4         |
+|            |                  |                  |                  |                  |                  |                  |                  |   TIM8_TRIG      |
+|            |                  |                  |                  |                  |                  |                  |                  |   TIM8_COM       |
+| Channel 8  | DSFDM1_FLT0      | DSFDM1_FLT1      | DSFDM1_FLT2      | DSFDM1_FLT3      | DSFDM1_FLT0      | DSFDM1_FLT1      | DSFDM1_FLT2      | DSFDM1_FLT3      |
+| Channel 9  | JPEG_IN          | JPEG_OUT         | SPI4_TX          | JPEG_IN          | JPEG_OUT         | SPI5_RX          | -                | -                |
+| Channel 10 | SAI1_B           | SAI2_B           | SAI2_A           | -                | -                | -                | SAI1_A           | -                |
+| Channel 11 | SDMMC2           | -                | QUADSPI          | -                | -                | SDMMC2           | -                | -                |
+|            |                  |                  |                  |                  |                  |                  |                  |                  |
+| Usage      | SPI4_RX_1        | TIM8_UP          |                  |                  | SPI4_TX_2        | TIM1_UP          |                  |                  |
+ */
+
+#define STM32_DMA_MAP(d,s,c)       ((d) << 6 | (s) << 3 | (c))
+
+#define DMA_CHAN0                 (0)
+#define DMA_CHAN4                 (4)
+#define DMA_CHAN5                 (5)
+
+#define DMAMAP_SPI2_RX             STM32_DMA_MAP(DMA1,DMA_STREAM3,DMA_CHAN0)
+#define DMAMAP_SPI2_TX             STM32_DMA_MAP(DMA1,DMA_STREAM4,DMA_CHAN0)
+#define DMAMAP_SPI3_RX_2           STM32_DMA_MAP(DMA1,DMA_STREAM2,DMA_CHAN0)
+#define DMAMAP_SPI3_TX_1           STM32_DMA_MAP(DMA1,DMA_STREAM5,DMA_CHAN0)
+#define DMAMAP_SPI4_RX_1           STM32_DMA_MAP(DMA2,DMA_STREAM0,DMA_CHAN4)
+#define DMAMAP_SPI4_TX_2           STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN5)
+
+// DMA1 Channel/Stream Selections
+//--------------------------------------------//---------------------------//----------------
+//      DMAMAP_TIM5_UP_1                      // DMA1, Stream 0, Channel 6    (DSHOT)
+//      AVAILABLE                             // DMA1, Stream 1
+#define DMAMAP_SPI3_RX DMAMAP_SPI3_RX_2       // DMA1, Stream 2, Channel 0    (SPI sensors ICM20602 2)
+//      DMAMAP_SPI2_RX                        // DMA1, Stream 3, Channel 0    (SPI sensors ICM20602 1)
+//      DMAMAP_SPI2_TX                        // DMA1, Stream 4, Channel 0    (SPI sensors ICM20602 1)
+#define DMAMAP_SPI3_TX DMAMAP_SPI3_TX_1       // DMA1, Stream 5, Channel 0    (SPI sensors ICM20602 2)
+//      DMAMAP_TIM4_UP                        // DMA1, Stream 6, Channel 2    (DSHOT)
+//      AVAILABLE                             // DMA1, Stream 7
+
+
+//  DMA2 Channel/Stream Selections
+//--------------------------------------------//---------------------------//----------------
+#define DMAMAP_SPI4_RX DMAMAP_SPI4_RX_1       // DMA2, Stream 0, Channel 4    (SPI OSD)
+//      DMAMAP_TIM8_UP                        // DMA2, Stream 1, Channel 7    (DSHOT)
+//      AVAILABLE                             // DMA1, Stream 2
+//      AVAILABLE                             // DMA1, Stream 3
+#define DMAMAP_SPI4_TX DMAMAP_SPI4_TX_2       // DMA2, Stream 4, Channel 5    (SPI OSD)
+//      AVAILABLE                             // DMA2, Stream 5
+//      AVAILABLE                             // DMA1, Stream 6
+//      AVAILABLE                             // DMA1, Stream 7
+
@@ -56,7 +56,6 @@ set(msg_files
 	esc_report.msg
 	esc_status.msg
 	estimator_innovations.msg
-	estimator_optical_flow_vel.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg
 	estimator_status.msg
@@ -120,6 +119,7 @@ set(msg_files
 	sensor_preflight_mag.msg
 	sensor_selection.msg
 	sensors_status_imu.msg
+	subsystem_info.msg
 	system_power.msg
 	task_stack_info.msg
 	tecs_status.msg
@@ -1,8 +0,0 @@
-uint64 timestamp			# time since system start (microseconds)
-uint64 timestamp_sample			# the timestamp of the raw data (microseconds)
-
-float32[2] vel_body			# velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
-float32[2] vel_ne			# same as vel_body but in local frame (m/s)
-float32[2] flow_uncompensated_integral	# integrated optical flow measurement (rad)
-float32[2] flow_compensated_integral	# integrated optical flow measurement compensated for angular motion (rad)
-float32[3] gyro_rate_integral		# gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
@@ -7,6 +7,11 @@ float32 dt                # delta time between samples (microseconds)

 float32 resolution_hz

-float32[2] peak_frequency_x # x axis peak frequencies
-float32[2] peak_frequency_y # y axis peak frequencies
-float32[2] peak_frequency_z # z axis peak frequencies
+uint8[3] peak_index_0
+uint8[3] peak_index_1
+
+float32[3] peak_frequency_0 # largest frequency peak found per sensor axis (0 if none)
+float32[3] peak_frequency_1 # second largest frequency peak found per sensor axis (0 if none)
+
+uint8[3] peak_index_quinns
+float32[3] peak_frequency_quinns
@@ -0,0 +1,39 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint64 SUBSYSTEM_TYPE_GYRO = 1
+uint64 SUBSYSTEM_TYPE_ACC = 2
+uint64 SUBSYSTEM_TYPE_MAG = 4
+uint64 SUBSYSTEM_TYPE_ABSPRESSURE = 8
+uint64 SUBSYSTEM_TYPE_DIFFPRESSURE = 16
+uint64 SUBSYSTEM_TYPE_GPS = 32
+uint64 SUBSYSTEM_TYPE_OPTICALFLOW = 64
+uint64 SUBSYSTEM_TYPE_CVPOSITION = 128
+uint64 SUBSYSTEM_TYPE_LASERPOSITION = 256
+uint64 SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512
+uint64 SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024
+uint64 SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048
+uint64 SUBSYSTEM_TYPE_YAWPOSITION = 4096
+uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 8192
+uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 16384
+uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 32768
+uint64 SUBSYSTEM_TYPE_RCRECEIVER = 65536
+uint64 SUBSYSTEM_TYPE_GYRO2 = 131072
+uint64 SUBSYSTEM_TYPE_ACC2 = 262144
+uint64 SUBSYSTEM_TYPE_MAG2 = 524288
+uint64 SUBSYSTEM_TYPE_GEOFENCE = 1048576
+uint64 SUBSYSTEM_TYPE_AHRS = 2097152
+uint64 SUBSYSTEM_TYPE_TERRAIN = 4194304
+uint64 SUBSYSTEM_TYPE_REVERSEMOTOR = 8388608
+uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
+uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
+uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
+uint64 SUBSYSTEM_TYPE_SATCOM = 134217728
+uint64 SUBSYSTEM_TYPE_PREARM_CHECK = 268435456
+uint64 SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 536870912
+
+bool present
+bool enabled
+bool ok
+uint64 subsystem_type
+
+uint8 ORB_QUEUE_LENGTH = 8
@@ -70,10 +70,6 @@ topic_name = spec.short_name

 #include <uORB/uORB.h>

-#ifdef __cplusplus
-#include <uORB/topics/uORBTopics.hpp>
-#endif
-
@##############################
@# Includes for dependencies
@##############################
@@ -139,11 +135,5 @@ ORB_DECLARE(@multi_topic);
@[end for]

 #ifdef __cplusplus
-@[for multi_topic in topics]@
-template<> struct ORBTypeMap<ORB_ID::@multi_topic> {
-    using type = @(uorb_struct);
-};
-@[end for]
-
 void print_message(const @uorb_struct& message);
 #endif
@@ -73,6 +73,4 @@ enum class ORB_ID : uint8_t {
 	INVALID
 };

-template<ORB_ID> struct ORBTypeMap;
-
 const struct orb_metadata *get_orb_meta(ORB_ID id);
@@ -150,6 +150,8 @@ rtps:
    id: 70
  - msg: px4io_status
    id: 71
+  - msg: subsystem_info
+    id: 72
  - msg: system_power
    id: 73
  - msg: task_stack_info
@@ -285,8 +287,6 @@ rtps:
    id: 136
  - msg: navigator_mission_item
    id: 137
-  - msg: estimator_optical_flow_vel
-    id: 138
  ########## multi topics: begin ##########
  - msg: actuator_controls_0
    id: 150
@@ -4,7 +4,8 @@ float32 roll_body		# body angle in NED frame (can be NaN for FW)
 float32 pitch_body		# body angle in NED frame (can be NaN for FW)
 float32 yaw_body		# body angle in NED frame (can be NaN for FW)

-float32 yaw_sp_move_rate	# rad/s (commanded by user)
+float32 yaw_sp_move_rate	# feed-forward NED yaw angular rate in rad/s
+bool absolute_heading_valid # True if absolute heading of attitude quaternion should be tracked

 # For quaternion-based attitude control
 float32[4] q_d			# Desired quaternion for quaternion control
@@ -33,45 +33,19 @@
 #
 ############################################################################

-# Set flags for the NuttX build system for Cygwin
-# They are already used by Config.mk
-ifneq (, $(findstring CYGWIN, $(shell uname)))
-  CONFIG_HOST_CYGWIN := y
-  CONFIG_WINDOWS_CYGWIN := y
-  CONFIG_CYGWIN_WINTOOL := y
-endif
-
 include $(TOPDIR)/.config
 include $(TOPDIR)/tools/Config.mk

-# Replace each space separated word in the string that contains "cygdrive"
-# with the windows path and escaped backslashes
-# e.g. [hello -I /cygdrive/c -I/cygdrive/c] -> [hello -I C:\\ ]
-define cygwin_to_windows_paths
-$(subst \,\\,$(foreach word,$(1),$(shell \
-if [[ "$(word)" == *"cygdrive"* ]]; then \
-  cygpath -w "$(word)"; \
-else \
-  echo $(word); \
-fi \
-)))
-endef
-
 CINCPATH := $(shell $(INCDIR) -s "$(CC)" $(TOPDIR)$(DELIM)include)
 CXXINCPATH := $(shell $(INCDIR) -s "$(CC)" $(TOPDIR)$(DELIM)include$(DELIM)cxx)

 ARCHINCLUDES += $(CINCPATH)
 ARCHXXINCLUDES += $(CINCPATH) $(CXXINCPATH)
-ARCHSCRIPT = -T$(BOARD_DIR)$(DELIM)scripts$(DELIM)flash.ld

-ifeq ($(CONFIG_BOARD_USE_PROBES),y)
-   ARCHINCLUDES += -I $(TOPDIR)/arch/$(CONFIG_ARCH)/src/$(CONFIG_ARCH_CHIP) -I $(TOPDIR)/arch/$(CONFIG_ARCH)/src/common
-   ARCHXXINCLUDES += -I $(TOPDIR)/arch/$(CONFIG_ARCH)/src/$(CONFIG_ARCH_CHIP) -I $(TOPDIR)/arch/$(CONFIG_ARCH)/src/common
-endif
-
-ifneq (, $(findstring CYGWIN, $(shell uname)))
-  ARCHINCLUDES := $(call cygwin_to_windows_paths,$(ARCHINCLUDES))
-  ARCHXXINCLUDES := $(call cygwin_to_windows_paths,$(ARCHXXINCLUDES))
+ifeq ($(CONFIG_CYGWIN_WINTOOL),y)
+  ARCHSCRIPT = -T "$(shell cygpath -w $(BOARD_DIR)$(DELIM)scripts$(DELIM)flash.ld)"
+else
+  ARCHSCRIPT = -T$(BOARD_DIR)$(DELIM)scripts$(DELIM)flash.ld
 endif

 CC = ${CMAKE_C_COMPILER}
@@ -87,6 +61,11 @@ OBJDUMP = ${CMAKE_OBJDUMP}
 ARCHCCVERSION = $(shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q')
 ARCHCCMAJOR = $(shell echo $(ARCHCCVERSION) | cut -d'.' -f1)

+ifeq ($(CONFIG_BOARD_USE_PROBES),y)
+   ARCHINCLUDES += -I$(TOPDIR)/arch/$(CONFIG_ARCH)/src/$(CONFIG_ARCH_CHIP) -I$(TOPDIR)/arch/$(CONFIG_ARCH)/src/common
+   ARCHXXINCLUDES += -I$(TOPDIR)/arch/$(CONFIG_ARCH)/src/$(CONFIG_ARCH_CHIP)  -I$(TOPDIR)/arch/$(CONFIG_ARCH)/src/common
+endif
+
 ifeq ($(CONFIG_DEBUG_CUSTOMOPT),y)
   MAXOPTIMIZATION = $(CONFIG_DEBUG_OPTLEVEL)
 else
@@ -33,7 +33,7 @@ set(tests
 	rc
 	search_min
 	servo
-	#lightware_laser
+	#sf0x
 	sleep
 	uorb
 	versioning
@@ -101,7 +101,7 @@ public:
 private:

 	// Publishers
-	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<camera_trigger_s>		_trigger_pub{ORB_ID(camera_trigger)};

 	// Subscribers
@@ -179,7 +179,7 @@ private:

 	orb_advert_t		_trigger_pub;

-	uORB::Publication<vehicle_command_ack_s>	_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_ack_s>	_cmd_ack_pub{ORB_ID(vehicle_command_ack)};

 	param_t			_p_mode;
 	param_t			_p_activation_time;
@@ -504,7 +504,7 @@ CameraTrigger::test()
 	vcmd.param5 = 1.0;
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;

-	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
 	vcmd_pub.publish(vcmd);
 }

@@ -38,8 +38,8 @@ add_subdirectory(ll40ls_pwm)
 add_subdirectory(mappydot)
 add_subdirectory(mb12xx)
 add_subdirectory(pga460)
-add_subdirectory(lightware_laser_i2c)
-add_subdirectory(lightware_laser_serial)
+add_subdirectory(sf0x)
+add_subdirectory(sf1xx)
 add_subdirectory(srf02)
 add_subdirectory(teraranger)
 add_subdirectory(tfmini)
@@ -1,486 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file lightware_laser_i2c.cpp
- *
- * @author ecmnet <ecm@gmx.de>
- * @author Vasily Evseenko <svpcom@gmail.com>
- *
- * Driver for the Lightware lidar range finder series.
- * Default I2C address 0x66 is used.
- */
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-#include <px4_platform_common/module.h>
-#include <drivers/device/i2c.h>
-#include <lib/parameters/param.h>
-#include <lib/perf/perf_counter.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/rangefinder/PX4Rangefinder.hpp>
-
-using namespace time_literals;
-
-/* Configuration Constants */
-#define LIGHTWARE_LASER_BASEADDR		0x66
-
-class LightwareLaser : public device::I2C, public I2CSPIDriver<LightwareLaser>
-{
-public:
-	LightwareLaser(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
-		       int address = LIGHTWARE_LASER_BASEADDR);
-
-	~LightwareLaser() override;
-
-	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-					     int runtime_instance);
-	static void print_usage();
-
-	int init() override;
-
-	void print_status() override;
-
-	void RunImpl();
-
-private:
-	// I2C (legacy) binary protocol command
-	static constexpr uint8_t I2C_LEGACY_CMD_READ_ALTITUDE = 0;
-
-	enum class Register : uint8_t {
-		// see http://support.lightware.co.za/sf20/#/commands
-		ProductName = 0,
-		DistanceOutput = 27,
-		DistanceData = 44,
-		MeasurementMode = 93,
-		ZeroOffset = 94,
-		LostSignalCounter = 95,
-		Protocol = 120,
-		ServoConnected = 121,
-	};
-
-	static constexpr uint16_t output_data_config = 0b11010110100;
-	struct OutputData {
-		int16_t first_return_median;
-		int16_t first_return_strength;
-		int16_t last_return_raw;
-		int16_t last_return_median;
-		int16_t last_return_strength;
-		int16_t background_noise;
-	};
-
-	enum class Type {
-		Generic = 0,
-		LW20c
-	};
-	enum class State {
-		Configuring,
-		Running
-	};
-
-	int probe() override;
-
-	void start();
-
-	int readRegister(Register reg, uint8_t *data, int len);
-
-	int configure();
-	int enableI2CBinaryProtocol();
-	int collect();
-
-	PX4Rangefinder _px4_rangefinder;
-
-	int _conversion_interval{-1};
-
-	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
-
-	Type _type{Type::Generic};
-	State _state{State::Configuring};
-	int _consecutive_errors{0};
-};
-
-LightwareLaser::LightwareLaser(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
-			       int address) :
-	I2C(DRV_DIST_DEVTYPE_LIGHTWARE_LASER, MODULE_NAME, bus, address, bus_frequency),
-	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(DRV_DIST_DEVTYPE_LIGHTWARE_LASER, rotation)
-{
-}
-
-LightwareLaser::~LightwareLaser()
-{
-	/* free perf counters */
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
-}
-
-int LightwareLaser::init()
-{
-	int ret = PX4_ERROR;
-	int32_t hw_model = 0;
-	param_get(param_find("SENS_EN_SF1XX"), &hw_model);
-
-	switch (hw_model) {
-	case 0:
-		PX4_WARN("disabled.");
-		return ret;
-
-	case 1:  /* SF10/a (25m 32Hz) */
-		_px4_rangefinder.set_min_distance(0.01f);
-		_px4_rangefinder.set_max_distance(25.0f);
-		_conversion_interval = 31250;
-		break;
-
-	case 2:  /* SF10/b (50m 32Hz) */
-		_px4_rangefinder.set_min_distance(0.01f);
-		_px4_rangefinder.set_max_distance(50.0f);
-		_conversion_interval = 31250;
-		break;
-
-	case 3:  /* SF10/c (100m 16Hz) */
-		_px4_rangefinder.set_min_distance(0.01f);
-		_px4_rangefinder.set_max_distance(100.0f);
-		_conversion_interval = 62500;
-		break;
-
-	case 4:
-		/* SF11/c (120m 20Hz) */
-		_px4_rangefinder.set_min_distance(0.01f);
-		_px4_rangefinder.set_max_distance(120.0f);
-		_conversion_interval = 50000;
-		break;
-
-	case 5:
-		/* SF/LW20/b (50m 48-388Hz) */
-		_px4_rangefinder.set_min_distance(0.001f);
-		_px4_rangefinder.set_max_distance(50.0f);
-		_conversion_interval = 20834;
-		break;
-
-	case 6:
-		/* SF/LW20/c (100m 48-388Hz) */
-		_px4_rangefinder.set_min_distance(0.001f);
-		_px4_rangefinder.set_max_distance(100.0f);
-		_conversion_interval = 20834;
-		_type = Type::LW20c;
-		break;
-
-	default:
-		PX4_ERR("invalid HW model %d.", hw_model);
-		return ret;
-	}
-
-	/* do I2C init (and probe) first */
-	return I2C::init();
-}
-
-int LightwareLaser::readRegister(Register reg, uint8_t *data, int len)
-{
-	const uint8_t cmd = (uint8_t)reg;
-	return transfer(&cmd, 1, data, len);
-}
-
-int LightwareLaser::probe()
-{
-	switch (_type) {
-
-	case Type::Generic: {
-			uint8_t cmd = I2C_LEGACY_CMD_READ_ALTITUDE;
-			return transfer(&cmd, 1, nullptr, 0);
-		}
-
-	case Type::LW20c:
-		// try to enable I2C binary protocol
-		int ret = enableI2CBinaryProtocol();
-
-		if (ret != 0) {
-			return ret;
-		}
-
-		// read the product name
-		uint8_t product_name[16];
-		ret = readRegister(Register::ProductName, product_name, sizeof(product_name));
-		product_name[sizeof(product_name) - 1] = '\0';
-		PX4_DEBUG("product: %s", product_name);
-
-		if (ret == 0 && (strncmp((const char *)product_name, "SF20", sizeof(product_name)) == 0 ||
-				 strncmp((const char *)product_name, "LW20", sizeof(product_name)) == 0)) {
-			return 0;
-		}
-
-		return -1;
-	}
-
-	return -1;
-}
-
-int LightwareLaser::enableI2CBinaryProtocol()
-{
-	const uint8_t cmd[] = {(uint8_t)Register::Protocol, 0xaa, 0xaa};
-	int ret = transfer(cmd, sizeof(cmd), nullptr, 0);
-
-	if (ret != 0) {
-		return ret;
-	}
-
-	// now read and check against the expected values
-	uint8_t value[2];
-	ret = transfer(cmd, 1, value, sizeof(value));
-
-	if (ret != 0) {
-		return ret;
-	}
-
-	PX4_DEBUG("protocol values: 0x%x 0x%x", value[0], value[1]);
-
-	return (value[0] == 0xcc && value[1] == 0x00) ? 0 : -1;
-}
-
-int LightwareLaser::configure()
-{
-	switch (_type) {
-	case Type::Generic: {
-			uint8_t cmd = I2C_LEGACY_CMD_READ_ALTITUDE;
-			int ret = transfer(&cmd, 1, nullptr, 0);
-
-			if (PX4_OK != ret) {
-				perf_count(_comms_errors);
-				PX4_DEBUG("i2c::transfer returned %d", ret);
-				return ret;
-			}
-
-			return ret;
-		}
-		break;
-
-	case Type::LW20c:
-
-		int ret = enableI2CBinaryProtocol();
-		const uint8_t cmd1[] = {(uint8_t)Register::ServoConnected, 0};
-		ret |= transfer(cmd1, sizeof(cmd1), nullptr, 0);
-		const uint8_t cmd2[] = {(uint8_t)Register::ZeroOffset, 0, 0, 0, 0};
-		ret |= transfer(cmd2, sizeof(cmd2), nullptr, 0);
-		const uint8_t cmd3[] = {(uint8_t)Register::MeasurementMode, 1}; // 48Hz
-		ret |= transfer(cmd3, sizeof(cmd3), nullptr, 0);
-		const uint8_t cmd4[] = {(uint8_t)Register::DistanceOutput, output_data_config & 0xff, (output_data_config >> 8) & 0xff, 0, 0};
-		ret |= transfer(cmd4, sizeof(cmd4), nullptr, 0);
-		const uint8_t cmd5[] = {(uint8_t)Register::LostSignalCounter, 0, 0, 0, 0}; // immediately report lost signal
-		ret |= transfer(cmd5, sizeof(cmd5), nullptr, 0);
-
-		return ret;
-		break;
-	}
-
-	return -1;
-}
-
-int LightwareLaser::collect()
-{
-	switch (_type) {
-	case Type::Generic: {
-			/* read from the sensor */
-			perf_begin(_sample_perf);
-			uint8_t val[2] {};
-			const hrt_abstime timestamp_sample = hrt_absolute_time();
-
-			if (transfer(nullptr, 0, &val[0], 2) < 0) {
-				perf_count(_comms_errors);
-				perf_end(_sample_perf);
-				return PX4_ERROR;
-			}
-
-			perf_end(_sample_perf);
-
-			uint16_t distance_cm = val[0] << 8 | val[1];
-			float distance_m = float(distance_cm) * 1e-2f;
-
-			_px4_rangefinder.update(timestamp_sample, distance_m);
-			break;
-		}
-
-	case Type::LW20c:
-
-		/* read from the sensor */
-		perf_begin(_sample_perf);
-		OutputData data;
-		const hrt_abstime timestamp_sample = hrt_absolute_time();
-
-		if (readRegister(Register::DistanceData, (uint8_t *)&data, sizeof(data)) < 0) {
-			perf_count(_comms_errors);
-			perf_end(_sample_perf);
-			return PX4_ERROR;
-		}
-
-		perf_end(_sample_perf);
-
-		// compare different outputs (median filter adds about 25ms delay)
-		PX4_DEBUG("fm: %4i, fs: %2i%%, lm: %4i, lr: %4i, fs: %2i%%, n: %i",
-			  data.first_return_median, data.first_return_strength, data.last_return_median, data.last_return_raw,
-			  data.last_return_strength, data.background_noise);
-
-		float distance_m = float(data.last_return_raw) * 1e-2f;
-		int8_t quality = data.last_return_strength;
-
-		_px4_rangefinder.update(timestamp_sample, distance_m, quality);
-		break;
-	}
-
-	return PX4_OK;
-}
-
-void LightwareLaser::start()
-{
-	/* schedule a cycle to start things */
-	ScheduleDelayed(_conversion_interval);
-}
-
-void LightwareLaser::RunImpl()
-{
-	switch (_state) {
-	case State::Configuring: {
-			if (configure() == 0) {
-				_state = State::Running;
-				ScheduleDelayed(_conversion_interval);
-
-			} else {
-				// retry after a while
-				PX4_DEBUG("Retrying...");
-				ScheduleDelayed(300_ms);
-			}
-
-			break;
-		}
-
-	case State::Running:
-		if (PX4_OK != collect()) {
-			PX4_DEBUG("collection error");
-
-			if (++_consecutive_errors > 3) {
-				_state = State::Configuring;
-				_consecutive_errors = 0;
-			}
-		}
-
-		ScheduleDelayed(_conversion_interval);
-		break;
-	}
-}
-
-void LightwareLaser::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-}
-
-void LightwareLaser::print_usage()
-{
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-
-I2C bus driver for Lightware SFxx series LIDAR rangefinders: SF10/a, SF10/b, SF10/c, SF11/c, SF/LW20.
-
-Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("lightware_laser_i2c", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-I2CSPIDriverBase *LightwareLaser::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-				      int runtime_instance)
-{
-	LightwareLaser* instance = new LightwareLaser(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);
-
-	if (instance == nullptr) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
-
-	if (instance->init() != PX4_OK) {
-		delete instance;
-		return nullptr;
-	}
-
-	instance->start();
-	return instance;
-}
-
-extern "C" __EXPORT int lightware_laser_i2c_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = LightwareLaser;
-	BusCLIArguments cli{true, false};
-	cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
-	cli.default_i2c_frequency = 400000;
-
-	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.orientation = atoi(cli.optarg());
-			break;
-		}
-	}
-
-	const char *verb = cli.optarg();
-
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}
@@ -1,7 +0,0 @@
-module_name: Lightware Laser Rangefinder (serial)
-serial_config:
-    - command: lightware_laser_serial start -d ${SERIAL_DEV}
-      port_config_param:
-        name: SENS_SF0X_CFG
-        group: Sensors
-
@@ -41,11 +41,11 @@

 #include "LidarLiteI2C.h"

-LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency,
+LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
 			   const int address) :
 	I2C(DRV_RNG_DEVTYPE_LL40LS, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(get_device_id(), orientation)
+	_px4_rangefinder(get_device_id(), rotation)
 {
 	_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
 	_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE);
@@ -94,7 +94,7 @@ static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms};
 class LidarLiteI2C : public device::I2C, public I2CSPIDriver<LidarLiteI2C>
 {
 public:
-	LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency,
+	LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
 		     const int address = LL40LS_BASEADDR);
 	virtual ~LidarLiteI2C();

@@ -72,7 +72,7 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html
 I2CSPIDriverBase *LidarLiteI2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				      int runtime_instance)
 {
-	LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);
+	LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.rotation, cli.bus_frequency);

 	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
@@ -105,7 +105,7 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[])
 	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
 		switch (ch) {
 		case 'R':
-			cli.orientation = (enum Rotation)atoi(cli.optarg());
+			cli.rotation = (enum Rotation)atoi(cli.optarg());
 			break;
 		}
 	}
@@ -31,15 +31,15 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__distance_sensor__lightware_laser_serial
-	MAIN lightware_laser_serial
+	MODULE drivers__sf0x
+	MAIN sf0x
 	COMPILE_FLAGS
 		-Wno-cast-align # TODO: fix and enable
 	SRCS
-		lightware_laser_serial.cpp
-		lightware_laser_serial.hpp
-		lightware_laser_serial_main.cpp
-		parser.cpp
+		SF0X.cpp
+		SF0X.hpp
+		sf0x_main.cpp
+		sf0x_parser.cpp
 	DEPENDS
 		drivers_rangefinder
 		px4_work_queue
@@ -48,5 +48,5 @@ px4_add_module(
 	)

 if(PX4_TESTING)
-	add_subdirectory(tests)
+	add_subdirectory(sf0x_tests)
 endif()
@@ -31,15 +31,15 @@
 *
 ****************************************************************************/

-#include "lightware_laser_serial.hpp"
+#include "SF0X.hpp"

 #include <fcntl.h>
 #include <termios.h>

 /* Configuration Constants */
-#define LW_TAKE_RANGE_REG		'd'
+#define SF0X_TAKE_RANGE_REG		'd'

-LightwareLaserSerial::LightwareLaserSerial(const char *port, uint8_t rotation) :
+SF0X::SF0X(const char *port, uint8_t rotation) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
 	_px4_rangefinder(0 /* device id not yet used */, rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
@@ -52,7 +52,7 @@ LightwareLaserSerial::LightwareLaserSerial(const char *port, uint8_t rotation) :
 	_port[sizeof(_port) - 1] = '\0';
 }

-LightwareLaserSerial::~LightwareLaserSerial()
+SF0X::~SF0X()
 {
 	stop();

@@ -61,7 +61,7 @@ LightwareLaserSerial::~LightwareLaserSerial()
 }

 int
-LightwareLaserSerial::init()
+SF0X::init()
 {
 	int32_t hw_model = 0;
 	param_get(param_find("SENS_EN_SF0X"), &hw_model);
@@ -108,10 +108,10 @@ LightwareLaserSerial::init()
 	return PX4_OK;
 }

-int LightwareLaserSerial::measure()
+int SF0X::measure()
 {
 	// Send the command to begin a measurement.
-	char cmd = LW_TAKE_RANGE_REG;
+	char cmd = SF0X_TAKE_RANGE_REG;
 	int ret = ::write(_fd, &cmd, 1);

 	if (ret != sizeof(cmd)) {
@@ -123,7 +123,7 @@ int LightwareLaserSerial::measure()
 	return PX4_OK;
 }

-int LightwareLaserSerial::collect()
+int SF0X::collect()
 {
 	perf_begin(_sample_perf);

@@ -161,7 +161,7 @@ int LightwareLaserSerial::collect()
 	bool valid = false;

 	for (int i = 0; i < ret; i++) {
-		if (OK == lightware_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
+		if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
 			valid = true;
 		}
 	}
@@ -179,7 +179,7 @@ int LightwareLaserSerial::collect()
 	return PX4_OK;
 }

-void LightwareLaserSerial::start()
+void SF0X::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
@@ -188,12 +188,12 @@ void LightwareLaserSerial::start()
 	ScheduleNow();
 }

-void LightwareLaserSerial::stop()
+void SF0X::stop()
 {
 	ScheduleClear();
 }

-void LightwareLaserSerial::Run()
+void SF0X::Run()
 {
 	/* fds initialized? */
 	if (_fd < 0) {
@@ -293,7 +293,7 @@ void LightwareLaserSerial::Run()
 	ScheduleDelayed(_interval);
 }

-void LightwareLaserSerial::print_info()
+void SF0X::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
@@ -32,11 +32,11 @@
 ****************************************************************************/

 /**
- * @file lightware_laser_serial.hpp
+ * @file SF0X.hpp
 * @author Lorenz Meier <lm@inf.ethz.ch>
 * @author Greg Hulands
 *
- * Driver for the Lightware laser rangefinder series
+ * Driver for the Lightware SF0x laser rangefinder series
 */

 #pragma once
@@ -48,13 +48,13 @@
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>

-#include "parser.h"
+#include "sf0x_parser.h"

-class LightwareLaserSerial : public px4::ScheduledWorkItem
+class SF0X : public px4::ScheduledWorkItem
 {
 public:
-	LightwareLaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
-	~LightwareLaserSerial() override;
+	SF0X(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	~SF0X() override;

 	int 			init();
 	void				print_info();
@@ -67,6 +67,7 @@ private:
 	int				measure();
 	int				collect();

+
 	PX4Rangefinder                  _px4_rangefinder;

 	char 				_port[20] {};
@@ -75,7 +76,7 @@ private:
 	int				_fd{-1};
 	char				_linebuf[10] {};
 	unsigned			_linebuf_index{0};
-	enum LW_PARSE_STATE		_parse_state {LW_PARSE_STATE0_UNSYNC};
+	enum SF0X_PARSE_STATE		_parse_state {SF0X_PARSE_STATE0_UNSYNC};
 	hrt_abstime			_last_read{0};

 	unsigned			_consecutive_fail_count;
@@ -83,4 +84,6 @@ private:
 	perf_counter_t			_sample_perf;
 	perf_counter_t			_comms_errors;

+
+
 };
@@ -0,0 +1,7 @@
+module_name: Lightware Laser Rangefinder
+serial_config:
+    - command: sf0x start -d ${SERIAL_DEV}
+      port_config_param:
+        name: SENS_SF0X_CFG
+        group: Sensors
+
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * Lightware Laser Rangefinder hardware model (serial)
+ * Lightware Laser Rangefinder hardware model
 *
 * @reboot_required true
 * @group Sensors
@@ -31,15 +31,15 @@
 *
 ****************************************************************************/

-#include "lightware_laser_serial.hpp"
+#include "SF0X.hpp"

 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>

-namespace lightware_laser
+namespace sf0x
 {

-LightwareLaserSerial *g_dev{nullptr};
+SF0X *g_dev{nullptr};

 static int start(const char *port, uint8_t rotation)
 {
@@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation)
 	}

 	/* create the driver */
-	g_dev = new LightwareLaserSerial(port, rotation);
+	g_dev = new SF0X(port, rotation);

 	if (g_dev == nullptr) {
 		return -1;
@@ -109,12 +109,12 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
 ### Examples

 Attempt to start driver on a specified serial device.
-$ lightware_laser_serial start -d /dev/ttyS1
+$ sf0x start -d /dev/ttyS1
 Stop driver
-$ lightware_laser_serial stop
+$ sf0x stop
 )DESCR_STR");

-	PRINT_MODULE_USAGE_NAME("lightware_laser_serial", "driver");
+	PRINT_MODULE_USAGE_NAME("sf0x", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
@@ -125,7 +125,7 @@ $ lightware_laser_serial stop

 } // namespace

-extern "C" __EXPORT int lightware_laser_serial_main(int argc, char *argv[])
+extern "C" __EXPORT int sf0x_main(int argc, char *argv[])
 {
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	const char *device_path = nullptr;
@@ -144,26 +144,26 @@ extern "C" __EXPORT int lightware_laser_serial_main(int argc, char *argv[])
 			break;

 		default:
-			lightware_laser::usage();
+			sf0x::usage();
 			return -1;
 		}
 	}

 	if (myoptind >= argc) {
-		lightware_laser::usage();
+		sf0x::usage();
 		return -1;
 	}

 	if (!strcmp(argv[myoptind], "start")) {
-		return lightware_laser::start(device_path, rotation);
+		return sf0x::start(device_path, rotation);

 	} else if (!strcmp(argv[myoptind], "stop")) {
-		return lightware_laser::stop();
+		return sf0x::stop();

 	} else if (!strcmp(argv[myoptind], "status")) {
-		return lightware_laser::status();
+		return sf0x::status();
 	}

-	lightware_laser::usage();
+	sf0x::usage();
 	return -1;
 }
@@ -32,19 +32,19 @@
 ****************************************************************************/

 /**
- * @file parser.cpp
+ * @file sf0x_parser.cpp
 * @author Lorenz Meier <lm@inf.ethz.ch>
 *
- * Driver for the Lightware laser rangefinder series
+ * Driver for the Lightware SF0x laser rangefinder series
 */

-#include "parser.h"
+#include "sf0x_parser.h"
 #include <string.h>
 #include <stdlib.h>

-//#define LW_DEBUG
+//#define SF0X_DEBUG

-#ifdef LW_DEBUG
+#ifdef SF0X_DEBUG
 #include <stdio.h>

 const char *parser_state[] = {
@@ -58,98 +58,98 @@ const char *parser_state[] = {
 };
 #endif

-int lightware_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum LW_PARSE_STATE *state, float *dist)
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
 {
 	int ret = -1;
 	char *end;

 	switch (*state) {
-	case LW_PARSE_STATE0_UNSYNC:
+	case SF0X_PARSE_STATE0_UNSYNC:
 		if (c == '\n') {
-			*state = LW_PARSE_STATE1_SYNC;
+			*state = SF0X_PARSE_STATE1_SYNC;
 			(*parserbuf_index) = 0;
 		}

 		break;

-	case LW_PARSE_STATE1_SYNC:
+	case SF0X_PARSE_STATE1_SYNC:
 		if (c >= '0' && c <= '9') {
-			*state = LW_PARSE_STATE2_GOT_DIGIT0;
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
 			parserbuf[*parserbuf_index] = c;
 			(*parserbuf_index)++;
 		}

 		break;

-	case LW_PARSE_STATE2_GOT_DIGIT0:
+	case SF0X_PARSE_STATE2_GOT_DIGIT0:
 		if (c >= '0' && c <= '9') {
-			*state = LW_PARSE_STATE2_GOT_DIGIT0;
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
 			parserbuf[*parserbuf_index] = c;
 			(*parserbuf_index)++;

 		} else if (c == '.') {
-			*state = LW_PARSE_STATE3_GOT_DOT;
+			*state = SF0X_PARSE_STATE3_GOT_DOT;
 			parserbuf[*parserbuf_index] = c;
 			(*parserbuf_index)++;

 		} else {
-			*state = LW_PARSE_STATE0_UNSYNC;
+			*state = SF0X_PARSE_STATE0_UNSYNC;
 		}

 		break;

-	case LW_PARSE_STATE3_GOT_DOT:
+	case SF0X_PARSE_STATE3_GOT_DOT:
 		if (c >= '0' && c <= '9') {
-			*state = LW_PARSE_STATE4_GOT_DIGIT1;
+			*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
 			parserbuf[*parserbuf_index] = c;
 			(*parserbuf_index)++;

 		} else {
-			*state = LW_PARSE_STATE0_UNSYNC;
+			*state = SF0X_PARSE_STATE0_UNSYNC;
 		}

 		break;

-	case LW_PARSE_STATE4_GOT_DIGIT1:
+	case SF0X_PARSE_STATE4_GOT_DIGIT1:
 		if (c >= '0' && c <= '9') {
-			*state = LW_PARSE_STATE5_GOT_DIGIT2;
+			*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
 			parserbuf[*parserbuf_index] = c;
 			(*parserbuf_index)++;

 		} else {
-			*state = LW_PARSE_STATE0_UNSYNC;
+			*state = SF0X_PARSE_STATE0_UNSYNC;
 		}

 		break;

-	case LW_PARSE_STATE5_GOT_DIGIT2:
+	case SF0X_PARSE_STATE5_GOT_DIGIT2:
 		if (c == '\r') {
-			*state = LW_PARSE_STATE6_GOT_CARRIAGE_RETURN;
+			*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;

 		} else {
-			*state = LW_PARSE_STATE0_UNSYNC;
+			*state = SF0X_PARSE_STATE0_UNSYNC;
 		}

 		break;

-	case LW_PARSE_STATE6_GOT_CARRIAGE_RETURN:
+	case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
 		if (c == '\n') {
 			parserbuf[*parserbuf_index] = '\0';
 			*dist = strtod(parserbuf, &end);
-			*state = LW_PARSE_STATE1_SYNC;
+			*state = SF0X_PARSE_STATE1_SYNC;
 			*parserbuf_index = 0;
 			ret = 0;

 		} else {
-			*state = LW_PARSE_STATE0_UNSYNC;
+			*state = SF0X_PARSE_STATE0_UNSYNC;
 		}

 		break;
 	}

-#ifdef LW_DEBUG
-	printf("state: LW_PARSE_STATE%s\n", parser_state[*state]);
+#ifdef SF0X_DEBUG
+	printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
 #endif

 	return ret;
-}
+}
@@ -32,22 +32,22 @@
 ****************************************************************************/

 /**
- * @file parser.cpp
+ * @file sf0x_parser.cpp
 * @author Lorenz Meier <lm@inf.ethz.ch>
 *
- * Declarations of parser for the Lightware laser rangefinder series
+ * Declarations of parser for the Lightware SF0x laser rangefinder series
 */

 #pragma once

-enum LW_PARSE_STATE {
-	LW_PARSE_STATE0_UNSYNC = 0,
-	LW_PARSE_STATE1_SYNC,
-	LW_PARSE_STATE2_GOT_DIGIT0,
-	LW_PARSE_STATE3_GOT_DOT,
-	LW_PARSE_STATE4_GOT_DIGIT1,
-	LW_PARSE_STATE5_GOT_DIGIT2,
-	LW_PARSE_STATE6_GOT_CARRIAGE_RETURN
+enum SF0X_PARSE_STATE {
+	SF0X_PARSE_STATE0_UNSYNC = 0,
+	SF0X_PARSE_STATE1_SYNC,
+	SF0X_PARSE_STATE2_GOT_DIGIT0,
+	SF0X_PARSE_STATE3_GOT_DOT,
+	SF0X_PARSE_STATE4_GOT_DIGIT1,
+	SF0X_PARSE_STATE5_GOT_DIGIT2,
+	SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
 };

-int lightware_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum LW_PARSE_STATE *state, float *dist);
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);
@@ -31,11 +31,11 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__distance_sensor__lightware_laser_serial__tests
-	MAIN lightware_laser_test
+	MODULE drivers__sf0x__sf0x_tests
+	MAIN sf0x_tests
 	COMPILE_FLAGS
 	SRCS
-		lightware_laser_test.cpp
-		../parser.cpp
+		SF0XTest.cpp
+		../sf0x_parser.cpp
 	DEPENDS
 	)
@@ -1,6 +1,6 @@
 #include <unit_test.h>

-#include "../parser.h"
+#include "../sf0x_parser.h"

 #include <systemlib/err.h>

@@ -8,25 +8,25 @@
 #include <cstring>
 #include <unistd.h>

-extern "C" __EXPORT int lightware_laser_test_main(int argc, char *argv[]);
+extern "C" __EXPORT int sf0x_tests_main(int argc, char *argv[]);

-class LightwareLaserTest : public UnitTest
+class SF0XTest : public UnitTest
 {
 public:
 	virtual bool run_tests();

 private:
-	bool runTest();
+	bool sf0xTest();
 };

-bool LightwareLaserTest::run_tests()
+bool SF0XTest::run_tests()
 {
-	ut_run_test(runTest);
+	ut_run_test(sf0xTest);

 	return (_tests_failed == 0);
 }

-bool LightwareLaserTest::runTest()
+bool SF0XTest::sf0xTest()
 {
 	const char _LINE_MAX = 20;
 	//char _linebuf[_LINE_MAX];
@@ -50,7 +50,7 @@ bool LightwareLaserTest::runTest()
 			       "\r\n"
 			      };

-	enum LW_PARSE_STATE state = LW_PARSE_STATE0_UNSYNC;
+	enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
 	float dist_m;
 	char _parserbuf[_LINE_MAX];
 	unsigned _parsebuf_index = 0;
@@ -61,7 +61,7 @@ bool LightwareLaserTest::runTest()
 		int parse_ret = -1;

 		for (int i = 0; i < (ssize_t)strlen(lines[l]); i++) {
-			parse_ret = lightware_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
+			parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);

 			if (parse_ret == 0) {
 				if (l == 0) {
@@ -88,4 +88,4 @@ bool LightwareLaserTest::runTest()
 	return true;
 }

-ut_declare_test_c(lightware_laser_test_main, LightwareLaserTest)
+ut_declare_test_c(sf0x_tests_main, SF0XTest)
@@ -31,10 +31,12 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__distance_sensor__lightware_laser_i2c
-	MAIN lightware_laser_i2c
+	MODULE drivers__sf1xx
+	MAIN sf1xx
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
 	SRCS
-		lightware_laser_i2c.cpp
+		sf1xx.cpp
 	DEPENDS
 	)

@@ -0,0 +1,360 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sf1xx.cpp
+ *
+ * @author ecmnet <ecm@gmx.de>
+ * @author Vasily Evseenko <svpcom@gmail.com>
+ *
+ * Driver for the Lightware SF1xx lidar range finder series.
+ * Default I2C address 0x66 is used.
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/module.h>
+#include <drivers/device/i2c.h>
+#include <lib/parameters/param.h>
+#include <lib/perf/perf_counter.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/rangefinder/PX4Rangefinder.hpp>
+
+/* Configuration Constants */
+#define SF1XX_BASEADDR		0x66
+
+class SF1XX : public device::I2C, public I2CSPIDriver<SF1XX>
+{
+public:
+	SF1XX(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
+	      int address = SF1XX_BASEADDR);
+
+	~SF1XX() override;
+
+	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+					     int runtime_instance);
+	static void print_usage();
+
+	int init() override;
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void print_status() override;
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void RunImpl();
+
+private:
+	int probe() override;
+
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address The I2C bus address to probe.
+	* @return True if the device is present.
+	*/
+	int probe_address(uint8_t address);
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void start();
+
+	int measure();
+	int collect();
+
+	PX4Rangefinder _px4_rangefinder;
+
+	bool _sensor_ok{false};
+
+	int _conversion_interval{-1};
+	int _measure_interval{0};
+
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
+};
+
+SF1XX::SF1XX(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
+	I2C(DRV_DIST_DEVTYPE_SF1XX, MODULE_NAME, bus, address, bus_frequency),
+	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
+	_px4_rangefinder(DRV_DIST_DEVTYPE_SF1XX, rotation)
+{
+}
+
+SF1XX::~SF1XX()
+{
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int SF1XX::init()
+{
+	int ret = PX4_ERROR;
+	int32_t hw_model = 0;
+	param_get(param_find("SENS_EN_SF1XX"), &hw_model);
+
+	switch (hw_model) {
+	case 0:
+		PX4_WARN("disabled.");
+		return ret;
+
+	case 1:  /* SF10/a (25m 32Hz) */
+		_px4_rangefinder.set_min_distance(0.01f);
+		_px4_rangefinder.set_max_distance(25.0f);
+		_conversion_interval = 31250;
+		break;
+
+	case 2:  /* SF10/b (50m 32Hz) */
+		_px4_rangefinder.set_min_distance(0.01f);
+		_px4_rangefinder.set_max_distance(50.0f);
+		_conversion_interval = 31250;
+		break;
+
+	case 3:  /* SF10/c (100m 16Hz) */
+		_px4_rangefinder.set_min_distance(0.01f);
+		_px4_rangefinder.set_max_distance(100.0f);
+		_conversion_interval = 62500;
+		break;
+
+	case 4:
+		/* SF11/c (120m 20Hz) */
+		_px4_rangefinder.set_min_distance(0.01f);
+		_px4_rangefinder.set_max_distance(120.0f);
+		_conversion_interval = 50000;
+		break;
+
+	case 5:
+		/* SF/LW20/b (50m 48-388Hz) */
+		_px4_rangefinder.set_min_distance(0.001f);
+		_px4_rangefinder.set_max_distance(50.0f);
+		_conversion_interval = 20834;
+		break;
+
+	case 6:
+		/* SF/LW20/c (100m 48-388Hz) */
+		_px4_rangefinder.set_min_distance(0.001f);
+		_px4_rangefinder.set_max_distance(100.0f);
+		_conversion_interval = 20834;
+		break;
+
+	default:
+		PX4_ERR("invalid HW model %d.", hw_model);
+		return ret;
+	}
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		return ret;
+	}
+
+	// Select altitude register
+	int ret2 = measure();
+
+	if (ret2 == 0) {
+		ret = OK;
+		_sensor_ok = true;
+	}
+
+	return ret;
+}
+
+int SF1XX::probe()
+{
+	return measure();
+}
+
+int SF1XX::measure()
+{
+	/*
+	 * Send the command '0' -- read altitude
+	 */
+	uint8_t cmd = 0;
+	int ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		PX4_DEBUG("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int SF1XX::collect()
+{
+	/* read from the sensor */
+	perf_begin(_sample_perf);
+	uint8_t val[2] {};
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	if (transfer(nullptr, 0, &val[0], 2) < 0) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return PX4_ERROR;
+	}
+
+	perf_end(_sample_perf);
+
+	uint16_t distance_cm = val[0] << 8 | val[1];
+	float distance_m = float(distance_cm) * 1e-2f;
+
+	_px4_rangefinder.update(timestamp_sample, distance_m);
+
+	return PX4_OK;
+}
+
+void SF1XX::start()
+{
+	if (_measure_interval == 0) {
+		_measure_interval = _conversion_interval;
+	}
+
+	/* set register to '0' */
+	measure();
+
+	/* schedule a cycle to start things */
+	ScheduleDelayed(_conversion_interval);
+}
+
+void SF1XX::RunImpl()
+{
+	/* Collect results */
+	if (OK != collect()) {
+		PX4_DEBUG("collection error");
+		/* if error restart the measurement state machine */
+		start();
+		return;
+	}
+
+	/* schedule a fresh cycle call when the measurement is done */
+	ScheduleDelayed(_conversion_interval);
+}
+
+void SF1XX::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+}
+
+void SF1XX::print_usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+I2C bus driver for Lightware SFxx series LIDAR rangefinders: SF10/a, SF10/b, SF10/c, SF11/c, SF/LW20.
+
+Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("sf1xx", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+I2CSPIDriverBase *SF1XX::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				      int runtime_instance)
+{
+	SF1XX* instance = new SF1XX(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (instance->init() != PX4_OK) {
+		delete instance;
+		return nullptr;
+	}
+
+	instance->start();
+	return instance;
+}
+
+extern "C" __EXPORT int sf1xx_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = SF1XX;
+	BusCLIArguments cli{true, false};
+	cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	cli.default_i2c_frequency = 400000;
+
+	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
+		switch (ch) {
+		case 'R':
+			cli.orientation = atoi(cli.optarg());
+			break;
+		}
+	}
+
+	const char *verb = cli.optarg();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_SF1XX);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -137,7 +137,7 @@
 #define DRV_DIST_DEVTYPE_LL40LS       0x70
 #define DRV_DIST_DEVTYPE_MAPPYDOT     0x71
 #define DRV_DIST_DEVTYPE_MB12XX       0x72
-#define DRV_DIST_DEVTYPE_LIGHTWARE_LASER 0x73
+#define DRV_DIST_DEVTYPE_SF1XX        0x73
 #define DRV_DIST_DEVTYPE_SRF02        0x74
 #define DRV_DIST_DEVTYPE_TERARANGER   0x75
 #define DRV_DIST_DEVTYPE_VL53L0X      0x76
@@ -176,7 +176,7 @@ private:
 	const Instance 			_instance;

 	uORB::Subscription		_orb_inject_data_sub{ORB_ID(gps_inject_data)};
-	uORB::Publication<gps_dump_s>	_dump_communication_pub{ORB_ID(gps_dump)};
+	uORB::PublicationQueued<gps_dump_s>	_dump_communication_pub{ORB_ID(gps_dump)};
 	gps_dump_s			*_dump_to_device{nullptr};
 	gps_dump_s			*_dump_from_device{nullptr};
 	bool				_should_dump_communication{false};			///< if true, dump communication
@@ -162,7 +162,7 @@ private:
 	char					_device[20];
 	orb_advert_t			_t_outputs;
 	orb_advert_t			_t_esc_status;
-	uORB::Publication<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
+	uORB::PublicationQueued<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
 	unsigned int			_num_outputs;
 	bool					_primary_pwm_device;
 	bool     				_motortest;
@@ -324,8 +324,8 @@ PMW3901::RunImpl()
 		return;
 	}

-	delta_x = (float)_flow_sum_x / 385.0f;		// proportional factor + convert from pixels to radians
-	delta_y = (float)_flow_sum_y / 385.0f;		// proportional factor + convert from pixels to radians
+	delta_x = (float)_flow_sum_x / 500.0f;		// proportional factor + convert from pixels to radians
+	delta_y = (float)_flow_sum_y / 500.0f;		// proportional factor + convert from pixels to radians

 	optical_flow_s report{};
 	report.timestamp = timestamp;
@@ -713,7 +713,7 @@ PX4IO::init()
 			vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;

 			/* send command once */
-			uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+			uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
 			vcmd_pub.publish(vcmd);

 			/* spin here until IO's state has propagated into the system */
@@ -746,7 +746,7 @@ PX4IO::init()
 		vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;

 		/* send command once */
-		uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+		uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
 		vcmd_pub.publish(vcmd);

 		/* spin here until IO's state has propagated into the system */
@@ -648,7 +648,7 @@ int RCInput::custom_command(int argc, char *argv[])
 	const char *verb = argv[0];

 	if (!strcmp(verb, "bind")) {
-		uORB::Publication<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
+		uORB::PublicationQueued<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
 		vehicle_command_s vcmd{};
 		vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
 		vcmd.timestamp = hrt_absolute_time();
@@ -79,8 +79,8 @@ private:

 	uORB::Subscription		_armed_sub{ORB_ID(actuator_armed)};
 	uORB::Publication<safety_s>	_to_safety{ORB_ID(safety)};
-	uORB::Publication<vehicle_command_s>	_to_command{ORB_ID(vehicle_command)};
-	uORB::Publication<led_control_s> _to_led_control{ORB_ID(led_control)};
+	uORB::PublicationQueued<vehicle_command_s>	_to_command{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<led_control_s> _to_led_control{ORB_ID(led_control)};
 	uORB::Publication<tune_control_s> _to_tune_control{ORB_ID(tune_control)};

 	safety_s _safety{};
@@ -359,6 +359,8 @@ void IridiumSBD::main_loop(int argc, char *argv[])
 			break;
 		}

+		publish_subsystem_status();
+
 		if (_new_state != _state) {
 			VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]);
 			_state = _new_state;
@@ -1114,13 +1116,31 @@ void IridiumSBD::publish_iridium_status()
 	}
 }

-int IridiumSBD::open_first(struct file *filep)
+void IridiumSBD::publish_subsystem_status()
+{
+	const bool present = true;
+	const bool enabled = true;
+	const bool ok = _status.last_heartbeat > 0; // maybe at some point here an additional check should be made
+
+	if ((_info.present != present) || (_info.enabled != enabled) || (_info.ok != ok)) {
+		_info.timestamp = hrt_absolute_time();
+		_info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_SATCOM;
+		_info.present = present;
+		_info.enabled = enabled;
+		_info.ok = ok;
+
+		_subsystem_pub.publish(_info);
+	}
+}
+
+
+int	IridiumSBD::open_first(struct file *filep)
 {
 	_cdev_used = true;
 	return CDev::open_first(filep);
 }

-int IridiumSBD::close_last(struct file *filep)
+int	IridiumSBD::close_last(struct file *filep)
 {
 	_cdev_used = false;
 	return CDev::close_last(filep);
@@ -41,6 +41,7 @@

 #include <uORB/Publication.hpp>
 #include <uORB/topics/iridiumsbd_status.h>
+#include <uORB/topics/subsystem_info.h>

 typedef enum {
 	SATCOM_OK = 0,
@@ -254,7 +255,9 @@ private:
 	 */
 	pollevent_t poll_state(struct file *filp);

-	void publish_iridium_status();
+	void publish_iridium_status(void);
+
+	void publish_subsystem_status();

 	/**
 	 * Notification of the first open of CDev.
@@ -302,6 +305,7 @@ private:
 	uint16_t _packet_length = 0;

 	uORB::Publication<iridiumsbd_status_s> _iridiumsbd_status_pub{ORB_ID(iridiumsbd_status)};
+	uORB::PublicationQueued<subsystem_info_s> _subsystem_pub{ORB_ID(subsystem_info)};

 	bool _test_pending = false;
 	char _test_command[32];
@@ -338,5 +342,6 @@ private:

 	bool _verbose = false;

-	iridiumsbd_status_s _status{};
+	iridiumsbd_status_s _status = {};
+	subsystem_info_s _info = {};
 };
@@ -166,7 +166,7 @@ void ToneAlarm::Run()
 				} else if (_play_tone && !tune_control.tune_override) {
 					// otherwise re-publish tune to process next
 					PX4_DEBUG("tune already playing, requeing tune: %d", tune_control.tune_id);
-					uORB::Publication<tune_control_s> tune_control_pub{ORB_ID(tune_control)};
+					uORB::PublicationQueued<tune_control_s> tune_control_pub{ORB_ID(tune_control)};
 					tune_control.timestamp = hrt_absolute_time();
 					tune_control_pub.publish(tune_control);
 				}
@@ -163,7 +163,7 @@ private:

 	// uORB topic handle for MAVLink parameter responses
 	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
-	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};

 	typedef uavcan::MethodBinder<UavcanServers *,
 		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
@@ -71,7 +71,7 @@ px4_add_module(
 	MODULE modules__gyro_fft
 	MAIN gyro_fft
 	STACK_MAIN
-		4096
+		8192
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
 	INCLUDES
@@ -45,13 +45,18 @@ GyroFFT::GyroFFT() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
 {
-	arm_rfft_init_q15(&_rfft_q15, FFT_LENGTH, 0, 1);
+	for (int axis = 0; axis < 3; axis++) {
+		arm_rfft_init_q15(&_rfft_q15[axis], FFT_LENGTH, 0, 1);
+	}

 	// init Hanning window
+	float hanning_window[FFT_LENGTH];
+
 	for (int n = 0; n < FFT_LENGTH; n++) {
-		const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1)));
-		arm_float_to_q15(&hanning_value, &_hanning_window[n], 1);
+		hanning_window[n] = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1)));
 	}
+
+	arm_float_to_q15(hanning_window, _hanning_window, FFT_LENGTH);
 }

 GyroFFT::~GyroFFT()
@@ -65,8 +70,8 @@ GyroFFT::~GyroFFT()
 bool GyroFFT::init()
 {
 	if (!SensorSelectionUpdate(true)) {
-		PX4_WARN("sensor_gyro_fifo callback registration failed!");
-		ScheduleDelayed(500_ms);
+		PX4_ERR("sensor_gyro_fifo callback registration failed!");
+		return false;
 	}

 	return true;
@@ -74,7 +79,7 @@ bool GyroFFT::init()

 bool GyroFFT::SensorSelectionUpdate(bool force)
 {
-	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
+	if (_sensor_selection_sub.updated() || force) {
 		sensor_selection_s sensor_selection{};
 		_sensor_selection_sub.copy(&sensor_selection);

@@ -134,41 +139,6 @@ static constexpr float tau(float x)
 	return (1.f / 4.f * p1 - sqrtf(6) / 24 * p2);
 }

-float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index)
-{
-	// find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/)
-	int16_t real[3] {fft[peak_index - 2], fft[peak_index], fft[peak_index + 2]};
-	int16_t imag[3] {fft[peak_index - 2 + 1], fft[peak_index + 1], fft[peak_index + 2 + 1]};
-
-	const int k = 1;
-
-	float divider = (real[k] * real[k] + imag[k] * imag[k]);
-
-	// ap = (X[k + 1].r * X[k].r + X[k+1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
-	float ap = (real[k + 1] * real[k] + imag[k + 1] * imag[k]) / divider;
-
-	// am = (X[k – 1].r * X[k].r + X[k – 1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
-	float am = (real[k - 1] * real[k] + imag[k - 1] * imag[k]) / divider;
-
-	float dp = -ap  / (1.f - ap);
-	float dm = am / (1.f - am);
-	float d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm);
-
-	float adjusted_bin = peak_index + d;
-	float peak_freq_adjusted = (_gyro_sample_rate_hz * adjusted_bin / (FFT_LENGTH * 2.f));
-
-	return peak_freq_adjusted;
-}
-
-static int float_cmp(const void *elem1, const void *elem2)
-{
-	if (*(const float *)elem1 < * (const float *)elem2) {
-		return -1;
-	}
-
-	return *(const float *)elem1 > *(const float *)elem2;
-}
-
 void GyroFFT::Run()
 {
 	if (should_exit()) {
@@ -194,10 +164,9 @@ void GyroFFT::Run()

 	SensorSelectionUpdate();

-	const float resolution_hz = _gyro_sample_rate_hz / FFT_LENGTH;
+	const float resolution_hz = _gyro_sample_rate_hz / (FFT_LENGTH * 2);

 	bool publish = false;
-	bool fft_updated = false;

 	// run on sensor gyro fifo updates
 	sensor_gyro_fifo_s sensor_gyro_fifo;
@@ -234,35 +203,31 @@ void GyroFFT::Run()
 				break;
 			}

-			int &buffer_index = _fft_buffer_index[axis];
-
 			for (int n = 0; n < N; n++) {
-				if (buffer_index < FFT_LENGTH) {
-					// convert int16_t -> q15_t (scaling isn't relevant)
-					_gyro_data_buffer[axis][buffer_index] = input[n] / 2;
-					buffer_index++;
-				}
+				int &buffer_index = _fft_buffer_index[axis];

-				// if we have enough samples begin processing, but only one FFT per cycle
-				if ((buffer_index >= FFT_LENGTH) && !fft_updated) {
+				_data_buffer[axis][buffer_index] = input[n] / 2;

-					arm_mult_q15(_gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, FFT_LENGTH);
+				buffer_index++;
+
+				// if we have enough samples, begin processing
+				if (buffer_index >= FFT_LENGTH) {
+
+					arm_mult_q15(_data_buffer[axis], _hanning_window, _fft_input_buffer, FFT_LENGTH);

 					perf_begin(_fft_perf);
-					arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);
+					arm_rfft_q15(&_rfft_q15[axis], _fft_input_buffer, _fft_outupt_buffer);
 					perf_end(_fft_perf);
-					fft_updated = true;

-					static constexpr uint16_t MIN_SNR = 10; // TODO:
-
-					static constexpr int MAX_NUM_PEAKS = 2;
-					uint32_t peaks_magnitude[MAX_NUM_PEAKS] {};
-					uint8_t peak_index[MAX_NUM_PEAKS] {};
+					static constexpr uint16_t MIN_SNR = 100; // TODO:
+					uint32_t max_peak_0 = 0;
+					uint8_t max_peak_index_0 = 0;
+					bool peak_0_found = false;

 					// start at 2 to skip DC
 					// output is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1]
-					for (uint8_t bucket_index = 2; bucket_index < (FFT_LENGTH / 2); bucket_index = bucket_index + 2) {
-						const float freq_hz = (bucket_index / 2) * resolution_hz;
+					for (uint16_t bucket_index = 2; bucket_index < FFT_LENGTH; bucket_index = bucket_index + 2) {
+						const float freq_hz = bucket_index * resolution_hz;

 						if (freq_hz > _param_imu_gyro_fft_max.get()) {
 							break;
@@ -271,68 +236,91 @@ void GyroFFT::Run()
 						if (freq_hz >= _param_imu_gyro_fft_min.get()) {
 							const int16_t real = _fft_outupt_buffer[bucket_index];
 							const int16_t complex = _fft_outupt_buffer[bucket_index + 1];
+							const uint32_t fft_value_squared = real * real + complex * complex;

-							const uint32_t fft_magnitude_squared = real * real + complex * complex;
+							if ((fft_value_squared > MIN_SNR) && (fft_value_squared >= max_peak_0)) {
+								max_peak_index_0 = bucket_index;
+								max_peak_0 = fft_value_squared;
+								peak_0_found = true;
+							}
+						}
+					}

-							if (fft_magnitude_squared > MIN_SNR) {
-								for (int i = 0; i < MAX_NUM_PEAKS; i++) {
-									if (fft_magnitude_squared > peaks_magnitude[i]) {
-										peaks_magnitude[i] = fft_magnitude_squared;
-										peak_index[i] = bucket_index;
-										publish = true;
-										break;
+					if (peak_0_found) {
+						{
+							// find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/)
+							int16_t real[3] {_fft_outupt_buffer[max_peak_index_0 - 2], _fft_outupt_buffer[max_peak_index_0], _fft_outupt_buffer[max_peak_index_0 + 2]};
+							int16_t imag[3] {_fft_outupt_buffer[max_peak_index_0 - 2 + 1], _fft_outupt_buffer[max_peak_index_0 + 1], _fft_outupt_buffer[max_peak_index_0 + 2 + 1]};
+
+							const int k = 1;
+
+							float divider = (real[k] * real[k] + imag[k] * imag[k]);
+
+							// ap = (X[k + 1].r * X[k].r + X[k+1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
+							float ap = (real[k + 1] * real[k] + imag[k + 1] * imag[k]) / divider;
+
+							// am = (X[k – 1].r * X[k].r + X[k – 1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
+							float am = (real[k - 1] * real[k] + imag[k - 1] * imag[k]) / divider;
+
+							float dp = -ap  / (1.f - ap);
+							float dm = am / (1.f - am);
+							float d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm);
+
+							uint8_t adjustedBinLocation = roundf(max_peak_index_0 + d);
+							float peakFreqAdjusted = (_gyro_sample_rate_hz * adjustedBinLocation / (FFT_LENGTH * 2));
+
+							_sensor_gyro_fft.peak_index_quinns[axis] = adjustedBinLocation;
+							_sensor_gyro_fft.peak_frequency_quinns[axis] = peakFreqAdjusted;
+						}
+
+
+						// find next peak
+						uint32_t max_peak_1 = 0;
+						uint8_t max_peak_index_1 = 0;
+						bool peak_1_found = false;
+
+						for (uint16_t bucket_index = 2; bucket_index < FFT_LENGTH; bucket_index = bucket_index + 2) {
+							if (bucket_index != max_peak_index_0) {
+								const float freq_hz = bucket_index * resolution_hz;
+
+								if (freq_hz > _param_imu_gyro_fft_max.get()) {
+									break;
+								}
+
+								if (freq_hz >= _param_imu_gyro_fft_min.get()) {
+									const int16_t real = _fft_outupt_buffer[bucket_index];
+									const int16_t complex = _fft_outupt_buffer[bucket_index + 1];
+									const uint32_t fft_value_squared = real * real + complex * complex;
+
+									if ((fft_value_squared > MIN_SNR) && (fft_value_squared >= max_peak_1)) {
+										max_peak_index_1 = bucket_index;
+										max_peak_1 = fft_value_squared;
+										peak_1_found = true;
 									}
 								}
 							}
 						}
-					}

-					if (publish) {
-						float *peak_frequencies;
+						if (peak_1_found) {
+							// if 2 peaks found then log them in order
+							_sensor_gyro_fft.peak_index_0[axis] = math::min(max_peak_index_0, max_peak_index_1);
+							_sensor_gyro_fft.peak_index_1[axis] = math::max(max_peak_index_0, max_peak_index_1);
+							_sensor_gyro_fft.peak_frequency_0[axis] = _sensor_gyro_fft.peak_index_0[axis] * resolution_hz;
+							_sensor_gyro_fft.peak_frequency_1[axis] = _sensor_gyro_fft.peak_index_1[axis] * resolution_hz;

-						switch (axis) {
-						case 0:
-							peak_frequencies = _sensor_gyro_fft.peak_frequency_x;
-							break;
-
-						case 1:
-							peak_frequencies = _sensor_gyro_fft.peak_frequency_y;
-							break;
-
-						case 2:
-							peak_frequencies = _sensor_gyro_fft.peak_frequency_z;
-							break;
+						} else {
+							// only 1 peak found
+							_sensor_gyro_fft.peak_index_0[axis] = max_peak_index_0;
+							_sensor_gyro_fft.peak_index_1[axis] = 0;
+							_sensor_gyro_fft.peak_frequency_0[axis] = max_peak_index_0 * resolution_hz;
+							_sensor_gyro_fft.peak_frequency_1[axis] = 0;
 						}

-						int peaks_found = 0;
-
-						for (int i = 0; i < MAX_NUM_PEAKS; i++) {
-							if ((peak_index[i] > 0) && (peak_index[i] < FFT_LENGTH) && (peaks_magnitude[i] > 0)) {
-								const float freq = EstimatePeakFrequency(_fft_outupt_buffer, peak_index[i]);
-
-								if (freq >= _param_imu_gyro_fft_min.get() && freq <= _param_imu_gyro_fft_max.get()) {
-									peak_frequencies[peaks_found] = freq;
-									peaks_found++;
-								}
-							}
-						}
-
-						// mark remaining slots empty
-						for (int i = peaks_found; i < MAX_NUM_PEAKS; i++) {
-							peak_frequencies[i] = NAN;
-						}
-
-						// publish in sorted order for convenience
-						if (peaks_found > 0) {
-							qsort(peak_frequencies, peaks_found, sizeof(float), float_cmp);
-						}
+						publish = true;
 					}

 					// reset
-					// shift buffer (75% overlap)
-					int overlap_start = FFT_LENGTH / 4;
-					memmove(&_gyro_data_buffer[axis][0], &_gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3);
-					buffer_index = overlap_start * 3;
+					buffer_index = 0;
 				}
 			}
 		}
@@ -74,9 +74,6 @@ public:
 	bool init();

 private:
-	static constexpr uint16_t FFT_LENGTH = 2048;
-
-	float EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index);
 	void Run() override;
 	bool SensorSelectionUpdate(bool force = false);
 	void VehicleIMUStatusUpdate();
@@ -98,9 +95,10 @@ private:

 	uint32_t _selected_sensor_device_id{0};

-	arm_rfft_instance_q15 _rfft_q15;
+	static constexpr uint16_t FFT_LENGTH = 1024;
+	arm_rfft_instance_q15 _rfft_q15[3];

-	q15_t _gyro_data_buffer[3][FFT_LENGTH] {};
+	q15_t _data_buffer[3][FFT_LENGTH] {};
 	q15_t _hanning_window[FFT_LENGTH] {};
 	q15_t _fft_input_buffer[FFT_LENGTH] {};
 	q15_t _fft_outupt_buffer[FFT_LENGTH * 2] {};
@@ -31,15 +31,6 @@
 *
 ****************************************************************************/

-/**
-* IMU gyro FFT enable.
-*
-* @boolean
-* @reboot_required true
-* @group Sensors
-*/
-PARAM_DEFINE_INT32(IMU_GYRO_FFT_EN, 0);
-
 /**
 * IMU gyro FFT minimum frequency.
 *
@@ -49,7 +40,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_FFT_EN, 0);
 * @reboot_required true
 * @group Sensors
 */
-PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 50.0f);
+PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 30.0f);

 /**
 * IMU gyro FFT maximum frequency.
@@ -124,7 +124,7 @@ protected:

 	uORB::Publication<vehicle_trajectory_waypoint_s> _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)};	/**< trajectory waypoint desired publication */
 	uORB::Publication<position_controller_status_s> _pub_pos_control_status{ORB_ID(position_controller_status)};	/**< position controller status publication */
-	uORB::Publication<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)};	/**< vehicle command do publication */
+	uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)};	/**< vehicle command do publication */

 	matrix::Vector3f _curr_wp = {}; /**< current position triplet */
 	matrix::Vector3f _position = {}; /**< current vehicle position */
@@ -129,7 +129,7 @@ private:

 	uORB::Publication<collision_constraints_s>	_constraints_pub{ORB_ID(collision_constraints)};		/**< constraints publication */
 	uORB::Publication<obstacle_distance_s>		_obstacle_distance_pub{ORB_ID(obstacle_distance_fused)};	/**< obstacle_distance publication */
-	uORB::Publication<vehicle_command_s>	_vehicle_command_pub{ORB_ID(vehicle_command)};			/**< vehicle command do publication */
+	uORB::PublicationQueued<vehicle_command_s>	_vehicle_command_pub{ORB_ID(vehicle_command)};			/**< vehicle command do publication */

 	uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
 	uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
@@ -66,7 +66,7 @@ private:
 	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3]);
 	void UpdateClipLimit();

-	uORB::PublicationMulti<sensor_accel_s> _sensor_pub;
+	uORB::PublicationQueuedMulti<sensor_accel_s> _sensor_pub;
 	uORB::PublicationMulti<sensor_accel_fifo_s>  _sensor_fifo_pub;

 	uint32_t		_device_id{0};
@@ -64,7 +64,7 @@ public:
 private:
 	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z);

-	uORB::PublicationMulti<sensor_gyro_s> _sensor_pub;
+	uORB::PublicationQueuedMulti<sensor_gyro_s> _sensor_pub;
 	uORB::PublicationMulti<sensor_gyro_fifo_s>  _sensor_fifo_pub;

 	uint32_t		_device_id{0};
@@ -61,7 +61,7 @@ public:
 	int get_class_instance() { return _class_device_instance; };

 private:
-	uORB::PublicationMulti<sensor_mag_s> _sensor_pub;
+	uORB::PublicationQueuedMulti<sensor_mag_s> _sensor_pub;

 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;
@@ -196,7 +196,7 @@ private:

 	uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */

-	uORB::Publication<vehicle_command_ack_s>	_pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_ack_s>	_pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)};

 	int _initTask(FlightTaskIndex task_index);
 };
@@ -256,26 +256,6 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_ext_yaw_handler->deactivate();
 	}

-	// Calculate the current vehicle state and check if it has updated.
-	State previous_state = _current_state;
-	_current_state = _getCurrentState();
-
-	if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) {
-		_updateInternalWaypoints();
-		_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
-		_yaw_lock = false;
-	}
-
-	if (_param_com_obs_avoid.get()
-	    && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-		_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
-				_triplet_next_wp,
-				_sub_triplet_setpoint.get().next.yaw,
-				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN,
-				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
-		_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
-	}
-
 	// set heading
 	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
 		_yaw_setpoint = _yaw;
@@ -313,6 +293,25 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_yawspeed_setpoint = NAN;
 	}

+	// Calculate the current vehicle state and check if it has updated.
+	State previous_state = _current_state;
+	_current_state = _getCurrentState();
+
+	if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) {
+		_updateInternalWaypoints();
+		_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
+	}
+
+	if (_param_com_obs_avoid.get()
+	    && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
+				_triplet_next_wp,
+				_sub_triplet_setpoint.get().next.yaw,
+				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN,
+				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
+		_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
+	}
+
 	return true;
 }

@@ -324,7 +323,7 @@ void FlightTaskAuto::_set_heading_from_mode()
 	switch (_param_mpc_yaw_mode.get()) {

 	case 0: // Heading points towards the current waypoint.
-	case 4: // Same as 0 but yaw first and then go
+	case 4: // Same as 0 but yaw fisrt and then go
 		v = Vector2f(_target) - Vector2f(_position);
 		break;

@@ -353,13 +352,14 @@ void FlightTaskAuto::_set_heading_from_mode()
 		// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
 		// radius, lock yaw to current yaw.
 		// This prevents excessive yawing.
-		if (!_yaw_lock) {
-			if (v.length() < _target_acceptance_radius) {
+		if (v.length() > _target_acceptance_radius) {
+			_compute_heading_from_2D_vector(_yaw_setpoint, v);
+			_yaw_lock = false;
+
+		} else {
+			if (!_yaw_lock) {
 				_yaw_setpoint = _yaw;
 				_yaw_lock = true;
-
-			} else {
-				_compute_heading_from_2D_vector(_yaw_setpoint, v);
 			}
 		}

@@ -72,8 +72,6 @@ bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint

 void FlightTaskAutoLineSmoothVel::reActivate()
 {
-	FlightTaskAutoMapper::reActivate();
-
 	// On ground, reset acceleration and velocity to zero
 	for (int i = 0; i < 2; ++i) {
 		_trajectory[i].reset(0.f, 0.f, _position(i));
@@ -63,7 +63,6 @@ bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_se

 void FlightTaskManualAltitudeSmoothVel::reActivate()
 {
-	FlightTaskManualAltitude::reActivate();
 	// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
 	// using the generated jerk, reset the z derivatives to zero
 	_smoothing.reset(0.f, 0.f, _position(2));
@@ -58,11 +58,11 @@ protected:
 	void _ekfResetHandlerPositionZ() override;
 	void _ekfResetHandlerVelocityZ() override;

-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
-					(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
-					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
-					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
-				       )
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
+		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
+	)

 private:
 	void _updateTrajConstraints();
@@ -66,7 +66,6 @@ bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_se

 void FlightTaskManualPositionSmoothVel::reActivate()
 {
-	FlightTaskManualPosition::reActivate();
 	// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
 	// using the generated jerk, reset the z derivatives to zero
 	_smoothing_xy.reset(Vector2f(), Vector2f(_velocity), Vector2f(_position));
@@ -72,7 +72,7 @@ _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))

 	// Enforce the existence of the test_motor topic, so we won't miss initial publications
 	test_motor_s test{};
-	uORB::Publication<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
+	uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
 	test_motor_pub.publish(test);
 	_motor_test.test_motor_sub.subscribe();
 }
@@ -11,7 +11,7 @@ class MarkdownTablesOutput():
                  "\n")
        for group in groups:
            result += '## %s\n\n' % group.GetName()
-            result += '<table>\n'
+            result += '<table style="width: 100%; table-layout:fixed; font-size:1.5rem; overflow: auto; display:block;">\n'
            result += ' <colgroup><col style="width: 23%"><col style="width: 46%"><col style="width: 11%"><col style="width: 11%"><col style="width: 9%"></colgroup>\n'
            result += ' <thead>\n'
            result += '   <tr><th>Name</th><th>Description</th><th>Min > Max (Incr.)</th><th>Default</th><th>Units</th></tr>\n'
@@ -352,7 +352,7 @@ class SourceParser(object):
                                'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s',  'rad s^2/m','rad s/m',
                                'bit/s', 'B/s',
                                'deg', 'deg*1e7', 'deg/s',
-                                'celcius', 'gauss', 'gauss/s', 'gauss^2',
+                                'celcius', 'gauss', 'gauss/s', 'mgauss', 'mgauss^2',
                                'hPa', 'kg', 'kg/m^2', 'kg m^2',
                                'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
                                'Ohm', 'V',
@@ -86,7 +86,7 @@ static void arm_auth_request_msg_send()
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
 	vcmd.target_system = arm_parameters.struct_value.authorizer_system_id;

-	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
 	vcmd_pub.publish(vcmd);
 }

@@ -40,6 +40,7 @@
 */

 #include "HealthFlags.h"
+#include <uORB/topics/subsystem_info.h>

 void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
 {
@@ -43,40 +43,7 @@

 #include <px4_platform_common/log.h>
 #include <uORB/topics/vehicle_status.h>
-
-struct subsystem_info_s {
-	// keep in sync with mavlink MAV_SYS_STATUS_SENSOR
-	static constexpr uint64_t SUBSYSTEM_TYPE_GYRO = 1 << 0;
-	static constexpr uint64_t SUBSYSTEM_TYPE_ACC = 1 << 1;
-	static constexpr uint64_t SUBSYSTEM_TYPE_MAG = 1 << 2;
-	static constexpr uint64_t SUBSYSTEM_TYPE_ABSPRESSURE = 1 << 3;
-	static constexpr uint64_t SUBSYSTEM_TYPE_DIFFPRESSURE = 1 << 4;
-	static constexpr uint64_t SUBSYSTEM_TYPE_GPS = 1 << 5;
-	static constexpr uint64_t SUBSYSTEM_TYPE_OPTICALFLOW = 1 << 6;
-	static constexpr uint64_t SUBSYSTEM_TYPE_CVPOSITION = 1 << 7;
-	static constexpr uint64_t SUBSYSTEM_TYPE_LASERPOSITION = 1 << 8;
-	static constexpr uint64_t SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 1 << 9;
-	static constexpr uint64_t SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1 << 10;
-	static constexpr uint64_t SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 1 << 11;
-	static constexpr uint64_t SUBSYSTEM_TYPE_YAWPOSITION = 1 << 12;
-	static constexpr uint64_t SUBSYSTEM_TYPE_ALTITUDECONTROL = 1 << 13;
-	static constexpr uint64_t SUBSYSTEM_TYPE_POSITIONCONTROL = 1 << 14;
-	static constexpr uint64_t SUBSYSTEM_TYPE_MOTORCONTROL = 1 << 15;
-	static constexpr uint64_t SUBSYSTEM_TYPE_RCRECEIVER = 1 << 16;
-	static constexpr uint64_t SUBSYSTEM_TYPE_GYRO2 = 1 << 17;
-	static constexpr uint64_t SUBSYSTEM_TYPE_ACC2 = 1 << 18;
-	static constexpr uint64_t SUBSYSTEM_TYPE_MAG2 = 1 << 19;
-	static constexpr uint64_t SUBSYSTEM_TYPE_GEOFENCE = 1 << 20;
-	static constexpr uint64_t SUBSYSTEM_TYPE_AHRS = 1 << 21;
-	static constexpr uint64_t SUBSYSTEM_TYPE_TERRAIN = 1 << 22;
-	static constexpr uint64_t SUBSYSTEM_TYPE_REVERSEMOTOR = 1 << 23;
-	static constexpr uint64_t SUBSYSTEM_TYPE_LOGGING = 1 << 24;
-	static constexpr uint64_t SUBSYSTEM_TYPE_SENSORBATTERY = 1 << 25;
-	static constexpr uint64_t SUBSYSTEM_TYPE_SENSORPROXIMITY = 1 << 26;
-	static constexpr uint64_t SUBSYSTEM_TYPE_SATCOM = 1 << 27;
-	static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28;
-	static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29;
-};
+#include <uORB/topics/subsystem_info.h>

 void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
 void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status);
@@ -42,6 +42,7 @@
 #include <lib/parameters/param.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/subsystem_info.h>

 using namespace time_literals;

@@ -58,15 +59,29 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 				    vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
 				    const hrt_abstime &time_since_boot)
 {
+	const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
+
 	reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
 			  && !status_flags.condition_calibration_enabled);

+	const bool checkSensors = !hil_enabled;
+	const bool checkDynamic = !hil_enabled;
+
+	bool checkAirspeed = false;
+
+	/* Perform airspeed check only if circuit breaker is not
+	 * engaged and it's not a rotary wing */
+	if (!status_flags.circuit_breaker_engaged_airspd_check &&
+	    (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
+		checkAirspeed = true;
+	}
+
 	bool failed = false;

 	failed = failed || !airframeCheck(mavlink_log_pub, status);

 	/* ---- MAG ---- */
-	{
+	if (checkSensors) {
 		int32_t sys_has_mag = 1;
 		param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);

@@ -96,7 +111,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	}

 	/* ---- ACCEL ---- */
-	{
+	if (checkSensors) {
 		/* check all sensors individually, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_accel_count; i++) {
 			const bool required = (i < max_mandatory_accel_count);
@@ -104,7 +119,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu

 			int32_t device_id = -1;

-			if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+			if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
 				if (required) {
 					failed = true;
 				}
@@ -115,7 +130,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	}

 	/* ---- GYRO ---- */
-	{
+	if (checkSensors) {
 		/* check all sensors individually, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
 			const bool required = (i < max_mandatory_gyro_count);
@@ -132,7 +147,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	}

 	/* ---- BARO ---- */
-	{
+	if (checkSensors) {
 		int32_t sys_has_baro = 1;
 		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);

@@ -155,17 +170,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu

 	/* ---- IMU CONSISTENCY ---- */
 	// To be performed after the individual sensor checks have completed
-	{
+	if (checkSensors) {
 		if (!imuConsistencyCheck(mavlink_log_pub, status, reportFailures)) {
 			failed = true;
 		}
 	}

 	/* ---- AIRSPEED ---- */
-	/* Perform airspeed check only if circuit breaker is not engaged and it's not a rotary wing */
-	if (!status_flags.circuit_breaker_engaged_airspd_check &&
-	    (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
-
+	if (checkAirspeed) {
 		int32_t optional = 0;
 		param_get(param_find("FW_ARSP_MODE"), &optional);

@@ -97,7 +97,7 @@ private:
 				      const bool optional, int32_t &device_id, const bool report_fail);
 	static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
 	static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-				       const bool optional, int32_t &device_id, const bool report_fail);
+				       const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail);
 	static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 			      const bool optional, int32_t &device_id, const bool report_fail);
 	static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
@@ -41,11 +41,12 @@
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/subsystem_info.h>

 using namespace time_literals;

 bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-					const bool optional, int32_t &device_id, const bool report_fail)
+					const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail)
 {
 	const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
 	bool calibration_valid = false;
@@ -73,17 +74,20 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s
 			}

 		} else {
-			const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
-							    + accel.get().y * accel.get().y
-							    + accel.get().z * accel.get().z);

-			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-				if (report_fail) {
-					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
+			if (dynamic) {
+				const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
+								    + accel.get().y * accel.get().y
+								    + accel.get().z * accel.get().z);
+
+				if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+					if (report_fail) {
+						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
+					}
+
+					/* this is frickin' fatal */
+					valid = false;
 				}
-
-				// this is fatal
-				valid = false;
 			}
 		}

@@ -39,6 +39,7 @@
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/subsystem_info.h>

 using namespace time_literals;

@@ -39,6 +39,7 @@
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/subsystem_info.h>

 using namespace time_literals;

@@ -40,6 +40,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/subsystem_info.h>

 bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
 			       const bool report_fail, const bool enforce_gps_required)
@@ -40,6 +40,7 @@
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/subsystem_info.h>

 using namespace time_literals;

@@ -39,6 +39,7 @@
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensors_status_imu.h>
+#include <uORB/topics/subsystem_info.h>

 bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
 		const bool report_status)
@@ -39,6 +39,7 @@
 #include <mathlib/mathlib.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/sensor_preflight_mag.h>

 // return false if the magnetomer measurements are inconsistent
@@ -40,6 +40,7 @@
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/subsystem_info.h>

 using namespace time_literals;

@@ -115,7 +115,7 @@ static struct vehicle_status_flags_s status_flags = {};
 void *commander_low_prio_loop(void *arg);

 static void answer_command(const vehicle_command_s &cmd, unsigned result,
-			   uORB::Publication<vehicle_command_ack_s> &command_ack_pub);
+			   uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);

 #if defined(BOARD_HAS_POWER_CONTROL)
 static orb_advert_t power_button_state_pub = nullptr;
@@ -184,7 +184,7 @@ static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2

 	vcmd.timestamp = hrt_absolute_time();

-	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};

 	return vcmd_pub.publish(vcmd);
 }
@@ -532,7 +532,7 @@ Commander::Commander() :

 bool
 Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local,
-			  uORB::Publication<vehicle_command_ack_s> &command_ack_pub)
+			  uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub)
 {
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id)
@@ -1722,6 +1722,14 @@ Commander::run()

 		battery_status_check();

+		/* update subsystem info which arrives from outside of commander*/
+		subsystem_info_s info;
+
+		while (_subsys_sub.update(&info))  {
+			set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
+			_status_changed = true;
+		}
+
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {

@@ -3377,7 +3385,7 @@ Commander::print_reject_arm(const char *msg)
 }

 void answer_command(const vehicle_command_s &cmd, unsigned result,
-		    uORB::Publication<vehicle_command_ack_s> &command_ack_pub)
+		    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub)
 {
 	switch (result) {
 	case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
@@ -3425,7 +3433,7 @@ void *commander_low_prio_loop(void *arg)
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));

 	/* command ack */
-	uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};

 	/* wakeup source(s) */
 	px4_pollfd_struct_t fds[1];
@@ -3795,12 +3803,6 @@ void Commander::data_link_check()
 								_status_changed = true;
 							}
 						}
-
-						const bool present = true;
-						const bool enabled = true;
-						const bool ok = (iridium_status.last_heartbeat > 0); // maybe at some point here an additional check should be made
-
-						set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SATCOM, present, enabled, ok, status);
 					}

 					break;
@@ -73,6 +73,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/power_button_state.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_acceleration.h>
@@ -140,7 +141,7 @@ private:
 	void estimator_check(const vehicle_status_flags_s &status_flags);

 	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
-			    uORB::Publication<vehicle_command_ack_s> &command_ack_pub);
+			    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);

 	unsigned handle_command_motor_test(const vehicle_command_s &cmd);

@@ -390,6 +391,7 @@ private:
 	uORB::Subscription					_parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription					_safety_sub{ORB_ID(safety)};
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription					_subsys_sub{ORB_ID(subsystem_info)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
@@ -420,7 +422,7 @@ private:

 	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};

-	uORB::Publication<vehicle_command_ack_s>		_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_ack_s>		_command_ack_pub{ORB_ID(vehicle_command_ack)};

 };

@@ -614,7 +614,7 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &ca
 				command_ack.target_component = cmd.source_component;
 				command_ack.timestamp = hrt_absolute_time();

-				uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
+				uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
 				command_ack_pub.publish(command_ack);

 				return ret;
@@ -159,7 +159,6 @@ EKF2::EKF2(bool replay_mode):
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();
 	_estimator_innovations_pub.advertise();
-	_estimator_optical_flow_vel_pub.advertise();
 	_estimator_sensor_bias_pub.advertise();
 	_estimator_states_pub.advertise();
 	_estimator_status_pub.advertise();
@@ -490,7 +489,6 @@ void EKF2::Run()
 		}

 		if (_optical_flow_sub.updated()) {
-			_new_optical_flow_data_received = true;
 			optical_flow_s optical_flow;

 			if (_optical_flow_sub.copy(&optical_flow)) {
@@ -1044,11 +1042,6 @@ void EKF2::Run()

 			publish_yaw_estimator_status(now);

-			if (_new_optical_flow_data_received) {
-				publish_estimator_optical_flow_vel(now);
-				_new_optical_flow_data_received = false;
-			}
-
 			if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) {
 				_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
 			}
@@ -1340,21 +1333,6 @@ void EKF2::publish_wind_estimate(const hrt_abstime &timestamp)
 	}
 }

-void EKF2::publish_estimator_optical_flow_vel(const hrt_abstime &timestamp)
-{
-	estimator_optical_flow_vel_s flow_vel{};
-	flow_vel.timestamp_sample = timestamp;
-
-	_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
-	_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
-	_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
-	_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
-	_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
-	flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-
-	_estimator_optical_flow_vel_pub.publish(flow_vel);
-}
-
 float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 {
 	float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
@@ -81,7 +81,6 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/yaw_estimator_status.h>
-#include <uORB/topics/estimator_optical_flow_vel.h>

 #include "Utility/PreFlightChecker.hpp"

@@ -126,7 +125,6 @@ private:
 	void publish_odometry(const hrt_abstime &timestamp, const imuSample &imu, const vehicle_local_position_s &lpos);
 	void publish_wind_estimate(const hrt_abstime &timestamp);
 	void publish_yaw_estimator_status(const hrt_abstime &timestamp);
-	void publish_estimator_optical_flow_vel(const hrt_abstime &timestamp);

 	/*
 	 * Calculate filtered WGS84 height from estimated AMSL height
@@ -151,9 +149,9 @@ private:
 	hrt_abstime _last_magcal_us = 0;	///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
 	hrt_abstime _total_cal_time_us = 0;	///< accumulated calibration time since the last save

-	float _last_valid_mag_cal[3] = {};	///< last valid XYZ magnetometer bias estimates (Gauss)
+	float _last_valid_mag_cal[3] = {};	///< last valid XYZ magnetometer bias estimates (mGauss)
 	bool _valid_cal_available[3] = {};	///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
-	float _last_valid_variance[3] = {};	///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2)
+	float _last_valid_variance[3] = {};	///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2)

 	// Used to control saving of mag declination to be used on next startup
 	bool _mag_decl_saved = false;	///< true when the magnetic declination has been saved
@@ -171,8 +169,6 @@ private:
 	bool new_ev_data_received = false;
 	vehicle_odometry_s _ev_odom{};

-	bool _new_optical_flow_data_received{false};
-
 	uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
 	uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
@@ -205,7 +201,6 @@ private:
 	uORB::Publication<estimator_innovations_s>		_estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovations_pub{ORB_ID(estimator_innovations)};
-	uORB::Publication<estimator_optical_flow_vel_s>		_estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
 	uORB::Publication<estimator_sensor_bias_s>		_estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
 	uORB::Publication<estimator_states_s>			_estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::PublicationData<estimator_status_s>		_estimator_status_pub{ORB_ID(estimator_status)};
@@ -414,13 +409,13 @@ private:
 		_param_ekf2_angerr_init,	///< 1-sigma tilt error after initial alignment using gravity vector (rad)

 		// EKF saved XYZ magnetometer bias values
-		(ParamFloat<px4::params::EKF2_MAGBIAS_X>) _param_ekf2_magbias_x,		///< X magnetometer bias (Gauss)
-		(ParamFloat<px4::params::EKF2_MAGBIAS_Y>) _param_ekf2_magbias_y,		///< Y magnetometer bias (Gauss)
-		(ParamFloat<px4::params::EKF2_MAGBIAS_Z>) _param_ekf2_magbias_z,		///< Z magnetometer bias (Gauss)
+		(ParamFloat<px4::params::EKF2_MAGBIAS_X>) _param_ekf2_magbias_x,		///< X magnetometer bias (mGauss)
+		(ParamFloat<px4::params::EKF2_MAGBIAS_Y>) _param_ekf2_magbias_y,		///< Y magnetometer bias (mGauss)
+		(ParamFloat<px4::params::EKF2_MAGBIAS_Z>) _param_ekf2_magbias_z,		///< Z magnetometer bias (mGauss)
 		(ParamInt<px4::params::EKF2_MAGBIAS_ID>)
 		_param_ekf2_magbias_id,		///< ID of the magnetometer sensor used to learn the bias values
 		(ParamFloat<px4::params::EKF2_MAGB_VREF>)
-		_param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (Gauss**2)
+		_param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2)
 		(ParamFloat<px4::params::EKF2_MAGB_K>)
 		_param_ekf2_magb_k,	///< maximum fraction of the learned magnetometer bias that is saved at each disarm

@@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 110);
 * @reboot_required true
 * @decimal 1
 */
-PARAM_DEFINE_FLOAT(EKF2_OF_DELAY, 20);
+PARAM_DEFINE_FLOAT(EKF2_OF_DELAY, 5);

 /**
 * Range finder measurement delay relative to IMU measurements
@@ -1072,7 +1072,7 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f);
 * @reboot_required true
 * @volatile
 * @category system
- * @unit gauss
+ * @unit mgauss
 * @decimal 3
 */
 PARAM_DEFINE_FLOAT(EKF2_MAGBIAS_X, 0.0f);
@@ -1087,7 +1087,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAGBIAS_X, 0.0f);
 * @reboot_required true
 * @volatile
 * @category system
- * @unit gauss
+ * @unit mgauss
 * @decimal 3
 */
 PARAM_DEFINE_FLOAT(EKF2_MAGBIAS_Y, 0.0f);
@@ -1102,7 +1102,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAGBIAS_Y, 0.0f);
 * @reboot_required true
 * @volatile
 * @category system
- * @unit gauss
+ * @unit mgauss
 * @decimal 3
 */
 PARAM_DEFINE_FLOAT(EKF2_MAGBIAS_Z, 0.0f);
@@ -1122,7 +1122,7 @@ PARAM_DEFINE_INT32(EKF2_MAGBIAS_ID, 0);
 *
 * @group EKF2
 * @reboot_required true
- * @unit gauss^2
+ * @unit mgauss^2
 * @decimal 8
 */
 PARAM_DEFINE_FLOAT(EKF2_MAGB_VREF, 2.5E-7f);
@@ -65,7 +65,7 @@ private:
 	/** Publish tune control to interrupt any sound */
 	void stop_tune();

-	uORB::Publication<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
+	uORB::PublicationQueued<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

 	bool 		_was_armed = false;
@@ -126,7 +126,7 @@ void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
 	command_ack.target_system = cmd.source_system;
 	command_ack.target_component = cmd.source_component;

-	uORB::Publication<vehicle_command_ack_s>	command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_ack_s>	command_ack_pub{ORB_ID(vehicle_command_ack)};
 	command_ack_pub.publish(command_ack);
 }

@@ -97,7 +97,7 @@ private:
 	int _old_nav_state = -1;
 	int _old_battery_status_warning = -1;

-	uORB::Publication<led_control_s> _led_control_pub{ORB_ID(led_control)};
+	uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};

 };

@@ -87,7 +87,7 @@ private:

 	int _stack_task_index{0};

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

@@ -77,7 +77,7 @@ private:
 	int publish_message();

 	ulog_stream_s _ulog_stream_data{};
-	uORB::Publication<ulog_stream_s> _ulog_stream_pub{ORB_ID(ulog_stream)};
+	uORB::PublicationQueued<ulog_stream_s> _ulog_stream_pub{ORB_ID(ulog_stream)};
 	int _ulog_stream_ack_sub{-1};
 	bool _need_reliable_transfer{false};
 	bool _is_started{false};
@@ -45,7 +45,7 @@ using namespace px4::logger;
 void LoggedTopics::add_default_topics()
 {
 	add_topic("actuator_armed");
-	add_topic("actuator_controls_0", 50);
+	add_topic("actuator_controls_0", 100);
 	add_topic("actuator_controls_1", 100);
 	add_topic("airspeed", 1000);
 	add_topic("airspeed_validated", 200);
@@ -60,7 +60,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_innovation_test_ratios", 200);
 	add_topic("estimator_innovation_variances", 200);
 	add_topic("estimator_innovations", 200);
-	add_topic("estimator_optical_flow_vel", 200);
 	add_topic("estimator_sensor_bias", 1000);
 	add_topic("estimator_states", 1000);
 	add_topic("estimator_status", 200);
@@ -94,9 +93,8 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_air_data", 200);
 	add_topic("vehicle_angular_velocity", 20);
 	add_topic("vehicle_attitude", 50);
-	add_topic("vehicle_attitude_setpoint", 50);
+	add_topic("vehicle_attitude_setpoint", 100);
 	add_topic("vehicle_command");
-	add_topic("vehicle_constraints", 1000);
 	add_topic("vehicle_control_mode");
 	add_topic("vehicle_global_position", 200);
 	add_topic("vehicle_gps_position", 500);
@@ -1971,7 +1971,7 @@ void Logger::ack_vehicle_command(vehicle_command_s *cmd, uint32_t result)
 	vehicle_command_ack.target_system = cmd->source_system;
 	vehicle_command_ack.target_component = cmd->source_component;

-	uORB::Publication<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
 	cmd_ack_pub.publish(vehicle_command_ack);
 }

--- a/Show More
+++ b/Show More