rate controller failure detector at takeoff

2026-05-31 14:30:04 +08:00 · 2020-12-13 20:45:48 -05:00
79 changed files with 1268 additions and 1297 deletions
@@ -15,7 +15,8 @@ jobs:
      matrix:
        config: [
          px4_fmu-v5_default,
-          tests, # includes px4_sitl
+          px4_sitl_default,
+          #tests, # includes px4_sitl
          ]
    steps:
    - uses: actions/checkout@v1
@@ -1,20 +1,20 @@
 # PX4 Drone Autopilot

-[![Releases](https://img.shields.io/github/release/PX4/PX4-Autopilot.svg)](https://github.com/PX4/PX4-Autopilot/releases) [![DOI](https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot)
+[![Releases](https://img.shields.io/github/release/PX4/Firmware.svg)](https://github.com/PX4/Firmware/releases) [![DOI](https://zenodo.org/badge/22634/PX4/Firmware.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware)

-[![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)
+[![Nuttx Targets](https://github.com/PX4/Firmware/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/Firmware/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/Firmware/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/Firmware/actions?query=workflow%3A%22SITL+Tests%22)

 [![Slack](https://px4-slack.herokuapp.com/badge.svg)](http://slack.px4.io)

-This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/master/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.
+This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/Firmware/tree/master/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.

-* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/master/LICENSE))
+* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/Firmware/blob/master/LICENSE))
 * [Supported airframes](https://docs.px4.io/master/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)):
  * [Multicopters](https://docs.px4.io/master/en/airframes/airframe_reference.html#copter)
  * [Fixed wing](https://docs.px4.io/master/en/airframes/airframe_reference.html#plane)
  * [VTOL](https://docs.px4.io/master/en/airframes/airframe_reference.html#vtol)
  * many more experimental types (Rovers, Blimps, Boats, Submarines, Autogyros, etc)
-* Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases)
+* Releases: [Downloads](https://github.com/PX4/Firmware/releases)


 ## PX4 Users
@@ -44,20 +44,20 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/master/en/co
    * [Lorenz Meier](https://github.com/LorenzMeier)
  * Architecture
    * [Daniel Agar](https://github.com/dagar)
-  * [Dev Call](https://github.com/PX4/PX4-Autopilot/labels/devcall)
+  * [Dev Call](https://github.com/PX4/Firmware/labels/devcall)
    * [Ramon Roche](https://github.com/mrpollo)
  * Communication Architecture
    * [Beat Kueng](https://github.com/bkueng)
    * [Julian Oes](https://github.com/JulianOes)
  * UI in QGroundControl
    * [Gus Grubba](https://github.com/dogmaphobic)
-  * [Multicopter Flight Control](https://github.com/PX4/PX4-Autopilot/labels/multicopter)
+  * [Multicopter Flight Control](https://github.com/PX4/Firmware/labels/multicopter)
    * [Mathieu Bresciani](https://github.com/bresch)
-  * [Multicopter Software Architecture](https://github.com/PX4/PX4-Autopilot/labels/multicopter)
+  * [Multicopter Software Architecture](https://github.com/PX4/Firmware/labels/multicopter)
    * [Matthias Grob](https://github.com/MaEtUgR)
-  * [VTOL Flight Control](https://github.com/PX4/PX4-Autopilot/labels/vtol)
+  * [VTOL Flight Control](https://github.com/PX4/Firmware/labels/vtol)
    * [Roman Bapst](https://github.com/RomanBapst)
-  * [Fixed Wing Flight Control](https://github.com/PX4/PX4-Autopilot/labels/fixedwing)
+  * [Fixed Wing Flight Control](https://github.com/PX4/Firmware/labels/fixedwing)
    * [Roman Bapst](https://github.com/RomanBapst)
  * OS / NuttX
    * [David Sidrane](https://github.com/davids5)
@@ -65,9 +65,9 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/master/en/co
    * [Daniel Agar](https://github.com/dagar)
  * Commander Architecture
    * [Julian Oes](https://github.com/julianoes)
-  * [UAVCAN](https://github.com/PX4/PX4-Autopilot/labels/uavcan)
+  * [UAVCAN](https://github.com/PX4/Firmware/labels/uavcan)
    * [Daniel Agar](https://github.com/dagar)
-  * [State Estimation](https://github.com/PX4/PX4-Autopilot/issues?q=is%3Aopen+is%3Aissue+label%3A%22state+estimation%22)
+  * [State Estimation](https://github.com/PX4/Firmware/issues?q=is%3Aopen+is%3Aissue+label%3A%22state+estimation%22)
    * [Paul Riseborough](https://github.com/priseborough)
  * Vision based navigation
    * [Julian Kent](https://github.com/jkflying)
@@ -76,7 +76,7 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/master/en/co
  * RTPS/ROS2 Interface
    * [Nuno Marques](https://github.com/TSC21)

-See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github).
+See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/Firmware/graphs/contributors) (Github).

 ## Supported Hardware

@@ -9,33 +9,40 @@

 if [ $AUTOCNF = yes ]
 then
+	param set FW_ARSP_MODE 1
+	param set FW_AIRSPD_MAX 25
+	param set FW_AIRSPD_MIN 14
+	param set FW_AIRSPD_TRIM 16
 	param set FW_L1_PERIOD 12
-	param set FW_MAN_P_MAX 30
-	param set FW_PR_FF 0.2
-	param set FW_PR_I 0.4
-	param set FW_PR_P 0.9
-	param set FW_PSP_OFF 3
-	param set FW_RR_FF 0.1
-	param set FW_RR_P 0.3

-	param set MC_YAW_P 1.6
+	param set MC_ROLLRATE_P 0.3

+	param set MIS_LTRMIN_ALT 10
 	param set MIS_TAKEOFF_ALT 10
+	param set MIS_YAW_TMT 10

 	param set MPC_ACC_HOR_MAX 2
-	param set MPC_XY_P 0.8
-	param set MPC_XY_VEL_P_ACC 3
-	param set MPC_XY_VEL_I_ACC 4
+	param set MPC_ACC_HOR_MAX 2
+	param set MPC_THR_MIN 0.1
+	param set MPC_TKO_SPEED 1
+	param set MPC_XY_P 0.15
 	param set MPC_XY_VEL_D_ACC 0.1
+	param set MPC_XY_VEL_I_ACC 4
+	param set MPC_XY_VEL_P_ACC 1
+	param set MPC_Z_VEL_MAX_DN 1.5
+	param set MPC_Z_VEL_P_ACC 16

-	param set NAV_LOITER_RAD 100
+	param set NAV_ACC_RAD 5
+	param set NAV_LOITER_RAD 80

-	param set VT_B_TRANS_DUR 8
-	param set VT_ELEV_MC_LOCK 0
-	param set VT_FWD_THRUST_EN 4
-	param set VT_MOT_ID 1234
+	param set VT_F_TRANS_DUR 5.0
+	param set VT_F_TRANS_THR 0.75
+	param set VT_FWD_THRUST_SC 1.1
+	param set VT_TILT_FW 1
 	param set VT_TILT_TRANS 0.6
+	param set VT_ELEV_MC_LOCK 0
 	param set VT_TYPE 1
+	param set VT_B_TRANS_DUR 8

 fi

@@ -11,7 +11,6 @@ pandas>=0.21
 pkgconfig
 psutil
 pygments
-wheel>=0.31.1
 pymavlink
 pyros-genmsg
 pyserial>=3.0
@@ -21,3 +20,4 @@ requests
 setuptools>=39.2.0
 six>=1.12.0
 toml>=0.9
+wheel>=0.31.1
@@ -1,5 +1,7 @@
 uint64 timestamp	# time since system start (microseconds)

+uint64 armed_time	# Arming timestamp
+
 bool armed		# Set to true if system is armed
 bool prearmed		# Set to true if the actuator safety is disabled but motors are not armed
 bool ready_to_arm	# Set to true if system is ready to be armed
@@ -5,3 +5,6 @@ float32 rollspeed_integ
 float32 pitchspeed_integ
 float32 yawspeed_integ
 float32 additional_integ1	# FW: wheel rate integrator (optional)
+
+float32[3] error                # rate error [radians/s]
+float32[3] error_integrated     # rate error integrated [radians]
@@ -3,11 +3,11 @@ uint64 timestamp_sample

 uint32 device_id          # unique device ID for the sensor that does not change between power cycles

-float32 x                 # acceleration in the FRD board frame X-axis in m/s^2
-float32 y                 # acceleration in the FRD board frame Y-axis in m/s^2
-float32 z                 # acceleration in the FRD board frame Z-axis in m/s^2
+float32 x                 # acceleration in the NED X board axis in m/s^2
+float32 y                 # acceleration in the NED Y board axis in m/s^2
+float32 z                 # acceleration in the NED Z board axis in m/s^2

-float32 temperature       # temperature in degrees Celsius
+float32 temperature       # temperature in degrees celsius

 uint32 error_count

@@ -8,8 +8,8 @@ float32 scale

 uint8 samples             # number of valid samples

-int16[32] x               # acceleration in the FRD board frame X-axis in m/s^2
-int16[32] y               # acceleration in the FRD board frame Y-axis in m/s^2
-int16[32] z               # acceleration in the FRD board frame Z-axis in m/s^2
+int16[32] x               # acceleration in the NED X board axis in m/s/s
+int16[32] y               # acceleration in the NED Y board axis in m/s/s
+int16[32] z               # acceleration in the NED Z board axis in m/s/s

 uint8 rotation            # Direction the sensor faces (see Rotation enum)
@@ -7,4 +7,4 @@ uint32 error_count

 float32 pressure	# static pressure measurement in millibar

-float32 temperature	# static temperature measurement in deg Celsius
+float32 temperature	# static temperature measurement in deg C
@@ -7,12 +7,12 @@ uint64 timestamp				# time since system start (microseconds)
 int32 RELATIVE_TIMESTAMP_INVALID = 2147483647	# (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid

 # gyro timstamp is equal to the timestamp of the message
-float32[3] gyro_rad			# average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
-uint32 gyro_integral_dt		# gyro measurement sampling period in microseconds
+float32[3] gyro_rad			# average angular rate measured in the XYZ body frame in rad/s over the last gyro sampling period
+uint32 gyro_integral_dt		# gyro measurement sampling period in us

 int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
-float32[3] accelerometer_m_s2		# average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
-uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in microseconds
+float32[3] accelerometer_m_s2		# average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period
+uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in us

 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
@@ -15,15 +15,15 @@ float32[3] gyro_offset_3	# gyro 3 XYZ offsets in the sensor frame in rad/s
 # Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
 uint32[4] accel_device_ids
-float32[3] accel_offset_0	# accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_1	# accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_2	# accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_3	# accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] accel_offset_0	# accelerometer 0 XYZ offsets in the sensor frame in m/s/s
+float32[3] accel_offset_1	# accelerometer 1 XYZ offsets in the sensor frame in m/s/s
+float32[3] accel_offset_2	# accelerometer 2 XYZ offsets in the sensor frame in m/s/s
+float32[3] accel_offset_3	# accelerometer 3 XYZ offsets in the sensor frame in m/s/s

 # Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
 uint32[4] baro_device_ids
-float32 baro_offset_0		# barometric pressure 0 offsets in the sensor frame in Pascals
-float32 baro_offset_1		# barometric pressure 1 offsets in the sensor frame in Pascals
-float32 baro_offset_2		# barometric pressure 2 offsets in the sensor frame in Pascals
-float32 baro_offset_3		# barometric pressure 3 offsets in the sensor frame in Pascals
+float32 baro_offset_0		# barometric pressure 0 offsets in the sensor frame in m/s/s
+float32 baro_offset_1		# barometric pressure 1 offsets in the sensor frame in m/s/s
+float32 baro_offset_2		# barometric pressure 2 offsets in the sensor frame in m/s/s
+float32 baro_offset_3		# barometric pressure 3 offsets in the sensor frame in m/s/s
@@ -3,11 +3,11 @@ uint64 timestamp_sample

 uint32 device_id          # unique device ID for the sensor that does not change between power cycles

-float32 x                 # angular velocity in the FRD board frame X-axis in rad/s
-float32 y                 # angular velocity in the FRD board frame Y-axis in rad/s
-float32 z                 # angular velocity in the FRD board frame Z-axis in rad/s
+float32 x                 # angular velocity in the NED X board axis in rad/s
+float32 y                 # angular velocity in the NED Y board axis in rad/s
+float32 z                 # angular velocity in the NED Z board axis in rad/s

-float32 temperature       # temperature in degrees Celsius
+float32 temperature       # temperature in degrees celsius

 uint32 error_count

@@ -8,8 +8,8 @@ float32 scale

 uint8 samples             # number of valid samples

-int16[32] x               # angular velocity in the FRD board frame X-axis in rad/s
-int16[32] y               # angular velocity in the FRD board frame Y-axis in rad/s
-int16[32] z               # angular velocity in the FRD board frame Z-axis in rad/s
+int16[32] x               # angular velocity in the NED X board axis in rad/s
+int16[32] y               # angular velocity in the NED Y board axis in rad/s
+int16[32] z               # angular velocity in the NED Z board axis in rad/s

 uint8 rotation            # Direction the sensor faces (see Rotation enum)
@@ -3,11 +3,11 @@ uint64 timestamp_sample

 uint32 device_id          # unique device ID for the sensor that does not change between power cycles

-float32 x                 # magnetic field in the FRD board frame X-axis in Gauss
-float32 y                 # magnetic field in the FRD board frame Y-axis in Gauss
-float32 z                 # magnetic field in the FRD board frame Z-axis in Gauss
+float32 x                 # magnetic field in the NED X board axis in Gauss
+float32 y                 # magnetic field in the NED Y board axis in Gauss
+float32 z                 # magnetic field in the NED Z board axis in Gauss

-float32 temperature       # temperature in degrees Celsius
+float32 temperature       # temperature in degrees celsius

 uint32 error_count

@@ -6,7 +6,7 @@ uint64 timestamp # time since system start (microseconds)
 uint32 accel_device_id_primary       # current primary accel device id for reference

 uint32[4] accel_device_ids
-float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in m/s^2.
+float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in (m/s/s).
 bool[4] accel_healthy


@@ -7,7 +7,7 @@ uint8 brick_valid		# brick bits power is good when bit 1
 uint8 usb_valid 		# USB is valid when 1
 uint8 servo_valid		# servo power is good when 1
 uint8 periph_5v_oc		# peripheral overcurrent when 1
-uint8 hipower_5v_oc		# high power peripheral overcurrent when 1
+uint8 hipower_5v_oc		# hi power peripheral overcurrent when 1

 uint8 BRICK1_VALID_SHIFTS=0
 uint8 BRICK1_VALID_MASK=1
@@ -3,4 +3,4 @@ uint64 timestamp		# time since system start (microseconds)

 uint64 timestamp_sample		# the timestamp of the raw data (microseconds)

-float32[3] xyz			# Bias corrected acceleration (including gravity) in the FRD body frame XYZ-axis in m/s^2
+float32[3] xyz			# Bias corrected acceleration (including gravity) in body axis (in m/s^2)
@@ -6,7 +6,7 @@ uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
 uint32 baro_device_id       # unique device ID for the selected barometer

 float32 baro_alt_meter			# Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH.
-float32 baro_temp_celcius		# Temperature in degrees Celsius
-float32 baro_pressure_pa		# Absolute pressure in Pascals
+float32 baro_temp_celcius		# Temperature in degrees celsius
+float32 baro_pressure_pa		# Absolute pressure in pascals

 float32 rho				# air density
@@ -2,4 +2,4 @@
 uint64 timestamp        # time since system start (microseconds)
 uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)

-float32[3] xyz          # angular acceleration about the FRD body frame XYZ-axis in rad/s^2
+float32[3] xyz          # angular acceleration about X, Y, Z body axis in rad/s^2,
@@ -2,6 +2,6 @@
 uint64 timestamp        # time since system start (microseconds)
 uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)

-float32[3] xyz		# Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s
+float32[3] xyz		# Bias corrected angular velocity about X, Y, Z body axis in rad/s

 # TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth
@@ -1,10 +1,10 @@
 # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use

-uint64 timestamp		    # time since system start (microseconds)
+uint64 timestamp		# time since system start (microseconds)

-uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)

-float32[4] q			    # Quaternion rotation from the FRD body frame to the NED earth frame
+float32[4] q			# Quaternion rotation from XYZ body frame to NED earth frame.
 float32[4] delta_q_reset 	# Amount by which quaternion has changed during last reset
 uint8 quat_reset_counter	# Quaternion reset counter

@@ -6,10 +6,10 @@ uint64 timestamp_sample
 uint32 accel_device_id          # Accelerometer unique device ID for the sensor that does not change between power cycles
 uint32 gyro_device_id           # Gyroscope unique device ID for the sensor that does not change between power cycles

-float32[3] delta_angle          # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt)
-float32[3] delta_velocity       # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt)
-uint16 delta_angle_dt           # integration period in microseconds
-uint16 delta_velocity_dt        # integration period in microseconds
+float32[3] delta_angle          # delta angle in the NED board axis in rad/s over the integration time frame (dt)
+float32[3] delta_velocity       # delta velocity in the NED board axis in m/s over the integration time frame (dt)
+uint16 delta_angle_dt           # integration period in us
+uint16 delta_velocity_dt        # integration period in us

 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
@@ -8,8 +8,8 @@ uint32[3] accel_clipping        # total clipping per axis
 uint32 accel_error_count
 uint32 gyro_error_count

-float32 accel_rate_hz
-float32 gyro_rate_hz
+uint16 accel_rate_hz
+uint16 gyro_rate_hz

 float32 accel_vibration_metric  # high frequency vibration level in the IMU delta velocity data (m/s)
 float32 gyro_vibration_metric   # high frequency vibration level in the IMU delta velocity data (m/s)
@@ -5,6 +5,6 @@ uint64 timestamp_sample     # the timestamp of the raw data (microseconds)

 uint32 device_id            # unique device ID for the selected magnetometer

-float32[3] magnetometer_ga  # Magnetic field in the FRD body frame XYZ-axis in Gauss
+float32[3] magnetometer_ga  # Magnetic field in NED body frame, in Gauss

 uint8 calibration_count     # Calibration changed counter. Monotonically increases whenever calibration changes.
@@ -11,12 +11,13 @@ uint8 ARMING_STATE_IN_AIR_RESTORE = 5
 uint8 ARMING_STATE_MAX = 6

 # FailureDetector status
-uint8 FAILURE_NONE = 0
-uint8 FAILURE_ROLL = 1 	        # (1 << 0)
-uint8 FAILURE_PITCH = 2	        # (1 << 1)
-uint8 FAILURE_ALT = 4 	        # (1 << 2)
-uint8 FAILURE_EXT = 8 	        # (1 << 3)
-uint8 FAILURE_ARM_ESC = 16      # (1 << 4)
+uint8 FAILURE_NONE      =  0
+uint8 FAILURE_ROLL      =  1  # (1 << 0)
+uint8 FAILURE_PITCH     =  2  # (1 << 1)
+uint8 FAILURE_ALT       =  4  # (1 << 2)
+uint8 FAILURE_EXT       =  8  # (1 << 3)
+uint8 FAILURE_ARM_ESC   = 16  # (1 << 4)
+uint8 FAILURE_RATE_CTRL = 32  # (1 << 5)

 # HIL
 uint8 HIL_STATE_OFF = 0
@@ -114,7 +115,3 @@ uint8 ARM_DISARM_REASON_UNIT_TEST = 13

 uint8 latest_arming_reason
 uint8 latest_disarming_reason
-
-uint64 armed_time       # Arming timestamp (microseconds)
-
-uint64 takeoff_time     # Takeoff timestamp (microseconds)
@@ -169,9 +169,11 @@ public:
 	{
 		int ret = 0;

-		// We don't need the task name at this point.
+#ifdef __PX4_NUTTX
+		// On NuttX task_create() adds the task name as first argument.
 		argc -= 1;
 		argv += 1;
+#endif

 		T *object = T::instantiate(argc, argv);
 		_object.store(object);
@@ -7,8 +7,6 @@ get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES)
 # CMAKE_CURRENT_BINARY_DIR ''./platforms/posix' to './bin'
 if (NOT CATKIN_DEVEL_PREFIX)
 	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
-	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG ${CMAKE_BINARY_DIR}/bin)
-	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE ${CMAKE_BINARY_DIR}/bin)
 endif()

 set(PX4_SHELL_COMMAND_PREFIX "px4-")
@@ -45,7 +45,7 @@ ExternalProject_Add(sitl_gazebo
 	USES_TERMINAL_BUILD true
 	EXCLUDE_FROM_ALL true
 	BUILD_ALWAYS 1
-	BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j1
+	BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR>
 )

 ExternalProject_Add(mavsdk_tests
@@ -1,7 +1,7 @@
 /****************************************************************************
 *
- *   Copyright (C) 2015-2020 Mark Charlebois. All rights reserved.
- *   Author: @author Mark Charlebois <charlebm@gmail.com>
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *   Author: @author Mark Charlebois <charlebm#gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -33,6 +33,7 @@
 ****************************************************************************/

 /**
+ * @file px4_posix_tasks.c
 * Implementation of existing task API for Linux
 */

@@ -59,18 +60,22 @@
 #include <px4_platform_common/posix.h>
 #include <systemlib/err.h>

+#define MAX_CMD_LEN 100
+
 #define PX4_MAX_TASKS 50
+#define SHELL_TASK_ID (PX4_MAX_TASKS+1)

 pthread_t _shell_task_id = 0;
 pthread_mutex_t task_mutex = PTHREAD_MUTEX_INITIALIZER;

 struct task_entry {
-	pthread_t pid{0};
-	std::string name{};
-	bool isused {false};
+	pthread_t pid;
+	std::string name;
+	bool isused;
+	task_entry() : isused(false) {}
 };

-static task_entry taskmap[PX4_MAX_TASKS] {};
+static task_entry taskmap[PX4_MAX_TASKS] = {};

 typedef struct {
 	px4_main_t entry;
@@ -84,11 +89,13 @@ static void *entry_adapter(void *ptr)
 {
 	pthdata_t *data = (pthdata_t *) ptr;

+	int rv;
+
 	// set the threads name
 #ifdef __PX4_DARWIN
-	int rv = pthread_setname_np(data->name);
+	rv = pthread_setname_np(data->name);
 #else
-	int rv = pthread_setname_np(pthread_self(), data->name);
+	rv = pthread_setname_np(pthread_self(), data->name);
 #endif

 	if (rv) {
@@ -107,8 +114,10 @@ static void *entry_adapter(void *ptr)
 px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
 			      char *const argv[])
 {
+
+	int i;
 	int argc = 0;
-	unsigned int len = strlen(name) + 1;
+	unsigned int len = 0;
 	struct sched_param param = {};
 	char *p = (char *)argv;

@@ -124,7 +133,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 		len += strlen(p) + 1;
 	}

-	unsigned long structsize = sizeof(pthdata_t) + (argc + 2) * sizeof(char *);
+	unsigned long structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *);

 	// not safe to pass stack data to the thread creation
 	pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len);
@@ -134,30 +143,22 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	}

 	memset(taskdata, 0, structsize + len);
+	unsigned long offset = ((unsigned long)taskdata) + structsize;

 	strncpy(taskdata->name, name, 16);
-	taskdata->name[15] = '\0';
+	taskdata->name[15] = 0;
 	taskdata->entry = entry;
-	taskdata->argc = argc + 1;
+	taskdata->argc = argc;

-	char *offset = (char *)taskdata + structsize;
-
-	// We match the NuttX task_spawn implementation which copies
-	// the name into argv[0] in order to provide a consistent API
-	// to all tasks/modules.
-	taskdata->argv[0] = offset;
-	strcpy(offset, name);
-	offset += strlen(name) + 1;
-
-	for (int i = 0; i < argc; ++i) {
+	for (i = 0; i < argc; i++) {
 		PX4_DEBUG("arg %d %s\n", i, argv[i]);
-		taskdata->argv[i + 1] = offset;
-		strcpy(offset, argv[i]);
+		taskdata->argv[i] = (char *)offset;
+		strcpy((char *)offset, argv[i]);
 		offset += strlen(argv[i]) + 1;
 	}

 	// Must add NULL at end of argv
-	taskdata->argv[argc + 1] = (char *)nullptr;
+	taskdata->argv[argc] = (char *)nullptr;

 	PX4_DEBUG("starting task %s", name);

@@ -225,8 +226,6 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int

 	px4_task_t taskid = 0;

-	int i;
-
 	for (i = 0; i < PX4_MAX_TASKS; ++i) {
 		if (!taskmap[i].isused) {
 			taskmap[i].name = name;
@@ -308,11 +307,10 @@ int px4_task_delete(px4_task_t id)

 void px4_task_exit(int ret)
 {
+	int i;
 	pthread_t pid = pthread_self();

 	// Get pthread ID from the opaque ID
-	int i;
-
 	for (i = 0; i < PX4_MAX_TASKS; ++i) {
 		if (taskmap[i].pid == pid) {
 			pthread_mutex_lock(&task_mutex);
@@ -424,7 +422,7 @@ const char *px4_get_taskname()

 int px4_prctl(int option, const char *arg2, px4_task_t pid)
 {
-	int rv = -1;
+	int rv;

 	switch (option) {
 	case PR_SET_NAME:
@@ -437,6 +435,7 @@ int px4_prctl(int option, const char *arg2, px4_task_t pid)
 		break;

 	default:
+		rv = -1;
 		PX4_WARN("FAILED SETTING TASK NAME");
 		break;
 	}
@@ -1137,9 +1137,12 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance)

 int GPS::run_trampoline_secondary(int argc, char *argv[])
 {
-	// the task name is the first argument
+
+#ifdef __PX4_NUTTX
+	// on NuttX task_create() adds the task name as first argument
 	argc -= 1;
 	argv += 1;
+#endif

 	GPS *gps = instantiate(argc, argv, Instance::Secondary);
 	if (gps) {
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * @file PAW3902.hpp
+ * @file paw3902.cpp
 *
 * Driver for the Pixart PAW3902 & PAW3903 optical flow sensors connected via SPI.
 */
@@ -50,20 +50,27 @@
 #include <lib/perf/perf_counter.h>
 #include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/optical_flow.h>

-using namespace time_literals;
-using namespace PixArt_PAW3902JF;
+/* Configuration Constants */
+
+#define PAW3902_SPI_BUS_SPEED (2000000L) // 2MHz

 #define DIR_WRITE(a) ((a) | (1 << 7))
 #define DIR_READ(a) ((a) & 0x7f)

+using namespace time_literals;
+using namespace PixArt_PAW3902JF;
+
+// PAW3902JF-TXQT is PixArt Imaging
+
 class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
 {
 public:
-	PAW3902(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode,
-		float yaw_rotation_degrees = NAN);
+	PAW3902(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency,
+		spi_mode_e spi_mode);
 	virtual ~PAW3902();

 	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
@@ -76,38 +83,37 @@ public:

 	void RunImpl();

-private:
+	void start();
+
+protected:
 	int probe() override;

-	uint8_t	RegisterRead(uint8_t reg, int retries = 3);
-	void RegisterWrite(uint8_t reg, uint8_t data);
-	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 5);
+private:

-	bool Reset();
+	uint8_t	registerRead(uint8_t reg);
+	void	registerWrite(uint8_t reg, uint8_t data);

-	bool ModeBright();
-	bool ModeLowLight();
-	bool ModeSuperLowLight();
+	bool reset();

-	bool ChangeMode(Mode newMode);
+	bool modeBright();
+	bool modeLowLight();
+	bool modeSuperLowLight();
+
+	bool changeMode(Mode newMode);

 	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};

-	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-	perf_counter_t	_comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
-	perf_counter_t	_false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
-	perf_counter_t	_mode_change_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change")};
-	perf_counter_t	_register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
+	perf_counter_t	_sample_perf;
+	perf_counter_t	_comms_errors;
+	perf_counter_t	_dupe_count_perf;

-	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
+	static constexpr uint64_t _collect_time{15000}; // 15 milliseconds, optical flow data publish rate

-	uint64_t _previous_collect_timestamp{0};
-	uint64_t _flow_dt_sum_usec{0};
-	uint8_t _flow_sample_counter{0};
-	uint16_t _flow_quality_sum{0};
+	uint64_t	_previous_collect_timestamp{0};
+	uint64_t	_flow_dt_sum_usec{0};
+	unsigned	_frame_count_since_last{0};

-	matrix::Dcmf	_rotation;
+	enum Rotation	_yaw_rotation {ROTATION_NONE};

 	int		_flow_sum_x{0};
 	int		_flow_sum_y{0};
@@ -117,4 +123,5 @@ private:
 	uint8_t 	_low_to_superlow_counter{0};
 	uint8_t 	_low_to_bright_counter{0};
 	uint8_t 	_superlow_to_low_counter{0};
+
 };
@@ -36,45 +36,45 @@
 namespace PixArt_PAW3902JF
 {

-static constexpr uint8_t PRODUCT_ID = 0x49;
+static constexpr uint8_t PRODUCT_ID = 0x49;	// shared with the PMW3901
 static constexpr uint8_t REVISION_ID = 0x01;
-static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6;

 static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126};	// 126 fps
 static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126};	// 126 fps
 static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};		// 50 fps

-static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
+static constexpr uint64_t T_SWW{11};	// 10.5 microseconds
+static constexpr uint64_t T_SRR{2};	// 1.5 microseconds

 enum Register : uint8_t {
-	Product_ID         = 0x00,
-	Revision_ID        = 0x01,
-	Motion             = 0x02,
-	Delta_X_L          = 0x03,
-	Delta_X_H          = 0x04,
-	Delta_Y_L          = 0x05,
-	Delta_Y_H          = 0x06,
-	Squal              = 0x07,
-	RawData_Sum        = 0x08,
-	Maximum_RawData    = 0x09,
-	Minimum_RawData    = 0x0A,
-	Shutter_Lower      = 0x0B,
-	Shutter_Upper      = 0x0C,
+	Product_ID	= 0x00,
+	Revision_ID	= 0x01,
+	Motion		= 0x02,
+	Delta_X_L	= 0x03,
+	Delta_X_H	= 0x04,
+	Delta_Y_L	= 0x05,
+	Delta_Y_H	= 0x06,
+	Squal		= 0x07,
+	RawData_Sum	= 0x08,
+	Maximum_RawData	= 0x09,
+	Minimum_RawData	= 0x0A,
+	Shutter_Lower	= 0x0B,
+	Shutter_Upper	= 0x0C,

-	Observation        = 0x15,
-	Motion_Burst       = 0x16,
+	Observation	= 0x15,
+	Motion_Burst	= 0x16,

-	Power_Up_Reset     = 0x3A,
+	Power_Up_Reset	= 0x3A,

-	Resolution	   = 0x4E,
+	Resolution	= 0x4E,

-	Inverse_Product_ID = 0x5F,
 };

 enum Product_ID_Bit : uint8_t {
 	Reset = 0x5A,
 };

+
 enum class Mode {
 	Bright = 0,
 	LowLight = 1,
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 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
@@ -34,20 +34,23 @@
 #include "PAW3902.hpp"
 #include <px4_platform_common/module.h>

-void PAW3902::print_usage()
+extern "C" __EXPORT int paw3902_main(int argc, char *argv[]);
+
+void
+PAW3902::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("paw3902", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }

 I2CSPIDriverBase *PAW3902::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				       int runtime_instance)
 {
-	PAW3902 *instance = new PAW3902(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.bus_frequency,
-					cli.spi_mode, cli.custom1);
+	PAW3902 *instance = new PAW3902(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
+					cli.bus_frequency, cli.spi_mode);

 	if (!instance) {
 		PX4_ERR("alloc failed");
@@ -62,18 +65,19 @@ I2CSPIDriverBase *PAW3902::instantiate(const BusCLIArguments &cli, const BusInst
 	return instance;
 }

-extern "C" __EXPORT int paw3902_main(int argc, char *argv[])
+int
+paw3902_main(int argc, char *argv[])
 {
-	int ch = 0;
+	int ch;
 	using ThisDriver = PAW3902;
 	BusCLIArguments cli{false, true};
 	cli.spi_mode = SPIDEV_MODE0;
-	cli.default_spi_frequency = SPI_SPEED;
+	cli.default_spi_frequency = PAW3902_SPI_BUS_SPEED;

-	while ((ch = cli.getopt(argc, argv, "Y:")) != EOF) {
+	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
 		switch (ch) {
-		case 'Y':
-			cli.custom1 = atoi(cli.optarg());
+		case 'R':
+			cli.rotation = (enum Rotation)atoi(cli.optarg());
 			break;
 		}
 	}
@@ -60,21 +60,7 @@ class List
 {
 public:

-	void add(T newNode) { addFront(newNode); }
-	void addFront(T newNode)
-	{
-		if (_head == nullptr) {
-			// list is empty, add as head
-			_head = newNode;
-			return;
-
-		} else {
-			newNode->setSibling(_head);
-			_head = newNode;
-		}
-	}
-
-	void addBack(T newNode)
+	void add(T newNode)
 	{
 		if (_head == nullptr) {
 			// list is empty, add as head
@@ -451,11 +451,11 @@ State FlightTaskAuto::_getCurrentState()
 		// Target is behind.
 		return_state = State::target_behind;

-	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _target_acceptance_radius) {
+	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
 		// Current position is more than cruise speed in front of previous setpoint.
 		return_state = State::previous_infront;

-	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _target_acceptance_radius) {
+	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
 		// Vehicle is more than cruise speed off track.
 		return_state = State::offtrack;

@@ -116,7 +116,6 @@ void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)

 void FlightTaskAutoLineSmoothVel::_generateSetpoints()
 {
-	_updateTurningCheck();
 	_prepareSetpoints();
 	_generateTrajectory();

@@ -126,26 +125,6 @@ void FlightTaskAutoLineSmoothVel::_generateSetpoints()
 	}
 }

-void FlightTaskAutoLineSmoothVel::_updateTurningCheck()
-{
-	const Vector2f vel_traj(_trajectory[0].getCurrentVelocity(),
-				_trajectory[1].getCurrentVelocity());
-	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
-				_trajectory[1].getCurrentPosition());
-	const Vector2f target_xy(_target);
-	const Vector2f u_vel_traj = vel_traj.unit_or_zero();
-	const Vector2f pos_to_target = Vector2f(target_xy - pos_traj);
-	const float cos_align = u_vel_traj * pos_to_target.unit_or_zero();
-
-	// The vehicle is turning if the angle between the velocity vector
-	// and the direction to the target is greater than 10 degrees, the
-	// velocity is large enough and the drone isn't in the acceptance
-	// radius of the last WP.
-	_is_turning = vel_traj.longerThan(0.2f)
-		      && cos_align < 0.98f
-		      && pos_to_target.longerThan(_target_acceptance_radius);
-}
-
 void FlightTaskAutoLineSmoothVel::_generateHeading()
 {
 	// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
@@ -161,7 +140,7 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
 	Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);

 	if ((vel_sp_xy.length() > .1f) &&
-	    (traj_to_target.length() > 2.f)) {
+	    (traj_to_target.length() > _target_acceptance_radius)) {
 		// Generate heading from velocity vector, only if it is long enough
 		// and if the drone is far enough from the target
 		_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);
@@ -204,10 +183,13 @@ float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const

 	// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
 	// (eg. Obstacle Avoidance)
+	bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
+	bool z_valid = PX4_ISFINITE(_position_setpoint(2));
+	bool z_modified =  z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;

 	Vector3f waypoints[3] = {pos_traj, _target, _next_wp};

-	if (isTargetModified()) {
+	if (xy_modified || z_modified) {
 		waypoints[2] = waypoints[1] = _position_setpoint;
 	}

@@ -242,64 +224,6 @@ float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
 	return max_speed;
 }

-Vector3f FlightTaskAutoLineSmoothVel::getCrossingPoint() const
-{
-	Vector3f pos_crossing_point{};
-
-	if (isTargetModified()) {
-		// Strictly follow the modified setpoint
-		pos_crossing_point = _position_setpoint;
-
-	} else {
-		if (_is_turning) {
-			// Get the crossing point using L1-style guidance
-			pos_crossing_point.xy() = getL1Point();
-			pos_crossing_point(2) = _target(2);
-
-		} else {
-			pos_crossing_point = _target;
-		}
-	}
-
-	return pos_crossing_point;
-}
-
-bool FlightTaskAutoLineSmoothVel::isTargetModified() const
-{
-	const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
-	const bool z_valid = PX4_ISFINITE(_position_setpoint(2));
-	const bool z_modified =  z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;
-
-	return xy_modified || z_modified;
-}
-
-Vector2f FlightTaskAutoLineSmoothVel::getL1Point() const
-{
-	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
-				_trajectory[1].getCurrentPosition());
-	const Vector2f target_xy(_target);
-	const Vector2f u_prev_to_target = Vector2f(target_xy - Vector2f(_prev_wp)).unit_or_zero();
-	const Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
-	const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
-	const Vector2f closest_pt = Vector2f(_prev_wp) + prev_to_closest;
-
-	// Compute along-track error using L1 distance and cross-track error
-	const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();
-
-	const float l1 = math::max(_target_acceptance_radius, 5.f);
-	float alongtrack_error = 0.f;
-
-	// Protect against sqrt of a negative number
-	if (l1 > crosstrack_error) {
-		alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);
-	}
-
-	// Position of the point on the line where L1 intersect the line between the two waypoints
-	const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;
-
-	return l1_point;
-}
-
 void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 {
 	// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
@@ -312,93 +236,74 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
 		// Wait for the yaw setpoint to be aligned
 		_velocity_setpoint.setAll(0.f);
-		return;
-	}

-	const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
-	const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));
+	} else {
+		const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
+		const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));

-	if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
-		// Use 3D position setpoint to generate a 3D velocity setpoint
-		Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
-				  _trajectory[1].getCurrentPosition(),
-				  _trajectory[2].getCurrentPosition());
+		if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
+			// Use 3D position setpoint to generate a 3D velocity setpoint
+			Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+					  _trajectory[1].getCurrentPosition(),
+					  _trajectory[2].getCurrentPosition());
+			Vector3f u_pos_traj_to_dest = (_position_setpoint - pos_traj).unit_or_zero();

-		const Vector3f u_pos_traj_to_dest((getCrossingPoint() - pos_traj).unit_or_zero());
+			const float xy_speed = _getMaxXYSpeed();
+			const float z_speed = _getMaxZSpeed();

-		float xy_speed = _getMaxXYSpeed();
-		const float z_speed = _getMaxZSpeed();
+			Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
+			math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
+			math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);

-		if (_is_turning) {
-			// Lock speed during turn
-			xy_speed = math::min(_max_speed_prev, xy_speed);
+			for (int i = 0; i < 3; i++) {
+				// If available, use the existing velocity as a feedforward, otherwise replace it
+				if (PX4_ISFINITE(_velocity_setpoint(i))) {
+					_velocity_setpoint(i) += vel_sp_constrained(i);

-		} else {
-			_max_speed_prev = xy_speed;
-		}
-
-		Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
-		math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
-		math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
-
-		for (int i = 0; i < 3; i++) {
-			// If available, use the existing velocity as a feedforward, otherwise replace it
-			if (PX4_ISFINITE(_velocity_setpoint(i))) {
-				_velocity_setpoint(i) += vel_sp_constrained(i);
-
-			} else {
-				_velocity_setpoint(i) = vel_sp_constrained(i);
+				} else {
+					_velocity_setpoint(i) = vel_sp_constrained(i);
+				}
 			}
 		}
-	}

-	else if (xy_pos_setpoint_valid) {
-		// Use 2D position setpoint to generate a 2D velocity setpoint
+		else if (xy_pos_setpoint_valid) {
+			// Use 2D position setpoint to generate a 2D velocity setpoint

-		// Get various path specific vectors
-		Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
-		Vector2f pos_traj_to_dest_xy = Vector2f(getCrossingPoint()) - pos_traj;
-		Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
+			// Get various path specific vectors
+			Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
+			Vector2f pos_traj_to_dest_xy = Vector2f(_position_setpoint.xy()) - pos_traj;
+			Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());

-		float xy_speed = _getMaxXYSpeed();
+			Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed();

-		if (_is_turning) {
-			// Lock speed during turn
-			xy_speed = math::min(_max_speed_prev, xy_speed);
+			for (int i = 0; i < 2; i++) {
+				// If available, use the existing velocity as a feedforward, otherwise replace it
+				if (PX4_ISFINITE(_velocity_setpoint(i))) {
+					_velocity_setpoint(i) += vel_sp_constrained_xy(i);

-		} else {
-			_max_speed_prev = xy_speed;
-		}
-
-		Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * xy_speed;
-
-		for (int i = 0; i < 2; i++) {
-			// If available, use the existing velocity as a feedforward, otherwise replace it
-			if (PX4_ISFINITE(_velocity_setpoint(i))) {
-				_velocity_setpoint(i) += vel_sp_constrained_xy(i);
-
-			} else {
-				_velocity_setpoint(i) = vel_sp_constrained_xy(i);
+				} else {
+					_velocity_setpoint(i) = vel_sp_constrained_xy(i);
+				}
 			}
 		}
-	}

-	else if (z_pos_setpoint_valid) {
-		// Use Z position setpoint to generate a Z velocity setpoint
+		else if (z_pos_setpoint_valid) {
+			// Use Z position setpoint to generate a Z velocity setpoint

-		const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
-		const float vel_sp_z = z_dir * _getMaxZSpeed();
+			const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
+			const float vel_sp_z = z_dir * _getMaxZSpeed();

-		// If available, use the existing velocity as a feedforward, otherwise replace it
-		if (PX4_ISFINITE(_velocity_setpoint(2))) {
-			_velocity_setpoint(2) += vel_sp_z;
+			// If available, use the existing velocity as a feedforward, otherwise replace it
+			if (PX4_ISFINITE(_velocity_setpoint(2))) {
+				_velocity_setpoint(2) += vel_sp_z;

-		} else {
-			_velocity_setpoint(2) = vel_sp_z;
+			} else {
+				_velocity_setpoint(2) = vel_sp_z;
+			}
 		}
-	}

-	_want_takeoff = _velocity_setpoint(2) < -0.3f;
+		_want_takeoff = _velocity_setpoint(2) < -0.3f;
+	}
 }

 void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
@@ -63,7 +63,6 @@ protected:

 	void _generateSetpoints() override; /**< Generate setpoints along line. */
 	void _generateHeading();
-	void _updateTurningCheck();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */

 	static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
@@ -73,13 +72,6 @@ protected:
 	float _getMaxXYSpeed() const;
 	float _getMaxZSpeed() const;

-	matrix::Vector3f getCrossingPoint() const;
-	bool isTargetModified() const;
-	matrix::Vector2f getL1Point() const;
-
-	float _max_speed_prev{};
-	bool _is_turning{false};
-
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _updateTrajConstraints();
 	void _generateTrajectory();
@@ -66,7 +66,7 @@ public:
 	 *
 	 * @param mixer			The mixer to be added.
 	 */
-	void				add_mixer(Mixer *mixer) { _mixers.addBack(mixer); }
+	void				add_mixer(Mixer *mixer) { _mixers.add(mixer); }

 	/**
 	 * Remove all the mixers from the group.
@@ -979,12 +979,22 @@ int param_save_default()
 		return res;
 	}

+	int shutdown_lock_ret = px4_shutdown_lock();
+
+	if (shutdown_lock_ret) {
+		PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret);
+	}
+
 	/* write parameters to temp file */
 	int fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);

 	if (fd < 0) {
 		PX4_ERR("failed to open param file: %s", filename);

+		if (shutdown_lock_ret == 0) {
+			px4_shutdown_unlock();
+		}
+
 		return PX4_ERROR;
 	}

@@ -1006,6 +1016,10 @@ int param_save_default()

 	PARAM_CLOSE(fd);

+	if (shutdown_lock_ret == 0) {
+		px4_shutdown_unlock();
+	}
+
 	return res;
 }

@@ -132,7 +132,7 @@ PARAM_DEFINE_INT32(SYS_PARAM_VER, 1);
 *
 * 0 : Set to 0 to do nothing
 * 1 : Set to 1 to start a calibration at next boot
- * This parameter is reset to zero when the temperature calibration starts.
+ * This parameter is reset to zero when the the temperature calibration starts.
 *
 * default (0, no calibration)
 *
@@ -147,7 +147,7 @@ PARAM_DEFINE_INT32(SYS_CAL_GYRO, 0);
 *
 * 0 : Set to 0 to do nothing
 * 1 : Set to 1 to start a calibration at next boot
- * This parameter is reset to zero when the temperature calibration starts.
+ * This parameter is reset to zero when the the temperature calibration starts.
 *
 * default (0, no calibration)
 *
@@ -162,7 +162,7 @@ PARAM_DEFINE_INT32(SYS_CAL_ACCEL, 0);
 *
 * 0 : Set to 0 to do nothing
 * 1 : Set to 1 to start a calibration at next boot
- * This parameter is reset to zero when the temperature calibration starts.
+ * This parameter is reset to zero when the the temperature calibration starts.
 *
 * default (0, no calibration)
 *
@@ -222,7 +222,7 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1);
 /**
 * Control if the vehicle has a barometer
 *
- * Disable this if the board has no barometer, such as some of the Omnibus
+ * Disable this if the board has no barometer, such as some of the the Omnibus
 * F4 SD variants.
 * If disabled, the preflight checks will not check for the presence of a
 * barometer.
@@ -1718,11 +1718,10 @@ Commander::run()
 			if (_armed.armed) {
 				if (!was_landed && _land_detector.landed) {
 					mavlink_log_info(&_mavlink_log_pub, "Landing detected");
-					_status.takeoff_time = 0;

 				} else if (was_landed && !_land_detector.landed) {
 					mavlink_log_info(&_mavlink_log_pub, "Takeoff detected");
-					_status.takeoff_time = hrt_absolute_time();
+					_time_at_takeoff = hrt_absolute_time();
 					_have_taken_off_since_arming = true;

 					// Set all position and velocity test probation durations to takeoff value
@@ -2452,18 +2451,18 @@ Commander::run()
 			_status.failure_detector_status = _failure_detector.getStatus();
 			_status_changed = true;

-			if (_armed.armed) {
-				if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
+			if (_armed.armed && (_failure_detector.getStatus() != FAILURE_NONE)) {
+				if (_status.failure_detector_status & FAILURE_ARM_ESCS) {
 					// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
-					if (hrt_elapsed_time(&_status.armed_time) < 500_ms) {
+					if (hrt_elapsed_time(&_armed.armed_time) < 500_ms) {
 						arm_disarm(false, true, arm_disarm_reason_t::FAILURE_DETECTOR);
 						mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request");
 					}
 				}

-				if (_status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH |
-								       vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
-					const bool is_second_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
+				if (_status.failure_detector_status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT | FAILURE_RATE_CTRL)) {
+
+					const bool is_second_after_takeoff = hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get());

 					if (is_second_after_takeoff && !_lockdown_triggered) {
 						// This handles the case where something fails during the early takeoff phase
@@ -2471,13 +2470,16 @@ Commander::run()
 						_lockdown_triggered = true;
 						mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown");

-					} else if (!_status_flags.circuit_breaker_flight_termination_disabled &&
-						   !_flight_termination_triggered && !_lockdown_triggered) {
+					} else if (_status.failure_detector_status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT)) {

-						_armed.force_failsafe = true;
-						_flight_termination_triggered = true;
-						mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight");
-						set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
+						if (!_status_flags.circuit_breaker_flight_termination_disabled && !_flight_termination_triggered
+						    && !_lockdown_triggered) {
+
+							_armed.force_failsafe = true;
+							_flight_termination_triggered = true;
+							mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight");
+							set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
+						}
 					}
 				}
 			}
@@ -3993,7 +3995,7 @@ void Commander::estimator_check()

 			} else {
 				// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
-				const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s);
+				const bool sufficient_time = (hrt_elapsed_time(&_time_at_takeoff) > 30_s);
 				const bool sufficient_speed = (lpos.vx * lpos.vx + lpos.vy * lpos.vy > 25.0f);

 				bool innovation_pass = estimator_status.vel_test_ratio < 1.0f && estimator_status.pos_test_ratio < 1.0f;
@@ -311,6 +311,7 @@ private:
 	hrt_abstime	_lvel_probation_time_us = POSVEL_PROBATION_MIN;

 	/* class variables used to check for navigation failure after takeoff */
+	hrt_abstime	_time_at_takeoff{0};		/**< last time we were on the ground */
 	hrt_abstime	_time_last_innov_pass{0};	/**< last time velocity or position innovations passed */
 	bool		_nav_test_passed{false};	/**< true if the post takeoff navigation test has passed */
 	bool		_nav_test_failed{false};	/**< true if the post takeoff navigation test has failed */
@@ -118,7 +118,7 @@ PARAM_DEFINE_INT32(COM_HLDL_LOSS_T, 120);
 /**
 * High Latency Datalink regain time threshold
 *
- * After a data link loss: after this number of seconds with a healthy datalink the 'datalink loss'
+ * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
 * flag is set back to false
 *
 * @group Commander
@@ -45,6 +45,9 @@ using namespace time_literals;
 FailureDetector::FailureDetector(ModuleParams *parent) :
 	ModuleParams(parent)
 {
+	_ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz
+	_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
+	_rate_ctrl_failure_hysteresis.set_hysteresis_time_from(false, 100_ms);
 }

 bool FailureDetector::update(const vehicle_status_s &vehicle_status)
@@ -54,18 +57,22 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status)
 	if (isAttitudeStabilized(vehicle_status)) {
 		updateAttitudeStatus();

-		if (_param_fd_ext_ats_en.get()) {
-			updateExternalAtsStatus();
-		}
-
 	} else {
 		_status &= ~(FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT);
 	}

+	if (_param_fd_ext_ats_en.get()) {
+		updateExternalAtsStatus();
+	}
+
 	if (_param_escs_en.get()) {
 		updateEscsStatus(vehicle_status);
 	}

+	if (_param_fd_rate_ctrl_en.get()) {
+		updateRateControlStatus(vehicle_status);
+	}
+
 	return _status != previous_status;
 }

@@ -136,10 +143,9 @@ void FailureDetector::updateExternalAtsStatus()
 		uint32_t pulse_width = pwm_input.pulse_width;
 		bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms);

-		hrt_abstime time_now = hrt_absolute_time();
+		const hrt_abstime time_now = hrt_absolute_time();

 		// Update hysteresis
-		_ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz
 		_ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now);

 		_status &= ~FAILURE_EXT;
@@ -160,7 +166,6 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
 		if (_esc_status_sub.update(&esc_status)) {
 			int all_escs_armed = (1 << esc_status.esc_count) - 1;

-			_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
 			_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);

 			if (_esc_failure_hysteresis.get_state()) {
@@ -174,3 +179,36 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
 		_status &= ~FAILURE_ARM_ESCS;
 	}
 }
+
+void FailureDetector::updateRateControlStatus(const vehicle_status_s &vehicle_status)
+{
+	rate_ctrl_status_s rcs;
+
+	if (_rate_ctrl_status_sub.update(&rcs) && (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+		static constexpr float rate_error_lim = math::radians(30.f);  // 30 degrees/second
+		static constexpr float angle_error_lim = math::radians(60.f); // 60 degrees
+
+		// roll rate error
+		const bool rr_pos_err = (rcs.error[0] > rate_error_lim) && (rcs.error_integrated[0] > angle_error_lim);
+		const bool rr_neg_err = (rcs.error[0] < -rate_error_lim) && (rcs.error_integrated[0] < -angle_error_lim);
+
+		// pitch rate error
+		const bool pr_pos_err = (rcs.error[1] > rate_error_lim) && (rcs.error_integrated[1] > angle_error_lim);
+		const bool pr_neg_err = (rcs.error[1] < -rate_error_lim) && (rcs.error_integrated[1] < -angle_error_lim);
+
+		const bool fail = rr_pos_err || rr_neg_err || pr_pos_err || pr_neg_err;
+
+		_rate_ctrl_failure_hysteresis.set_state_and_update(fail, rcs.timestamp);
+
+		if (_rate_ctrl_failure_hysteresis.get_state()) {
+			_status |= FAILURE_RATE_CTRL;
+
+		} else {
+			_status &= ~FAILURE_RATE_CTRL;
+		}
+
+	} else {
+		_rate_ctrl_failure_hysteresis.set_state_and_update(false, hrt_absolute_time());
+		_status &= ~FAILURE_RATE_CTRL;
+	}
+}
@@ -43,30 +43,28 @@

 #pragma once

-#include <matrix/matrix/math.hpp>
-#include <mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/hysteresis/hysteresis.h>
 #include <px4_platform_common/module_params.h>
-#include <hysteresis/hysteresis.h>
-

 // subscriptions
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/pwm_input.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>

-typedef enum {
+enum failure_detector_bitmak {
 	FAILURE_NONE = vehicle_status_s::FAILURE_NONE,
 	FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL,
 	FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH,
 	FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
 	FAILURE_EXT = vehicle_status_s::FAILURE_EXT,
-	FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC
-} failure_detector_bitmak;
-
-using uORB::SubscriptionData;
+	FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC,
+	FAILURE_RATE_CTRL = vehicle_status_s::FAILURE_RATE_CTRL,
+};

 class FailureDetector : public ModuleParams
 {
@@ -81,17 +79,20 @@ private:
 	void updateAttitudeStatus();
 	void updateExternalAtsStatus();
 	void updateEscsStatus(const vehicle_status_s &vehicle_status);
+	void updateRateControlStatus(const vehicle_status_s &vehicle_status);

 	uint8_t _status{FAILURE_NONE};

+	systemlib::Hysteresis _esc_failure_hysteresis{false};
+	systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
+	systemlib::Hysteresis _rate_ctrl_failure_hysteresis{false};
 	systemlib::Hysteresis _roll_failure_hysteresis{false};
 	systemlib::Hysteresis _pitch_failure_hysteresis{false};
-	systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
-	systemlib::Hysteresis _esc_failure_hysteresis{false};

-	uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
+	uORB::Subscription _rate_ctrl_status_sub{ORB_ID(rate_ctrl_status)};
+	uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};

 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@@ -100,6 +101,7 @@ private:
 		(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
 		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
-		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en
+		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
+		(ParamBool<px4::params::FD_RATE_CTRL_EN>) _param_fd_rate_ctrl_en
 	)
 };
@@ -141,3 +141,13 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
 * @group Failure Detector
 */
 PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
+
+/**
+ * Enable rate controller checks at takeoff.
+ * If enabled the failure detector will monitor the rate controller at takeoff and immediately disarm if there's an error.
+ *
+ * @boolean
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_RATE_CTRL_EN, 0);
@@ -231,10 +231,10 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
 			}

 			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-				status->armed_time = hrt_absolute_time();
+				armed->armed_time = hrt_absolute_time();

 			} else {
-				status->armed_time = 0;
+				armed->armed_time = 0;
 			}
 		}
 	}
@@ -711,8 +711,8 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	float terrain_vpos = _ekf.getTerrainVertPos();

 	// Distance to bottom surface (ground) in meters
-	// constrain the distance to ground to _rng_gnd_clearance
-	lpos.dist_bottom = math::max(terrain_vpos - lpos.z, _param_ekf2_min_rng.get());
+	//  constrain the distance to ground to _rng_gnd_clearance
+	lpos.dist_bottom = math::min(terrain_vpos - lpos.z, _param_ekf2_min_rng.get());

 	if (!_had_valid_terrain) {
 		_had_valid_terrain = lpos.dist_bottom_valid;
@@ -215,7 +215,7 @@ MavlinkFTP::_process_request(
 		break;

 	case kCmdCreateFile:
-		errorCode = _workOpen(payload, O_CREAT | O_TRUNC | O_WRONLY);
+		errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
 		break;

 	case kCmdOpenFileWO:
@@ -1838,9 +1838,16 @@ Mavlink::task_main(int argc, char *argv[])

 	_interface_name = nullptr;

-	// We don't care about the name and verb at this point.
+#ifdef __PX4_NUTTX
+	/* the NuttX optarg handler does not
+	 * ignore argv[0] like the POSIX handler
+	 * does, nor does it deal with non-flag
+	 * verbs well. So we remove the application
+	 * name and the verb.
+	 */
 	argc -= 2;
 	argv += 2;
+#endif

 	/* don't exit from getopt loop to leave getopt global variables in consistent state,
 	 * set error flag instead */
@@ -84,7 +84,7 @@ private:

 		bool lpos_updated = false;

-		vehicle_local_position_s local_pos{};
+		vehicle_local_position_s local_pos;

 		if (_local_pos_sub.copy(&local_pos)) {

@@ -34,7 +34,7 @@
 #ifndef FLIGHT_INFORMATION_HPP
 #define FLIGHT_INFORMATION_HPP

-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>

 class MavlinkStreamFlightInformation : public MavlinkStream
 {
@@ -58,31 +58,26 @@ private:
 		_param_com_flight_uuid = param_find("COM_FLIGHT_UUID");
 	}

-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	param_t _param_com_flight_uuid{PARAM_INVALID};
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	param_t _param_com_flight_uuid;

 	bool send() override
 	{
-		vehicle_status_s vehicle_status{};
-
-		if (_vehicle_status_sub.copy(&vehicle_status) && vehicle_status.timestamp != 0) {
-			mavlink_flight_information_t flight_info{};
-			flight_info.time_boot_ms = hrt_absolute_time() / 1000;
-			flight_info.arming_time_utc = vehicle_status.armed_time;
-			flight_info.takeoff_time_utc = vehicle_status.takeoff_time;
+		actuator_armed_s actuator_armed{};
+		bool ret = _armed_sub.copy(&actuator_armed);

+		if (ret && actuator_armed.timestamp != 0) {
 			int32_t flight_uuid;
+			param_get(_param_com_flight_uuid, &flight_uuid);

-			if (param_get(_param_com_flight_uuid, &flight_uuid) == PX4_OK) {
-				flight_info.flight_uuid = static_cast<uint64_t>(flight_uuid);
-			}
-
+			mavlink_flight_information_t flight_info{};
+			flight_info.flight_uuid = static_cast<uint64_t>(flight_uuid);
+			flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time / 1000;
+			flight_info.time_boot_ms = hrt_absolute_time() / 1000;
 			mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info);
-
-			return true;
 		}

-		return false;
+		return ret;
 	}
 };

@@ -59,7 +59,7 @@ MulticopterHoverThrustEstimator::~MulticopterHoverThrustEstimator()

 bool MulticopterHoverThrustEstimator::init()
 {
-	if (!_vehicle_local_position_sub.registerCallback()) {
+	if (!_vehicle_local_position_setpoint_sub.registerCallback()) {
 		PX4_ERR("vehicle_local_position_setpoint callback registration failed!");
 		return false;
 	}
@@ -91,46 +91,13 @@ void MulticopterHoverThrustEstimator::updateParams()
 void MulticopterHoverThrustEstimator::Run()
 {
 	if (should_exit()) {
-		_vehicle_local_position_sub.unregisterCallback();
+		_vehicle_local_position_setpoint_sub.unregisterCallback();
 		exit_and_cleanup();
 		return;
 	}

-	if (_vehicle_land_detected_sub.updated()) {
-		vehicle_land_detected_s vehicle_land_detected;
-
-		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
-			_landed = vehicle_land_detected.landed;
-
-			if (_landed) {
-				_in_air = false;
-			}
-		}
-	}
-
-	if (!_vehicle_local_position_sub.updated()) {
-		return;
-	}
-
-	vehicle_local_position_s local_pos{};
-
-	if (_vehicle_local_position_sub.copy(&local_pos)) {
-		// This is only necessary because the landed
-		// flag of the land detector does not guarantee that
-		// the vehicle does not touch the ground anymore.
-		// There is no check for the dist_bottom validity as
-		// this value is always good enough after takeoff for
-		// this use case.
-		// TODO: improve the landed flag
-		if (!_landed) {
-			if (local_pos.dist_bottom > 1.f) {
-				_in_air = true;
-			}
-		}
-	}
-
-	// new local position setpoint needed every iteration
-	if (!_vehicle_local_position_setpoint_sub.updated()) {
+	// new local position estimate and setpoint needed every iteration
+	if (!_vehicle_local_pos_sub.updated() || !_vehicle_local_position_setpoint_sub.updated()) {
 		return;
 	}

@@ -146,6 +113,14 @@ void MulticopterHoverThrustEstimator::Run()

 	perf_begin(_cycle_perf);

+	if (_vehicle_land_detected_sub.updated()) {
+		vehicle_land_detected_s vehicle_land_detected;
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+			_landed = vehicle_land_detected.landed;
+		}
+	}
+
 	if (_vehicle_status_sub.updated()) {
 		vehicle_status_s vehicle_status;

@@ -154,30 +129,40 @@ void MulticopterHoverThrustEstimator::Run()
 		}
 	}

+	// always copy latest position estimate
+	vehicle_local_position_s local_pos{};
+
+	if (_vehicle_local_pos_sub.copy(&local_pos)) {
+		// This is only necessary because the landed
+		// flag of the land detector does not guarantee that
+		// the vehicle does not touch the ground anymore
+		// TODO: improve the landed flag
+		if (local_pos.dist_bottom_valid) {
+			_in_air = local_pos.dist_bottom > 1.f;
+		}
+	}
+
 	const float dt = (local_pos.timestamp - _timestamp_last) * 1e-6f;
 	_timestamp_last = local_pos.timestamp;

-	if (_armed && _in_air && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
+	if (_armed && !_landed && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {

 		_hover_thrust_ekf.predict(dt);

 		vehicle_local_position_setpoint_s local_pos_sp;

-		if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) {
+		if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
 			if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
 				// Inform the hover thrust estimator about the measured vertical
 				// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
-				// Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects
-				if (fabsf(local_pos.vz) < 2.f) {
-					_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
-				}
+				ZeroOrderHoverThrustEkf::status status;
+				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status);

-				const bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f)
-						   && (_hover_thrust_ekf.getInnovationTestRatio() < 1.f);
+				const bool valid = _in_air && (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f);
 				_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
 				_valid = _valid_hysteresis.get_state();

-				publishStatus(local_pos.timestamp);
+				publishStatus(local_pos.timestamp, status);
 			}
 		}

@@ -199,19 +184,20 @@ void MulticopterHoverThrustEstimator::Run()
 	perf_end(_cycle_perf);
 }

-void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime &timestamp_sample)
+void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime &timestamp_sample,
+		const ZeroOrderHoverThrustEkf::status &status)
 {
-	hover_thrust_estimate_s status_msg{};
+	hover_thrust_estimate_s status_msg;

 	status_msg.timestamp_sample = timestamp_sample;

-	status_msg.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate();
-	status_msg.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar();
+	status_msg.hover_thrust = status.hover_thrust;
+	status_msg.hover_thrust_var = status.hover_thrust_var;

-	status_msg.accel_innov = _hover_thrust_ekf.getInnovation();
-	status_msg.accel_innov_var = _hover_thrust_ekf.getInnovationVar();
-	status_msg.accel_innov_test_ratio = _hover_thrust_ekf.getInnovationTestRatio();
-	status_msg.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar();
+	status_msg.accel_innov = status.innov;
+	status_msg.accel_innov_var = status.innov_var;
+	status_msg.accel_innov_test_ratio = status.innov_test_ratio;
+	status_msg.accel_noise_var = status.accel_noise_var;

 	status_msg.valid = _valid;

@@ -88,19 +88,19 @@ private:

 	void reset();

-	void publishStatus(const hrt_abstime &timestamp_sample);
+	void publishStatus(const hrt_abstime &timestamp_sample, const ZeroOrderHoverThrustEkf::status &status);
 	void publishInvalidStatus();

 	ZeroOrderHoverThrustEkf _hover_thrust_ekf{};

 	uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};

-	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
+	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_setpoint_sub{this, ORB_ID(vehicle_local_position_setpoint)};

 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Subscription _vehicle_local_pos_sub{ORB_ID(vehicle_local_position)};

 	hrt_abstime _timestamp_last{0};

@@ -49,7 +49,7 @@ void ZeroOrderHoverThrustEkf::predict(const float dt)
 	_dt = dt;
 }

-void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust)
+void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust, status &status_return)
 {
 	const float H = computeH(thrust);
 	const float innov_var = computeInnovVar(H);
@@ -75,10 +75,7 @@ void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust)
 	updateLpf(residual, signed_innov_test_ratio);
 	updateMeasurementNoise(residual, H);

-	// save for logging
-	_innov = innov;
-	_innov_var = innov_var;
-	_innov_test_ratio = innov_test_ratio;
+	status_return = packStatus(innov, innov_var, innov_test_ratio);
 }

 inline float ZeroOrderHoverThrustEkf::computeH(const float thrust) const
@@ -154,3 +151,18 @@ inline void ZeroOrderHoverThrustEkf::updateMeasurementNoise(const float residual
 	const float P = _state_var;
 	_acc_var = math::constrain((1.f - alpha) * _acc_var  + alpha * (res_no_bias * res_no_bias + H * P * H), 1.f, 400.f);
 }
+
+inline ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkf::packStatus(const float innov, const float innov_var,
+		const float innov_test_ratio) const
+{
+	// Send back status for logging
+	status ret{};
+	ret.hover_thrust = _hover_thr;
+	ret.hover_thrust_var = _state_var;
+	ret.innov = innov;
+	ret.innov_var = innov_var;
+	ret.innov_test_ratio = innov_test_ratio;
+	ret.accel_noise_var = _acc_var;
+
+	return ret;
+}
@@ -70,13 +70,22 @@
 class ZeroOrderHoverThrustEkf
 {
 public:
+	struct status {
+		float hover_thrust;
+		float hover_thrust_var;
+		float innov;
+		float innov_var;
+		float innov_test_ratio;
+		float accel_noise_var;
+	};
+
 	ZeroOrderHoverThrustEkf() = default;
 	~ZeroOrderHoverThrustEkf() = default;

 	void resetAccelNoise() { _acc_var = 5.f; };

-	void predict(float _dt);
-	void fuseAccZ(float acc_z, float thrust);
+	void predict(float dt);
+	void fuseAccZ(float acc_z, float thrust, status &status_return);

 	void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
 	void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; }
@@ -86,9 +95,6 @@ public:

 	float getHoverThrustEstimate() const { return _hover_thr; }
 	float getHoverThrustEstimateVar() const { return _state_var; }
-	float getInnovation() const { return _innov; }
-	float getInnovationVar() const { return _innov_var; }
-	float getInnovationTestRatio() const { return _innov_test_ratio; }
 	float getAccelNoiseVar() const { return _acc_var; }

 private:
@@ -100,10 +106,6 @@ private:
 	float _acc_var{5.f}; ///< Acceleration variance (m^2/s^3)
 	float _dt{0.02f};

-	float _innov{0.f}; ///< Measurement innovation (m/s^2)
-	float _innov_var{0.f}; ///< Measurement innovation variance (m^2/s^3)
-	float _innov_test_ratio{0.f}; ///< Noramlized Innovation Squared test ratio
-
 	float _residual_lpf{}; ///< used to remove the constant bias of the residual
 	float _signed_innov_test_ratio_lpf{}; ///< used as a delay to trigger the recovery logic

@@ -129,6 +131,8 @@ private:
 	void updateLpf(float residual, float signed_innov_test_ratio);
 	void updateMeasurementNoise(float residual, float H);

+	status packStatus(float innov, float innov_var, float innov_test_ratio) const;
+
 	static constexpr float _noise_learning_time_constant = 2.f; ///< in seconds
 	static constexpr float _lpf_time_constant = 1.f; ///< in seconds
 };
@@ -47,22 +47,13 @@ using namespace matrix;
 class ZeroOrderHoverThrustEkfTest : public ::testing::Test
 {
 public:
-	struct Status {
-		float hover_thrust;
-		float hover_thrust_var;
-		float innov;
-		float innov_var;
-		float innov_test_ratio;
-		float accel_noise_var;
-	};
-
 	ZeroOrderHoverThrustEkfTest()
 	{
 		_random_generator.seed(42);
 	}
 	float computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust);
-	Status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f,
-		      float thr_noise = 0.f);
+	ZeroOrderHoverThrustEkf::status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f,
+					       float thr_noise = 0.f);

 private:
 	ZeroOrderHoverThrustEkf _ekf{};
@@ -80,28 +71,19 @@ float ZeroOrderHoverThrustEkfTest::computeAccelFromThrustAndHoverThrust(float th
 	return CONSTANTS_ONE_G * thrust / hover_thrust - CONSTANTS_ONE_G;
 }

-ZeroOrderHoverThrustEkfTest::Status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust,
-		float time,
+ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust, float time,
 		float accel_noise, float thr_noise)
 {
-	Status status{};
+	ZeroOrderHoverThrustEkf::status status{};

 	for (float t = 0.f; t <= time; t += _dt) {
 		_ekf.predict(_dt);
 		float noisy_thrust =  thrust + thr_noise * _standard_normal_distribution(_random_generator);
 		float accel_theory = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true);
 		float noisy_accel =  accel_theory + accel_noise * _standard_normal_distribution(_random_generator);
-		_ekf.fuseAccZ(noisy_accel, noisy_thrust);
+		_ekf.fuseAccZ(noisy_accel, noisy_thrust, status);
 	}

-	status.hover_thrust = _ekf.getHoverThrustEstimate();
-	status.hover_thrust_var = _ekf.getHoverThrustEstimateVar();
-
-	status.innov = _ekf.getInnovation();
-	status.innov_var = _ekf.getInnovationVar();
-	status.innov_test_ratio = _ekf.getInnovationTestRatio();
-	status.accel_noise_var = _ekf.getAccelNoiseVar();
-
 	return status;
 }

@@ -112,7 +94,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticCase)
 	const float hover_thrust_true = 0.5f;

 	// WHEN: we input noiseless data and run the filter
-	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, 2.f);
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f);

 	// THEN: The estimate should not move and its variance decrease quickly
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-4f);
@@ -128,7 +110,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergence)
 	const float hover_thrust_true = 0.72f;

 	// WHEN: we input noiseless data and run the filter
-	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, 2.f);
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f);

 	// THEN: the state should converge to the true value and its variance decrease
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-2f);
@@ -147,7 +129,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergenceWithNoise)
 	const float t_sim = 10.f;

 	// WHEN: we input noisy accel data and run the filter
-	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);

 	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
@@ -165,7 +147,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testLargeAccelNoiseAndBias)
 	const float t_sim = 15.f;

 	// WHEN: we input noisy accel data and run the filter
-	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);

 	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 7e-2);
@@ -185,7 +167,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testThrustAndAccelNoise)
 	const float t_sim = 15.f;

 	// WHEN: we input noisy accel and thrust data, and run the filter
-	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);

 	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
@@ -206,7 +188,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testHoverThrustJump)
 	float t_sim = 10.f;

 	// WHEN: we input noisy accel and thrust data, and run the filter
-	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
 	// THEN: change the hover thrust and the current thrust (the velocity controller responds quickly)
 	// Note that this is an extreme jump in hover thrust
 	thrust = 0.3;
@@ -243,8 +243,13 @@ MulticopterRateControl::Run()
 			const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed);

 			// publish rate controller status
-			rate_ctrl_status_s rate_ctrl_status{};
-			_rate_control.getRateControlStatus(rate_ctrl_status);
+			rate_ctrl_status_s &rate_ctrl_status = _rate_control.getRateControlStatus();
+
+			if (!_v_control_mode.flag_armed || _landed) {
+				Vector3f().copyTo(rate_ctrl_status.error);
+				Vector3f().copyTo(rate_ctrl_status.error_integrated);
+			}
+
 			rate_ctrl_status.timestamp = hrt_absolute_time();
 			_controller_status_pub.publish(rate_ctrl_status);

@@ -71,6 +71,12 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
 		updateIntegral(rate_error, dt);
 	}

+	rate_error.copyTo(_rate_ctrl_status.error);
+
+	_rate_ctrl_status.error_integrated[0] += rate_error(0) * dt;
+	_rate_ctrl_status.error_integrated[1] += rate_error(1) * dt;
+	_rate_ctrl_status.error_integrated[2] += rate_error(2) * dt;
+
 	return torque;
 }

@@ -104,11 +110,8 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
 			_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
 		}
 	}
-}

-void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
-{
-	rate_ctrl_status.rollspeed_integ = _rate_int(0);
-	rate_ctrl_status.pitchspeed_integ = _rate_int(1);
-	rate_ctrl_status.yawspeed_integ = _rate_int(2);
+	_rate_ctrl_status.rollspeed_integ = _rate_int(0);
+	_rate_ctrl_status.pitchspeed_integ = _rate_int(1);
+	_rate_ctrl_status.yawspeed_integ = _rate_int(2);
 }
@@ -98,7 +98,7 @@ public:
 	 * Get status message of controller for logging/debugging
 	 * @param rate_ctrl_status status message to fill with internal states
 	 */
-	void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);
+	rate_ctrl_status_s &getRateControlStatus() { return _rate_ctrl_status; }

 private:
 	void updateIntegral(matrix::Vector3f &rate_error, const float dt);
@@ -115,4 +115,6 @@ private:

 	bool _mixer_saturation_positive[3] {};
 	bool _mixer_saturation_negative[3] {};
+
+	rate_ctrl_status_s _rate_ctrl_status{};
 };
@@ -35,8 +35,6 @@

 #include <px4_platform_common/log.h>

-#include <uORB/topics/vehicle_imu_status.h>
-
 using namespace matrix;
 using namespace time_literals;

@@ -47,7 +45,7 @@ VehicleAcceleration::VehicleAcceleration() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
-	CheckAndUpdateFilters();
+	_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
 }

 VehicleAcceleration::~VehicleAcceleration()
@@ -67,6 +65,7 @@ bool VehicleAcceleration::Start()
 	}

 	if (!SensorSelectionUpdate(true)) {
+		_selected_sensor_sub_index = 0;
 		_sensor_sub.registerCallback();
 	}

@@ -82,46 +81,58 @@ void VehicleAcceleration::Stop()
 	Deinit();
 }

-void VehicleAcceleration::CheckAndUpdateFilters()
+void VehicleAcceleration::CheckFilters()
 {
-	bool sample_rate_changed = false;
+	if (_interval_count > 1000) {
+		bool reset_filters = false;

-	// get sample rate from vehicle_imu_status publication
-	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-		uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
+		// calculate sensor update rate
+		const float sample_interval_avg = _interval_sum / _interval_count;

-		const float sample_rate_hz = imu_status.get().accel_rate_hz;
+		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
+
+			_update_rate_hz = 1.e6f / sample_interval_avg;

-		if ((imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id())
-		    && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
 			// check if sample rate error is greater than 1%
-			if ((fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
-				PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
-				_filter_sample_rate = sample_rate_hz;
-				sample_rate_changed = true;
+			if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
+				reset_filters = true;
+			}

-				// determine number of sensor samples that will get closest to the desired rate
+			if (reset_filters || (_required_sample_updates == 0)) {
 				if (_param_imu_integ_rate.get() > 0) {
+					// determine number of sensor samples that will get closest to the desired rate
 					const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
-					const float sample_interval_avg = 1e6f / sample_rate_hz;
 					const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
 										(float)sensor_accel_s::ORB_QUEUE_LENGTH);

 					_sensor_sub.set_required_updates(samples);
+					_required_sample_updates = samples;

 				} else {
 					_sensor_sub.set_required_updates(1);
+					_required_sample_updates = 1;
 				}
-
-				break;
 			}
 		}
-	}

-	// update software low pass filters
-	if (sample_rate_changed || (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.1f)) {
-		_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
-		_lp_filter.reset(_acceleration_prev);
+		if (!reset_filters) {
+			// accel low pass cutoff frequency changed
+			if (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f) {
+				reset_filters = true;
+			}
+		}
+
+		if (reset_filters) {
+			PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
+			_filter_sample_rate = _update_rate_hz;
+
+			// update software low pass filters
+			_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
+			_lp_filter.reset(_acceleration_prev);
+		}
+
+		// reset sample interval accumulator
+		_timestamp_sample_last = 0;
 	}
 }

@@ -140,7 +151,7 @@ void VehicleAcceleration::SensorBiasUpdate(bool force)
 		estimator_sensor_bias_s bias;

 		if (_estimator_sensor_bias_sub.copy(&bias)) {
-			if (bias.accel_device_id == _calibration.device_id()) {
+			if (bias.accel_device_id == _selected_sensor_device_id) {
 				_bias = Vector3f{bias.accel_bias};

 			} else {
@@ -152,27 +163,31 @@ void VehicleAcceleration::SensorBiasUpdate(bool force)

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

-		if ((sensor_selection.accel_device_id != 0) && (_calibration.device_id() != sensor_selection.accel_device_id)) {
+		if (_selected_sensor_device_id != sensor_selection.accel_device_id) {
 			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 				uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};

-				const uint32_t device_id = sensor_accel_sub.get().device_id;
-
-				if ((device_id != 0) && (device_id == sensor_selection.accel_device_id)) {
+				if ((sensor_accel_sub.get().device_id != 0) && (sensor_accel_sub.get().device_id == sensor_selection.accel_device_id)) {

 					if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
-						PX4_DEBUG("selected sensor changed %d -> %d", _calibration.device_id(), device_id);
+						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
+
+						// record selected sensor (array index)
+						_selected_sensor_sub_index = i;
+						_selected_sensor_device_id = sensor_selection.accel_device_id;

 						// clear bias and corrections
 						_bias.zero();

-						_calibration.set_device_id(device_id);
+						_calibration.set_device_id(sensor_accel_sub.get().device_id);

-						CheckAndUpdateFilters();
+						// reset sample interval accumulator on sensor change
+						_timestamp_sample_last = 0;
+						_required_sample_updates = 0;

 						return true;
 					}
@@ -180,7 +195,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 			}

 			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id);
-			_calibration.set_device_id(0);
+			_selected_sensor_device_id = 0;
+			_selected_sensor_sub_index = 0;
 		}
 	}

@@ -198,8 +214,6 @@ void VehicleAcceleration::ParametersUpdate(bool force)
 		updateParams();

 		_calibration.ParametersUpdate();
-
-		CheckAndUpdateFilters();
 	}
 }

@@ -220,6 +234,20 @@ void VehicleAcceleration::Run()

 	while (_sensor_sub.update(&sensor_data)) {

+		// collect sample interval average for filters
+		if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
+			_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
+			_interval_count++;
+
+		} else {
+			_interval_sum = 0.f;
+			_interval_count = 0.f;
+		}
+
+		_timestamp_sample_last = sensor_data.timestamp_sample;
+
+		CheckFilters();
+
 		// Apply calibration and filter
 		//  - calibration offsets, scale factors, and thermal scale (if available)
 		//  - estimated in run bias (if available)
@@ -245,9 +273,9 @@ void VehicleAcceleration::Run()

 void VehicleAcceleration::PrintStatus()
 {
-	PX4_INFO("selected sensor: %d, rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]",
-		 _calibration.device_id(), (double)_filter_sample_rate,
-		 (double)_bias(0), (double)_bias(1), (double)_bias(2));
+	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
+		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
+	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));

 	_calibration.PrintStatus();
 }
@@ -68,7 +68,7 @@ public:
 private:
 	void Run() override;

-	void CheckAndUpdateFilters();
+	void CheckFilters();
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
 	bool SensorSelectionUpdate(bool force = false);
@@ -86,14 +86,25 @@ private:

 	calibration::Accelerometer _calibration{};

-	matrix::Vector3f _bias{};
+	matrix::Vector3f _bias{0.f, 0.f, 0.f};

-	matrix::Vector3f _acceleration_prev{};
+	matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f};
+
+	static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
+	float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
+
+	uint8_t _required_sample_updates{0}; /**< number or sensor publications required for configured rate */
+
+	math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f};

-	static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
 	float _filter_sample_rate{kInitialRateHz};

-	math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.f};
+	uint32_t _selected_sensor_device_id{0};
+	uint8_t _selected_sensor_sub_index{0};
+
+	hrt_abstime _timestamp_sample_last{0};
+	float _interval_sum{0.f};
+	float _interval_count{0.f};

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
@@ -35,8 +35,6 @@

 #include <px4_platform_common/log.h>

-#include <uORB/topics/vehicle_imu_status.h>
-
 using namespace matrix;
 using namespace time_literals;

@@ -47,7 +45,10 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	CheckAndUpdateFilters();
+	_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
+	_notch_filter_velocity.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
+
+	_lp_filter_acceleration.set_cutoff_frequency(kInitialRateHz, _param_imu_dgyro_cutoff.get());
 }

 VehicleAngularVelocity::~VehicleAngularVelocity()
@@ -67,6 +68,7 @@ bool VehicleAngularVelocity::Start()
 	}

 	if (!SensorSelectionUpdate(true)) {
+		_selected_sensor_sub_index = 0;
 		_sensor_sub.registerCallback();
 	}

@@ -82,45 +84,75 @@ void VehicleAngularVelocity::Stop()
 	Deinit();
 }

-void VehicleAngularVelocity::CheckAndUpdateFilters()
+void VehicleAngularVelocity::CheckFilters()
 {
-	bool sample_rate_changed = false;
+	if (_interval_count > 1000) {
+		bool reset_filters = false;

-	// get sample rate from vehicle_imu_status publication
-	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-		uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
+		// calculate sensor update rate
+		const float sample_interval_avg = _interval_sum / _interval_count;

-		const float sample_rate_hz = imu_status.get().gyro_rate_hz;
+		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
+
+			_update_rate_hz = 1.e6f / sample_interval_avg;

-		if ((imu_status.get().gyro_device_id != 0) && (imu_status.get().gyro_device_id == _calibration.device_id())
-		    && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
 			// check if sample rate error is greater than 1%
-			if ((fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
-				PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
-				_filter_sample_rate = sample_rate_hz;
-				sample_rate_changed = true;
-				break;
+			if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
+				reset_filters = true;
+			}
+
+			if (reset_filters || (_required_sample_updates == 0)) {
+				if (_param_imu_gyro_rate_max.get() > 0) {
+					// determine number of sensor samples that will get closest to the desired rate
+					const float configured_interval_us = 1e6f / _param_imu_gyro_rate_max.get();
+					const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
+										(float)sensor_gyro_s::ORB_QUEUE_LENGTH);
+
+					_sensor_sub.set_required_updates(samples);
+					_required_sample_updates = samples;
+
+				} else {
+					_sensor_sub.set_required_updates(1);
+					_required_sample_updates = 1;
+				}
 			}
 		}
-	}

-	// update software low pass filters
-	if (sample_rate_changed || (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.1f)) {
-		_lp_filter_velocity.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get());
-		_lp_filter_velocity.reset(_angular_velocity_prev);
-	}
+		if (!reset_filters) {
+			// gyro low pass cutoff frequency changed
+			if (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) {
+				reset_filters = true;
+			}

-	if (sample_rate_changed
-	    || (fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.1f)
-	    || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.1f)
-	   ) {
-		_notch_filter_velocity.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
-		_notch_filter_velocity.reset(_angular_velocity_prev);
-	}
+			// gyro notch filter frequency or bandwidth changed
+			if ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
+			    || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)) {
+				reset_filters = true;
+			}

-	if (sample_rate_changed || (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.1f)) {
-		_lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get());
-		_lp_filter_acceleration.reset(_angular_acceleration_prev);
+			// gyro derivative low pass cutoff changed
+			if (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
+				reset_filters = true;
+			}
+		}
+
+		if (reset_filters) {
+			PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
+			_filter_sample_rate = _update_rate_hz;
+
+			// update software low pass filters
+			_lp_filter_velocity.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get());
+			_lp_filter_velocity.reset(_angular_velocity_prev);
+
+			_notch_filter_velocity.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
+			_notch_filter_velocity.reset(_angular_velocity_prev);
+
+			_lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get());
+			_lp_filter_acceleration.reset(_angular_acceleration_prev);
+		}
+
+		// reset sample interval accumulator
+		_timestamp_sample_last = 0;
 	}
 }

@@ -139,7 +171,7 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 		estimator_sensor_bias_s bias;

 		if (_estimator_sensor_bias_sub.copy(&bias)) {
-			if (bias.gyro_device_id == _calibration.device_id()) {
+			if (bias.gyro_device_id == _selected_sensor_device_id) {
 				_bias = Vector3f{bias.gyro_bias};

 			} else {
@@ -151,27 +183,31 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force)

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

-		if ((sensor_selection.gyro_device_id != 0) && (_calibration.device_id() != sensor_selection.gyro_device_id)) {
+		if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
 			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 				uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};

-				const uint32_t device_id = sensor_gyro_sub.get().device_id;
-
-				if ((device_id != 0) && (device_id == sensor_selection.gyro_device_id)) {
+				if ((sensor_gyro_sub.get().device_id != 0) && (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id)) {

 					if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
-						PX4_DEBUG("selected sensor changed %d -> %d", _calibration.device_id(), device_id);
+						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
+
+						// record selected sensor (array index)
+						_selected_sensor_sub_index = i;
+						_selected_sensor_device_id = sensor_selection.gyro_device_id;

 						// clear bias and corrections
 						_bias.zero();

-						_calibration.set_device_id(device_id);
+						_calibration.set_device_id(sensor_gyro_sub.get().device_id);

-						CheckAndUpdateFilters();
+						// reset sample interval accumulator on sensor change
+						_timestamp_sample_last = 0;
+						_required_sample_updates = 0;

 						return true;
 					}
@@ -179,7 +215,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 			}

 			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.gyro_device_id);
-			_calibration.set_device_id(0);
+			_selected_sensor_device_id = 0;
+			_selected_sensor_sub_index = 0;
 		}
 	}

@@ -197,8 +234,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 		updateParams();

 		_calibration.ParametersUpdate();
-
-		CheckAndUpdateFilters();
 	}
 }

@@ -219,6 +254,18 @@ void VehicleAngularVelocity::Run()

 	while (_sensor_sub.update(&sensor_data)) {

+		// collect sample interval average for filters
+		if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
+			_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
+			_interval_count++;
+
+		} else {
+			_interval_sum = 0.f;
+			_interval_count = 0.f;
+		}
+
+		_timestamp_sample_last = sensor_data.timestamp_sample;
+
 		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 		const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f);
 		_timestamp_sample_prev = sensor_data.timestamp_sample;
@@ -243,6 +290,7 @@ void VehicleAngularVelocity::Run()
 		_angular_acceleration_prev = angular_acceleration_raw;
 		const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};

+		CheckFilters();

 		// publish once all new samples are processed
 		if (!_sensor_sub.updated()) {
@@ -280,9 +328,9 @@ void VehicleAngularVelocity::Run()

 void VehicleAngularVelocity::PrintStatus()
 {
-	PX4_INFO("selected sensor: %d, rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]",
-		 _calibration.device_id(), (double)_filter_sample_rate,
-		 (double)_bias(0), (double)_bias(1), (double)_bias(2));
+	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
+		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
+	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));

 	_calibration.PrintStatus();
 }
@@ -70,7 +70,7 @@ public:
 private:
 	void Run() override;

-	void CheckAndUpdateFilters();
+	void CheckFilters();
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
 	bool SensorSelectionUpdate(bool force = false);
@@ -89,22 +89,33 @@ private:

 	calibration::Gyroscope _calibration{};

-	matrix::Vector3f _bias{};
+	matrix::Vector3f _bias{0.f, 0.f, 0.f};

-	matrix::Vector3f _angular_acceleration_prev{};
-	matrix::Vector3f _angular_velocity_prev{};
+	matrix::Vector3f _angular_acceleration_prev{0.f, 0.f, 0.f};
+	matrix::Vector3f _angular_velocity_prev{0.f, 0.f, 0.f};
 	hrt_abstime _timestamp_sample_prev{0};

 	hrt_abstime _last_publish{0};
-	static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
-	float _filter_sample_rate{kInitialRateHz};
+	static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
+	float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
+
+	uint8_t _required_sample_updates{0}; /**< number or sensor publications required for configured rate */

 	// angular velocity filters
-	math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.f};
+	math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.0f};
 	math::NotchFilter<matrix::Vector3f> _notch_filter_velocity{};

 	// angular acceleration filter
-	math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 30.f};
+	math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 30.0f};
+
+	float _filter_sample_rate{kInitialRateHz};
+
+	uint32_t _selected_sensor_device_id{0};
+	uint8_t _selected_sensor_sub_index{0};
+
+	hrt_abstime _timestamp_sample_last{0};
+	float _interval_sum{0.f};
+	float _interval_count{0.f};

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
@@ -226,7 +226,7 @@ void VehicleIMU::Run()
 			if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
 				update_integrator_config = true;
 				publish_status = true;
-				_status.gyro_rate_hz = 1e6f / _gyro_interval.update_interval;
+				_status.gyro_rate_hz = roundf(1e6f / _gyro_interval.update_interval);
 			}
 		}

@@ -272,7 +272,7 @@ void VehicleIMU::Run()
 			if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
 				update_integrator_config = true;
 				publish_status = true;
-				_status.accel_rate_hz = 1e6f / _accel_interval.update_interval;
+				_status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
 			}
 		}

@@ -69,14 +69,14 @@ int Simulator::start(int argc, char *argv[])
 	_instance = new Simulator();

 	if (_instance) {
-		if (argc == 5 && strcmp(argv[3], "-u") == 0) {
+		if (argc == 4 && strcmp(argv[2], "-u") == 0) {
 			_instance->set_ip(InternetProtocol::UDP);
-			_instance->set_port(atoi(argv[4]));
+			_instance->set_port(atoi(argv[3]));
 		}

-		if (argc == 5 && strcmp(argv[3], "-c") == 0) {
+		if (argc == 4 && strcmp(argv[2], "-c") == 0) {
 			_instance->set_ip(InternetProtocol::TCP);
-			_instance->set_port(atoi(argv[4]));
+			_instance->set_port(atoi(argv[3]));
 		}

 		_instance->run();
@@ -163,9 +163,16 @@ static int vmount_thread_main(int argc, char *argv[])

 	InputTest *test_input = nullptr;

-	// We don't need the task name.
+#ifdef __PX4_NUTTX
+	/* the NuttX optarg handler does not
+	 * ignore argv[0] like the POSIX handler
+	 * does, nor does it deal with non-flag
+	 * verbs well. So we Remove the application
+	 * name and the verb.
+	 */
 	argc -= 1;
 	argv += 1;
+#endif

 	if (argc > 0 && !strcmp(argv[0], "test")) {
 		PX4_INFO("Starting in test mode");
@@ -309,11 +309,6 @@ void Standard::update_transition_state()
 		if (_motor_state != motor_state::ENABLED) {
 			_motor_state = set_motor_state(_motor_state, motor_state::ENABLED);
 		}
-
-		// set idle speed for MC actuators
-		if (!_flag_idle_mc) {
-			_flag_idle_mc = set_idle_mc();
-		}
 	}

 	mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);
@@ -342,77 +337,78 @@ void Standard::update_fw_state()
 */
 void Standard::fill_actuator_outputs()
 {
-	auto &mc_in = _actuators_mc_in->control;
-	auto &fw_in = _actuators_fw_in->control;
-
-	auto &mc_out = _actuators_out_0->control;
-	auto &fw_out = _actuators_out_1->control;
-
-	const bool elevon_lock = (_params->elevons_mc_lock == 1);
-
-	switch (_vtol_schedule.flight_mode) {
-	case vtol_mode::MC_MODE:
-
-		// MC out = MC in
-		mc_out[actuator_controls_s::INDEX_ROLL]         = mc_in[actuator_controls_s::INDEX_ROLL];
-		mc_out[actuator_controls_s::INDEX_PITCH]        = mc_in[actuator_controls_s::INDEX_PITCH];
-		mc_out[actuator_controls_s::INDEX_YAW]          = mc_in[actuator_controls_s::INDEX_YAW];
-		mc_out[actuator_controls_s::INDEX_THROTTLE]     = mc_in[actuator_controls_s::INDEX_THROTTLE];
-		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = mc_in[actuator_controls_s::INDEX_LANDING_GEAR];
-
-		// FW out = 0, other than roll and pitch depending on elevon lock
-		fw_out[actuator_controls_s::INDEX_ROLL]         = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH]        = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_PITCH];
-		fw_out[actuator_controls_s::INDEX_YAW]          = 0;
-		fw_out[actuator_controls_s::INDEX_THROTTLE]     = _pusher_throttle;
-		fw_out[actuator_controls_s::INDEX_FLAPS]        = 0;
-		fw_out[actuator_controls_s::INDEX_AIRBRAKES]    = 0;
-
-		break;
-
-	case vtol_mode::TRANSITION_TO_FW:
-
-	// FALLTHROUGH
-	case vtol_mode::TRANSITION_TO_MC:
-		// MC out = MC in (weighted)
-		mc_out[actuator_controls_s::INDEX_ROLL]         = mc_in[actuator_controls_s::INDEX_ROLL]     * _mc_roll_weight;
-		mc_out[actuator_controls_s::INDEX_PITCH]        = mc_in[actuator_controls_s::INDEX_PITCH]    * _mc_pitch_weight;
-		mc_out[actuator_controls_s::INDEX_YAW]          = mc_in[actuator_controls_s::INDEX_YAW]      * _mc_yaw_weight;
-		mc_out[actuator_controls_s::INDEX_THROTTLE]     = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
-		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = 0;
-
-		// FW out = FW in, with VTOL transition controlling throttle and airbrakes
-		fw_out[actuator_controls_s::INDEX_ROLL]         = fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH]        = fw_in[actuator_controls_s::INDEX_PITCH];
-		fw_out[actuator_controls_s::INDEX_YAW]          = fw_in[actuator_controls_s::INDEX_YAW];
-		fw_out[actuator_controls_s::INDEX_THROTTLE]     = _pusher_throttle;
-		fw_out[actuator_controls_s::INDEX_FLAPS]        = fw_in[actuator_controls_s::INDEX_FLAPS];
-		fw_out[actuator_controls_s::INDEX_AIRBRAKES]    = _reverse_output;
-
-		break;
-
-	case vtol_mode::FW_MODE:
-		// MC out = 0
-		mc_out[actuator_controls_s::INDEX_ROLL]         = 0;
-		mc_out[actuator_controls_s::INDEX_PITCH]        = 0;
-		mc_out[actuator_controls_s::INDEX_YAW]          = 0;
-		mc_out[actuator_controls_s::INDEX_THROTTLE]     = 0;
-		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = 0;
-
-		// FW out = FW in
-		fw_out[actuator_controls_s::INDEX_ROLL]         = fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH]        = fw_in[actuator_controls_s::INDEX_PITCH];
-		fw_out[actuator_controls_s::INDEX_YAW]          = fw_in[actuator_controls_s::INDEX_YAW];
-		fw_out[actuator_controls_s::INDEX_THROTTLE]     = fw_in[actuator_controls_s::INDEX_THROTTLE];
-		fw_out[actuator_controls_s::INDEX_FLAPS]        = fw_in[actuator_controls_s::INDEX_FLAPS];
-		fw_out[actuator_controls_s::INDEX_AIRBRAKES]    = 0;
-		break;
-	}
-
+	// multirotor controls
+	_actuators_out_0->timestamp = hrt_absolute_time();
 	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+
+	// roll
+	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
+	// pitch
+	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+	// yaw
+	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
+	// throttle
+	_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
+
+
+	// fixed wing controls
+	_actuators_out_1->timestamp = hrt_absolute_time();
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;

-	_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
+	if (_vtol_schedule.flight_mode != vtol_mode::MC_MODE) {
+		// roll
+		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
+
+		// pitch
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
+		// yaw
+		_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_YAW];
+
+		_actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = _reverse_output;
+
+	} else {
+
+		if (_params->elevons_mc_lock) {
+			// zero outputs when inactive
+			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f;
+			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0.0f;
+			_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f;
+			_actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = 0.0f;
+
+		} else {
+			// roll
+			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
+				_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
+
+			// pitch
+			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+				_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
+
+			_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f;
+			_actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = 0.0f;
+		}
+	}
+
+	// set the fixed wing throttle control
+	if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+
+		// take the throttle value commanded by the fw controller
+		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
+
+	} else {
+		// otherwise we may be ramping up the throttle during the transition to fw mode
+		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
+	}
+
+
 }

 void
@@ -232,8 +232,8 @@ void Tailsitter::update_transition_state()

 		const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;

-		if (!_flag_idle_mc) {
-			_flag_idle_mc = set_idle_mc();
+		if (!flag_idle_mc) {
+			flag_idle_mc = set_idle_mc();
 		}

 		if (tilt > 0.01f) {
@@ -280,34 +280,36 @@ void Tailsitter::update_fw_state()
 */
 void Tailsitter::fill_actuator_outputs()
 {
-	auto &mc_in = _actuators_mc_in->control;
-	auto &fw_in = _actuators_fw_in->control;
+	_actuators_out_0->timestamp = hrt_absolute_time();
+	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;

-	auto &mc_out = _actuators_out_0->control;
-	auto &fw_out = _actuators_out_1->control;
+	_actuators_out_1->timestamp = hrt_absolute_time();
+	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;

-	mc_out[actuator_controls_s::INDEX_ROLL]  = mc_in[actuator_controls_s::INDEX_ROLL]  * _mc_roll_weight;
-	mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
-	mc_out[actuator_controls_s::INDEX_YAW]   = mc_in[actuator_controls_s::INDEX_YAW]   * _mc_yaw_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
+			* _mc_roll_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
+			_mc_yaw_weight;

 	if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
-		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];

 	} else {
-		mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE];
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
 	}

 	if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
-		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
-		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
+		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0;
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0;

 	} else {
-		fw_out[actuator_controls_s::INDEX_ROLL]  = fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
+		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
 	}
-
-	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
-	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
-
-	_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
 }
@@ -351,8 +351,8 @@ void Tiltrotor::update_transition_state()


 		// set idle speed for rotary wing mode
-		if (!_flag_idle_mc) {
-			_flag_idle_mc = set_idle_mc();
+		if (!flag_idle_mc) {
+			flag_idle_mc = set_idle_mc();
 		}

 		// tilt rotors back
@@ -388,6 +388,8 @@ void Tiltrotor::update_transition_state()
 	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
 	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
 	_mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
+
+
 }

 void Tiltrotor::waiting_on_tecs()
@@ -401,47 +403,51 @@ void Tiltrotor::waiting_on_tecs()
 */
 void Tiltrotor::fill_actuator_outputs()
 {
-	auto &mc_in = _actuators_mc_in->control;
-	auto &fw_in = _actuators_fw_in->control;
-
-	auto &mc_out = _actuators_out_0->control;
-	auto &fw_out = _actuators_out_1->control;
-
 	// Multirotor output
-	mc_out[actuator_controls_s::INDEX_ROLL]  = mc_in[actuator_controls_s::INDEX_ROLL]  * _mc_roll_weight;
-	mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
-	mc_out[actuator_controls_s::INDEX_YAW]   = mc_in[actuator_controls_s::INDEX_YAW]   * _mc_yaw_weight;
+	_actuators_out_0->timestamp = hrt_absolute_time();
+	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+
+	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;

 	if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
-		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];

 		/* allow differential thrust if enabled */
 		if (_params->diff_thrust == 1) {
-			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
+			_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] =
+				_actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
 		}

 	} else {
-		mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
 	}

 	// Fixed wing output
-	fw_out[4] = _tilt_control;
-
-	if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
-		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
-		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
-		fw_out[actuator_controls_s::INDEX_YAW]   = 0;
-
-	} else {
-		fw_out[actuator_controls_s::INDEX_ROLL]  = fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
-		fw_out[actuator_controls_s::INDEX_YAW]   = fw_in[actuator_controls_s::INDEX_YAW];
-	}
-
-	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+	_actuators_out_1->timestamp = hrt_absolute_time();
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;

-	_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
+	_actuators_out_1->control[4] = _tilt_control;
+
+	if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f;
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0.0f;
+		_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f;
+
+	} else {
+		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
+		_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_YAW];
+	}
 }

 /*
@@ -455,4 +461,5 @@ float Tiltrotor::thrust_compensation_for_tilt()

 	// increase vertical thrust by 1/cos(tilt), limmit to [-1,0]
 	return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f);
+
 }
@@ -123,8 +123,8 @@ bool VtolType::init()

 void VtolType::update_mc_state()
 {
-	if (!_flag_idle_mc) {
-		_flag_idle_mc = set_idle_mc();
+	if (!flag_idle_mc) {
+		flag_idle_mc = set_idle_mc();
 	}

 	if (_motor_state != motor_state::ENABLED) {
@@ -142,8 +142,8 @@ void VtolType::update_mc_state()

 void VtolType::update_fw_state()
 {
-	if (_flag_idle_mc) {
-		_flag_idle_mc = !set_idle_fw();
+	if (flag_idle_mc) {
+		flag_idle_mc = !set_idle_fw();
 	}

 	if (_motor_state != motor_state::DISABLED) {
@@ -214,7 +214,7 @@ protected:

 	struct Params 					*_params;

-	bool _flag_idle_mc = false;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
+	bool flag_idle_mc = false;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"

 	bool _pusher_active = false;
 	float _mc_roll_weight = 1.0f;	// weight for multicopter attitude controller roll output
@@ -53,7 +53,6 @@ public:
 	virtual bool run_tests();

 	bool test_add();
-	bool test_add_back();
 	bool test_remove();
 	bool test_range_based_for();
 	bool test_reinsert();
@@ -63,7 +62,6 @@ public:
 bool ListTest::run_tests()
 {
 	ut_run_test(test_add);
-	ut_run_test(test_add_back);
 	ut_run_test(test_remove);
 	ut_run_test(test_range_based_for);
 	ut_run_test(test_reinsert);
@@ -92,45 +90,6 @@ bool ListTest::test_add()
 	// verify full size (100)
 	ut_assert_true(list1.size() == 100);

-	int i = 99;
-
-	for (auto t : list1) {
-		// verify all elements were inserted in order
-		ut_compare("stored i", i, t->i);
-		i--;
-	}
-
-	// delete all elements
-	list1.clear();
-
-	// verify list has been cleared
-	ut_assert_true(list1.empty());
-	ut_compare("size 0", list1.size(), 0);
-
-	return true;
-}
-
-bool ListTest::test_add_back()
-{
-	List<testContainer *> list1;
-
-	// size should be 0 initially
-	ut_compare("size initially 0", list1.size(), 0);
-	ut_assert_true(list1.empty());
-
-	// insert 100
-	for (int i = 0; i < 100; i++) {
-		testContainer *t = new testContainer();
-		t->i = i;
-		list1.addBack(t);
-
-		ut_compare("size increasing with i", list1.size(), i + 1);
-		ut_assert_true(!list1.empty());
-	}
-
-	// verify full size (100)
-	ut_assert_true(list1.size() == 100);
-
 	int i = 0;

 	for (auto t : list1) {
@@ -225,27 +184,27 @@ bool ListTest::test_range_based_for()
 		ut_assert_true(!list1.empty());
 	}

-	// first element should be 0 (last added)
-	ut_compare("first 0", list1.getHead()->i, 0);
+	// first element should be 99 (first added)
+	ut_compare("first 0", list1.getHead()->i, 99);

 	// verify all elements were inserted in order
-	int i = 0;
+	int i = 99;
 	auto t1 = list1.getHead();

 	while (t1 != nullptr) {
 		ut_compare("check count", i, t1->i);
 		t1 = t1->getSibling();
-		i++;
+		i--;
 	}

 	// verify full size (100)
 	ut_compare("size check", list1.size(), 100);

-	i = 0;
+	i = 99;

 	for (auto t2 : list1) {
 		ut_compare("range based for i", i, t2->i);
-		i++;
+		i--;
 	}

 	// verify full size (100)
@@ -68,9 +68,9 @@ task_main(int argc, char *argv[])
 {
 	char buffer[DM_MAX_DATA_SIZE];

-	PX4_INFO("Starting dataman test task %s", argv[2]);
+	PX4_INFO("Starting dataman test task %s", argv[1]);
 	/* try to read an invalid item */
-	int my_id = atoi(argv[2]);
+	int my_id = atoi(argv[1]);

 	/* try to read an invalid item */
 	if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {