fix rangefinder params

* move P-init to ekf-derivation script

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -15,7 +15,6 @@ param set-default NAV_ACC_RAD 0.5
 
 # Differential Parameters
 param set-default RD_WHEEL_TRACK 0.3
-param set-default RD_MAX_THR_YAW_R 1.5
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 
@@ -31,14 +30,16 @@ param set-default RO_YAW_RATE_P 0.25
 param set-default RO_YAW_RATE_LIM 180
 param set-default RO_YAW_ACCEL_LIM 120
 param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.43
 
 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 5
 
-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-defatul RO_SPEED_RED 1
 
 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -1,51 +1,68 @@
 #!/bin/sh
-# @name Gazebo lawnmower
+# @name Gazebo - Zero Turn Lawnmower
 # @type Rover
 # @class Rover
 
 . ${R}etc/init.d/rc.rover_differential_defaults
 
 PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
-PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=lawn}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
 
 param set-default SIM_GZ_EN 1 # Gazebo bridge
 
+param set-default NAV_ACC_RAD 0.5
+
 # We can arm and drive in manual mode when it slides and GPS check fails:
 param set-default COM_ARM_WO_GPS 1
 
-# Rover parameters
+# Differential Parameters
 param set-default RD_WHEEL_TRACK 0.9
-param set-default RD_YAW_RATE_I 0.1
-param set-default RD_YAW_RATE_P 5
-param set-default RD_MAX_ACCEL 1
-param set-default RD_MAX_JERK 3
-param set-default RD_MAX_SPEED 8
-param set-default RD_YAW_P 5
-param set-default RD_YAW_I 0.1
-param set-default RD_MAX_YAW_RATE 30
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 
+# Rover Control Parameters
+param set-default RO_ACCEL_LIM 1.5
+param set-default RO_DECEL_LIM 5
+param set-default RO_JERK_LIM 15
+param set-default RO_MAX_THR_SPEED 2.7
+
+# Rover Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.2
+param set-default RO_YAW_RATE_P 1.0
+param set-default RO_YAW_RATE_LIM 60
+param set-default RO_YAW_ACCEL_LIM 50
+param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.0
+
+# Rover Attitude Control Parameters
+param set-default RO_YAW_P 5
+
+# Rover Velocity Control Parameters
+param set-default RO_SPEED_LIM 2.1
+param set-default RO_SPEED_I 0.01
+param set-default RO_SPEED_P 0.1
+param set-defatul RO_SPEED_RED 1.0
+
 # Pure pursuit parameters
-param set-default PP_LOOKAHD_MAX 30
-param set-default PP_LOOKAHD_MIN 2
 param set-default PP_LOOKAHD_GAIN 1
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1
 
 # Actuator mapping - set SITL motors/servos output parameters:
 
 # "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
 param set-default SIM_GZ_WH_FUNC1 101 # right wheel
-#param set-default SIM_GZ_WH_MIN1 0
-#param set-default SIM_GZ_WH_MAX1 200
-#param set-default SIM_GZ_WH_DIS1 100
-#param set-default SIM_GZ_WH_FAIL1 100
+param set-default SIM_GZ_WH_MIN1 87
+param set-default SIM_GZ_WH_MAX1 113
+param set-default SIM_GZ_WH_DIS1 100
+param set-default SIM_GZ_WH_FAIL1 100
 
 param set-default SIM_GZ_WH_FUNC2 102 # left wheel
-#param set-default SIM_GZ_WH_MIN2 0
-#param set-default SIM_GZ_WH_MAX2 200
-#param set-default SIM_GZ_WH_DIS2 100
-#param set-default SIM_GZ_WH_FAIL2 100
+param set-default SIM_GZ_WH_MIN2 87
+param set-default SIM_GZ_WH_MAX2 113
+param set-default SIM_GZ_WH_DIS2 100
+param set-default SIM_GZ_WH_FAIL2 100
 
 param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
 

ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann

@@ -30,14 +30,16 @@ param set-default RO_MAX_THR_SPEED 3.1
 param set-default RO_YAW_RATE_I 0.1
 param set-default RO_YAW_RATE_P 1
 param set-default RO_YAW_RATE_LIM 180
+param set-default RO_YAW_RATE_CORR 1
 
 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 3
 
-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 3
 param set-default RO_SPEED_I 0.1
 param set-default RO_SPEED_P 1
+param set-default RO_SPEED_RED 1
 
 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1

ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum

@@ -15,8 +15,6 @@ param set-default NAV_ACC_RAD 0.5
 
 # Mecanum Parameters
 param set-default RM_WHEEL_TRACK 0.3
-param set-default RM_MAX_THR_YAW_R 1.2
-param set-default RM_MISS_SPD_GAIN 1
 
 # Rover Control Parameters
 param set-default RO_ACCEL_LIM 3
@@ -30,14 +28,16 @@ param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 120
 param set-default RO_YAW_ACCEL_LIM 240
 param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.75
 
 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 5
 
-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2
 param set-default RO_SPEED_I 0.5
 param set-default RO_SPEED_P 1
+param set-default RO_SPEED_RED 1
 
 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 0.5

ROMFS/px4fmu_common/init.d-posix/airframes/4016_gz_x500_lidar_down

@@ -8,3 +8,6 @@
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar_down}
 
 . ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+param set-default EKF2_RNG_POS_Z -0.177
+param set-default EKF2_MIN_RNG 0

ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover

@@ -26,7 +26,6 @@ param set-default NAV_ACC_RAD 0.5
 
 # Differential Parameters
 param set-default RD_WHEEL_TRACK 0.3
-param set-default RD_MAX_THR_YAW_R 0.7
 param set-default RD_TRANS_DRV_TRN 0.785398
 param set-default RD_TRANS_TRN_DRV 0.139626
 
@@ -42,14 +41,16 @@ param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 250
 param set-default RO_YAW_ACCEL_LIM 600
 param set-default RO_YAW_DECEL_LIM 600
+param set-default RO_YAW_RATE_CORR 2.7
 
 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 2.5
 
-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 1.6
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-default RO_SPEED_RED 1
 
 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1

ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho

@@ -33,14 +33,16 @@ param set-default RO_MAX_THR_SPEED 2.8
 param set-default RO_YAW_RATE_I 0.1
 param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 120
+param set-default RO_YAW_RATE_CORR 1
 
 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 2.5
 
-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2.5
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-default RO_SPEED_RED 1
 
 # Pure pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1

boards/cuav/7-nano/default.px4board

@@ -20,6 +20,7 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -42,8 +43,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_MODE_MANAGER=y
 CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/cuav/7-nano/init/rc.board_sensors

@@ -12,10 +12,17 @@ else
 fi
 
 iim42652 -s -R 22 start
-bmi088 -A -R 29 -s start
-bmi088 -G -R 29 -s start
 
-ist8310 -I -R 18 start
+bmi088 -A -R 29 -s start
+if ! bmi088 -G -R 29 -s start
+then
+	iim42653 -s -b 2 -R 22 start
+fi
+
+if ! ist8310 -I -R 18 start
+then
+	iis2mdc -I -R 37 start
+fi
 
 bmp581 -s start
 icp201xx -I start

boards/cuav/7-nano/src/spi.cpp

@@ -42,6 +42,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin2}, SPI::DRDY{GPIO::PortG, GPIO::Pin3}),
 		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortD, GPIO::Pin12}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
 	}, {GPIO::PortI, GPIO::Pin11}),
 	// initSPIBus(SPI::Bus::SPI3,{
 	// 	// no devices

docs/en/advanced_config/bootloader_update.md

@@ -9,11 +9,45 @@ This topic explains how to build the PX4 bootloader, and several methods for fla
 
 ::: info
 
-- Most boards will need to use the [Debug Probe](#debug-probe-bootloader-update) to update the bootloader.
+- You can use [QGC Bootloader Update](#qgc-bootloader-update-sys-bl-update) with firmware that includes the [`bl-update` module](../modules/modules_command.md#bl-update).
+  This is the easiest way to update the bootloader, provided the board is able to load firmware.
+- You can also use the [Debug Probe](#debug-probe-bootloader-update) to update the bootloader.
+  This is useful for updating/fixing the bootloader when the board is bricked.
 - On [FMUv6X-RT](../flight_controller/pixhawk6x-rt.md) you can [install bootloader/unbrick boards via USB](bootloader_update_v6xrt.md).
   This is useful if you don't have a debug probe.
-- On FMUv2 and some custom firmware (only) you can use [QGC Bootloader Update](#qgc-bootloader-update).
-  :::
+
+:::
+
+## QGC Bootloader Update (`SYS_BL_UPDATE`)
+
+The easiest way to update the bootloader is to first use _QGroundControl_ to install firmware that contains the desired/latest bootloader.
+You can then initiate bootloader update on next restart by setting the parameter: [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
+
+This approach can be used if the [`bl-update` module](../modules/modules_command.md#bl-update) is present in the firmware.
+The easiest way to check this is just to see if the [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE) parameter is [found in QGroundControl](../advanced_config/parameters.md#finding-a-parameter).
+
+:::warning
+Boards that include the module will have the line `CONFIG_SYSTEMCMDS_BL_UPDATE=y` in their `default.px4board` file (for examples [see this search](https://github.com/search?q=repo%3APX4%2FPX4-Autopilot+path%3A**%2Fdefault.px4board+CONFIG_SYSTEMCMDS_BL_UPDATE%3Dy&type=code)).
+You can enable this key in your own custom firmware if needed.
+:::
+
+The steps are:
+
+1. Insert an SD card (enables boot logging to debug any problems).
+1. [Update the Firmware](../config/firmware.md#custom) with an image containing the new/desired bootloader.
+
+   ::: info
+   The updated bootloader might be included the default firmware for your board or supplied in custom firmware.
+   :::
+
+1. Wait for the vehicle to reboot.
+1. [Find and enable](../advanced_config/parameters.md) the parameter [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
+1. Reboot (disconnect/reconnect the board).
+   The bootloader update will only take a few seconds.
+
+Generally at this point you may then want to [update the firmware](../config/firmware.md) again using the correct/newly installed bootloader.
+
+An specific example of this process for updating the [FMUv2 bootloader](#fmuv2-bootloader-update) is given below.
 
 ## Building the PX4 Bootloader
 
@@ -49,11 +83,9 @@ The instructions in the repo README explain how to use it.
 The following steps explain how you can "manually" update the bootloader using a [compatible Debug Probe](../debug/swd_debug.md#debug-probes-for-px4-hardware):
 
 1. Get a binary containing the bootloader (either from dev team or [build it yourself](#building-the-px4-bootloader)).
-
-1. Get a [Debug Probe](../debug/swd_debug.md#debug-probes-for-px4-hardware).
+2. Get a [Debug Probe](../debug/swd_debug.md#debug-probes-for-px4-hardware).
    Connect the probe your PC via USB and setup the `gdbserver`.
-
-1. Go into the directory containing the binary and run the command for your target bootloader in the terminal:
+3. Go into the directory containing the binary and run the command for your target bootloader in the terminal:
 
    - FMUv6X
 
@@ -78,7 +110,7 @@ The following steps explain how you can "manually" update the bootloader using a
    Bootloaders from [PX4/PX4-Bootloader](https://github.com/PX4/PX4-Bootloader) are named with the pattern `*_bl.elf`.
    :::
 
-1. The _gdb terminal_ appears and it should display the following output:
+4. The _gdb terminal_ appears and it should display the following output:
 
    ```sh
    GNU gdb (GNU Tools for Arm Embedded Processors 7-2017-q4-major) 8.0.50.20171128-git
@@ -98,28 +130,27 @@ The following steps explain how you can "manually" update the bootloader using a
    Reading symbols from px4fmuv5_bl.elf...done.
    ```
 
-1. Find your `<dronecode-probe-id>` by running an `ls` command in the **/dev/serial/by-id** directory.
-
-1. Now connect to the debug probe with the following command:
+5. Find your `<dronecode-probe-id>` by running an `ls` command in the **/dev/serial/by-id** directory.
+6. Now connect to the debug probe with the following command:
 
    ```sh
    tar ext /dev/serial/by-id/<dronecode-probe-id>
    ```
 
-1. Power on the Pixhawk with another USB cable and connect the probe to the `FMU-DEBUG` port.
+7. Power on the Pixhawk with another USB cable and connect the probe to the `FMU-DEBUG` port.
 
    ::: info
    If using a Dronecode probe you may need to remove the case in order to connect to the `FMU-DEBUG` port (e.g. on Pixhawk 4 you would do this using a T6 Torx screwdriver).
    :::
 
-1. Use the following command to scan for the Pixhawk`s SWD and connect to it:
+8. Use the following command to scan for the Pixhawk`s SWD and connect to it:
 
    ```sh
    (gdb) mon swdp_scan
    (gdb) attach 1
    ```
 
-1. Load the binary into the Pixhawk:
+9. Load the binary into the Pixhawk:
 
    ```sh
    (gdb) load
@@ -127,38 +158,10 @@ The following steps explain how you can "manually" update the bootloader using a
 
 After the bootloader has updated you can [Load PX4 Firmware](../config/firmware.md) using _QGroundControl_.
 
-## QGC Bootloader Update
-
-The easiest approach is to first use _QGroundControl_ to install firmware that contains the desired/latest bootloader.
-You can then initiate bootloader update on next restart by setting the parameter: [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
-
-This approach can only be used if [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE) is present in firmware.
-
-:::warning
-Currently only FMUv2 and some custom firmware includes the desired bootloader.
-:::
-
-The steps are:
-
-1. Insert an SD card (enables boot logging to debug any problems).
-1. [Update the Firmware](../config/firmware.md#custom) with an image containing the new/desired bootloader.
-
-   ::: info
-   The updated bootloader might be supplied in custom firmware (i.e. from the dev team), or it or may be included in the latest main branch.
-   :::
-
-1. Wait for the vehicle to reboot.
-1. [Find and enable](../advanced_config/parameters.md) the parameter [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
-1. Reboot (disconnect/reconnect the board).
-   The bootloader update will only take a few seconds.
-
-Generally at this point you may then want to [update the firmware](../config/firmware.md) again using the correct/newly installed bootloader.
-
-An specific example of this process for updating the FMUv2 bootloader is given below.
-
-### FMUv2 Bootloader Update
+## FMUv2 Bootloader Update
 
 If _QGroundControl_ installs the FMUv2 target (see console during installation), and you have a newer board, you may need to update the bootloader in order to access all the memory on your flight controller.
+This example explains how you can use [QGC Bootloader Update](qgc-bootloader-update-sys-bl-update) to update the bootloader.
 
 ::: info
 Early FMUv2 [Pixhawk-series](../flight_controller/pixhawk_series.md#fmu_versions) flight controllers had a [hardware issue](../flight_controller/silicon_errata.md#fmuv2-pixhawk-silicon-errata) that restricted them to using 1MB of flash memory.
@@ -168,17 +171,17 @@ The problem is fixed on newer boards, but you may need to update the factory-pro
 To update the bootloader:
 
 1. Insert an SD card (enables boot logging to debug any problems).
-1. [Update the Firmware](../config/firmware.md) to PX4 _master_ version (when updating the firmware, check **Advanced settings** and then select **Developer Build (master)** from the dropdown list).
+2. [Update the Firmware](../config/firmware.md) to PX4 _master_ version (when updating the firmware, check **Advanced settings** and then select **Developer Build (master)** from the dropdown list).
    _QGroundControl_ will automatically detect that the hardware supports FMUv2 and install the appropriate Firmware.
 
    ![FMUv2 update](../../assets/qgc/setup/firmware/bootloader_update.jpg)
 
    Wait for the vehicle to reboot.
 
-1. [Find and enable](../advanced_config/parameters.md) the parameter [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
-1. Reboot (disconnect/reconnect the board).
+3. [Find and enable](../advanced_config/parameters.md) the parameter [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
+4. Reboot (disconnect/reconnect the board).
    The bootloader update will only take a few seconds.
-1. Then [Update the Firmware](../config/firmware.md) again.
+5. Then [Update the Firmware](../config/firmware.md) again.
    This time _QGroundControl_ should autodetect the hardware as FMUv3 and update the Firmware appropriately.
 
    ![FMUv3 update](../../assets/qgc/setup/firmware/bootloader_fmu_v3_update.jpg)

docs/en/computer_vision/collision_prevention.md

@@ -3,7 +3,7 @@
 _Collision Prevention_ may be used to automatically slow and stop a vehicle before it can crash into an obstacle.
 It can be enabled for multicopter vehicles when using acceleration-based [Position mode](../flight_modes_mc/position.md) (or VTOL vehicles in MC mode).
 
-It can be enabled for multicopter vehicles in [Position mode](../flight_modes_mc/position.md), and can use sensor data from an offboard companion computer, offboard rangefinders over MAVLink, a rangefinder attached to the flight controller, or any combination of the above.
+It can be enabled for multicopter vehicles in [Position mode](../flight_modes_mc/position.md) (with [MPC_POS_MODE](#MPC_POS_MODE) set to `Acceleration based`), and can use sensor data from an offboard companion computer, offboard rangefinders over MAVLink, a rangefinder attached to the flight controller, or any combination of the above.
 
 Collision prevention may restrict vehicle maximum speed if the sensor range isn't large enough!
 It also prevents motion in directions where no sensor data is available (i.e. if you have no rear-sensor data, you will not be able to fly backwards).
@@ -26,15 +26,14 @@ Users are notified through _QGroundControl_ while _Collision Prevention_ is acti
 
 PX4 software setup is covered in the next section.
 If you are using a distance sensor attached to your flight controller for collision prevention, it will need to be attached and configured as described in [PX4 Distance Sensor](#rangefinder).
-If you are using a companion computer to provide obstacle information see [companion setup](#companion
+If you are using a companion computer to provide obstacle information see [companion setup](#companion) below.
 
-## Supported Rangefinders  {#rangefinder}
+## Supported Rangefinders {#rangefinder}
 
 ### Lanbao PSK-CM8JL65-CC5 [Discontinued]
 
 At time of writing PX4 allows you to use the [Lanbao PSK-CM8JL65-CC5](../sensor/cm8jl65_ir_distance_sensor.md) IR distance sensor for collision prevention “out of the box”, with minimal additional configuration:
 
-
 - First [attach and configure the sensor](../sensor/cm8jl65_ir_distance_sensor.md), and enable collision prevention (as described above, using [CP_DIST](#CP_DIST)).
 - Set the sensor orientation using [SENS_CM8JL65_R_0](../advanced_config/parameter_reference.md#SENS_CM8JL65_R_0).
 
@@ -51,7 +50,6 @@ Other sensors may be enabled, but this requires modification of driver code to s
   This should be done by mimicking the `SENS_CM8JL65_R_0` parameter (though you might also hard-code the orientation in the sensor _module.yaml_ file to something like `sf0x start -d ${SERIAL_DEV} -R 25` - where 25 is equivalent to `ROTATION_DOWNWARD_FACING`).
 - Modify the driver to set the _field of view_ in the distance sensor UORB topic (`distance_sensor_s.h_fov`).
 
-
 ## PX4 (Software) Setup
 
 Configure collision prevention by [setting the following parameters](../advanced_config/parameters.md) in _QGroundControl_:
@@ -62,7 +60,7 @@ Configure collision prevention by [setting the following parameters](../advanced
 | <a id="CP_DELAY"></a>[CP_DELAY](../advanced_config/parameter_reference.md#CP_DELAY)                | Set the sensor and velocity setpoint tracking delay. See [Delay Tuning](#delay_tuning) below.                                                                                                                                                                                                   |
 | <a id="CP_GUIDE_ANG"></a>[CP_GUIDE_ANG](../advanced_config/parameter_reference.md#CP_GUIDE_ANG)    | Set the angle (to both sides of the commanded direction) within which the vehicle may deviate if it finds fewer obstacles in that direction. See [Guidance Tuning](#angle_change_tuning) below.                                                                                                 |
 | <a id="CP_GO_NO_DATA"></a>[CP_GO_NO_DATA](../advanced_config/parameter_reference.md#CP_GO_NO_DATA) | Set to 1 to allow the vehicle to move in directions where there is no sensor coverage (default is 0/`False`).                                                                                                                                                                                   |
-| <a id="MPC_POS_MODE"></a>[MPC_POS_MODE](../advanced_config/parameter_reference.md#MPC_POS_MODE)    | Set to `Direct velocity` or `Smoothed velocity` to enable Collision Prevention in Position Mode (default is `Acceleration based`).                                                                                                                                                              |
+| <a id="MPC_POS_MODE"></a>[MPC_POS_MODE](../advanced_config/parameter_reference.md#MPC_POS_MODE)    | Must be set to `Acceleration based`.                                                                                                                                                                                                                                                           |
 
 ## Algorithm Description
 
@@ -131,7 +129,6 @@ The guidance feature will never direct the vehicle in a direction without sensor
 If the vehicle feels stuck with only a single distance sensor pointing forwards, this is probably because the guidance cannot safely adapt the direction due to lack of information.
 :::
 
-
 ## Algorithm Description
 
 The data from all sensors are fused into an internal representation of 72 sectors around the vehicle, each containing either the sensor data and information about when it was last observed, or an indication that no data for the sector was available.
@@ -212,7 +209,7 @@ The steps are:
      type: px4_msgs::msg::ObstacleDistance
    ```
 
-   For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in [uXRCE-DDS](../middleware/uxrce_dds.md) (PX4-ROS 2/DDS Bridge)_.
+   For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in [uXRCE-DDS](../middleware/uxrce_dds.md) (PX4-ROS 2/DDS Bridge)\_.
 
 3. Open PlotJuggler and navigate to the **Tools > Reactive Script Editor** section.
    In the **Script Editor** tab, add following scripts in the appropriate sections:

docs/en/peripherals/dshot.md

@@ -36,7 +36,7 @@ Remove propellers before changing ESC configuration parameters!
 
 Enable DShot for your required outputs in the [Actuator Configuration](../config/actuators.md).
 
-DShot comes with different speed options: _DShot150_, _DShot300_, _DShot600_ and _DShot1200_, where the number indicates the speed in kilo-bits/second.
+DShot comes with different speed options: _DShot150_, _DShot300_, and _DShot600_ where the number indicates the speed in kilo-bits/second.
 You should set the parameter to the highest speed supported by your ESC (according to its datasheet).
 
 Then connect the battery and arm the vehicle.
@@ -110,18 +110,19 @@ The most important ones are:
 
   :::
 
-## Telemetry
-
-Some ESCs are capable of sending telemetry back to the flight controller, including:
-
-- temperature
-- voltage
-- current
-- accumulated current consumption
-- RPM values
+## ESC Telemetry
 
+Some ESCs are capable of sending telemetry back to the flight controller through a UART RX port.
 These DShot ESCs will have an additional telemetry wire.
 
+The provided telemetry includes:
+
+- Temperature
+- Voltage
+- Current
+- Accumulated current consumption
+- RPM values
+
 To enable this feature (on ESCs that support it):
 
 1. Join all the telemetry wires from all the ESCs together, and then connect them to one of the RX pins on an unused flight controller serial port.
@@ -147,3 +148,40 @@ ERROR [dshot] No data received. If telemetry is setup correctly, try again.
 
 Check manufacturer documentation for confirmation/details.
 :::
+
+## Bidirectional DShot (Telemetry)
+
+<Badge type="tip" text="PX4 v1.16" />
+
+Bidirectional DShot is a protocol that can provide telemetry including: high rate ESC RPM data, voltage, current, and temperature with a single wire.
+
+The PX4 implementation currently enables only ESC RPM (eRPM) data collection from each ESC at high frequencies.
+This telemetry significantly improves the performance of [Dynamic Notch Filters](../config_mc/filter_tuning.md#dynamic-notch-filters) and enables more precise vehicle tuning.
+
+::: info
+The [ESC Telemetry](#esc-telemetry) described above is currently still necessary if you want voltage, current, or temperature information.
+It's setup and use is independent of bidirectional DShot.
+:::
+
+### Hardware Setup
+
+The ESC must be connected to FMU outputs only.
+These will be labeled `MAIN` on flight controllers that only have one PWM bus, and `AUX` on controllers that have both `MAIN` and `AUX` ports (i.e. FCs that have an IO board).
+
+:::warning **Limited hardware support**
+This feature is only supported on flight controllers with the following processors:
+
+- STM32H7: First four FMU outputs
+  - Must be connected to the first 4 FMU outputs, and these outputs must also be mapped to the same timer.
+  - [KakuteH7](../flight_controller/kakuteh7v2.md) is not supported because the outputs are not mapped to the same timer.
+- [i.MXRT](../flight_controller/nxp_mr_vmu_rt1176.md) (V6X-RT & Tropic): 8 FMU outputs.
+
+No other boards are supported.
+:::
+
+### Configuration {#bidirectional-dshot-configuration}
+
+To enable bidirectional DShot, set the [DSHOT_BIDIR_EN](../advanced_config/parameter_reference.md#DSHOT_BIDIR_EN) parameter.
+
+The system calculates actual motor RPM from the received eRPM data using the [MOT_POLE_COUNT](../advanced_config/parameter_reference.md#MOT_POLE_COUNT) parameter.
+This parameter must be set correctly for accurate RPM reporting.

docs/en/releases/main.md

@@ -40,7 +40,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 
 ### Common
 
-- TBD
+- [QGroundControl Bootloader Update](../advanced_config/bootloader_update.md#qgc-bootloader-update-sys-bl-update) via the [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE) parameter has been re-enabled after being broken for a number of releases. ([PX4-Autopilot#25032: build: romf: fix generation of rc.board_bootloader_upgrade](https://github.com/PX4/PX4-Autopilot/pull/25032)).
 
 ### Control
 

docs/en/sensor_bus/i2c_general.md

@@ -4,15 +4,15 @@
 
 It is recommended for:
 
-* Connecting offboard components that require low bandwidth and low latency communication, e.g. [rangefinders](../sensor/rangefinders.md), [magnetometers](../gps_compass/magnetometer.md), [airspeed sensors](../sensor/airspeed.md) and [tachometers](../sensor/tachometers.md) .
+* Connecting offboard components that require low bandwidth and low latency communication, e.g. [rangefinders](../sensor/rangefinders.md), [magnetometers](../gps_compass/magnetometer.md), [airspeed sensors](../sensor/airspeed.md) and [tachometers](../sensor/tachometers.md).
 * Compatibility with peripheral devices that only support I2C.
 * Allowing multiple devices to attach to a single bus, which is useful for conserving ports.
 
 I2C allows multiple master devices to connect to multiple slave devices using only 2 wires per connection (SDA, SCL).
-in theory a bus can support 128 devices, each accessed via its unique address.
+in theory, a bus can support 128 devices, each accessed via its unique address.
 
 ::: info
-UAVCAN would normally be preferred where higher data rates are required, and on larger vehicles where sensors are be mounted further from the flight controller.
+UAVCAN would normally be preferred where higher data rates are required, and on larger vehicles where sensors are mounted further from the flight controller.
 :::
 
 
@@ -20,7 +20,7 @@ UAVCAN would normally be preferred where higher data rates are required, and on
 
 I2C uses a pair of wires: SDA (serial data) and SCL (serial clock).
 The bus is of open-drain type, meaning that devices ground the data line.
-It uses a pullup resistor to push it to `log.1` (idle state) - every wire has it usually located on the bus terminating devices.
+It uses a pull-up resistor to push it to `log.1` (idle state) - every wire has it usually located on the bus terminating devices.
 One bus can connect to multiple I2C devices.
 The individual devices are connected without any crossing.
 
@@ -52,41 +52,44 @@ If two I2C devices on a bus have the same ID there will be a clash, and neither
 This usually occurs because a user needs to attach two sensors of the same type to the bus, but may also happen if devices use duplicate addresses by default.
 
 Particular I2C devices may allow you to select a new address for one of the devices to avoid the clash.
-Some devices do not support this option, or do not have broad options for the addresses that can be used (i.e. cannot be used to avoid a clash).
+Some devices do not support this option or do not have broad options for the addresses that can be used (i.e. cannot be used to avoid a clash).
 
 If you can't change the addresses, one option is to use an [I2C Address Translator](#i2c-address-translators).
 
 ### Insufficient Transfer Capacity
 
-The bandwidth available for each individual device generally decreases as more devices are added. The exact decrease depends on the bandwidth used by each individual device. Therefore it is possible to connect many low bandwidth devices, like [tachometers](../sensor/tachometers.md).
+The bandwidth available for each device generally decreases as more devices are added. The exact decrease depends on the bandwidth used by each individual device. Therefore it is possible to connect many low-bandwidth devices, like [tachometers](../sensor/tachometers.md).
 If too many devices are added, it can cause transmission errors and network unreliability.
 
 There are several ways to reduce the problem:
-* Dividing the devices into groups, each with approximately the same number of devices and connecting each group to one autopilot port
+* Dividing the devices into groups, each with approximately the same number of devices, and connecting each group to one autopilot port
 * Increase bus speed limit (usually set to 100kHz for external I2C bus)
 
 ### Excessive Wiring Capacitance
 
-The electrical capacity of bus wiring increases as more devices/wires are added. The exact decrease depends on total length of bus wiring and wiring specific capacitance.
+The electrical capacity of bus wiring increases as more devices/wires are added. The exact decrease depends on the total length of bus wiring and wiring-specific capacitance.
 The problem can be analyzed using an oscilloscope, where we see that the edges of SDA/SCL signals are no longer sharp.
 
 There are several ways to reduce the problem:
-* Dividing the devices into groups, each with approximately the same number of devices and connecting each group to one autopilot port
-* Using the shortest and the highest quality I2C cables possible
-* Separating the devices with a weak open-drain driver to smaller bus with lower capacitance
-* [I2C Bus Accelerators](#i2c-bus-accelerators)
+* Dividing the devices into groups, each with approximately the same number of devices, and connecting each group to one autopilot port
+* Using the shorter and higher quality I2C cables, see the [cable wiring page](../assembly/cable_wiring.md#i2c-cables) for details
+* Separating the devices with a weak open-drain driver to smaller buses with lower capacitance by using [I2C Bus Accelerators](#i2c-bus-accelerators)
 
 ## I2C Bus Accelerators
 
-I2C bus accelerators are separate circuits that can be used to support longer wiring length on an I2C bus.
+I2C bus accelerators are separate circuits that can be used to support longer wiring lengths on an I2C bus.
 They work by physically dividing an I2C network into 2 parts and using their own transistors to amplify I2C signals.
 
 Available accelerators include:
-- [Thunderfly TFI2CEXT01](https://github.com/ThunderFly-aerospace/TFI2CEXT01):
+- [Thunderfly TFI2CEXT01](https://docs.thunderfly.cz/avionics/TFI2CEXT01/):
   ![I2C bus extender](../../assets/peripherals/i2c_tfi2cext/tfi2cext01a_bottom.jpg)
   - This has Dronecode connectors and is hence very easy to add to a Pixhawk I2C setup.
   - The module has no settings (it works out of the box).
 
+### I2C Level Converter function
+
+Some I2C devices have 5V on the data lines, while the Pixhawk connector standard port expects these lines to be 3.3 V.
+You can use the TFI2CEXT01 as a level converter to connect 5V devices to a Pixhawk I2C port. This feature is possible because the SCL and SDA lines of TFI2CEXT01 are 5V tolerant. 
 
 ## I2C Address Translators
 
@@ -96,21 +99,12 @@ The work by listening for I2C communication and transforming the address when a
 Supported I2C Address Translators include:
 
 - [Thunderfly TFI2CADT01](../sensor_bus/translator_tfi2cadt.md)
-
+  - This has Dronecode connectors and is very easy to add to a Pixhawk I2C setup.
 
 ## I2C Bus Splitters
 
-I2C Bus Splitters are circuit boards that split the I2C port on your flight controller into multiple ports.
-They are useful if you want to use multiple I2C peripherals on a flight controller that has only one I2C port (or too few), such as an airspeed sensor and a distance sensor.
-
-You can find an appropriate board using an internet search.
-
-## I2C Level Converter
-
-Some I2C devices have 5V on the data lines, while the Pixhawk connector standard port expects these lines to be 3.3 V.
-You can use an I2C level converter to connect 5V devices to a Pixhawk I2C port.
-
-You can find an appropriate covnerter using an internet search.
+I2C Bus Splitters are devices that split the I2C port on your flight controller into multiple connectors.
+They are useful if you want to use multiple I2C peripherals on a flight controller that has only one I2C port (or too few), such as an airspeed sensor and a distance sensor. Both devices [I2C Address Translator](../sensor_bus/translator_tfi2cadt.md) and [I2C Bus Accelerators](#i2c-bus-accelerators) could also be used as I2C splitters because they have multiple I2C connectors for connecting additional I2C devices.
 
 ## I2C Development
 

docs/en/sensor_bus/translator_tfi2cadt.md

@@ -1,12 +1,11 @@
 # TFI2CADT01 - I²C Address Translator
 
-[TFI2CADT01](https://github.com/ThunderFly-aerospace/TFI2CADT01) is an address translator module that is compatible with Pixhawk and PX4.
+[TFI2CADT01](https://docs.thunderfly.cz/avionics/TFI2CADT01/) is an address translator module that is compatible with Pixhawk and PX4.
 
 Address translation allows multiple I2C devices with the same address to coexist on an I2C network.
 The module may be needed if using several devices that have the same hard-coded address.
 
-The module has an input and an output side.
-A sensor is connected to the master device on one side.
+The module has an input and an output side and a sensor is connected to the master device on one side.
 On the output side sensors, whose addresses are to be translated, can be connected.
 The module contains two pairs of connectors, each pair responsible for different translations.
 
@@ -14,7 +13,7 @@ The module contains two pairs of connectors, each pair responsible for different
 
 ::: info
 [TFI2CADT01](https://github.com/ThunderFly-aerospace/TFI2CADT01) is designed as open-source hardware with GPLv3 license.
-It is commercially available from [ThunderFly](https://www.thunderfly.cz/) company or from [Tindie eshop](https://www.tindie.com/products/thunderfly/tfi2cadt01-i2c-address-translator/).
+It is commercially available from [ThunderFly](https://www.thunderfly.cz/) company or from [Tindie eshop](https://www.tindie.com/products/26353/).
 :::
 
 ## Address Translation Method
@@ -31,7 +30,7 @@ If you need your own value for address translation, changing the configuration r
 The tachometer sensor [TFRPM01](../sensor/thunderfly_tachometer.md) can be set to two different addresses using a solder jumper.
 If the autopilot has three buses, only 6 sensors can be connected and no bus remains free (2 available addresses * 3 I2C ports).
 In some multicopters or VTOL solutions, there is a need to measure the RPM of 8 or more elements.
-The [TFI2CADT01](https://www.tindie.com/products/thunderfly/tfi2cadt01-i2c-address-translator/) is highly recommended in this case.
+The [TFI2CADT01](https://www.tindie.com/products/26353/) is highly recommended in this case.
 
 ![Multiple sensors](../../assets/peripherals/i2c_tfi2cadt/tfi2cadt01_multi_tfrpm01.jpg)
 
@@ -58,7 +57,7 @@ graph TD
 
 ::: info
 TFI2CADT01 does not contain any I2C buffer or accelerator.
-As it adds additional capacitance on the bus, we advise combining it with some bus booster, e.g. [TFI2CEXT01](https://github.com/ThunderFly-aerospace/TFI2CEXT01).
+As it adds additional capacitance on the bus, we advise combining it with some bus booster, e.g. [TFI2CEXT01](https://docs.thunderfly.cz/avionics/TFI2CEXT01/).
 :::
 
 ### Other Resources

msg/BatteryInfo.msg

@@ -0,0 +1,9 @@
+# Battery information
+#
+# Static or near-invariant battery information.
+# Should be streamed at low rate.
+
+uint64 timestamp # [us] Time since system start
+
+uint8 id # Must match the id in the battery_status message for the same battery
+char[32] serial_number # [@invalid 0 All bytes] Serial number of the battery pack in ASCII characters, 0 terminated

msg/CMakeLists.txt

@@ -47,6 +47,7 @@ set(msg_files
 	Airspeed.msg
 	AirspeedWind.msg
 	AutotuneAttitudeControlStatus.msg
+	BatteryInfo.msg
 	ButtonEvent.msg
 	CameraCapture.msg
 	CameraStatus.msg
@@ -226,6 +227,7 @@ set(msg_files
 	VehicleImu.msg
 	VehicleImuStatus.msg
 	VehicleLocalPositionSetpoint.msg
+	TaskLocalPositionSetpoint.msg
 	VehicleMagnetometer.msg
 	VehicleOpticalFlow.msg
 	VehicleOpticalFlowVel.msg

msg/CellularStatus.msg

@@ -2,37 +2,37 @@
 #
 # This is currently used only for logging cell status from MAVLink.
 
-uint64 timestamp # [us] Time since system start.
+uint64 timestamp # [us] Time since system start
 
-uint16 status # [@enum STATUS_FLAG] Status bitmap.
-uint16 STATUS_FLAG_UNKNOWN = 1 # State unknown or not reportable.
-uint16 STATUS_FLAG_FAILED = 2 # Modem is unusable.
-uint16 STATUS_FLAG_INITIALIZING = 4 # Modem is being initialized.
-uint16 STATUS_FLAG_LOCKED = 8 # Modem is locked.
-uint16 STATUS_FLAG_DISABLED = 16 # Modem is not enabled and is powered down.
-uint16 STATUS_FLAG_DISABLING = 32 # Modem is currently transitioning to the STATUS_FLAG_DISABLED state.
-uint16 STATUS_FLAG_ENABLING = 64 # Modem is currently transitioning to the STATUS_FLAG_ENABLED state.
-uint16 STATUS_FLAG_ENABLED = 128 # Modem is enabled and powered on but not registered with a network provider and not available for data connections.
-uint16 STATUS_FLAG_SEARCHING = 256 # Modem is searching for a network provider to register.
-uint16 STATUS_FLAG_REGISTERED = 512 # Modem is registered with a network provider, and data connections and messaging may be available for use.
-uint16 STATUS_FLAG_DISCONNECTING = 1024 # Modem is disconnecting and deactivating the last active packet data bearer. This state will not be entered if more than one packet data bearer is active and one of the active bearers is deactivated.
-uint16 STATUS_FLAG_CONNECTING = 2048 # Modem is activating and connecting the first packet data bearer. Subsequent bearer activations when another bearer is already active do not cause this state to be entered.
-uint16 STATUS_FLAG_CONNECTED = 4096 # One or more packet data bearers is active and connected.
+uint16 status # [@enum STATUS_FLAG] Status bitmap
+uint16 STATUS_FLAG_UNKNOWN = 1 # State unknown or not reportable
+uint16 STATUS_FLAG_FAILED = 2 # Modem is unusable
+uint16 STATUS_FLAG_INITIALIZING = 4 # Modem is being initialized
+uint16 STATUS_FLAG_LOCKED = 8 # Modem is locked
+uint16 STATUS_FLAG_DISABLED = 16 # Modem is not enabled and is powered down
+uint16 STATUS_FLAG_DISABLING = 32 # Modem is currently transitioning to the STATUS_FLAG_DISABLED state
+uint16 STATUS_FLAG_ENABLING = 64 # Modem is currently transitioning to the STATUS_FLAG_ENABLED state
+uint16 STATUS_FLAG_ENABLED = 128 # Modem is enabled and powered on but not registered with a network provider and not available for data connections
+uint16 STATUS_FLAG_SEARCHING = 256 # Modem is searching for a network provider to register
+uint16 STATUS_FLAG_REGISTERED = 512 # Modem is registered with a network provider, and data connections and messaging may be available for use
+uint16 STATUS_FLAG_DISCONNECTING = 1024 # Modem is disconnecting and deactivating the last active packet data bearer. This state will not be entered if more than one packet data bearer is active and one of the active bearers is deactivated
+uint16 STATUS_FLAG_CONNECTING = 2048 # Modem is activating and connecting the first packet data bearer. Subsequent bearer activations when another bearer is already active do not cause this state to be entered
+uint16 STATUS_FLAG_CONNECTED = 4096 # One or more packet data bearers is active and connected
 
-uint8 failure_reason # [@enum FAILURE_REASON] Failure reason.
-uint8 FAILURE_REASON_NONE = 0 # No error.
-uint8 FAILURE_REASON_UNKNOWN = 1 # Error state is unknown.
-uint8 FAILURE_REASON_SIM_MISSING = 2 # SIM is required for the modem but missing.
-uint8 FAILURE_REASON_SIM_ERROR = 3 # SIM is available, but not usable for connection.
+uint8 failure_reason # [@enum FAILURE_REASON] Failure reason
+uint8 FAILURE_REASON_NONE = 0 # No error
+uint8 FAILURE_REASON_UNKNOWN = 1 # Error state is unknown
+uint8 FAILURE_REASON_SIM_MISSING = 2 # SIM is required for the modem but missing
+uint8 FAILURE_REASON_SIM_ERROR = 3 # SIM is available, but not usable for connection
 
-uint8 type # [@enum CELLULAR_NETWORK_RADIO_TYPE] Cellular network radio type.
+uint8 type # [@enum CELLULAR_NETWORK_RADIO_TYPE] Cellular network radio type
 uint8 CELLULAR_NETWORK_RADIO_TYPE_NONE = 0 # None
 uint8 CELLULAR_NETWORK_RADIO_TYPE_GSM = 1 # GSM
 uint8 CELLULAR_NETWORK_RADIO_TYPE_CDMA = 2 # CDMA
 uint8 CELLULAR_NETWORK_RADIO_TYPE_WCDMA = 3 # WCDMA
 uint8 CELLULAR_NETWORK_RADIO_TYPE_LTE = 4 # LTE
 
-uint8 quality # [dBm] Cellular network RSSI/RSRP, absolute value.
-uint16 mcc # [@invalid UINT16_MAX] Mobile country code.
-uint16 mnc # [@invalid UINT16_MAX] Mobile network code.
-uint16 lac # [@invalid 0] Location area code.
+uint8 quality # [dBm] Cellular network RSSI/RSRP, absolute value
+uint16 mcc # [@invalid UINT16_MAX] Mobile country code
+uint16 mnc # [@invalid UINT16_MAX] Mobile network code
+uint16 lac # [@invalid 0] Location area code

msg/EstimatorStatus.msg

@@ -39,14 +39,12 @@ uint8 CS_FIXED_WING = 17	# 17 - true when thought to be operating as a fixed win
 uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetometer has been declared faulty and is no longer being used
 uint8 CS_ASPD = 19		# 19 - true when airspeed measurements are being fused
 uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect induced static pressure rise is active
-uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected
 uint8 CS_GPS_YAW = 22		# 22 - true when yaw (not ground course) data from a GPS receiver is being fused
 uint8 CS_MAG_ALIGNED = 23	# 23 - true when the in-flight mag field alignment has been completed
 uint8 CS_EV_VEL = 24		# 24 - true when local frame velocity data fusion from external vision measurements is intended
 uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measurement for the magnetometer Z component
 uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
 uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
-uint8 CS_RNG_FAULT = 28		# 28 - true when the range finder has been declared faulty and is no longer being used
 uint8 CS_GNSS_VEL = 44		# 44 - true if GNSS velocity measurements are being fused
 
 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults

msg/EstimatorStatusFlags.msg

@@ -25,14 +25,12 @@ bool cs_fixed_wing            # 17 - true when the vehicle is operating as a fix
 bool cs_mag_fault             # 18 - true when the magnetometer has been declared faulty and is no longer being used
 bool cs_fuse_aspd             # 19 - true when airspeed measurements are being fused
 bool cs_gnd_effect            # 20 - true when protection from ground effect induced static pressure rise is active
-bool cs_rng_stuck             # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 bool cs_gnss_yaw              # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed
 bool cs_ev_vel                # 24 - true when local frame velocity data fusion from external vision measurements is intended
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gnss_yaw_fault        # 27 - true when the GNSS heading has been declared faulty and is no longer being used
-bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing

msg/PositionSetpoint.msg

@@ -36,3 +36,5 @@ float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
+
+# TOPICS position_setpoint task_position_setpoint

msg/RoverSteeringSetpoint.msg

@@ -1,5 +1,3 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Ackermann only, Positiv: Steer right, Negativ: Steer left)
-
-float32 normalized_speed_diff     # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Differential/Mecanum only, Positiv = Turn right, Negativ: Turn left)
+float32 normalized_steering_setpoint # [-1, 1] Positiv = Turn right, Negativ: Turn left (Ackermann: Steering angle, Differential/Mecanum: Speed difference between the left and right wheels)

msg/TaskLocalPositionSetpoint.msg

@@ -0,0 +1,19 @@
+# Local position setpoint in NED frame
+# Telemetry of PID position controller to monitor tracking.
+# NaN means the state was not controlled
+
+uint64 timestamp	# time since system start (microseconds)
+
+float32 x		# in meters NED
+float32 y		# in meters NED
+float32 z		# in meters NED
+
+float32 vx		# in meters/sec
+float32 vy		# in meters/sec
+float32 vz		# in meters/sec
+
+float32[3] acceleration # in meters/sec^2
+float32[3] thrust	# normalized thrust vector in NED
+
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec

msg/VehicleConstraints.msg

@@ -7,3 +7,5 @@ float32 speed_up # in meters/sec
 float32 speed_down # in meters/sec
 
 bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)
+
+bool lock_dist_bottom # altitude is locked to the current distance to ground

msg/px4_msgs_old/msg/BatteryStatusV0.msg

@@ -0,0 +1,79 @@
+# Battery status
+#
+# Battery status information for up to 4 battery instances.
+# These are populated from power module and smart battery device drivers, and one battery updated from MAVLink.
+# Battery instance information is also logged and streamed in MAVLink telemetry.
+
+uint32 MESSAGE_VERSION = 0
+uint8 MAX_INSTANCES = 4
+
+uint64 timestamp # [us] Time since system start
+bool connected # Whether or not a battery is connected. For power modules this is based on a voltage threshold.
+float32 voltage_v # [V] [@invalid 0] Battery voltage
+float32 current_a # [A] [@invalid -1] Battery current
+float32 current_average_a # [A] [@invalid -1] Battery current average (for FW average in level flight)
+float32 discharged_mah # [mAh] [@invalid -1] Discharged amount
+float32 remaining # [@range 0,1] [@invalid -1] Remaining capacity
+float32 scale # [@range 1,] [@invalid -1] Scaling factor to compensate for lower actuation power caused by voltage sag
+float32 time_remaining_s # [s] [@invalid NaN] Predicted time remaining until battery is empty under previous averaged load
+float32 temperature # [°C] [@invalid NaN] Temperature of the battery
+uint8 cell_count # [@invalid 0] Number of cells
+
+
+uint8 source # [@enum SOURCE] Battery source
+uint8 SOURCE_POWER_MODULE = 0 # Power module
+uint8 SOURCE_EXTERNAL = 1 # External
+uint8 SOURCE_ESCS = 2 # ESCs
+
+uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
+uint16 capacity # [mAh] Capacity of the battery when fully charged
+uint16 cycle_count # Number of discharge cycles the battery has experienced
+uint16 average_time_to_empty # [minutes] Predicted remaining battery capacity based on the average rate of discharge
+uint16 serial_number # Serial number of the battery pack
+uint16 manufacture_date # Manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512
+uint16 state_of_health # [%] [@range 0, 100] State of health. FullChargeCapacity/DesignCapacity
+uint16 max_error # [%] [@range 1, 100] Max error, expected margin of error in the state-of-charge calculation
+uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed
+uint16 interface_error # Interface error counter
+
+float32[14] voltage_cell_v # [V] [@invalid 0] Battery individual cell voltages
+float32 max_cell_voltage_delta # Max difference between individual cell voltages
+
+bool is_powering_off # Power off event imminent indication, false if unknown
+bool is_required # Set if the battery is explicitly required before arming
+
+uint8 warning # [@enum WARNING STATE] Current battery warning
+uint8 WARNING_NONE = 0 # No battery low voltage warning active
+uint8 WARNING_LOW = 1 # Low voltage warning
+uint8 WARNING_CRITICAL = 2 # Critical voltage, return / abort immediately
+uint8 WARNING_EMERGENCY = 3 # Immediate landing required
+uint8 WARNING_FAILED = 4 # Battery has failed completely
+uint8 STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field
+uint8 STATE_CHARGING = 7 # Battery is charging
+
+uint16 faults # [@enum FAULT] Smart battery supply status/fault flags (bitmask) for health indication
+uint8 FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
+uint8 FAULT_SPIKES = 1 # Voltage spikes
+uint8 FAULT_CELL_FAIL= 2 # One or more cells have failed
+uint8 FAULT_OVER_CURRENT = 3 # Over-current
+uint8 FAULT_OVER_TEMPERATURE = 4 # Over-temperature
+uint8 FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
+uint8 FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage)
+uint8 FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
+uint8 FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
+uint8 FAULT_HARDWARE_FAILURE = 9 # Hardware problem
+uint8 FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming
+uint8 FAULT_COUNT = 11 # Counter. Keep this as last element
+
+float32 full_charge_capacity_wh # [Wh] Compensated battery capacity
+float32 remaining_capacity_wh # [Wh] Compensated battery capacity remaining
+uint16 over_discharge_count # Number of battery overdischarge
+float32 nominal_voltage # [V] Nominal voltage of the battery pack
+
+float32 internal_resistance_estimate # [Ohm] Internal resistance per cell estimate
+float32 ocv_estimate # [V] Open circuit voltage estimate
+float32 ocv_estimate_filtered # [V] Filtered open circuit voltage estimate
+float32 volt_based_soc_estimate # [@range 0, 1] Normalized volt based state of charge estimate
+float32 voltage_prediction # [V] Predicted voltage
+float32 prediction_error # [V] Prediction error
+float32 estimation_covariance_norm # Norm of the covariance matrix

msg/translation_node/translations/all_translations.h

@@ -6,12 +6,9 @@
 
 #include <translation_util.h>
 
-//#include "example_translation_direct_v1.h"
-//#include "example_translation_multi_v2.h"
-//#include "example_translation_service_v1.h"
-
-#include "translation_vehicle_status_v1.h"
 #include "translation_airspeed_validated_v1.h"
-#include "translation_vehicle_attitude_setpoint_v1.h"
 #include "translation_arming_check_reply_v1.h"
+#include "translation_battery_status_v1.h"
 #include "translation_event_v1.h"
+#include "translation_vehicle_attitude_setpoint_v1.h"
+#include "translation_vehicle_status_v1.h"

msg/translation_node/translations/translation_battery_status_v1.h

@@ -0,0 +1,114 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+// Translate BatteryStatus v0 <--> v1
+#include <px4_msgs_old/msg/battery_status_v0.hpp>
+#include <px4_msgs/msg/battery_status.hpp>
+
+class BatteryStatusV1Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::BatteryStatusV0;
+	static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+	using MessageNewer = px4_msgs::msg::BatteryStatus;
+	static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+	static constexpr const char* kTopic = "fmu/out/battery_status";
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+		msg_newer.timestamp = msg_older.timestamp;
+		msg_newer.connected = msg_older.connected;
+		msg_newer.voltage_v = msg_older.voltage_v;
+		msg_newer.current_a = msg_older.current_a;
+		msg_newer.current_average_a = msg_older.current_average_a;
+		msg_newer.discharged_mah = msg_older.discharged_mah;
+		msg_newer.remaining = msg_older.remaining;
+		msg_newer.scale = msg_older.scale;
+		msg_newer.time_remaining_s = msg_older.time_remaining_s;
+		msg_newer.temperature = msg_older.temperature;
+		msg_newer.cell_count = msg_older.cell_count;
+		msg_newer.source = msg_older.source;
+		msg_newer.priority = msg_older.priority;
+		msg_newer.capacity = msg_older.capacity;
+		msg_newer.cycle_count = msg_older.cycle_count;
+		msg_newer.average_time_to_empty = msg_older.average_time_to_empty;
+		// The serial number moved to the battery_info message and is char[32] instead of uint16
+		msg_newer.manufacture_date = msg_older.manufacture_date;
+		msg_newer.state_of_health = msg_older.state_of_health;
+		msg_newer.max_error = msg_older.max_error;
+		msg_newer.id = msg_older.id;
+		msg_newer.interface_error = msg_older.interface_error;
+
+		for (int i = 0; i < 14; ++i) {
+			msg_newer.voltage_cell_v[i] = msg_older.voltage_cell_v[i];
+		}
+
+		msg_newer.max_cell_voltage_delta = msg_older.max_cell_voltage_delta;
+		msg_newer.is_powering_off = msg_older.is_powering_off;
+		msg_newer.is_required = msg_older.is_required;
+		msg_newer.warning = msg_older.warning;
+		msg_newer.faults = msg_older.faults;
+		msg_newer.full_charge_capacity_wh = msg_older.full_charge_capacity_wh;
+		msg_newer.remaining_capacity_wh = msg_older.remaining_capacity_wh;
+		msg_newer.over_discharge_count = msg_older.over_discharge_count;
+		msg_newer.nominal_voltage = msg_older.nominal_voltage;
+		msg_newer.internal_resistance_estimate = msg_older.internal_resistance_estimate;
+		msg_newer.ocv_estimate = msg_older.ocv_estimate;
+		msg_newer.ocv_estimate_filtered = msg_older.ocv_estimate_filtered;
+		msg_newer.volt_based_soc_estimate = msg_older.volt_based_soc_estimate;
+		msg_newer.voltage_prediction = msg_older.voltage_prediction;
+		msg_newer.prediction_error = msg_older.prediction_error;
+		msg_newer.estimation_covariance_norm = msg_older.estimation_covariance_norm;
+	}
+
+	static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+		msg_older.timestamp = msg_newer.timestamp;
+		msg_older.connected = msg_newer.connected;
+		msg_older.voltage_v = msg_newer.voltage_v;
+		msg_older.current_a = msg_newer.current_a;
+		msg_older.current_average_a = msg_newer.current_average_a;
+		msg_older.discharged_mah = msg_newer.discharged_mah;
+		msg_older.remaining = msg_newer.remaining;
+		msg_older.scale = msg_newer.scale;
+		msg_older.time_remaining_s = msg_newer.time_remaining_s;
+		msg_older.temperature = msg_newer.temperature;
+		msg_older.cell_count = msg_newer.cell_count;
+		msg_older.source = msg_newer.source;
+		msg_older.priority = msg_newer.priority;
+		msg_older.capacity = msg_newer.capacity;
+		msg_older.cycle_count = msg_newer.cycle_count;
+		msg_older.average_time_to_empty = msg_newer.average_time_to_empty;
+		msg_older.serial_number = 0; // The serial number moved to the battery_info message and is char[32] instead of uint16
+		msg_older.manufacture_date = msg_newer.manufacture_date;
+		msg_older.state_of_health = msg_newer.state_of_health;
+		msg_older.max_error = msg_newer.max_error;
+		msg_older.id = msg_newer.id;
+		msg_older.interface_error = msg_newer.interface_error;
+
+		for (int i = 0; i < 14; ++i) {
+			msg_older.voltage_cell_v[i] = msg_newer.voltage_cell_v[i];
+		}
+
+		msg_older.max_cell_voltage_delta = msg_newer.max_cell_voltage_delta;
+		msg_older.is_powering_off = msg_newer.is_powering_off;
+		msg_older.is_required = msg_newer.is_required;
+		msg_older.warning = msg_newer.warning;
+		msg_older.faults = msg_newer.faults;
+		msg_older.full_charge_capacity_wh = msg_newer.full_charge_capacity_wh;
+		msg_older.remaining_capacity_wh = msg_newer.remaining_capacity_wh;
+		msg_older.over_discharge_count = msg_newer.over_discharge_count;
+		msg_older.nominal_voltage = msg_newer.nominal_voltage;
+		msg_older.internal_resistance_estimate = msg_newer.internal_resistance_estimate;
+		msg_older.ocv_estimate = msg_newer.ocv_estimate;
+		msg_older.ocv_estimate_filtered = msg_newer.ocv_estimate_filtered;
+		msg_older.volt_based_soc_estimate = msg_newer.volt_based_soc_estimate;
+		msg_older.voltage_prediction = msg_newer.voltage_prediction;
+		msg_older.prediction_error = msg_newer.prediction_error;
+		msg_older.estimation_covariance_norm = msg_newer.estimation_covariance_norm;
+	}
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(BatteryStatusV1Translation);

msg/versioned/BatteryStatus.msg

@@ -4,7 +4,7 @@
 # These are populated from power module and smart battery device drivers, and one battery updated from MAVLink.
 # Battery instance information is also logged and streamed in MAVLink telemetry.
 
-uint32 MESSAGE_VERSION = 0
+uint32 MESSAGE_VERSION = 1
 uint8 MAX_INSTANCES = 4
 
 uint64 timestamp # [us] Time since system start
@@ -29,7 +29,6 @@ uint8 priority # Zero based priority is the connection on the Power Controller V
 uint16 capacity # [mAh] Capacity of the battery when fully charged
 uint16 cycle_count # Number of discharge cycles the battery has experienced
 uint16 average_time_to_empty # [minutes] Predicted remaining battery capacity based on the average rate of discharge
-uint16 serial_number # Serial number of the battery pack
 uint16 manufacture_date # Manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512
 uint16 state_of_health # [%] [@range 0, 100] State of health. FullChargeCapacity/DesignCapacity
 uint16 max_error # [%] [@range 1, 100] Max error, expected margin of error in the state-of-charge calculation

range_alt_notes.txt

@@ -0,0 +1,20 @@
+## FlightTaskManualAltitude
+- dist_bottom_var -- currently terrain variance, I see occasionally dist_bottom diverge and then clamp back to expected
+- ground slowdown (_respectGroundSlowdown) uses mpc_land_alt ... weird, remove?
+- _respectMaxAltitude ... weird, remove? We can instead use dist_bottom validity
+- _respectMinAltitude ... weird, remove? No need for a function
+
+
+## FlightTask
+- _dist_to_bottom and _dist_to_ground ... confusing, unify
+-
+
+## FlightTaskAuto
+- reuse logic for range alt lock
+- _prepareLandSetpoints -- Slow down automatic descend close to ground ... use only with terrain estimate available? (baro estimate unreliable)
+
+## EKF
+- Vz does not reflect true Vz due to noisy baro
+- Vz errors cause rangefinder kinematic consistency check to fail
+- Position setpoint error remains over long periods (might be related to Vz issues above)
+

src/drivers/batt_smbus/batt_smbus.cpp

@@ -77,6 +77,7 @@ BATT_SMBUS::BATT_SMBUS(const I2CSPIDriverConfig &config, SMBus *interface) :
 
 BATT_SMBUS::~BATT_SMBUS()
 {
+	orb_unadvertise(_battery_info_topic);
 	orb_unadvertise(_batt_topic);
 	perf_free(_cycle);
 
@@ -165,7 +166,6 @@ void BATT_SMBUS::RunImpl()
 	if (ret == PX4_OK) {
 		new_report.capacity = _batt_capacity;
 		new_report.cycle_count = _cycle_count;
-		new_report.serial_number = _serial_number;
 		new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
 		new_report.cell_count = _cell_count;
 		new_report.state_of_health = _state_of_health;
@@ -192,6 +192,13 @@ void BATT_SMBUS::RunImpl()
 		int instance = 0;
 		orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance);
 
+		battery_info_s battery_info{};
+		battery_info.timestamp = new_report.timestamp;
+		battery_info.id = new_report.id;
+		snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, _serial_number);
+		orb_publish_auto(ORB_ID(battery_info), &_battery_info_topic, &battery_info, &instance);
+
+
 		_last_report = new_report;
 	}
 }

src/drivers/batt_smbus/batt_smbus.h

@@ -52,6 +52,7 @@
 #include <px4_platform_common/param.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/topics/battery_info.h>
 #include <uORB/topics/battery_status.h>
 
 #include <board_config.h>
@@ -242,7 +243,7 @@ private:
 	/** @param _last_report Last published report, used for test(). */
 	battery_status_s _last_report{};
 
-	/** @param _batt_topic uORB battery topic. */
+	orb_advert_t _battery_info_topic{nullptr};
 	orb_advert_t _batt_topic{nullptr};
 
 	/** @param _cell_count Number of series cell. */

src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp

@@ -94,7 +94,6 @@ public:
 		bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE;
 		bat_status.source = 1; // External
 		bat_status.capacity = bat_info.full_charge_capacity_wh;
-		bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits
 		bat_status.state_of_health = bat_info.state_of_health_pct; // External
 		bat_status.id = bat_info.battery_id;
 
@@ -118,9 +117,17 @@ public:
 
 		_battery_status_pub.publish(bat_status);
 		print_message(ORB_ID(battery_status), bat_status);
+
+		battery_info_s battery_info{};
+		battery_info.timestamp = bat_status.timestamp;
+		battery_info.id = bat_status.id;
+		snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu32,
+			 bat_info.model_instance_id);
+		_battery_info_pub.publish(battery_info);
 	};
 
 private:
+	uORB::PublicationMulti<battery_info_s> _battery_info_pub{ORB_ID(battery_info)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
 
 };

src/drivers/cyphal/Subscribers/udral/Battery.hpp

@@ -41,6 +41,7 @@
 
 #pragma once
 
+#include <uORB/topics/battery_info.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/PublicationMulti.hpp>
 
@@ -120,6 +121,10 @@ public:
 			// TODO uORB publication rate limiting
 			_battery_status_pub.publish(bat_status);
 
+			_battery_info.timestamp = bat_status.timestamp;
+			_battery_info.id = bat_status.id;
+			_battery_info_pub.publish(_battery_info);
+
 		} else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) {
 			reg_udral_service_battery_Status_0_2 bat {};
 			size_t bat_size_in_bytes = receive.payload_size;
@@ -163,7 +168,7 @@ public:
 
 			bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh
 			bat_status.cycle_count = parameters.cycle_count;
-			bat_status.serial_number = parameters.unique_id & 0xFFFF;
+			snprintf(_battery_info.serial_number, sizeof(_battery_info.serial_number), "%" PRIu64, parameters.unique_id);
 			bat_status.state_of_health = parameters.state_of_health_pct;
 			bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic
 			bat_status.id = 0; //TODO instancing
@@ -174,6 +179,7 @@ public:
 private:
 	float _nominal_voltage = NAN;
 
+	uORB::PublicationMulti<battery_info_s> _battery_info_pub{ORB_ID(battery_info)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
 
 	SubjectSubscription _status_sub;
@@ -182,5 +188,6 @@ private:
 	const char *_status_name = "battery_status";
 	const char *_parameters_name = "battery_parameters";
 
+	battery_info_s _battery_info{};
 	battery_status_s bat_status {0};
 };

src/drivers/differential_pressure/auav/AUAV.cpp

@@ -240,14 +240,14 @@ int AUAV::read_calibration_eeprom(const uint8_t eeprom_address, uint16_t &data)
 void AUAV::process_calib_data_raw(const calib_data_raw_t calib_data_raw)
 {
 	/* Conversion of calib data as described in the datasheet */
-	_calib_data.a = (float)((calib_data_raw.a_hw << 16) | calib_data_raw.a_lw) / 0x7FFFFFFF;
-	_calib_data.b = (float)((calib_data_raw.b_hw << 16) | calib_data_raw.b_lw) / 0x7FFFFFFF;
-	_calib_data.c = (float)((calib_data_raw.c_hw << 16) | calib_data_raw.c_lw) / 0x7FFFFFFF;
-	_calib_data.d = (float)((calib_data_raw.d_hw << 16) | calib_data_raw.d_lw) / 0x7FFFFFFF;
+	_calib_data.a = (float)((int32_t)((calib_data_raw.a_hw << 16) | calib_data_raw.a_lw)) / 0x7FFFFFFF;
+	_calib_data.b = (float)((int32_t)((calib_data_raw.b_hw << 16) | calib_data_raw.b_lw)) / 0x7FFFFFFF;
+	_calib_data.c = (float)((int32_t)((calib_data_raw.c_hw << 16) | calib_data_raw.c_lw)) / 0x7FFFFFFF;
+	_calib_data.d = (float)((int32_t)((calib_data_raw.d_hw << 16) | calib_data_raw.d_lw)) / 0x7FFFFFFF;
 
-	_calib_data.tc50h = (float)(calib_data_raw.tc50 >> 8) / 0x7F;
-	_calib_data.tc50l = (float)(calib_data_raw.tc50 & 0xFF) / 0x7F;
-	_calib_data.es = (float)(calib_data_raw.es & 0xFF) / 0x7F;
+	_calib_data.tc50h = (float)((int8_t)(calib_data_raw.tc50 >> 8)) / 0x7F;
+	_calib_data.tc50l = (float)((int8_t)(calib_data_raw.tc50 & 0xFF)) / 0x7F;
+	_calib_data.es = (float)((int8_t)(calib_data_raw.es & 0xFF)) / 0x7F;
 }
 
 float AUAV::correct_pressure(const uint32_t pressure_raw, const uint32_t temperature_raw) const

src/drivers/dshot/DShotTelemetry.cpp

@@ -51,6 +51,7 @@ DShotTelemetry::~DShotTelemetry()
 
 int DShotTelemetry::init(const char *uart_device, bool swap_rxtx)
 {
+	int ret = OK;
 	deinit();
 	_uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY);
 
@@ -59,23 +60,31 @@ int DShotTelemetry::init(const char *uart_device, bool swap_rxtx)
 		return -errno;
 	}
 
+	ret = setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE);
+
+	if (ret) {
+		PX4_ERR("failed to set baurate: %s err: %d", uart_device, ret);
+		return ret;
+	}
+
 	if (swap_rxtx) {
 		// Swap RX/TX pins if the device supports it
-		int rv = ioctl(_uart_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+		ret = ioctl(_uart_fd, TIOCSSWAP, SER_SWAP_ENABLED);
 
 		// For other devices we can still place RX on TX pin via half-duplex single-wire mode
-		if (rv) { rv = ioctl(_uart_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); }
+		if (ret) { ret = ioctl(_uart_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); }
 
-		if (rv) {
-			PX4_ERR("failed to swap rx/tx pins: %s err: %d", uart_device, rv);
-			return rv;
+		if (ret) {
+			PX4_ERR("failed to swap rx/tx pins: %s err: %d", uart_device, ret);
+			return ret;
 		}
 	}
 
 	_num_timeouts = 0;
 	_num_successful_responses = 0;
 	_current_motor_index_request = -1;
-	return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE);
+
+	return ret;
 }
 
 void DShotTelemetry::deinit()

src/drivers/magnetometer/Kconfig

@@ -12,7 +12,6 @@ menu "Magnetometer"
         select DRIVERS_MAGNETOMETER_LIS3MDL
         select DRIVERS_MAGNETOMETER_LSM303AGR
         select DRIVERS_MAGNETOMETER_RM3100
-        select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L
         select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA
         select DRIVERS_MAGNETOMETER_ST_IIS2MDC
         ---help---

src/drivers/magnetometer/bosch/bmm350/BMM350.cpp

@@ -34,6 +34,8 @@
 #include "BMM350.hpp"
 using namespace time_literals;
 
+#define MAX(a,b)          (a > b ? a : b)
+
 BMM350::BMM350(const I2CSPIDriverConfig &config) :
 	I2C(config),
 	I2CSPIDriver(config),
@@ -318,7 +320,7 @@ void BMM350::RunImpl()
 				_state = STATE::RESET;
 			}
 
-			ScheduleDelayed(OdrToUs(_mag_odr_mode));
+			ScheduleDelayed(MAX(6000_us, OdrToUs(_mag_odr_mode)));
 			break;
 		}
 

src/drivers/magnetometer/vtrantech/CMakeLists.txt

@@ -1,34 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-add_subdirectory(vcm1193l)

src/drivers/smart_battery/batmon/batmon.cpp

@@ -191,7 +191,6 @@ void Batmon::RunImpl()
 	if (ret == PX4_OK) {
 		new_report.capacity = _batt_capacity;
 		new_report.cycle_count = _cycle_count;
-		new_report.serial_number = _serial_number;
 		new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
 		new_report.cell_count = _cell_count;
 		new_report.state_of_health = _state_of_health;
@@ -220,6 +219,12 @@ void Batmon::RunImpl()
 		orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance);
 
 		_last_report = new_report;
+
+		battery_info_s battery_info{};
+		battery_info.timestamp = new_report.timestamp;
+		battery_info.id = new_report.id;
+		snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, _serial_number);
+		orb_publish_auto(ORB_ID(battery_info), &_battery_info_topic, &battery_info, &instance);
 	}
 }
 

src/drivers/uavcan/sensors/battery.cpp

@@ -125,7 +125,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	// _battery_status[instance].cycle_count = msg.;
 	_battery_status[instance].time_remaining_s = NAN;
 	// _battery_status[instance].average_time_to_empty = msg.;
-	_battery_status[instance].serial_number = msg.model_instance_id;
 	_battery_status[instance].id = msg.getSrcNodeID().get();
 
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
@@ -143,8 +142,14 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	determineWarning(_battery_status[instance].remaining);
 	_battery_status[instance].warning = _warning;
 
+	_battery_info[instance].timestamp = _battery_status[instance].timestamp;
+	_battery_info[instance].id = _battery_status[instance].id;
+	snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu32,
+		 msg.model_instance_id);
+
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
 		publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+		_battery_info_pub[instance].publish(_battery_info[instance]);
 	}
 }
 
@@ -238,8 +243,13 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equi
 	/* Override data that is expected to arrive from UAVCAN msg*/
 	_battery_status[instance] = _battery[instance]->getBatteryStatus();
 	_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
-	_battery_status[instance].serial_number = msg.model_instance_id;
 	_battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery
 
 	publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+
+	_battery_info[instance].timestamp = _battery_status[instance].timestamp;
+	_battery_info[instance].id = _battery_status[instance].id;
+	snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu32,
+		 msg.model_instance_id);
+	_battery_info_pub[instance].publish(_battery_info[instance]);
 }

src/drivers/uavcan/sensors/battery.hpp

@@ -38,6 +38,7 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
+#include <uORB/topics/battery_info.h>
 #include <uORB/topics/battery_status.h>
 #include <uavcan/equipment/power/BatteryInfo.hpp>
 #include <ardupilot/equipment/power/BatteryInfoAux.hpp>
@@ -95,6 +96,11 @@ private:
 	float _discharged_mah_loop = 0.f;
 	uint8_t _warning;
 	hrt_abstime _last_timestamp;
+
+	// Separate battery info publication because UavcanSensorBridgeBase only supports publishing one topic
+	uORB::PublicationMulti<battery_info_s> _battery_info_pub[battery_status_s::MAX_INSTANCES] {ORB_ID(battery_info), ORB_ID(battery_info), ORB_ID(battery_info), ORB_ID(battery_info)};
+
+	battery_info_s _battery_info[battery_status_s::MAX_INSTANCES] {};
 	battery_status_s _battery_status[battery_status_s::MAX_INSTANCES] {};
 	BatteryDataType _batt_update_mod[battery_status_s::MAX_INSTANCES] {};
 

src/lib/drivers/smbus_sbs/SBS.hpp

@@ -45,6 +45,7 @@
 
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <lib/drivers/smbus/SMBus.hpp>
+#include <uORB/topics/battery_info.h>
 #include <uORB/topics/battery_status.h>
 #include <px4_platform_common/param.h>
 #include <lib/atmosphere/atmosphere.h>
@@ -66,7 +67,7 @@ public:
 
 	friend SMBus;
 
-	int populate_smbus_data(battery_status_s &msg);
+	int populate_smbus_data(battery_status_s &msg, battery_info_s &battery_info);
 
 	virtual void RunImpl(); // Can be overridden by derived implimentation
 
@@ -136,7 +137,7 @@ protected:
 
 	perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batmon_cycle")}; // TODO
 
-	/** @param _batt_topic uORB battery topic. */
+	orb_advert_t _battery_info_topic{nullptr};
 	orb_advert_t _batt_topic{nullptr};
 
 	/** @param _cell_count Number of series cell (retrieved from cell_count PX4 params) */
@@ -173,8 +174,10 @@ SMBUS_SBS_BaseClass<T>::SMBUS_SBS_BaseClass(const I2CSPIDriverConfig &config, SM
 	I2CSPIDriver<T>(config),
 	_interface(interface)
 {
+	battery_info_s battery_info{};
 	battery_status_s new_report = {};
 	int SBS_instance_number = 0;
+	_battery_info_topic = orb_advertise_multi(ORB_ID(battery_info), &battery_info, &SBS_instance_number);
 	_batt_topic = orb_advertise_multi(ORB_ID(battery_status), &new_report, &SBS_instance_number);
 	_interface->init();
 }
@@ -251,7 +254,7 @@ int SMBUS_SBS_BaseClass<T>::get_startup_info()
 }
 
 template<class T>
-int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)
+int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data, battery_info_s &battery_info)
 {
 
 	// Temporary variable for storing SMBUS reads.
@@ -285,7 +288,7 @@ int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)
 
 	// Read serial number.
 	ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, result);
-	data.serial_number = result;
+	snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, result);
 
 	// Read battery temperature and covert to Celsius.
 	ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
@@ -312,12 +315,17 @@ void SMBUS_SBS_BaseClass<T>::RunImpl()
 
 	new_report.connected = true;
 
-	int ret = populate_smbus_data(new_report);
+	battery_info_s battery_info{};
+	battery_info.timestamp = now;
+	battery_info.id = new_report.id;
+
+	int ret = populate_smbus_data(new_report, battery_info);
 
 	new_report.cell_count = _cell_count;
 
 	// Only publish if no errors.
 	if (!ret) {
 		orb_publish(ORB_ID(battery_status), _batt_topic, &new_report);
+		orb_publish(ORB_ID(battery_info), _battery_info_topic, &battery_info);
 	}
 }

src/lib/parameters/px4params/srcparser.py

@@ -184,7 +184,6 @@ class SourceParser(object):
     re_cut_type_specifier = re.compile(r'[a-z]+$')
     re_is_a_number = re.compile(r'^-?[0-9\.]')
     re_remove_dots = re.compile(r'\.+$')
-    re_remove_carriage_return = re.compile('\n+')
 
     valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "increment", "reboot_required", "value", "boolean", "bit", "category", "volatile"])
 
@@ -311,7 +310,6 @@ class SourceParser(object):
                                 raise Exception('short description too long (150 max, is {:}, parameter: {:})'.format(len(short_desc), name))
                             param.SetField("short_desc", self.re_remove_dots.sub('', short_desc))
                         if long_desc is not None:
-                            long_desc = self.re_remove_carriage_return.sub(' ', long_desc)
                             param.SetField("long_desc", long_desc)
                         for tag in tags:
                             if tag == "group":

src/lib/rover_control/RoverControl.cpp

@@ -161,7 +161,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const
 }
 
 float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint,
-		  const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel,
+		  const float vehicle_yaw_rate, const float max_thr_speed, const float yaw_rate_corr, const float max_yaw_accel,
+		  const float max_yaw_decel,
 		  const float wheel_track, const float dt)
 {
 	// Apply acceleration and deceleration limit
@@ -194,11 +195,11 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
 	// Transform yaw rate into speed difference
 	float speed_diff_normalized{0.f};
 
-	if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
-		const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track /
-					 2.f;
-		speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_yaw_r,
-					max_thr_yaw_r, -1.f, 1.f);
+	if (wheel_track > FLT_EPSILON && max_thr_speed > FLT_EPSILON) { // Feedforward
+		const float speed_diff = (adjusted_yaw_rate_setpoint.getState() * wheel_track /
+					  2.f) * yaw_rate_corr;
+		speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_speed,
+					max_thr_speed, -1.f, 1.f);
 	}
 
 	// Feedback control

src/lib/rover_control/RoverControl.hpp

@@ -100,7 +100,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float
  * @param pid_yaw_rate Yaw rate PID (Updated by this function).
  * @param yaw_rate_setpoint Yaw rate setpoint [rad/s].
  * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s].
- * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s].
+ * @param max_thr_speed Speed at maximum throttle [m/s].
+ * @param yaw_rate_corr Yaw rate correction factor to convert yaw rate to speed difference.
  * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2].
  * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2].
  * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m].
@@ -108,7 +109,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float
  * @return Normalized speed difference setpoint [-1, 1].
  */
 float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate,
-		  float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel,
+		  float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_speed, float yaw_rate_corr, float max_yaw_accel,
+		  float max_yaw_decel,
 		  float wheel_track, float dt);
 
 /**

src/lib/rover_control/rovercontrol_params.c

@@ -131,6 +131,22 @@ PARAM_DEFINE_FLOAT(RO_YAW_ACCEL_LIM, -1.f);
  */
 PARAM_DEFINE_FLOAT(RO_YAW_DECEL_LIM, -1.f);
 
+/**
+ * Yaw rate correction factor
+ *
+ * Multiplicative correction factor for the feedforward mapping of the yaw rate controller.
+ * Increase this value (x > 1) if the measured yaw rate is lower than the setpoint, decrease (0 < x < 1) otherwise.
+ * Note: Tuning this is particularly useful for skid-steered rovers, or rovers with misaligned wheels/steering axes
+ * that cause a lot of friction when turning.
+ *
+ * @min 0.01
+ * @max 10000
+ * @increment 0.01
+ * @decimal 2
+ * @group Rover Rate Control
+ */
+PARAM_DEFINE_FLOAT(RO_YAW_RATE_CORR, 1.f);
+
 /**
  * Proportional gain for closed loop yaw controller
  *
@@ -253,3 +269,21 @@ PARAM_DEFINE_FLOAT(RO_JERK_LIM, -1.f);
  * @group Rover Velocity Control
  */
 PARAM_DEFINE_FLOAT(RO_SPEED_TH, 0.1f);
+
+/**
+ * Tuning parameter for the speed reduction based on the course error
+ *
+ * Reduced_speed = RO_MAX_THR_SPEED * (1 - normalized_course_error * RO_SPEED_RED)
+ * The normalized course error is the angle between the current course and the bearing setpoint
+ * interpolated from [0, 180] -> [0, 1].
+ * Higher value -> More speed reduction.
+ * Note: This is also used to calculate the speed at which the vehicle arrives at a waypoint in auto modes.
+ * Set to -1 to disable bearing error based speed reduction.
+ *
+ * @min -1
+ * @max 100
+ * @increment 0.01
+ * @decimal 2
+ * @group Rover Velocity Control
+ */
+PARAM_DEFINE_FLOAT(RO_SPEED_RED, -1.f);

src/modules/commander/Commander.cpp

@@ -2110,6 +2110,7 @@ void Commander::landDetectorUpdate()
 				events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected");
 				_vehicle_status.takeoff_time = hrt_absolute_time();
 				_have_taken_off_since_arming = true;
+				_home_position.setTakeoffTime(_vehicle_status.takeoff_time);
 			}
 
 			// automatically set or update home position

src/modules/commander/HomePosition.cpp

@@ -39,12 +39,8 @@
 #include <lib/geo/geo.h>
 #include "commander_helper.h"
 
-using namespace time_literals;
-
-HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags)
-	: _failsafe_flags(failsafe_flags)
-{
-}
+HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags): ModuleParams(nullptr),
+	_failsafe_flags(failsafe_flags) {}
 
 bool HomePosition::hasMovedFromCurrentHomeLocation()
 {
@@ -308,6 +304,22 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 	_local_position_sub.update();
 	_global_position_sub.update();
 
+	if (_vehicle_air_data_sub.updated()) {
+		vehicle_air_data_s baro_data;
+		_vehicle_air_data_sub.copy(&baro_data);
+		const float baro_alt = baro_data.baro_alt_meter;
+
+		if (_last_baro_timestamp != 0) {
+			const float dt = baro_data.timestamp - _last_baro_timestamp;
+			_lpf_baro.update(baro_alt, dt);
+
+		} else {
+			_lpf_baro.reset(baro_alt);
+		}
+
+		_last_baro_timestamp = baro_data.timestamp;
+	}
+
 	if (_vehicle_gps_position_sub.updated()) {
 		sensor_gps_s vehicle_gps_position;
 		_vehicle_gps_position_sub.copy(&vehicle_gps_position);
@@ -319,12 +331,56 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 		_gps_epv = vehicle_gps_position.epv;
 
 		const hrt_abstime now = hrt_absolute_time();
-		const bool time = (now < vehicle_gps_position.timestamp + 1_s);
-		const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
-		const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
-		const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
-		const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
-		_gps_position_for_home_valid = time && fix && eph && epv && evh;
+		const bool time_valid = now < (vehicle_gps_position.timestamp + 1_s);
+		const bool fix_valid = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
+		const bool eph_valid = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
+		const bool epv_valid = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
+		const bool evh_valid = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
+
+		_gps_position_for_home_valid = time_valid && fix_valid && eph_valid && epv_valid && evh_valid;
+
+		if (_param_com_home_en.get() && _gps_position_for_home_valid && _last_gps_timestamp != 0 && _last_baro_timestamp != 0
+		    && _takeoff_time != 0 && now < _takeoff_time + kHomePositionCorrectionTimeWindow) {
+
+			const float gps_alt = static_cast<float>(_gps_alt);
+
+			if (!PX4_ISFINITE(_gps_vel_integral)) {
+				_gps_vel_integral = gps_alt; // initialize the gps-vel-integral at same altitude as gps-pos
+				_baro_gps_static_offset = gps_alt - _lpf_baro.getState();
+			}
+
+			_gps_vel_integral += 1e-6f * (vehicle_gps_position.timestamp - _last_gps_timestamp) * (-vehicle_gps_position.vel_d_m_s);
+
+			// correct baro_alt with offset from GPS alt from when the drift integral was initialized
+			const float baro_alt_corrected = _lpf_baro.getState() + _baro_gps_static_offset;
+			const float gps_alt_with_home_offset = gps_alt + _home_altitude_offset_applied;
+
+			// Apply home altitude correction only if the GPS velocity-integrated altitude and baro altitude
+			// are more consistent with each other than either is with the GPS altitude (with home offset).
+			if (fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(baro_alt_corrected - gps_alt_with_home_offset) &&
+			    fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(_gps_vel_integral - gps_alt_with_home_offset)) {
+
+				const float offset_new = baro_alt_corrected - gps_alt - _home_altitude_offset_applied;
+
+				if (fabsf(offset_new) > kAltitudeDifferenceThreshold) {
+
+					home_position_s home = _home_position_pub.get();
+					home.alt -= offset_new;
+					home.z += offset_new;
+					home.timestamp = now;
+					home.manual_home = false;
+					home.update_count = _home_position_pub.get().update_count + 1U;
+
+					_home_position_pub.update(home);
+					_home_altitude_offset_applied = baro_alt_corrected - gps_alt; // offset present when home position was last corrected
+				}
+			}
+
+		} else {
+			_gps_vel_integral = NAN;
+		}
+
+		_last_gps_timestamp = vehicle_gps_position.timestamp;
 	}
 
 	const vehicle_local_position_s &lpos = _local_position_sub.get();

src/modules/commander/HomePosition.hpp

@@ -40,6 +40,11 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/failsafe_flags.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <px4_platform_common/module_params.h>
+
+using namespace time_literals;
 
 static constexpr int kHomePositionGPSRequiredFixType = 2;
 static constexpr float kHomePositionGPSRequiredEPH = 5.f;
@@ -47,8 +52,11 @@ static constexpr float kHomePositionGPSRequiredEPV = 10.f;
 static constexpr float kHomePositionGPSRequiredEVH = 1.f;
 static constexpr float kMinHomePositionChangeEPH = 1.f;
 static constexpr float kMinHomePositionChangeEPV = 1.5f;
+static constexpr float kLpfBaroTimeConst = 5.f;
+static constexpr float kAltitudeDifferenceThreshold = 1.f; // altitude difference after which home position gets updated
+static constexpr uint64_t kHomePositionCorrectionTimeWindow = 120_s;
 
-class HomePosition
+class HomePosition: public ModuleParams
 {
 public:
 	HomePosition(const failsafe_flags_s &failsafe_flags);
@@ -57,6 +65,7 @@ public:
 	bool setHomePosition(bool force = false);
 	void setInAirHomePosition();
 	bool setManually(double lat, double lon, float alt, float yaw);
+	void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; }
 
 	void update(bool set_automatically, bool check_if_changed);
 
@@ -76,6 +85,15 @@ private:
 
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+
+	uint64_t _last_gps_timestamp{0};
+	uint64_t _last_baro_timestamp{0};
+	AlphaFilter<float> _lpf_baro{kLpfBaroTimeConst};
+	float _gps_vel_integral{NAN};
+	float _home_altitude_offset_applied{0.f};
+	float _baro_gps_static_offset{0.f};
+	uint64_t _takeoff_time{0};
 
 	uORB::PublicationData<home_position_s>			_home_position_pub{ORB_ID(home_position)};
 
@@ -88,4 +106,8 @@ private:
 	double							_gps_alt{0};
 	float							_gps_eph{0.f};
 	float							_gps_epv{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en
+	)
 };

src/modules/commander/commander_params.c

@@ -143,8 +143,9 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
  *
  * Set home position automatically if possible.
  *
- * During missions, the home position is locked and will not reset during intermediate landings.
+ * During missions, the latitude/longitude of the home position is locked and will not reset during intermediate landings.
  * It will only update once the mission is complete or landed outside of a mission.
+ * However, the altitude is still being adjusted to correct for GNSS vertical drift in the first 2 minutes after takeoff.
  *
  * @group Commander
  * @reboot_required true

src/modules/ekf2/CMakeLists.txt

@@ -211,7 +211,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
-		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/jake_range_height_control.cpp
 		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -124,7 +124,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		aid_sources/range_finder/range_finder_consistency_check.cpp
-		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/jake_range_height_control.cpp
 		aid_sources/range_finder/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -58,7 +58,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 	const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
 	const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6);
 
-	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.drag_ctrl == 0))) {
+	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.ekf2_drag_ctrl == 0))) {
 		_control_status.flags.wind = false;
 	}
 
@@ -72,7 +72,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 	if (_control_status.flags.fixed_wing) {
 		if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
 			if (!_control_status.flags.fuse_aspd) {
-				_yawEstimator.setTrueAirspeed(_params.EKFGSF_tas_default);
+				_yawEstimator.setTrueAirspeed(_params.ekf2_gsf_tas);
 			}
 
 		} else {
@@ -82,7 +82,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 
 #endif // CONFIG_EKF2_GNSS
 
-	if (_params.arsp_thr <= 0.f) {
+	if (_params.ekf2_arsp_thr <= 0.f) {
 		stopAirspeedFusion();
 		return;
 	}
@@ -99,7 +99,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		const bool continuing_conditions_passing = _control_status.flags.in_air
 				&& !_control_status.flags.fake_pos;
 
-		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
+		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.ekf2_arsp_thr;
 		const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& is_airspeed_significant
@@ -151,7 +151,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const
 {
 	// Variance for true airspeed measurement - (m/sec)^2
-	const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
+	const float R = sq(math::constrain(_params.ekf2_eas_noise, 0.5f, 5.0f) *
 			   math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
 
 	float innov = 0.f;
@@ -165,7 +165,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
 			      R,                                       // observation variance
 			      innov,                                   // innovation
 			      innov_var,                               // innovation variance
-			      math::max(_params.tas_innov_gate, 1.f)); // innovation gate
+			      math::max(_params.ekf2_tas_gate, 1.f)); // innovation gate
 }
 
 void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)
@@ -243,7 +243,7 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
 	constexpr float sideslip_var = sq(math::radians(15.0f));
 
 	const float euler_yaw = getEulerYaw(_R_to_earth);
-	const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f)
+	const float airspeed_var = sq(math::constrain(_params.ekf2_eas_noise, 0.5f, 5.0f)
 				      * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
 
 	matrix::SquareMatrix<float, State::wind_vel.dof> P_wind;

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -57,7 +57,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 		const float measurement = baro_sample.hgt;
 #endif
 
-		const float measurement_var = sq(_params.baro_noise);
+		const float measurement_var = sq(_params.ekf2_baro_noise);
 
 		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
 
@@ -83,14 +83,14 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 						baro_sample.time_us,
 						-(measurement - bias_est.getBias()),      // observation
 						measurement_var + bias_est.getBiasVar(),  // observation variance
-						math::max(_params.baro_innov_gate, 1.f)); // innovation gate
+						math::max(_params.ekf2_baro_gate, 1.f)); // innovation gate
 
 		// Compensate for positive static pressure transients (negative vertical position innovations)
 		// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
-		if (_control_status.flags.gnd_effect && (_params.gnd_effect_deadzone > 0.f)) {
+		if (_control_status.flags.gnd_effect && (_params.ekf2_gnd_eff_dz > 0.f)) {
 
 			const float deadzone_start = 0.0f;
-			const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
+			const float deadzone_end = deadzone_start + _params.ekf2_gnd_eff_dz;
 
 			if (aid_src.innovation < -deadzone_start) {
 				if (aid_src.innovation <= -deadzone_end) {
@@ -111,7 +111,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 		}
 
 		// determine if we should use height aiding
-		const bool continuing_conditions_passing = (_params.baro_ctrl == 1)
+		const bool continuing_conditions_passing = (_params.ekf2_baro_ctrl == 1)
 				&& measurement_valid
 				&& (_baro_counter > _obs_buffer_length)
 				&& !_control_status.flags.baro_fault;
@@ -222,11 +222,11 @@ float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const f
 		const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
 
 		const Vector3f K_pstatic_coef(
-			airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
-			airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
-			_params.static_pressure_coef_z);
+			airspeed_body(0) >= 0.f ? _params.ekf2_pcoef_xp : _params.ekf2_pcoef_xn,
+			airspeed_body(1) >= 0.f ? _params.ekf2_pcoef_yp : _params.ekf2_pcoef_yn,
+			_params.ekf2_pcoef_z);
 
-		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
+		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.ekf2_aspd_max));
 
 		const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
 

src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp

@@ -45,7 +45,7 @@
 
 void Ekf::controlDragFusion(const imuSample &imu_delayed)
 {
-	if ((_params.drag_ctrl > 0) && _drag_buffer) {
+	if ((_params.ekf2_drag_ctrl > 0) && _drag_buffer) {
 
 		if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
 			_control_status.flags.wind = true;
@@ -65,17 +65,19 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)
 
 void Ekf::fuseDrag(const dragSample &drag_sample)
 {
-	const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
+	const float R_ACC = fmaxf(_params.ekf2_drag_noise,
+				  0.5f); // observation noise variance in specific force drag (m/sec**2)**2
 	const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
 
 	// correct rotor momentum drag for increase in required rotor mass flow with altitude
 	// obtained from momentum disc theory
-	const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / atmosphere::kAirDensitySeaLevelStandardAtmos), 0.f);
+	const float mcoef_corrrected = fmaxf(_params.ekf2_mcoef * sqrtf(rho / atmosphere::kAirDensitySeaLevelStandardAtmos),
+					     0.f);
 
 	// drag model parameters
-	const bool using_bcoef_x = _params.bcoef_x > 1.0f;
-	const bool using_bcoef_y = _params.bcoef_y > 1.0f;
-	const bool using_mcoef   = _params.mcoef   > 0.001f;
+	const bool using_bcoef_x = _params.ekf2_bcoef_x > 1.0f;
+	const bool using_bcoef_y = _params.ekf2_bcoef_y > 1.0f;
+	const bool using_mcoef   = _params.ekf2_mcoef   > 0.001f;
 
 	if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
 		return;
@@ -92,11 +94,11 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 	Vector2f bcoef_inv{0.f, 0.f};
 
 	if (using_bcoef_x) {
-		bcoef_inv(0) = 1.f / _params.bcoef_x;
+		bcoef_inv(0) = 1.f / _params.ekf2_bcoef_x;
 	}
 
 	if (using_bcoef_y) {
-		bcoef_inv(1) = 1.f / _params.bcoef_y;
+		bcoef_inv(1) = 1.f / _params.ekf2_bcoef_y;
 	}
 
 	if (using_bcoef_x && using_bcoef_y) {

src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp

@@ -52,14 +52,14 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
 
 		// determine if we should use the horizontal position observations
-		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum);
+		bool quality_sufficient = (_params.ekf2_ev_qmin <= 0) || (ev_sample.quality >= _params.ekf2_ev_qmin);
 
 		const bool starting_conditions_passing = quality_sufficient
 				&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
-				&& ((_params.ev_quality_minimum <= 0)
-				    || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
-				&& ((_params.ev_quality_minimum <= 0)
-				    || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
+				&& ((_params.ekf2_ev_qmin <= 0)
+				    || (_ev_sample_prev.quality >= _params.ekf2_ev_qmin)) // previous quality sufficient
+				&& ((_params.ekf2_ev_qmin <= 0)
+				    || (_ext_vision_buffer->get_newest().quality >= _params.ekf2_ev_qmin)) // newest quality sufficient
 				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
 
 		updateEvAttitudeErrorFilter(ev_sample, ev_reset);
@@ -68,19 +68,19 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 
 		switch (ev_sample.vel_frame) {
 		case VelocityFrame::BODY_FRAME_FRD: {
-				EvVelBodyFrameFrd ev_vel_body(*this, ev_sample, _params.ev_vel_noise, imu_sample);
+				EvVelBodyFrameFrd ev_vel_body(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
 				controlEvVelFusion(ev_vel_body, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
 				break;
 			}
 
 		case VelocityFrame::LOCAL_FRAME_NED: {
-				EvVelLocalFrameNed ev_vel_ned(*this, ev_sample, _params.ev_vel_noise, imu_sample);
+				EvVelLocalFrameNed ev_vel_ned(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
 				controlEvVelFusion(ev_vel_ned, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
 				break;
 			}
 
 		case VelocityFrame::LOCAL_FRAME_FRD: {
-				EvVelLocalFrameFrd ev_vel_frd(*this, ev_sample, _params.ev_vel_noise, imu_sample);
+				EvVelLocalFrameFrd ev_vel_frd(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
 				controlEvVelFusion(ev_vel_frd, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
 				break;
 			}

src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp

@@ -75,13 +75,13 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 	}
 
 	const float measurement = pos(2) - pos_offset_earth(2);
-	float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f));
+	float measurement_var = math::max(pos_cov(2, 2), sq(_params.ekf2_evp_noise), sq(0.01f));
 
 #if defined(CONFIG_EKF2_GNSS)
 
 	// increase minimum variance if GPS active
 	if (_control_status.flags.gps_hgt) {
-		measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise));
+		measurement_var = math::max(measurement_var, sq(_params.ekf2_gps_p_noise));
 	}
 
 #endif // CONFIG_EKF2_GNSS
@@ -92,7 +92,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 					ev_sample.time_us,
 					measurement - bias_est.getBias(),           // observation
 					measurement_var + bias_est.getBiasVar(),    // observation variance
-					math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
+					math::max(_params.ekf2_evp_gate, 1.f)); // innovation gate
 
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
@@ -102,7 +102,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 		bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 	}
 
-	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
+	const bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
 			&& measurement_valid;
 
 	const bool starting_conditions_passing = common_starting_conditions_passing

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -49,7 +49,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
 
 	// determine if we should use EV position aiding
-	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
+	bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
 					     && _control_status.flags.tilt_align
 					     && PX4_ISFINITE(ev_sample.pos(0))
 					     && PX4_ISFINITE(ev_sample.pos(1));
@@ -131,7 +131,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 	// increase minimum variance if GNSS is active (position reference)
 	if (_control_status.flags.gnss_pos) {
 		for (int i = 0; i < 2; i++) {
-			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
+			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.ekf2_gps_p_noise));
 		}
 	}
 
@@ -142,8 +142,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 	const Vector2f measurement{pos(0), pos(1)};
 
 	const Vector2f measurement_var{
-		math::max(pos_cov(0, 0), sq(_params.ev_pos_noise), sq(0.01f)),
-		math::max(pos_cov(1, 1), sq(_params.ev_pos_noise), sq(0.01f))
+		math::max(pos_cov(0, 0), sq(_params.ekf2_evp_noise), sq(0.01f)),
+		math::max(pos_cov(1, 1), sq(_params.ekf2_evp_noise), sq(0.01f))
 	};
 
 	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
@@ -169,7 +169,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 			      pos_obs_var,                                            // observation variance
 			      position_estimate - position,                           // innovation
 			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
-			      math::max(_params.ev_pos_innov_gate, 1.f));             // innovation gate
+			      math::max(_params.ekf2_evp_gate, 1.f));             // innovation gate
 
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h

@@ -45,10 +45,10 @@
 class ExternalVisionVel
 {
 public:
-	ExternalVisionVel(Ekf &ekf_instance, const extVisionSample &vision_sample, const float ev_vel_noise,
+	ExternalVisionVel(Ekf &ekf_instance, const extVisionSample &vision_sample, const float ekf2_evv_noise,
 			  const imuSample &imu_sample) : _ekf(ekf_instance), _sample(vision_sample)
 	{
-		_min_variance = sq(ev_vel_noise);
+		_min_variance = sq(ekf2_evv_noise);
 		const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _ekf._state.gyro_bias;
 		Vector3f position_offset_body = _ekf._params.ev_pos_body - _ekf._params.imu_pos_body;
 		_velocity_offset_body = angular_velocity % position_offset_body;
@@ -93,9 +93,9 @@ public:
 class EvVelBodyFrameFrd : public ExternalVisionVel
 {
 public:
-	EvVelBodyFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
+	EvVelBodyFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
 			  const imuSample &imu_sample) :
-		ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
+		ExternalVisionVel(ekf_instance, vision_sample, ekf2_evv_noise, imu_sample)
 	{
 		_measurement = _sample.vel - _velocity_offset_body;
 		_measurement_var = _sample.velocity_var;
@@ -132,9 +132,9 @@ public:
 class EvVelLocalFrameNed : public ExternalVisionVel
 {
 public:
-	EvVelLocalFrameNed(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
+	EvVelLocalFrameNed(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
 			   const imuSample &imu_sample) :
-		ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
+		ExternalVisionVel(ekf_instance, vision_sample, ekf2_evv_noise, imu_sample)
 	{
 		const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body;
 
@@ -149,9 +149,9 @@ public:
 class EvVelLocalFrameFrd : public ExternalVisionVel
 {
 public:
-	EvVelLocalFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
+	EvVelLocalFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
 			   const imuSample &imu_sample) :
-		ExternalVisionVel(ekf_instance,	vision_sample, ev_vel_noise, imu_sample)
+		ExternalVisionVel(ekf_instance,	vision_sample, ekf2_evv_noise, imu_sample)
 	{
 		const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body;
 

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -51,15 +51,15 @@ void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_c
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
 
 	// determine if we should use EV velocity aiding
-	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
+	bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
 					     && _control_status.flags.tilt_align
 					     && ev._sample.vel.isAllFinite()
-					     && !ev._sample.vel.longerThan(_params.velocity_limit);
+					     && !ev._sample.vel.longerThan(_params.ekf2_vel_lim);
 
 
 	continuing_conditions_passing &= ev._measurement.isAllFinite() && ev._measurement_var.isAllFinite();
 
-	float gate = math::max(_params.ev_vel_innov_gate, 1.f);
+	float gate = math::max(_params.ekf2_evv_gate, 1.f);
 
 	if (_control_status.flags.ev_vel) {
 		if (continuing_conditions_passing) {

src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp

@@ -45,7 +45,7 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
 	static constexpr const char *AID_SRC_NAME = "EV yaw";
 
 	float obs = getEulerYaw(ev_sample.quat);
-	float obs_var = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
+	float obs_var = math::max(ev_sample.orientation_var(2), _params.ekf2_eva_noise, sq(0.01f));
 
 	float innov = wrap_pi(getEulerYaw(_R_to_earth) - obs);
 	float innov_var = 0.f;
@@ -59,14 +59,14 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
 			      obs_var,                                     // observation variance
 			      innov,                                       // innovation
 			      innov_var,                                   // innovation variance
-			      math::max(_params.heading_innov_gate, 1.f)); // innovation gate
+			      math::max(_params.ekf2_hdg_gate, 1.f)); // innovation gate
 
 	if (ev_reset) {
 		_control_status.flags.ev_yaw_fault = false;
 	}
 
 	// determine if we should use EV yaw aiding
-	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
+	bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
 					     && _control_status.flags.tilt_align
 					     && !_control_status.flags.ev_yaw_fault
 					     && PX4_ISFINITE(aid_src.observation)

src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp

@@ -48,7 +48,7 @@ void Ekf::controlFakeHgtFusion()
 
 	if (fake_hgt_data_ready) {
 
-		const float obs_var = sq(_params.pos_noaid_noise);
+		const float obs_var = sq(_params.ekf2_noaid_noise);
 		const float innov_gate = 3.f;
 
 		updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate);
@@ -110,7 +110,7 @@ void Ekf::resetHeightToLastKnown()
 {
 	_information_events.flags.reset_pos_to_last_known = true;
 	ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude());
-	resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
+	resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.ekf2_noaid_noise));
 }
 
 void Ekf::stopFakeHgtFusion()

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -52,7 +52,7 @@ void Ekf::controlFakePosFusion()
 		Vector2f obs_var;
 
 		if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-			obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, 1.f));
+			obs_var(0) = obs_var(1) = sq(fmaxf(_params.ekf2_noaid_noise, 1.f));
 
 		} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 			// Accelerate tilt fine alignment by fusing more

src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp

@@ -113,29 +113,29 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
 	_check_fail_status.flags.fix = (gnss.fix_type < 3);
 
 	// Check the number of satellites
-	_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats);
+	_check_fail_status.flags.nsats = (gnss.nsats < _params.ekf2_req_nsats);
 
 	// Check the position dilution of precision
-	_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop);
+	_check_fail_status.flags.pdop = (gnss.pdop > _params.ekf2_req_pdop);
 
 	// Check the reported horizontal and vertical position accuracy
-	_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc);
-	_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc);
+	_check_fail_status.flags.hacc = (gnss.hacc > _params.ekf2_req_eph);
+	_check_fail_status.flags.vacc = (gnss.vacc > _params.ekf2_req_epv);
 
 	// Check the reported speed accuracy
-	_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc);
+	_check_fail_status.flags.sacc = (gnss.sacc > _params.ekf2_req_sacc);
 
 	_check_fail_status.flags.spoofed = gnss.spoofed;
 
 	runOnGroundGnssChecks(gnss);
 
 	// force horizontal speed failure if above the limit
-	if (gnss.vel.xy().longerThan(_params.velocity_limit)) {
+	if (gnss.vel.xy().longerThan(_params.ekf2_vel_lim)) {
 		_check_fail_status.flags.hspeed = true;
 	}
 
 	// force vertical speed failure if above the limit
-	if (fabsf(gnss.vel(2)) > _params.velocity_limit) {
+	if (fabsf(gnss.vel(2)) > _params.ekf2_vel_lim) {
 		_check_fail_status.flags.vspeed = true;
 	}
 
@@ -198,7 +198,7 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss)
 		}
 
 		// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
-		const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
+		const Vector3f vel_limit(_params.ekf2_req_hdrift, _params.ekf2_req_hdrift, _params.ekf2_req_vdrift);
 		Vector3f delta_pos(delta_pos_n, delta_pos_e, (_alt_prev - gnss.alt));
 
 		// Apply a low pass filter
@@ -209,26 +209,26 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss)
 
 		// hdrift: calculate the horizontal drift speed and fail if too high
 		_horizontal_position_drift_rate_m_s = Vector2f(_lat_lon_alt_deriv_filt.xy()).norm();
-		_check_fail_status.flags.hdrift = (_horizontal_position_drift_rate_m_s > _params.req_hdrift);
+		_check_fail_status.flags.hdrift = (_horizontal_position_drift_rate_m_s > _params.ekf2_req_hdrift);
 
 		// vdrift: fail if the vertical drift speed is too high
 		_vertical_position_drift_rate_m_s = fabsf(_lat_lon_alt_deriv_filt(2));
-		_check_fail_status.flags.vdrift = (_vertical_position_drift_rate_m_s > _params.req_vdrift);
+		_check_fail_status.flags.vdrift = (_vertical_position_drift_rate_m_s > _params.ekf2_req_vdrift);
 
 		// hspeed: check the magnitude of the filtered horizontal GNSS velocity
 		const Vector2f vel_ne = matrix::constrain(Vector2f(gnss.vel.xy()),
-					-10.0f * _params.req_hdrift,
-					10.0f * _params.req_hdrift);
+					-10.0f * _params.ekf2_req_hdrift,
+					10.0f * _params.ekf2_req_hdrift);
 		_vel_ne_filt = vel_ne * filter_coef + _vel_ne_filt * (1.0f - filter_coef);
 		_filtered_horizontal_velocity_m_s = _vel_ne_filt.norm();
-		_check_fail_status.flags.hspeed = (_filtered_horizontal_velocity_m_s > _params.req_hdrift);
+		_check_fail_status.flags.hspeed = (_filtered_horizontal_velocity_m_s > _params.ekf2_req_hdrift);
 
 		// vspeed: check the magnitude of the filtered vertical GNSS velocity
-		const float gnss_vz_limit = 10.f * _params.req_vdrift;
+		const float gnss_vz_limit = 10.f * _params.ekf2_req_vdrift;
 		const float gnss_vz = math::constrain(gnss.vel(2), -gnss_vz_limit, gnss_vz_limit);
 		_vel_d_filt = gnss_vz * filter_coef + _vel_d_filt * (1.f - filter_coef);
 
-		_check_fail_status.flags.vspeed = (fabsf(_vel_d_filt) > _params.req_vdrift);
+		_check_fail_status.flags.vspeed = (fabsf(_vel_d_filt) > _params.ekf2_req_vdrift);
 
 	} else {
 		// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation

src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp

@@ -43,10 +43,11 @@ namespace estimator
 class GnssChecks final
 {
 public:
-	GnssChecks(int32_t &check_mask, int32_t &req_nsats, float &req_pdop, float &req_hacc, float &req_vacc, float &req_sacc,
-		   float &req_hdrift, float &req_vdrift, float &velocity_limit, uint32_t &min_health_time_us,
+	GnssChecks(int32_t &check_mask, int32_t &ekf2_req_nsats, float &ekf2_req_pdop, float &ekf2_req_eph, float &ekf2_req_epv,
+		   float &ekf2_req_sacc,
+		   float &ekf2_req_hdrift, float &ekf2_req_vdrift, float &ekf2_vel_lim, uint32_t &min_health_time_us,
 		   filter_control_status_u &control_status):
-		_params{check_mask, req_nsats, req_pdop, req_hacc, req_vacc, req_sacc, req_hdrift, req_vdrift, velocity_limit, min_health_time_us},
+		_params{check_mask, ekf2_req_nsats, ekf2_req_pdop, ekf2_req_eph, ekf2_req_epv, ekf2_req_sacc, ekf2_req_hdrift, ekf2_req_vdrift, ekf2_vel_lim, min_health_time_us},
 		_control_status(control_status)
 	{};
 
@@ -143,14 +144,14 @@ private:
 
 	struct Params {
 		const int32_t &check_mask;
-		const int32_t &req_nsats;
-		const float &req_pdop;
-		const float &req_hacc;
-		const float &req_vacc;
-		const float &req_sacc;
-		const float &req_hdrift;
-		const float &req_vdrift;
-		const float &velocity_limit;
+		const int32_t &ekf2_req_nsats;
+		const float &ekf2_req_pdop;
+		const float &ekf2_req_eph;
+		const float &ekf2_req_epv;
+		const float &ekf2_req_sacc;
+		const float &ekf2_req_hdrift;
+		const float &ekf2_req_vdrift;
+		const float &ekf2_vel_lim;
 		const uint32_t &min_health_time_us;
 	};
 

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -50,13 +50,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 	if (_gps_data_ready) {
 
 		// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
-		float noise = math::max(gps_sample.vacc, 1.5f * _params.gps_pos_noise); // use 1.5 as a typical ratio of vacc/hacc
+		float noise = math::max(gps_sample.vacc, 1.5f * _params.ekf2_gps_p_noise); // use 1.5 as a typical ratio of vacc/hacc
 
 		if (!isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.gps_hgt)) {
 			// if we are not using another source of aiding, then we are reliant on the GPS
 			// observations to constrain attitude errors and must limit the observation noise value.
-			if (noise > _params.pos_noaid_noise) {
-				noise = _params.pos_noaid_noise;
+			if (noise > _params.ekf2_noaid_noise) {
+				noise = _params.ekf2_noaid_noise;
 			}
 		}
 
@@ -74,7 +74,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 						gps_sample.time_us,
 						-(measurement - bias_est.getBias()),
 						measurement_var + bias_est.getBiasVar(),
-						math::max(_params.gps_pos_innov_gate, 1.f));
+						math::max(_params.ekf2_gps_p_gate, 1.f));
 
 		// update the bias estimator before updating the main filter but after
 		// using its current state to compute the vertical position innovation
@@ -89,7 +89,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 						       && _local_origin_lat_lon.isInitialized()
 						       && _gnss_checks.passed();
 
-		const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
+		const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
 				&& common_conditions_passing;
 
 		const bool starting_conditions_passing = continuing_conditions_passing

src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

@@ -47,7 +47,7 @@
 
 void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 {
-	if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
+	if (!(_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
 	    || _control_status.flags.gnss_yaw_fault) {
 
 		stopGnssYawFusion();
@@ -151,7 +151,7 @@ void Ekf::updateGnssYaw(const gnssSample &gnss_sample)
 			      R_YAW,                                       // observation variance
 			      wrap_pi(heading_pred - measured_hdg),        // innovation
 			      heading_innov_var,                           // innovation variance
-			      math::max(_params.heading_innov_gate, 1.f)); // innovation gate
+			      math::max(_params.ekf2_hdg_gate, 1.f)); // innovation gate
 }
 
 void Ekf::fuseGnssYaw(float antenna_yaw_offset)

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -41,7 +41,7 @@
 
 void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 {
-	if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
+	if (!_gps_buffer || (_params.ekf2_gps_ctrl == 0)) {
 		stopGnssFusion();
 		return;
 	}
@@ -124,7 +124,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 
 void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset)
 {
-	const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
+	const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
 			&& _control_status.flags.tilt_align
 			&& _control_status.flags.yaw_align;
 	const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed();
@@ -169,7 +169,7 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
 
 void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset)
 {
-	const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
+	const bool gnss_pos_enabled = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
 
 	const bool continuing_conditions_passing = gnss_pos_enabled
 			&& _control_status.flags.tilt_align
@@ -215,10 +215,10 @@ void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_samp
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 	const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
 
-	const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f));
+	const float vel_var = sq(math::max(gnss_sample.sacc, _params.ekf2_gps_v_noise, 0.01f));
 	const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
 
-	const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
+	const float innovation_gate = math::max(_params.ekf2_gps_v_gate, 1.f);
 
 	updateAidSourceStatus(aid_src,
 			      gnss_sample.time_us,                  // sample timestamp
@@ -235,7 +235,7 @@ void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_samp
 				   && (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted
 
 	if (bad_acc_vz_rejected
-	    && (gnss_sample.sacc < _params.req_sacc)
+	    && (gnss_sample.sacc < _params.ekf2_req_sacc)
 	   ) {
 		const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
 		aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
@@ -253,13 +253,13 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
 	const Vector2f innovation = (_gpos - measurement_corrected).xy();
 
 	// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
-	float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
+	float pos_noise = math::max(gnss_sample.hacc, _params.ekf2_gps_p_noise);
 
 	if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) {
 		// if we are not using another source of aiding, then we are reliant on the GNSS
 		// observations to constrain attitude errors and must limit the observation noise value.
-		if (pos_noise > _params.pos_noaid_noise) {
-			pos_noise = _params.pos_noaid_noise;
+		if (pos_noise > _params.ekf2_noaid_noise) {
+			pos_noise = _params.ekf2_noaid_noise;
 		}
 	}
 
@@ -273,7 +273,7 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
 			      pos_obs_var,                                            // observation variance
 			      innovation,                                             // innovation
 			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
-			      math::max(_params.gps_pos_innov_gate, 1.f));            // innovation gate
+			      math::max(_params.ekf2_gps_p_gate, 1.f));            // innovation gate
 }
 
 void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
@@ -283,14 +283,14 @@ void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
 	const Vector2f vel_xy(aid_src_vel.observation);
 
 	if ((vel_var > 0.f)
-	    && (vel_var < _params.req_sacc)
+	    && (vel_var < _params.ekf2_req_sacc)
 	    && vel_xy.isAllFinite()) {
 
 		_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);
 
 		// Try to align yaw using estimate if available
-		if (((_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
-		     || (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
+		if (((_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
+		     || (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
 		    && !_control_status.flags.yaw_align
 		    && _control_status.flags.tilt_align) {
 			if (resetYawToEKFGSF()) {
@@ -379,7 +379,7 @@ bool Ekf::shouldResetGpsFusion() const
 
 	const bool is_reset_required = has_horizontal_aiding_timed_out
 				       || (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max)
-					   && (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
+					   && (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
 
 	const bool is_inflight_nav_failure = _control_status.flags.in_air
 					     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)

src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp

@@ -50,7 +50,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 {
 	// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
 	const Vector3f measurement = Vector3f(imu.delta_vel / imu.delta_vel_dt - _state.accel_bias).unit();
-	const float measurement_var = math::max(sq(_params.gravity_noise), sq(0.01f));
+	const float measurement_var = math::max(sq(_params.ekf2_grav_noise), sq(0.01f));
 
 	const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
 	const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -54,7 +54,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 		_control_status.flags.mag_aligned_in_flight = false;
 	}
 
-	if (_params.mag_fusion_type == MagFuseType::NONE) {
+	if (_params.ekf2_mag_type == MagFuseType::NONE) {
 		stopMagFusion();
 		return;
 	}
@@ -114,7 +114,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
-		if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+		if (_params.ekf2_synt_mag_z && (_params.ekf2_decl_type & GeoDeclinationMask::USE_GEO_DECL)
 		    && (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
 		   ) {
 			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);
@@ -131,7 +131,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 		_fault_status.flags.bad_mag_z = false;
 
 		// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
-		const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
+		const float R_MAG = math::max(sq(_params.ekf2_mag_noise), sq(0.01f));
 
 		// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
 		Vector3f mag_innov;
@@ -147,7 +147,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 				      Vector3f(R_MAG, R_MAG, R_MAG),           // observation variance
 				      mag_innov,                               // innovation
 				      innov_var,                               // innovation variance
-				      math::max(_params.mag_innov_gate, 1.f)); // innovation gate
+				      math::max(_params.ekf2_mag_gate, 1.f)); // innovation gate
 
 		// Perform an innovation consistency check and report the result
 		_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);
@@ -155,9 +155,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 		_innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f);
 
 		// determine if we should use mag fusion
-		bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::INIT)
-						      || (_params.mag_fusion_type == MagFuseType::AUTO)
-						      || (_params.mag_fusion_type == MagFuseType::HEADING))
+		bool continuing_conditions_passing = ((_params.ekf2_mag_type == MagFuseType::INIT)
+						      || (_params.ekf2_mag_type == MagFuseType::AUTO)
+						      || (_params.ekf2_mag_type == MagFuseType::HEADING))
 						     && _control_status.flags.tilt_align
 						     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
 						     && mag_sample.mag.longerThan(0.f)
@@ -184,12 +184,12 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 							       && !_control_status.flags.gnss_yaw;
 
 			_control_status.flags.mag_3D = common_conditions_passing
-						       && (_params.mag_fusion_type == MagFuseType::AUTO)
+						       && (_params.ekf2_mag_type == MagFuseType::AUTO)
 						       && _control_status.flags.mag_aligned_in_flight;
 
 			_control_status.flags.mag_hdg = common_conditions_passing
-							&& ((_params.mag_fusion_type == MagFuseType::HEADING)
-							    || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
+							&& ((_params.ekf2_mag_type == MagFuseType::HEADING)
+							    || (_params.ekf2_mag_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
 		}
 
 		// TODO: allow clearing mag_fault if mag_3d is good?
@@ -237,17 +237,17 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 						// observation variance (rad**2)
 						const float R_DECL = sq(0.5f);
 
-						if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+						if ((_params.ekf2_decl_type & GeoDeclinationMask::USE_GEO_DECL)
 						    && PX4_ISFINITE(_wmm_declination_rad)
 						   ) {
 							// using declination from the world magnetic model
 							fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states, update_tilt);
 
-						} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
-							   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+						} else if ((_params.ekf2_decl_type & GeoDeclinationMask::SAVE_GEO_DECL)
+							   && PX4_ISFINITE(_params.ekf2_mag_decl) && (fabsf(_params.ekf2_mag_decl) > 0.f)
 							  ) {
 							// using previously saved declination
-							fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states, update_tilt);
+							fuseDeclination(math::radians(_params.ekf2_mag_decl), R_DECL, update_all_states, update_tilt);
 
 						} else {
 							// if there is no aiding coming from an inertial frame we need to fuse some declination
@@ -460,7 +460,7 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
 	Vector3f mag_bias{0.f, 0.f, 0.f};
 	const Vector3f mag_bias_var = getMagBiasVariance();
 
-	if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
+	if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.ekf2_mag_noise))) {
 		mag_bias = _state.mag_B;
 	}
 
@@ -482,10 +482,10 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
 		_mag_heading_innov_lpf.reset(0.f);
 	}
 
-	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
+	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.ekf2_head_noise) {
 		// Check if there has been enough change in horizontal velocity to make yaw observable
 
-		if (isNorthEastAidingActive() && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) {
+		if (isNorthEastAidingActive() && (_accel_horiz_lpf.getState().longerThan(_params.ekf2_mag_acclim))) {
 			// yaw angle must be observable to consider consistency
 			_control_status.flags.mag_heading_consistent = true;
 		}
@@ -499,7 +499,7 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 {
 	_control_status.flags.mag_field_disturbed = false;
 
-	if (_params.mag_check == 0) {
+	if (_params.ekf2_mag_check == 0) {
 		// skip all checks
 		return true;
 	}
@@ -507,14 +507,14 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 	bool is_check_failing = false;
 	_mag_strength = mag_sample.length();
 
-	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
+	if (_params.ekf2_mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
 		if (PX4_ISFINITE(_wmm_field_strength_gauss)) {
-			if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) {
+			if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.ekf2_mag_chk_str)) {
 				_control_status.flags.mag_field_disturbed = true;
 				is_check_failing = true;
 			}
 
-		} else if (_params.mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
+		} else if (_params.ekf2_mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
 			is_check_failing = true;
 
 		} else {
@@ -531,9 +531,9 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 	const Vector3f mag_earth = _R_to_earth * mag_sample;
 	_mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
 
-	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
+	if (_params.ekf2_mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
 		if (PX4_ISFINITE(_wmm_inclination_rad)) {
-			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
+			const float inc_tol_rad = radians(_params.ekf2_mag_chk_inc);
 			const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);
 
 			if (fabsf(inc_error_rad) > inc_tol_rad) {
@@ -541,7 +541,7 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 				is_check_failing = true;
 			}
 
-		} else if (_params.mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
+		} else if (_params.ekf2_mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
 			is_check_failing = true;
 
 		} else {
@@ -569,7 +569,7 @@ void Ekf::resetMagHeading(const Vector3f &mag)
 	Vector3f mag_bias{0.f, 0.f, 0.f};
 	const Vector3f mag_bias_var = getMagBiasVariance();
 
-	if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
+	if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.ekf2_mag_noise))) {
 		mag_bias = _state.mag_B;
 	}
 
@@ -583,7 +583,7 @@ void Ekf::resetMagHeading(const Vector3f &mag)
 	// calculate the observed yaw angle and yaw variance
 	const float declination = getMagDeclination();
 	float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
-	float yaw_new_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
+	float yaw_new_variance = math::max(sq(_params.ekf2_head_noise), sq(0.01f));
 
 	ECL_INFO("reset mag heading %.3f -> %.3f rad (bias:[%.3f, %.3f, %.3f], declination:%.1f)",
 		 (double)getEulerYaw(_R_to_earth), (double)yaw_new,
@@ -606,17 +606,17 @@ float Ekf::getMagDeclination()
 		// Use value consistent with earth field state
 		return atan2f(_state.mag_I(1), _state.mag_I(0));
 
-	} else if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+	} else if ((_params.ekf2_decl_type & GeoDeclinationMask::USE_GEO_DECL)
 		   && PX4_ISFINITE(_wmm_declination_rad)
 		  ) {
 		// if available use value returned by geo library
 		return _wmm_declination_rad;
 
-	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
-		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+	} else if ((_params.ekf2_decl_type & GeoDeclinationMask::SAVE_GEO_DECL)
+		   && PX4_ISFINITE(_params.ekf2_mag_decl) && (fabsf(_params.ekf2_mag_decl) > 0.f)
 		  ) {
 		// using saved mag declination
-		return math::radians(_params.mag_declination_deg);
+		return math::radians(_params.ekf2_mag_decl);
 	}
 
 	// otherwise unavailable

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -93,7 +93,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 
 			// we need to re-initialise covariances and abort this fusion step
 			if (update_all_states) {
-				resetQuatCov(_params.mag_heading_noise);
+				resetQuatCov(_params.ekf2_head_noise);
 			}
 
 			resetMagEarthCov();

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -42,7 +42,7 @@
 
 void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 {
-	if (!_flow_buffer || (_params.flow_ctrl != 1)) {
+	if (!_flow_buffer || (_params.ekf2_of_ctrl != 1)) {
 		stopFlowFusion();
 		return;
 	}
@@ -56,7 +56,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		_ref_body_rate = -(imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias());
 
 		// ensure valid flow sample gyro rate before proceeding
-		switch (static_cast<FlowGyroSource>(_params.flow_gyro_src)) {
+		switch (static_cast<FlowGyroSource>(_params.ekf2_of_gyr_src)) {
 		default:
 
 		/* FALLTHROUGH */
@@ -80,8 +80,8 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		const flowSample &flow_sample = _flow_sample_delayed;
 
 		const int32_t min_quality = _control_status.flags.in_air
-					    ? _params.flow_qual_min
-					    : _params.flow_qual_min_gnd;
+					    ? _params.ekf2_of_qmin
+					    : _params.ekf2_of_qmin_gnd;
 
 		const bool is_quality_good = (flow_sample.quality >= min_quality);
 
@@ -113,7 +113,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				      Vector2f{R_LOS, R_LOS},                              // observation variance
 				      predictFlow(flow_gyro_corrected) - flow_compensated, // innovation
 				      innov_var,                                           // innovation variance
-				      math::max(_params.flow_innov_gate, 1.f));            // innovation gate
+				      math::max(_params.ekf2_of_gate, 1.f));            // innovation gate
 
 		// logging
 		_flow_rate_compensated = flow_compensated;
@@ -144,7 +144,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					       && !flow_sample.flow_rate.longerThan(_flow_max_rate)
 					       && !flow_compensated.longerThan(_flow_max_rate);
 
-		const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
+		const bool continuing_conditions_passing = (_params.ekf2_of_ctrl == 1)
 				&& _control_status.flags.tilt_align
 				&& is_within_sensor_dist;
 

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -73,7 +73,7 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			// when close to the ground (singularity at 0) and the innovation can suddenly become really
 			// large and destabilize the filter
 			_aid_src_optical_flow.test_ratio[1] = sq(_aid_src_optical_flow.innovation[1]) / (sq(
-					_params.flow_innov_gate) * _aid_src_optical_flow.innovation_variance[1]);
+					_params.ekf2_of_gate) * _aid_src_optical_flow.innovation_variance[1]);
 
 			if (_aid_src_optical_flow.test_ratio[1] > 1.f) {
 				continue;
@@ -164,14 +164,14 @@ Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const
 float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) const
 {
 	// calculate the observation noise variance - scaling noise linearly across flow quality range
-	const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
-	const float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
+	const float R_LOS_best = fmaxf(_params.ekf2_of_n_min, 0.05f);
+	const float R_LOS_worst = fmaxf(_params.ekf2_of_n_max, 0.05f);
 
 	// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
-	float weighting = (255.f - (float)_params.flow_qual_min);
+	float weighting = (255.f - (float)_params.ekf2_of_qmin);
 
 	if (weighting >= 1.f) {
-		weighting = math::constrain((float)(flow_sample.quality - _params.flow_qual_min) / weighting, 0.f, 1.f);
+		weighting = math::constrain((float)(flow_sample.quality - _params.ekf2_of_qmin) / weighting, 0.f, 1.f);
 
 	} else {
 		weighting = 0.0f;

src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp

@@ -1,84 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Sensor.hpp
- * Abstract class for sensors
- *
- * @author Mathieu Bresciani <brescianimathieu@gmail.com>
- *
- */
-
-#ifndef EKF_SENSOR_HPP
-#define EKF_SENSOR_HPP
-
-#include <cstdint>
-
-namespace estimator
-{
-namespace sensor
-{
-
-class Sensor
-{
-public:
-	virtual ~Sensor() {};
-
-	/*
-	 * run sanity checks on the current data
-	 * this has to be called immediately after
-	 * setting new data
-	 */
-	virtual void runChecks() {};
-
-	/*
-	 * return true if the sensor is healthy
-	 */
-	virtual bool isHealthy() const = 0;
-
-	/*
-	 * return true if the delayed sample is healthy
-	 * and can be fused in the estimator
-	 */
-	virtual bool isDataHealthy() const = 0;
-
-	/*
-	 * return true if the sensor data rate is
-	 * stable and high enough
-	 */
-	virtual bool isRegularlySendingData() const = 0;
-};
-
-} // namespace sensor
-} // namespace estimator
-#endif // !EKF_SENSOR_HPP

src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp

@@ -0,0 +1,428 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file range_height_control.cpp
+ * Control functions for ekf range finder height fusion
+ */
+
+#include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_h.h"
+#include "ekf_derivation/generated/compute_hagl_innov_var.h"
+
+void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
+{
+	// Check if rangefinder is available/enabled
+	if (!_range_buffer) {
+		// PX4_INFO("no buff");
+		return;
+	}
+
+	// Pop rangefinder measurement from buffer of samples into active sample
+	sensor::rangeSample sample = {};
+	if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
+		if (_range_sensor.timedOut(imu_sample.time_us)) {
+			// Disable fusion if it's currently enabled
+			if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
+				PX4_INFO("stopping RNG fusion, sensor timed out");
+				stopRangeAltitudeFusion();
+				stopRangeTerrainFusion();
+			}
+			// PX4_INFO("timed out1");
+		}
+		return;
+	}
+
+	// PX4_INFO("got one!");
+	// PX4_INFO("rng: %f", (double)sample.rng);
+	// PX4_INFO("quality: %d", sample.quality);
+
+	// Set the raw sample
+	_range_sensor.setSample(sample);
+
+	// TODO: move setting params to init function
+	// Set all of the parameters
+	_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
+	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
+
+	// Update sensor to earth rotation
+	_range_sensor.updateSensorToEarthRotation(_R_to_earth);
+
+
+	// TODO: refactor into validity_checks()
+	// Gate sample consumption on these checks
+	bool quality_ok = sample.quality > 0; // TODO: what about unknown? (-1)
+	bool tilt_ok = _range_sensor.isTiltOk();
+	bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal();
+	// - Not stuck value
+	// - Not fog detected
+
+	// If quality, tilt, or value are outside of bounds -- throw away measurement
+	if (!quality_ok || !tilt_ok || !range_ok) {
+		if (_range_sensor.timedOut(imu_sample.time_us)) {
+			// Disable fusion if it's currently enabled
+			if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
+				PX4_INFO("stopping RNG fusion, sensor data invalid");
+				stopRangeAltitudeFusion();
+				stopRangeTerrainFusion();
+			}
+			PX4_INFO("timed out2");
+		}
+
+		if (!quality_ok) {
+			PX4_INFO("!quality_ok");
+		}
+		if (!tilt_ok) {
+			PX4_INFO("!tilt_ok");
+		}
+		if (!range_ok) {
+			PX4_INFO("!range_ok");
+		} // commander takeoff
+		return;
+	}
+
+
+	// TODO: refactor into apply_body_offset()
+	// Correct the range data for position offset relative to the IMU
+	const Vector3f rng_pos_body = { _params.ekf2_rng_pos_x, _params.ekf2_rng_pos_y, _params.ekf2_rng_pos_z };
+	const Vector3f imu_pos_body = _params.imu_pos_body;
+	const Vector3f pos_offset_body = rng_pos_body - imu_pos_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+	sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt();
+
+	// Provide sample from buffer to object
+	_range_sensor.setSample(sample);
+
+
+	// TODO: refactor into consintency_filter_update() that runs consistency status
+	// Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate
+	_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
+
+	const float z = _gpos.altitude();
+	const float vz = _state.vel(2);
+	const float dist = _range_sensor.getDistBottom(); // NOTE: applies rotation into world frame
+	const float dist_var = 0.05;
+	const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
+	const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
+
+	// Run the kinematic consistency check
+	_rng_consistency_check.run(z, z_var, vz, vz_var, dist, dist_var, imu_sample.time_us);
+
+	// Track kinematic consistency
+	// _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+	_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
+	updateRangeHagl(_aid_src_rng_hgt);
+
+	if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) {
+		PX4_INFO("INFINIFY OBSERVATION INVALID");
+	}
+
+	// Fuse Range data as Primary height source
+	// NOTE: terrain is not estimated in this mode
+	if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
+		fuseRangeAsHeightSource();
+	} else {
+		// Fuse Range data as aiding height source (Primary GPS or Baro)
+		fuseRangeAsHeightAiding();
+	}
+}
+
+void Ekf::fuseRangeAsHeightAiding()
+{
+	bool range_aid_conditional = _params.rng_ctrl == RngCtrl::CONDITIONAL;
+	bool range_aid_enabled = _params.rng_ctrl == RngCtrl::ENABLED;
+
+	bool range_aid_conditions_passed = rangeAidConditionsPassed();
+	bool kinematically_consistent = _control_status.flags.rng_kin_consistent;
+
+	bool do_range_aid = kinematically_consistent &&
+						(range_aid_enabled || (range_aid_conditional && range_aid_conditions_passed));
+
+	bool fuse_measurement = false;
+
+
+	// Variables to use below
+	bool innovation_rejected = _aid_src_rng_hgt.innovation_rejected;
+	bool optical_flow_for_terrain = _control_status.flags.opt_flow_terrain;
+
+	// Fuse Range into Altitude if:
+	// - passes range_aid_conditionalchecks
+	// - kinematically consistent
+	if (do_range_aid) {
+		// Start fusion
+		if (!_control_status.flags.rng_hgt) {
+			// Fusion init logic
+			PX4_INFO("starting RNG Altitude fusion");
+			_control_status.flags.rng_hgt = true;
+
+			// TODO: do we really need to stop terrain fusion here?
+			stopRangeTerrainFusion();
+
+			// TODO: review for correctness
+			if (innovation_rejected && kinematically_consistent) {
+				// Reset aid source
+				PX4_INFO("range alt fusion, resetting aid source");
+				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+			}
+		}
+
+		// Fuse
+		fuse_measurement = true;
+
+	} else {
+		// Stop fusion
+		stopRangeAltitudeFusion();
+
+		// if (!range_aid_conditions_passed) {
+		// 	PX4_INFO("range aid conditions failed");
+		// }
+		// if (!kinematically_consistent) {
+		// 	PX4_INFO("kinematically inconsistent");
+		// }
+	}
+
+	// Fuse Range into Terrain if:
+	// - kinematically consistent
+	if (kinematically_consistent) {
+
+		// Start fusion
+		if (!_control_status.flags.rng_terrain) {
+			// Fusion init logic
+			PX4_INFO("starting RNG Terrain fusion");
+			_control_status.flags.rng_terrain = true;
+
+			// Reset terrain when we first init
+			static bool first_init = true;
+			if (!optical_flow_for_terrain && first_init) {
+				first_init = false;
+				// Reset aid source and then reset terrain estimate
+				// PX4_INFO("FIRST range terrain fusion, resetting terrain to range");
+				PX4_INFO("FIRST range terrain fusion, resetting terrain to range");
+
+				resetTerrainToRng(_aid_src_rng_hgt);
+				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+				resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
+			}
+		}
+
+		// Reset terrain to range if innovation is rejected
+		if (!optical_flow_for_terrain && innovation_rejected
+				&& kinematically_consistent) {
+			PX4_INFO("range terrain fusion, resetting terrain to range");
+			resetTerrainToRng(_aid_src_rng_hgt);
+			resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+		}
+
+		// Fuse
+		fuse_measurement = true;
+
+	} else {
+		// Stop fusion
+		stopRangeTerrainFusion();
+	}
+
+	if (fuse_measurement) {
+		// PX4_INFO("fusing: %f", (double)_aid_src_rng_hgt.observation);
+		// PX4_INFO("_control_status.flags.rng_hgt: %d", _control_status.flags.rng_hgt);
+		// PX4_INFO("_control_status.flags.rng_terrain: %d", _control_status.flags.rng_terrain);
+		// PX4_INFO("_height_sensor_ref: %d", (int)_height_sensor_ref);
+
+		fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
+
+		// commander takeoff
+
+	}
+}
+
+void Ekf::fuseRangeAsHeightSource()
+{
+	// When primary height source is RANGE, we always fuse it
+	// I don't think conditional range aid mode makes sense in the context of RANGE = primary
+
+	// Fusion init logic
+	if (_height_sensor_ref != HeightSensor::RANGE) {
+
+		_height_sensor_ref = HeightSensor::RANGE;
+		_information_events.flags.reset_hgt_to_rng = true;
+
+		// Reset aid source innovation
+		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+
+		// Reset altitude to RANGE
+		resetAltitudeTo(_aid_src_rng_hgt.observation, _aid_src_rng_hgt.observation_variance);
+		_control_status.flags.rng_hgt = true;
+
+		// Cannot have terrain estimate fusion while RANGE is primary
+		stopRangeTerrainFusion();
+		_state.terrain = 0.f;
+
+		// TODO: needed? It's set above in --> resetAidSourceStatusZeroInnovation()
+		// _aid_src_rng_hgt.time_last_fuse = imu_sample.time_us;
+	}
+
+	// TODO:
+	// When RNG is primary height source
+	if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
+		PX4_INFO("RNG height fusion reset required, all height sources failing");
+
+		uint64_t timestamp = _aid_src_rng_hgt.timestamp;
+		_information_events.flags.reset_hgt_to_rng = true;
+		resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
+		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+
+		// reset vertical velocity if no valid sources available
+		if (!isVerticalVelocityAidingActive()) {
+			PX4_INFO("resetting vertical velocity");
+			resetVerticalVelocityToZero();
+		}
+
+		_aid_src_rng_hgt.time_last_fuse = timestamp;
+	}
+}
+
+bool Ekf::rangeAidConditionsPassed()
+{
+	// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
+	// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
+	float range_hagl_max = _params.max_hagl_for_range_aid;
+	float max_vel_xy = _params.max_vel_for_range_aid;
+
+	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
+
+	bool is_hagl_stable = (hagl_test_ratio < 1.f);
+
+	if (!_control_status.flags.rng_hgt) {
+		range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
+		max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
+		is_hagl_stable = (hagl_test_ratio < 0.01f);
+	}
+
+	const bool is_in_range = (getHagl() < range_hagl_max);
+
+	bool is_below_max_speed = true;
+
+	if (isHorizontalAidingActive()) {
+		is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
+	}
+
+	return is_in_range && is_hagl_stable && is_below_max_speed;
+}
+
+void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
+{
+	// Since the distance is not a direct observation of the terrain state but is based
+	// on the height state, a reset should consider the height uncertainty. This can be
+	// done by manipulating the Kalman gain to inject all the innovation in the terrain state
+	// and create the correct correlation with the terrain state with a covariance update.
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 0.f);
+
+	const float old_terrain = _state.terrain;
+
+	VectorState H;
+	sym::ComputeHaglH(&H);
+
+	VectorState K;
+	K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset"
+
+	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
+
+	// record the state change
+	const float delta_terrain = _state.terrain - old_terrain;
+
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+
+	aid_src.time_last_fuse = _time_delayed_us;
+}
+
+void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
+{
+	const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
+	const float measurement_variance = getRngVar();
+
+	float innovation_variance;
+	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
+
+	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
+	updateAidSourceStatus(aid_src,
+			      _range_sensor.sample()->time_us, // sample timestamp
+			      measurement,                               // observation
+			      measurement_variance,                      // observation variance
+			      getHagl() - measurement,                   // innovation
+			      innovation_variance,                       // innovation variance
+			      innov_gate);                               // innovation gate
+
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
+	}
+}
+
+float Ekf::getRngVar() const
+{
+	const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
+	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
+	return dist_var;
+}
+
+void Ekf::stopRangeAltitudeFusion()
+{
+	if (_control_status.flags.rng_hgt) {
+		PX4_INFO("stopping RNG Altitude fusion");
+		_control_status.flags.rng_hgt = false;
+		if (_height_sensor_ref == HeightSensor::RANGE) {
+			_height_sensor_ref = HeightSensor::UNKNOWN;
+		}
+	}
+}
+
+void Ekf::stopRangeTerrainFusion()
+{
+	if (_control_status.flags.rng_terrain) {
+		PX4_INFO("stopping RNG Terrain fusion");
+		_control_status.flags.rng_terrain = false;
+	}
+}

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

@@ -36,56 +36,123 @@
  */
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
+#include "ekf_derivation/generated/range_validation_filter_h.h"
+#include "ekf_derivation/generated/range_validation_filter_P_init.h"
 
-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
-		bool horizontal_motion, uint64_t time_us)
+using namespace matrix;
+
+void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom,
+				       const float dist_bottom_var)
 {
-	if (horizontal_motion) {
-		_time_last_horizontal_motion = time_us;
-	}
+	printf("RangeFinderConsistencyCheck::init\n");
 
+	_P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
+	_Ht = sym::RangeValidationFilterH<float>();
+
+	_x(RangeFilter::z.idx) = z;
+	_x(RangeFilter::terrain.idx) = z - dist_bottom;
+	_initialized = true;
+	_test_ratio_lpf.reset(0.f);
+	_t_since_first_sample = 0.f;
+}
+
+void RangeFinderConsistencyCheck::update(const float z, const float z_var, const float vz, const float vz_var,
+		const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
+{
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
 
-	if ((_time_last_update_us == 0)
-	    || (dt < 0.001f) || (dt > 0.5f)) {
+	if (dt > 1.f) {
 		_time_last_update_us = time_us;
-		_dist_bottom_prev = dist_bottom;
+		_initialized = false;
+		return;
+
+	} else if (!_initialized) {
+		init(z, z_var, dist_bottom, dist_bottom_var);
 		return;
 	}
 
-	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
-	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
-
-	// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
-	const float var = 2.f * dist_bottom_var / (dt * dt);
-	_innov_var = var + vz_var;
-
-	const float normalized_innov_sq = (_innov * _innov) / _innov_var;
-	_test_ratio = normalized_innov_sq / (_gate * _gate);
-	_signed_test_ratio_lpf.setParameters(dt, _signed_test_ratio_tau);
-	const float signed_test_ratio = matrix::sign(_innov) * _test_ratio;
-	_signed_test_ratio_lpf.update(signed_test_ratio);
-
-	updateConsistency(vz, time_us);
-
+	// prediction step
 	_time_last_update_us = time_us;
-	_dist_bottom_prev = dist_bottom;
-}
+	_x(RangeFilter::z.idx) -= dt * vz;
+	_P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
+	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
 
-void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
-{
-	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
-		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
-			_is_kinematically_consistent = false;
-			_time_last_inconsistent_us = time_us;
+	// iterate through both measurements (z and dist_bottom)
+	const Vector2f measurements{z, dist_bottom};
+
+	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
+
+		// dist_bottom
+		Vector2f H = _Ht;
+		float R = dist_bottom_var;
+
+		// z, direct state measurement
+		if (measurement_idx == 0) {
+			H(RangeFilter::z.idx) = 1.f;
+			H(RangeFilter::terrain.idx) = 0.f;
+			R = z_var;
 		}
 
-	} else {
-		if ((fabsf(vz) > _min_vz_for_valid_consistency)
-		    && (_test_ratio < 1.f)
-		    && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
-		   ) {
-			_is_kinematically_consistent = true;
+		// residual
+		const float measurement_pred = H * _x;
+		const float y = measurements(measurement_idx) - measurement_pred;
+
+		// for H as col-vector:
+		// innovation variance S = H^T * P * H + R
+		// kalman gain K = P * H / S
+		const float S = (H.transpose() * _P * H + R)(0, 0);
+		Vector2f K = (_P * H / S);
+
+		if (measurement_idx == 0) {
+			K(RangeFilter::z.idx) = 1.f;
+
+		} else if (measurement_idx == 1) {
+			_innov = y;
+			const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
+			_test_ratio_lpf.setParameters(dt, _t_to_init);
+			_test_ratio_lpf.update(sign(_innov) * test_ratio);
 		}
+
+		// update step
+		_x(RangeFilter::z.idx)       += K(RangeFilter::z.idx) * y;
+		_x(RangeFilter::terrain.idx) += K(RangeFilter::terrain.idx) * y;
+
+		// covariance update with Joseph form:
+		// P = (I - K H) P (I - K H)^T + K R K^T
+		Matrix2f I;
+		I.setIdentity();
+		Matrix2f IKH = I - K.multiplyByTranspose(H);
+		_P = IKH * _P * IKH.transpose() + (K * R).multiplyByTranspose(K);
 	}
+
+	evaluateState(dt, vz, vz_var);
+}
+
+void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
+{
+	// start the consistency check after 1s
+	if (_t_since_first_sample < _t_to_init) {
+		_t_since_first_sample += dt;
+		return;
+	}
+
+	if (fabsf(_test_ratio_lpf.getState()) > 1.f) {
+		printf("_test_ratio_lpf failed (>1)\n");
+		_t_since_first_sample = 0.f;
+		_state = KinematicState::INCONSISTENT;
+		return;
+	}
+
+	_state = KinematicState::CONSISTENT;
+}
+
+void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var,
+				      const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
+{
+	if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
+		_last_posD_reset_count = current_posD_reset_count;
+		_initialized = false;
+	}
+
+	update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
 }

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp

@@ -48,36 +48,61 @@ public:
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;
 
-	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
+	enum class KinematicState : int {
+		INCONSISTENT = 0,
+		CONSISTENT = 1,
+		UNKNOWN = 2
+	};
 
-	void setGate(float gate) { _gate = gate; }
+	float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
+	float getInnov() const { return _initialized ? _innov : 0.f; }
+	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
+	bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT && _t_since_first_sample > _t_to_init; }
+	bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; }
+	void setGate(const float gate) { _gate = gate; }
+	void run(float z, float z_var, float vz, float vz_var,
+		 float dist_bottom, float dist_bottom_var, uint64_t time_us);
+	void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; }
+	void reset()
+	{
+		if (_initialized && _state == KinematicState::CONSISTENT) {
+			_state = KinematicState::UNKNOWN;
+		}
 
-	float getTestRatio() const { return _test_ratio; }
-	float getSignedTestRatioLpf() const { return _signed_test_ratio_lpf.getState(); }
-	float getInnov() const { return _innov; }
-	float getInnovVar() const { return _innov_var; }
-	bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
+		_initialized = false;
+	}
+
+	uint8_t current_posD_reset_count{0};
 
 private:
-	void updateConsistency(float vz, uint64_t time_us);
 
-	uint64_t _time_last_update_us{};
-	float _dist_bottom_prev{};
-
-	float _test_ratio{};
-	AlphaFilter<float> _signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
-	float _gate{.2f};
-	float _innov{};
-	float _innov_var{};
-
-	bool _is_kinematically_consistent{true};
-	uint64_t _time_last_inconsistent_us{};
-	uint64_t _time_last_horizontal_motion{};
-
-	static constexpr float _signed_test_ratio_tau = 2.f;
-
-	static constexpr float _min_vz_for_valid_consistency = .5f;
-	static constexpr uint64_t _consistency_hyst_time_us = 1e6;
+	void update(float z, float z_var, float vz, float vz_var, float dist_bottom,
+		    float dist_bottom_var, uint64_t time_us);
+	void init(float z, float z_var, float dist_bottom, float dist_bottom_var);
+	void evaluateState(float dt, float vz, float vz_var);
+	float _terrain_process_noise{0.0f};
+	matrix::SquareMatrix<float, 2> _P{};
+	matrix::Vector2f _Ht{};
+	matrix::Vector2f _x{};
+	bool _initialized{false};
+	float _innov{0.f};
+	float _innov_var{0.f};
+	uint64_t _time_last_update_us{0};
+	static constexpr float time_constant{1.f};
+	AlphaFilter<float> _test_ratio_lpf{time_constant};
+	float _gate{1.0f};
+	KinematicState _state{KinematicState::UNKNOWN};
+	float _t_since_first_sample{0.f};
+	uint8_t _last_posD_reset_count{0};
+	static constexpr float _t_to_init{1.f};
 };
 
+namespace RangeFilter
+{
+struct IdxDof { unsigned idx; unsigned dof; };
+static constexpr IdxDof z{0, 1};
+static constexpr IdxDof terrain{1, 1};
+static constexpr uint8_t size{2};
+}
+
 #endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -44,18 +44,23 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
-	bool rng_data_ready = false;
+	if (!_range_buffer) {
+		return;
+	}
 
-	if (_range_buffer) {
-		// Get range data from buffer and check validity
-		rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
-		_range_sensor.setDataReadiness(rng_data_ready);
+	// Get range data from buffer and check validity
+	bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.sample());
+
+	_range_sensor.setSample();
+
+	_range_sensor.setDataReadiness(rng_data_ready);
+
+	if (_range_sensor.isDataReady()) {
 
-		// update range sensor angle parameters in case they have changed
 		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 		_range_sensor.setMaxFogDistance(_params.rng_fog);
+		_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
 
 		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 
@@ -65,39 +70,35 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
-			if (_control_status.flags.in_air) {
-				const bool horizontal_motion = _control_status.flags.fixed_wing
-							       || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
+			// TODO: this is a constant
+			const float dist_var = getRngVar();
+			_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
 
-				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
-				const float var = sq(_params.range_noise) + dist_dependant_var;
+			const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
+			const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
 
-				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
-							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
-			}
+			// TODO: review -- variance
+			_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
+						   dist_var, imu_sample.time_us);
+
+		} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
+			_range_sensor.setRange(_params.rng_gnd_clearance);
+			_range_sensor.setValidity(true);
 
 		} else {
-			// If we are supposed to be using range finder data but have bad range measurements
-			// and are on the ground, then synthesise a measurement at the expected on ground value
-			if (!_control_status.flags.in_air
-			    && _range_sensor.isRegularlySendingData()
-			    && _range_sensor.isDataReady()) {
-
-				_range_sensor.setRange(_params.rng_gnd_clearance);
-				_range_sensor.setValidity(true); // bypass the checks
-			}
+			_rng_consistency_check.reset();
 		}
-
-		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
-
-	} else {
-		return;
 	}
 
+	_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
+						&& _rng_consistency_check.isNotKinematicallyInconsistent();
+
+
 	auto &aid_src = _aid_src_rng_hgt;
 
-	if (rng_data_ready && _range_sensor.getSampleAddress()) {
+	if (_range_sensor.isDataReady() && _range_sensor.sample()) {
 
 		updateRangeHagl(aid_src);
 		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
@@ -107,23 +108,26 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				&& _control_status.flags.tilt_align
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy()
-				&& _rng_consistency_check.isKinematicallyConsistent();
+				&& _rng_consistency_check.isNotKinematicallyInconsistent();
+
+		// SUS: _rng_consistency_check.isNotKinematicallyInconsistent()
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();
 
-
 		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 						      && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
 						      && isConditionalRangeAidSuitable();
 
+		// SUS: isConditionalRangeAidSuitable()
+
 		const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 					  && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
 
 		if (_control_status.flags.rng_hgt) {
 			if (!(do_conditional_range_aid || do_range_aid)) {
-				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
+				PX4_INFO("stopping %s fusion", HGT_SRC_NAME);
 				stopRngHgtFusion();
 			}
 
@@ -131,13 +135,14 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
 				if (do_conditional_range_aid) {
 					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
-					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
+					PX4_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
+					    && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -145,7 +150,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				} else if (do_range_aid) {
 					// Range finder is the primary height source, the ground is now the datum used
 					// to compute the local vertical position
-					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
+					PX4_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 
 					_information_events.flags.reset_hgt_to_rng = true;
@@ -160,10 +165,11 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 			} else {
 				if (do_conditional_range_aid || do_range_aid) {
-					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
+					PX4_INFO("starting %s height fusion", HGT_SRC_NAME);
 					_control_status.flags.rng_hgt = true;
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
+					    && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -180,7 +186,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 				if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
 					// All height sources are failing
-					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
+					PX4_INFO("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_rng = true;
 					resetAltitudeTo(aid_src.observation - _state.terrain);
@@ -196,18 +202,18 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
 					if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) {
-						ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
+						PX4_INFO("stopping %s fusion, fusion failing", HGT_SRC_NAME);
 						stopRngHgtFusion();
 						stopRngTerrFusion();
 
-					} else {
+					} else if (_rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
 				}
 
 			} else {
-				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
+				PX4_INFO("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
 				stopRngTerrFusion();
 			}
@@ -221,7 +227,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					}
 
 				} else {
-					if (aid_src.innovation_rejected) {
+					if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -234,7 +240,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 	} else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain)
 		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
-		ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
+		PX4_INFO("stopping %s fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
 		stopRngTerrFusion();
 	}
@@ -250,7 +256,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
 	updateAidSourceStatus(aid_src,
-			      _range_sensor.getSampleAddress()->time_us, // sample timestamp
+			      _range_sensor.sample()->time_us, // sample timestamp
 			      measurement,                               // observation
 			      measurement_variance,                      // observation variance
 			      getHagl() - measurement,                   // innovation
@@ -268,11 +274,9 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 float Ekf::getRngVar() const
 {
-	return fmaxf(
-		       P(State::pos.idx + 2, State::pos.idx + 2)
-		       + sq(_params.range_noise)
-		       + sq(_params.range_noise_scaler * _range_sensor.getRange()),
-		       0.f);
+	const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
+	return dist_var;
 }
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp

@@ -47,10 +47,34 @@ namespace estimator
 namespace sensor
 {
 
-void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
+void SensorRangeFinder::setSample(const rangeSample &sample)
 {
-	updateSensorToEarthRotation(R_to_earth);
-	updateValidity(current_time_us);
+	_sample = sample;
+}
+
+bool SensorRangeFinder::timedOut(uint64_t time_now) const
+{
+	if (_sample.time_us > time_now) {
+		return false;
+	}
+
+	// TODO: 200ms?
+	return time_now > _sample.time_us + 200'000;
+}
+
+void SensorRangeFinder::setPitchOffset(float new_pitch_offset)
+{
+	if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) {
+		_sin_pitch_offset = sinf(new_pitch_offset);
+		_cos_pitch_offset = cosf(new_pitch_offset);
+		_pitch_offset_rad = new_pitch_offset;
+	}
+}
+
+void SensorRangeFinder::setLimits(float min_distance, float max_distance)
+{
+	_rng_valid_min_val = min_distance;
+	_rng_valid_max_val = max_distance;
 }
 
 void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
@@ -60,112 +84,5 @@ void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_ear
 	_cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset;
 }
 
-void SensorRangeFinder::updateValidity(uint64_t current_time_us)
-{
-	updateDtDataLpf(current_time_us);
-
-	if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
-		_is_sample_valid = false;
-		_is_regularly_sending_data = false;
-		return;
-	}
-
-	_is_regularly_sending_data = true;
-
-	// Don't run the checks unless we have retrieved new data from the buffer
-	if (_is_sample_ready) {
-		_is_sample_valid = false;
-
-		_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
-
-		if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) {
-			return;
-		}
-
-		updateStuckCheck();
-		updateFogCheck(getDistBottom(), _sample.time_us);
-
-		if (!_is_stuck && !_is_blocked) {
-			_is_sample_valid = true;
-			_time_last_valid_us = _sample.time_us;
-		}
-	}
-}
-
-bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const
-{
-	return current_time_us - _time_bad_quality_us > _quality_hyst_us;
-}
-
-void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us)
-{
-	// Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant.
-	float alpha = 0.5f * _dt_update;
-	_dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us);
-
-	// Apply spike protection to the filter state.
-	_dt_data_lpf = fminf(_dt_data_lpf, 4e6f);
-}
-
-inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const
-{
-	return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL;
-}
-
-inline bool SensorRangeFinder::isDataInRange() const
-{
-	return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val);
-}
-
-void SensorRangeFinder::updateStuckCheck()
-{
-	if (!isStuckDetectorEnabled()) {
-		// Stuck detector disabled
-		_is_stuck = false;
-		return;
-	}
-
-	// Check for "stuck" range finder measurements when range was not valid for certain period
-	// This handles a failure mode observed with some lidar sensors
-	if (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) {
-
-		// require a variance of rangefinder values to check for "stuck" measurements
-		if (_stuck_max_val - _stuck_min_val > _stuck_threshold) {
-			_stuck_min_val = 0.0f;
-			_stuck_max_val = 0.0f;
-			_is_stuck = false;
-
-		} else {
-			if (_sample.rng > _stuck_max_val) {
-				_stuck_max_val = _sample.rng;
-			}
-
-			if (_stuck_min_val < 0.1f || _sample.rng < _stuck_min_val) {
-				_stuck_min_val = _sample.rng;
-			}
-
-			_is_stuck = true;
-		}
-	}
-}
-
-void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
-{
-	if (_max_fog_dist > 0.f) {
-
-		const float median_dist = _median_dist.apply(dist_bottom);
-		const float factor = 2.f; // magic hardcoded factor
-
-		if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) {
-			_is_blocked = true;
-
-		} else if (_is_blocked && median_dist > factor * _max_fog_dist) {
-			_is_blocked = false;
-		}
-
-		_prev_median_dist = median_dist;
-	}
-}
-
 } // namespace sensor
 } // namespace estimator

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp

@@ -41,8 +41,6 @@
 #ifndef EKF_SENSOR_RANGE_FINDER_HPP
 #define EKF_SENSOR_RANGE_FINDER_HPP
 
-#include "Sensor.hpp"
-
 #include <matrix/math.hpp>
 #include <lib/mathlib/math/filter/MedianFilter.hpp>
 
@@ -57,105 +55,62 @@ struct rangeSample {
 	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
 };
 
-static constexpr uint64_t RNG_MAX_INTERVAL =
-	200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
 
-class SensorRangeFinder : public Sensor
+class SensorRangeFinder
 {
 public:
 	SensorRangeFinder() = default;
-	~SensorRangeFinder() override = default;
+	~SensorRangeFinder() = default;
 
-	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
-	bool isHealthy() const override { return _is_sample_valid; }
-	bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
-	bool isDataReady() const { return _is_sample_ready; }
-	bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
-	bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; }
+	struct Parameters {
+		float ekf2_imu_pos_x{};
+		float ekf2_imu_pos_y{};
+		float ekf2_imu_pos_z{};
+
+		float ekf2_rng_pos_x{};
+		float ekf2_rng_pos_y{};
+		float ekf2_rng_pos_z{};
+
+		float ekf2_rng_pitch{};
+
+		float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt
+	};
+
+	void updateParameters(Parameters& parameters) { _parameters = parameters; };
+
+	bool timedOut(uint64_t time_now) const;
+	void setSample(const rangeSample &sample);
 
-	void setSample(const rangeSample &sample)
-	{
-		_sample = sample;
-		_is_sample_ready = true;
-	}
 
 	// This is required because of the ring buffer
 	// TODO: move the ring buffer here
-	rangeSample *getSampleAddress() { return &_sample; }
+	rangeSample *sample() { return &_sample; }
 
-	void setPitchOffset(float new_pitch_offset)
-	{
-		if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) {
-			_sin_pitch_offset = sinf(new_pitch_offset);
-			_cos_pitch_offset = cosf(new_pitch_offset);
-			_pitch_offset_rad = new_pitch_offset;
-		}
-	}
+	void setPitchOffset(float new_pitch_offset);
 
 	void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; }
 
-	void setLimits(float min_distance, float max_distance)
-	{
-		_rng_valid_min_val = min_distance;
-		_rng_valid_max_val = max_distance;
-	}
-
-	void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; }
-
-	void setQualityHysteresis(float valid_quality_threshold_s)
-	{
-		_quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f);
-	}
-
 	float getCosTilt() const { return _cos_tilt_rng_to_earth; }
 
-	void setRange(float rng) { _sample.rng = rng; }
-	float getRange() const { return _sample.rng; }
+	void setLimits(float min_distance, float max_distance);
+
+	// void setRange(float rng) { _sample.rng = rng; }
+	// float getRange() const { return _sample.rng; }
 
 	float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }
 
-	void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; }
-	void setValidity(bool is_valid) { _is_sample_valid = is_valid; }
-
 	float getValidMinVal() const { return _rng_valid_min_val; }
 	float getValidMaxVal() const { return _rng_valid_max_val; }
 
-	void setFaulty(bool faulty = true) { _is_faulty = faulty; }
-
-private:
 	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
 
-	void updateValidity(uint64_t current_time_us);
-	void updateDtDataLpf(uint64_t current_time_us);
-	bool isSampleOutOfDate(uint64_t current_time_us) const;
-	bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
 	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
-	bool isDataInRange() const;
-	bool isQualityOk(uint64_t current_time_us) const;
-	void updateStuckCheck();
-	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
+
+private:
 
 	rangeSample _sample{};
 
-	bool _is_sample_ready{};	///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
-	bool _is_sample_valid{};	///< true if range finder sample retrieved from buffer is valid
-	bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval
-	uint64_t _time_last_valid_us{};	///< time the last range finder measurement was ready (uSec)
-	bool _is_faulty{false};         ///< the sensor should not be used anymore
-
-	/*
-	 * Stuck check
-	 */
-	bool _is_stuck{};
-	float _stuck_threshold{0.1f};	///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m), set to zero to disable
-	float _stuck_min_val{};		///< minimum value for new rng measurement when being stuck
-	float _stuck_max_val{};		///< maximum value for new rng measurement when being stuck
-
-	/*
-	 * Data regularity check
-	 */
-	static constexpr float _dt_update{0.01f}; 	///< delta time since last ekf update TODO: this should be a parameter
-	float _dt_data_lpf{};	///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
+	Parameters _parameters{};
 
 	/*
 	 * Tilt check
@@ -172,20 +127,6 @@ private:
 	float _rng_valid_min_val{};	///< minimum distance that the rangefinder can measure (m)
 	float _rng_valid_max_val{};	///< maximum distance that the rangefinder can measure (m)
 
-	/*
-	 * Quality check
-	 */
-	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
-	uint64_t _quality_hyst_us{};		///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
-
-	/*
-	 * Fog check
-	 */
-	bool _is_blocked{false};
-	float _max_fog_dist{0.f};	//< maximum distance at which the range finder could detect fog (m)
-	math::MedianFilter<float, 5> _median_dist{};
-	float _prev_median_dist{0.f};
-
 };
 
 } // namespace sensor

src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp

@@ -49,7 +49,7 @@
 
 void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 {
-	_control_status.flags.fuse_beta = _params.beta_fusion_enabled
+	_control_status.flags.fuse_beta = _params.ekf2_fuse_beta
 					  && (_control_status.flags.fixed_wing || _control_status.flags.fuse_aspd)
 					  && _control_status.flags.in_air
 					  && !_control_status.flags.fake_pos;
@@ -77,7 +77,7 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
 {
 	float observation = 0.f;
-	const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance
+	const float R = math::max(sq(_params.ekf2_beta_noise), sq(0.01f)); // observation noise variance
 	const float epsilon = 1e-3f;
 	float innov;
 	float innov_var;
@@ -89,7 +89,7 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
 			      R,                                        // observation variance
 			      innov,                                    // innovation
 			      innov_var,                                // innovation variance
-			      math::max(_params.beta_innov_gate, 1.f)); // innovation gate
+			      math::max(_params.ekf2_beta_gate, 1.f)); // innovation gate
 }
 
 bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)

src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp

@@ -61,7 +61,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 		computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
 
 		if (!_control_status.flags.tilt_align
-		    || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
+		    || (aid_src_status.innovation_variance - obs_var) > sq(_params.ekf2_head_noise)) {
 			// The yaw variance is too large, fuse fake measurement
 			fuseYaw(aid_src_status, H_YAW);
 		}

src/modules/ekf2/EKF/common.h

@@ -100,13 +100,13 @@ enum class VelocityFrame : uint8_t {
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 enum GeoDeclinationMask : uint8_t {
-	// Bit locations for mag_declination_source
+	// Bit locations for ekf2_decl_type
 	USE_GEO_DECL  = (1 << 0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
 	SAVE_GEO_DECL = (1 << 1) ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
 };
 
 enum MagFuseType : uint8_t {
-	// Integer definitions for mag_fusion_type
+	// Integer definitions for ekf2_mag_type
 	AUTO    = 0,   	///< The selection of either heading or 3D magnetometer fusion will be automatic
 	HEADING = 1,   	///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
 	NONE    = 5,   	///< Do not use magnetometer under any circumstance.
@@ -148,7 +148,7 @@ enum class GnssCtrl : uint8_t {
 	YAW  = (1 << 3)
 };
 
-enum class RngCtrl : uint8_t {
+enum RngCtrl : uint8_t {
 	DISABLED    = 0,
 	CONDITIONAL = 1,
 	ENABLED     = 2
@@ -266,83 +266,83 @@ struct systemFlagUpdate {
 
 struct parameters {
 
-	int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
+	int32_t ekf2_predict_us{10000}; ///< filter update interval in microseconds
 
 	int32_t imu_ctrl{static_cast<int32_t>(ImuCtrl::GyroBias) | static_cast<int32_t>(ImuCtrl::AccelBias)};
 
-	float velocity_limit{100.f};           ///< velocity state limit (m/s)
+	float ekf2_vel_lim{100.f};           ///< velocity state limit (m/s)
 
 	// measurement source control
 	int32_t height_sensor_ref{static_cast<int32_t>(HeightSensor::BARO)};
 	int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
 
-	float delay_max_ms{110.f};              ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec)
+	float ekf2_delay_max{110.f};              ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec)
 
 	// input noise
-	float gyro_noise{1.5e-2f};              ///< IMU angular rate noise used for covariance prediction (rad/sec)
-	float accel_noise{3.5e-1f};             ///< IMU acceleration noise use for covariance prediction (m/sec**2)
+	float ekf2_gyr_noise{1.5e-2f};              ///< IMU angular rate noise used for covariance prediction (rad/sec)
+	float ekf2_acc_noise{3.5e-1f};             ///< IMU acceleration noise use for covariance prediction (m/sec**2)
 
 	// process noise
-	float gyro_bias_p_noise{1.0e-3f};       ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
-	float accel_bias_p_noise{1.0e-2f};      ///< process noise for IMU accelerometer bias prediction (m/sec**3)
+	float ekf2_gyr_b_noise{1.0e-3f};       ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
+	float ekf2_acc_b_noise{1.0e-2f};      ///< process noise for IMU accelerometer bias prediction (m/sec**3)
 
 #if defined(CONFIG_EKF2_WIND)
 	const float initial_wind_uncertainty {1.0f};    ///< 1-sigma initial uncertainty in wind velocity (m/sec)
-	float wind_vel_nsd{1.0e-2f};        ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
+	float ekf2_wind_nsd{1.0e-2f};        ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
 	const float wind_vel_nsd_scaler{0.5f};      ///< scaling of wind process noise with vertical velocity
 #endif // CONFIG_EKF2_WIND
 
 	// initialization errors
-	float switch_on_gyro_bias{0.1f};        ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
-	float switch_on_accel_bias{0.2f};       ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
-	float initial_tilt_err{0.1f};           ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
+	float ekf2_gbias_init{0.1f};        ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
+	float ekf2_abias_init{0.2f};       ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
+	float ekf2_angerr_init{0.1f};           ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
 
 #if defined(CONFIG_EKF2_BAROMETER)
-	int32_t baro_ctrl {1};
-	float baro_delay_ms{0.0f};              ///< barometer height measurement delay relative to the IMU (mSec)
-	float baro_noise{2.0f};                 ///< observation noise for barometric height fusion (m)
+	int32_t ekf2_baro_ctrl {1};
+	float ekf2_baro_delay{0.0f};              ///< barometer height measurement delay relative to the IMU (mSec)
+	float ekf2_baro_noise{2.0f};                 ///< observation noise for barometric height fusion (m)
 	float baro_bias_nsd{0.13f};             ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
-	float baro_innov_gate{5.0f};            ///< barometric and GPS height innovation consistency gate size (STD)
+	float ekf2_baro_gate{5.0f};            ///< barometric and GPS height innovation consistency gate size (STD)
 
-	float gnd_effect_deadzone{5.0f};        ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
-	float gnd_effect_max_hgt{0.5f};         ///< Height above ground at which baro ground effect becomes insignificant (m)
+	float ekf2_gnd_eff_dz{5.0f};        ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
+	float ekf2_gnd_max_hgt{0.5f};         ///< Height above ground at which baro ground effect becomes insignificant (m)
 
 # if defined(CONFIG_EKF2_BARO_COMPENSATION)
 	// static barometer pressure position error coefficient along body axes
-	float static_pressure_coef_xp{0.0f};    // (-)
-	float static_pressure_coef_xn{0.0f};    // (-)
-	float static_pressure_coef_yp{0.0f};    // (-)
-	float static_pressure_coef_yn{0.0f};    // (-)
-	float static_pressure_coef_z{0.0f};     // (-)
+	float ekf2_pcoef_xp{0.0f};    // (-)
+	float ekf2_pcoef_xn{0.0f};    // (-)
+	float ekf2_pcoef_yp{0.0f};    // (-)
+	float ekf2_pcoef_yn{0.0f};    // (-)
+	float ekf2_pcoef_z{0.0f};     // (-)
 
 	// upper limit on airspeed used for correction  (m/s**2)
-	float max_correction_airspeed{20.0f};
+	float ekf2_aspd_max{20.0f};
 # endif // CONFIG_EKF2_BARO_COMPENSATION
 #endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_GNSS)
-	int32_t gnss_ctrl {static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
-	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
+	int32_t ekf2_gps_ctrl {static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
+	float ekf2_gps_delay{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
 
 	Vector3f gps_pos_body{};                ///< xyz position of the GPS antenna in body frame (m)
 
 	// position and velocity fusion
-	float gps_vel_noise{0.5f};           ///< minimum allowed observation noise for gps velocity fusion (m/sec)
-	float gps_pos_noise{0.5f};              ///< minimum allowed observation noise for gps position fusion (m)
+	float ekf2_gps_v_noise{0.5f};           ///< minimum allowed observation noise for gps velocity fusion (m/sec)
+	float ekf2_gps_p_noise{0.5f};              ///< minimum allowed observation noise for gps position fusion (m)
 	float gps_hgt_bias_nsd{0.13f};          ///< process noise for gnss height bias estimation (m/s/sqrt(Hz))
-	float gps_pos_innov_gate{5.0f};         ///< GPS horizontal position innovation consistency gate size (STD)
-	float gps_vel_innov_gate{5.0f};         ///< GPS velocity innovation consistency gate size (STD)
+	float ekf2_gps_p_gate{5.0f};         ///< GPS horizontal position innovation consistency gate size (STD)
+	float ekf2_gps_v_gate{5.0f};         ///< GPS velocity innovation consistency gate size (STD)
 
 	// these parameters control the strictness of GPS quality checks used to determine if the GPS is
 	// good enough to set a local origin and commence aiding
-	int32_t gps_check_mask{21};             ///< bitmask used to control which GPS quality checks are used
-	float req_hacc{5.0f};                   ///< maximum acceptable horizontal position error (m)
-	float req_vacc{8.0f};                   ///< maximum acceptable vertical position error (m)
-	float req_sacc{1.0f};                   ///< maximum acceptable speed error (m/s)
-	int32_t req_nsats{6};                   ///< minimum acceptable satellite count
-	float req_pdop{2.0f};                   ///< maximum acceptable position dilution of precision
-	float req_hdrift{0.3f};                 ///< maximum acceptable horizontal drift speed (m/s)
-	float req_vdrift{0.5f};                 ///< maximum acceptable vertical drift speed (m/s)
+	int32_t ekf2_gps_check{21};             ///< bitmask used to control which GPS quality checks are used
+	float ekf2_req_eph{5.0f};                   ///< maximum acceptable horizontal position error (m)
+	float ekf2_req_epv{8.0f};                   ///< maximum acceptable vertical position error (m)
+	float ekf2_req_sacc{1.0f};                   ///< maximum acceptable speed error (m/s)
+	int32_t ekf2_req_nsats{6};                   ///< minimum acceptable satellite count
+	float ekf2_req_pdop{2.0f};                   ///< maximum acceptable position dilution of precision
+	float ekf2_req_hdrift{0.3f};            ///< maximum acceptable horizontal drift speed (m/s)
+	float ekf2_req_vdrift{0.5f};            ///< maximum acceptable vertical drift speed (m/s)
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	// GNSS heading fusion
@@ -350,51 +350,51 @@ struct parameters {
 # endif // CONFIG_EKF2_GNSS_YAW
 
 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
-	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
+	float ekf2_gsf_tas{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
 	const unsigned EKFGSF_reset_delay{1000000};     ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
 	const float EKFGSF_yaw_err_max{0.262f};         ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
 
 #endif // CONFIG_EKF2_GNSS
 
-	float pos_noaid_noise{10.0f};           ///< observation noise for non-aiding position fusion (m)
+	float ekf2_noaid_noise{10.0f};           ///< observation noise for non-aiding position fusion (m)
 
-	float heading_innov_gate{2.6f};         ///< heading fusion innovation consistency gate size (STD)
-	float mag_heading_noise{3.0e-1f};       ///< measurement noise used for simple heading fusion (rad)
+	float ekf2_hdg_gate{2.6f};         ///< heading fusion innovation consistency gate size (STD)
+	float ekf2_head_noise{3.0e-1f};       ///< measurement noise used for simple heading fusion (rad)
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	float mag_delay_ms {0.0f};              ///< magnetometer measurement delay relative to the IMU (mSec)
+	float ekf2_mag_delay {0.0f};              ///< magnetometer measurement delay relative to the IMU (mSec)
 
-	float mage_p_noise{1.0e-3f};            ///< process noise for earth magnetic field prediction (Gauss/sec)
-	float magb_p_noise{1.0e-4f};            ///< process noise for body magnetic field prediction (Gauss/sec)
+	float ekf2_mag_e_noise{1.0e-3f};            ///< process noise for earth magnetic field prediction (Gauss/sec)
+	float ekf2_mag_b_noise{1.0e-4f};            ///< process noise for body magnetic field prediction (Gauss/sec)
 
 	// magnetometer fusion
-	float mag_noise{5.0e-2f};               ///< measurement noise used for 3-axis magnetometer fusion (Gauss)
-	float mag_declination_deg{0.0f};        ///< magnetic declination (degrees)
-	float mag_innov_gate{3.0f};             ///< magnetometer fusion innovation consistency gate size (STD)
-	int32_t mag_declination_source{3};      ///< bitmask used to control the handling of declination data
-	int32_t mag_fusion_type{0};             ///< integer used to specify the type of magnetometer fusion used
-	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
+	float ekf2_mag_noise{5.0e-2f};               ///< measurement noise used for 3-axis magnetometer fusion (Gauss)
+	float ekf2_mag_decl{0.0f};        ///< magnetic declination (degrees)
+	float ekf2_mag_gate{3.0f};             ///< magnetometer fusion innovation consistency gate size (STD)
+	int32_t ekf2_decl_type{3};      ///< bitmask used to control the handling of declination data
+	int32_t ekf2_mag_type{0};             ///< integer used to specify the type of magnetometer fusion used
+	float ekf2_mag_acclim{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
 
 	// compute synthetic magnetomter Z value if possible
-	int32_t synthesize_mag_z{0};
-	int32_t mag_check{0};
-	float mag_check_strength_tolerance_gs{0.2f};
-	float mag_check_inclination_tolerance_deg{20.f};
+	int32_t ekf2_synt_mag_z{0};
+	int32_t ekf2_mag_check{0};
+	float ekf2_mag_chk_str{0.2f};
+	float ekf2_mag_chk_inc{20.f};
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	// airspeed fusion
-	float airspeed_delay_ms{100.0f};        ///< airspeed measurement delay relative to the IMU (mSec)
-	float tas_innov_gate{5.0f};             ///< True Airspeed innovation consistency gate size (STD)
-	float eas_noise{1.4f};                  ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
-	float arsp_thr{2.0f};                   ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
+	float ekf2_asp_delay{100.0f};        ///< airspeed measurement delay relative to the IMU (mSec)
+	float ekf2_tas_gate{5.0f};             ///< True Airspeed innovation consistency gate size (STD)
+	float ekf2_eas_noise{1.4f};                  ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
+	float ekf2_arsp_thr{2.0f};                   ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_SIDESLIP)
 	// synthetic sideslip fusion
-	int32_t beta_fusion_enabled{0};
-	float beta_innov_gate{5.0f};            ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
-	float beta_noise{0.3f};                 ///< synthetic sideslip noise (rad)
+	int32_t ekf2_fuse_beta{0};
+	float ekf2_beta_gate{5.0f};            ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
+	float ekf2_beta_noise{0.3f};                 ///< synthetic sideslip noise (rad)
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)
 #endif // CONFIG_EKF2_SIDESLIP
 
@@ -415,30 +415,32 @@ struct parameters {
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
-	float rng_sens_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
-	float range_noise_scaler{0.0f};         ///< scaling from range measurement to noise (m/m)
+	float ekf2_rng_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
+	float ekf2_rng_sfe{0.0f};         ///< scaling from range measurement to noise (m/m)
 	float max_hagl_for_range_aid{5.0f};     ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
 	float max_vel_for_range_aid{1.0f};      ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
 	float range_aid_innov_gate{1.0f};       ///< gate size used for innovation consistency checks for range aid fusion
-	float range_valid_quality_s{1.0f};      ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
-	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
+	float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check
 	float rng_fog{0.f};                 	///< max distance which a blocked range sensor measures (fog, dirt) [m]
 
-	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
+	float ekf2_rng_pos_x{0.f};
+	float ekf2_rng_pos_y{0.f};
+	float ekf2_rng_pos_z{0.f};
+
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// vision position fusion
-	int32_t ev_ctrl{0};
-	float ev_delay_ms{175.0f};              ///< off-board vision measurement delay relative to the IMU (mSec)
+	int32_t ekf2_ev_ctrl{0};
+	float ekf2_ev_delay{175.0f};              ///< off-board vision measurement delay relative to the IMU (mSec)
 
-	float ev_vel_noise{0.1f};               ///< minimum allowed observation noise for EV velocity fusion (m/sec)
-	float ev_pos_noise{0.1f};               ///< minimum allowed observation noise for EV position fusion (m)
-	float ev_att_noise{0.1f};               ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
-	int32_t ev_quality_minimum{0};          ///< vision minimum acceptable quality integer
-	float ev_vel_innov_gate{3.0f};          ///< vision velocity fusion innovation consistency gate size (STD)
-	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
+	float ekf2_evv_noise{0.1f};               ///< minimum allowed observation noise for EV velocity fusion (m/sec)
+	float ekf2_evp_noise{0.1f};               ///< minimum allowed observation noise for EV position fusion (m)
+	float ekf2_eva_noise{0.1f};               ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
+	int32_t ekf2_ev_qmin{0};          ///< vision minimum acceptable quality integer
+	float ekf2_evv_gate{3.0f};          ///< vision velocity fusion innovation consistency gate size (STD)
+	float ekf2_evp_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
 
 	Vector3f ev_pos_body{};                 ///< xyz position of VI-sensor focal point in body frame (m)
@@ -446,20 +448,20 @@ struct parameters {
 
 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	// gravity fusion
-	float gravity_noise{1.0f};              ///< accelerometer measurement gaussian noise (m/s**2)
+	float ekf2_grav_noise{1.0f};              ///< accelerometer measurement gaussian noise (m/s**2)
 #endif // CONFIG_EKF2_GRAVITY_FUSION
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	int32_t flow_ctrl {0};
-	int32_t flow_gyro_src {static_cast<int32_t>(FlowGyroSource::Auto)};
-	float flow_delay_ms{5.0f};              ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
+	int32_t ekf2_of_ctrl {0};
+	int32_t ekf2_of_gyr_src {static_cast<int32_t>(FlowGyroSource::Auto)};
+	float ekf2_of_delay{5.0f};              ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
 
 	// optical flow fusion
-	float flow_noise{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)
-	float flow_noise_qual_min{0.5f};        ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
-	int32_t flow_qual_min{1};               ///< minimum acceptable quality integer from  the flow sensor
-	int32_t flow_qual_min_gnd{0};           ///< minimum acceptable quality integer from  the flow sensor when on ground
-	float flow_innov_gate{3.0f};            ///< optical flow fusion innovation consistency gate size (STD)
+	float ekf2_of_n_min{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)
+	float ekf2_of_n_max{0.5f};        ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
+	int32_t ekf2_of_qmin{1};               ///< minimum acceptable quality integer from  the flow sensor
+	int32_t ekf2_of_qmin_gnd{0};           ///< minimum acceptable quality integer from  the flow sensor when on ground
+	float ekf2_of_gate{3.0f};            ///< optical flow fusion innovation consistency gate size (STD)
 
 	Vector3f flow_pos_body{};               ///< xyz position of range sensor focal point in body frame (m)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
@@ -468,26 +470,26 @@ struct parameters {
 	Vector3f imu_pos_body{};                ///< xyz position of IMU in body frame (m)
 
 	// accel bias learning control
-	float acc_bias_lim{0.4f};               ///< maximum accel bias magnitude (m/sec**2)
-	float acc_bias_learn_acc_lim{25.0f};    ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)
-	float acc_bias_learn_gyr_lim{3.0f};     ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
-	float acc_bias_learn_tc{0.5f};          ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
+	float ekf2_abl_lim{0.4f};               ///< maximum accel bias magnitude (m/sec**2)
+	float ekf2_abl_acclim{25.0f};    ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)
+	float ekf2_abl_gyrlim{3.0f};     ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
+	float ekf2_abl_tau{0.5f};          ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
 
-	float gyro_bias_lim{0.4f};              ///< maximum gyro bias magnitude (rad/sec)
+	float ekf2_gyr_b_lim{0.4f};              ///< maximum gyro bias magnitude (rad/sec)
 
 	const unsigned reset_timeout_max{7'000'000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
 	const unsigned no_aid_timeout_max{1'000'000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
 	const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec)
 
-	int32_t valid_timeout_max{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
+	int32_t ekf2_noaid_tout{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	// multi-rotor drag specific force fusion
-	int32_t drag_ctrl{0};
-	float drag_noise{2.5f};                 ///< observation noise variance for drag specific force measurements (m/sec**2)**2
-	float bcoef_x{100.0f};                  ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
-	float bcoef_y{100.0f};                  ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
-	float mcoef{0.1f};                      ///< rotor momentum drag coefficient for the X and Y axes (1/s)
+	int32_t ekf2_drag_ctrl{0};
+	float ekf2_drag_noise{2.5f};                 ///< observation noise variance for drag specific force measurements (m/sec**2)**2
+	float ekf2_bcoef_x{100.0f};                  ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
+	float ekf2_bcoef_y{100.0f};                  ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
+	float ekf2_mcoef{0.1f};                      ///< rotor momentum drag coefficient for the X and Y axes (1/s)
 #endif // CONFIG_EKF2_DRAG_FUSION
 
 	// control of accel error detection and mitigation (IMU clipping)
@@ -497,7 +499,7 @@ struct parameters {
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	// auxiliary velocity fusion
-	float auxvel_delay_ms{5.0f};            ///< auxiliary velocity measurement delay relative to the IMU (mSec)
+	float ekf2_avel_delay{5.0f};            ///< auxiliary velocity measurement delay relative to the IMU (mSec)
 	const float auxvel_noise{0.5f};         ///< minimum observation noise, uses reported noise if greater (m/s)
 	const float auxvel_gate{5.0f};          ///< velocity fusion innovation consistency gate size (STD)
 #endif // CONFIG_EKF2_AUXVEL
@@ -570,8 +572,6 @@ uint64_t mag_fault               :
 		uint64_t fuse_aspd               : 1; ///< 19 - true when airspeed measurements are being fused
 uint64_t gnd_effect              :
 		1; ///< 20 - true when protection from ground effect induced static pressure rise is active
-uint64_t rng_stuck               :
-		1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 uint64_t gnss_yaw                 :
 		1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 		uint64_t mag_aligned_in_flight   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
@@ -582,8 +582,6 @@ uint64_t synthetic_mag_z         :
 		uint64_t vehicle_at_rest         : 1; ///< 26 - true when the vehicle is at rest
 uint64_t gnss_yaw_fault           :
 		1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
-uint64_t rng_fault               :
-		1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
 uint64_t inertial_dead_reckoning :
 		1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 		uint64_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements

src/modules/ekf2/EKF/covariance.cpp

@@ -57,7 +57,7 @@ void Ekf::initialiseCovariance()
 
 	// velocity
 #if defined(CONFIG_EKF2_GNSS)
-	const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f));
+	const float vel_var = sq(fmaxf(_params.ekf2_gps_v_noise, 0.01f));
 #else
 	const float vel_var = sq(0.5f);
 #endif
@@ -65,20 +65,20 @@ void Ekf::initialiseCovariance()
 
 	// position
 #if defined(CONFIG_EKF2_BAROMETER)
-	float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
+	float z_pos_var = sq(fmaxf(_params.ekf2_baro_noise, 0.01f));
 #else
 	float z_pos_var = sq(1.f);
 #endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_GNSS)
-	const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
+	const float xy_pos_var = sq(fmaxf(_params.ekf2_gps_p_noise, 0.01f));
 
 	if (_control_status.flags.gps_hgt) {
-		z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
+		z_pos_var = sq(fmaxf(1.5f * _params.ekf2_gps_p_noise, 0.01f));
 	}
 
 #else
-	const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f));
+	const float xy_pos_var = sq(fmaxf(_params.ekf2_noaid_noise, 0.01f));
 #endif
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -116,11 +116,11 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	const float dt = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
 
 	// gyro noise variance
-	float gyro_noise = _params.gyro_noise;
+	float gyro_noise = _params.ekf2_gyr_noise;
 	const float gyro_var = sq(gyro_noise);
 
 	// accel noise variance
-	float accel_noise = _params.accel_noise;
+	float accel_noise = _params.ekf2_acc_noise;
 	Vector3f accel_var;
 
 	for (unsigned i = 0; i < 3; i++) {
@@ -144,7 +144,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	// gyro bias: add process noise
 	{
-		const float gyro_bias_sig = dt * _params.gyro_bias_p_noise;
+		const float gyro_bias_sig = dt * _params.ekf2_gyr_b_noise;
 		const float gyro_bias_process_noise = sq(gyro_bias_sig);
 
 		for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
@@ -158,7 +158,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	// accel bias: add process noise
 	{
-		const float accel_bias_sig = dt * _params.accel_bias_p_noise;
+		const float accel_bias_sig = dt * _params.ekf2_acc_b_noise;
 		const float accel_bias_process_noise = sq(accel_bias_sig);
 
 		for (unsigned index = 0; index < State::accel_bias.dof; index++) {
@@ -173,25 +173,25 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// mag_I: add process noise
-	float mag_I_sig = dt * _params.mage_p_noise;
+	float mag_I_sig = dt * _params.ekf2_mag_e_noise;
 	float mag_I_process_noise = sq(mag_I_sig);
 
 	for (unsigned index = 0; index < State::mag_I.dof; index++) {
 		const unsigned i = State::mag_I.idx + index;
 
-		if (P(i, i) < sq(_params.mag_noise)) {
+		if (P(i, i) < sq(_params.ekf2_mag_noise)) {
 			P(i, i) += mag_I_process_noise;
 		}
 	}
 
 	// mag_B: add process noise
-	float mag_B_sig = dt * _params.magb_p_noise;
+	float mag_B_sig = dt * _params.ekf2_mag_b_noise;
 	float mag_B_process_noise = sq(mag_B_sig);
 
 	for (unsigned index = 0; index < State::mag_B.dof; index++) {
 		const unsigned i = State::mag_B.idx + index;
 
-		if (P(i, i) < sq(_params.mag_noise)) {
+		if (P(i, i) < sq(_params.ekf2_mag_noise)) {
 			P(i, i) += mag_B_process_noise;
 		}
 	}
@@ -203,7 +203,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	// wind vel: add process noise
 	const float height_rate = _height_rate_lpf.update(_state.vel(2), imu_delayed.delta_vel_dt);
-	const float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate));
+	const float wind_vel_nsd_scaled = _params.ekf2_wind_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate));
 	const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
 
 	for (unsigned index = 0; index < State::wind_vel.dof; index++) {
@@ -227,6 +227,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
 						 1)));
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+		_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
+#endif // CONFIG_EKF2_RANGE_FINDER
 	}
 
 #endif // CONFIG_EKF2_TERRAIN
@@ -313,7 +317,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max,
 
 void Ekf::resetQuatCov(const float yaw_noise)
 {
-	const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));
+	const float tilt_var = sq(math::max(_params.ekf2_angerr_init, 0.01f));
 	float yaw_var = sq(0.01f);
 
 	// update the yaw angle variance using the variance of the measurement
@@ -334,19 +338,19 @@ void Ekf::resetGyroBiasCov()
 {
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
+	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.ekf2_gbias_init));
 }
 
 void Ekf::resetGyroBiasZCov()
 {
-	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
+	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.ekf2_gbias_init));
 }
 
 void Ekf::resetAccelBiasCov()
 {
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
+	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.ekf2_abias_init));
 }
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -354,13 +358,13 @@ void Ekf::resetMagEarthCov()
 {
 	ECL_INFO("reset mag earth covariance");
 
-	P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.mag_noise));
+	P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.ekf2_mag_noise));
 }
 
 void Ekf::resetMagBiasCov()
 {
 	ECL_INFO("reset mag bias covariance");
 
-	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
+	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.ekf2_mag_noise));
 }
 #endif // CONFIG_EKF2_MAGNETOMETER

src/modules/ekf2/EKF/ekf.cpp

@@ -77,13 +77,6 @@ void Ekf::reset()
 	_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
 #endif // CONFIG_EKF2_TERRAIN
 
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
-	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
-	_range_sensor.setMaxFogDistance(_params.rng_fog);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
 
@@ -151,7 +144,7 @@ bool Ekf::update()
 
 		// calculate an average filter update time
 		// limit input between -50% and +100% of nominal value
-		const float filter_update_s = 1e-6f * _params.filter_update_interval_us;
+		const float filter_update_s = 1e-6f * _params.ekf2_predict_us;
 		const float input = math::constrain(0.5f * (imu_sample_delayed.delta_vel_dt + imu_sample_delayed.delta_ang_dt),
 						    0.5f * filter_update_s,
 						    2.f * filter_update_s);
@@ -270,7 +263,7 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_state.pos(2) = -_gpos.altitude();
 
 	// constrain states
-	_state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit);
+	_state.vel = matrix::constrain(_state.vel, -_params.ekf2_vel_lim, _params.ekf2_vel_lim);
 
 	// calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere
 	_accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt);
@@ -372,24 +365,36 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
 
 void Ekf::updateParameters()
 {
-	_params.gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
-	_params.accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
+	_params.ekf2_gyr_noise = math::constrain(_params.ekf2_gyr_noise, 0.f, 1.f);
+	_params.ekf2_acc_noise = math::constrain(_params.ekf2_acc_noise, 0.f, 1.f);
 
-	_params.gyro_bias_p_noise = math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
-	_params.accel_bias_p_noise = math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
+	_params.ekf2_gyr_b_noise = math::constrain(_params.ekf2_gyr_b_noise, 0.f, 1.f);
+	_params.ekf2_acc_b_noise = math::constrain(_params.ekf2_acc_b_noise, 0.f, 1.f);
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	_params.mage_p_noise = math::constrain(_params.mage_p_noise, 0.f, 1.f);
-	_params.magb_p_noise = math::constrain(_params.magb_p_noise, 0.f, 1.f);
+	_params.ekf2_mag_e_noise = math::constrain(_params.ekf2_mag_e_noise, 0.f, 1.f);
+	_params.ekf2_mag_b_noise = math::constrain(_params.ekf2_mag_b_noise, 0.f, 1.f);
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_WIND)
-	_params.wind_vel_nsd = math::constrain(_params.wind_vel_nsd, 0.f, 1.f);
+	_params.ekf2_wind_nsd = math::constrain(_params.ekf2_wind_nsd, 0.f, 1.f);
 #endif // CONFIG_EKF2_WIND
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 	_aux_global_position.updateParameters();
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+#if defined (CONFIG_EKF2_RANGE_FINDER)
+
+	sensor::SensorRangeFinder::Parameters params = {};
+	// params.ekf2_imu_pos_x = _param_ekf2_imu_pos_x.get();
+	// params.ekf2_imu_pos_y = _param_ekf2_imu_pos_y.get();
+	// params.ekf2_imu_pos_z = _param_ekf2_imu_pos_z.get();
+	// params.ekf2_rng_pos_x = _param_ekf2_rng_pos_x.get();
+	// params.ekf2_rng_pos_y = _param_ekf2_rng_pos_y.get();
+	// params.ekf2_rng_pos_z = _param_ekf2_rng_pos_z.get();
+	params.ekf2_rng_pitch = _params.ekf2_rng_pitch;
+	_range_sensor.updateParameters(params);
+#endif
 }
 
 template<typename T>

src/modules/ekf2/EKF/ekf.h

@@ -117,7 +117,7 @@ public:
 
 	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
 	float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); }
-	float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); }
+	float getHaglRateInnovRatio() const { return _rng_consistency_check.getTestRatioLpf(); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -266,13 +266,13 @@ public:
 	// gyro bias
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
 	Vector3f getGyroBiasVariance() const { return getStateVariance<State::gyro_bias>(); } // get the gyroscope bias variance in rad/s
-	float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
-	float getGyroNoise() const { return _params.gyro_noise; }
+	float getGyroBiasLimit() const { return _params.ekf2_gyr_b_lim; }
+	float getGyroNoise() const { return _params.ekf2_gyr_noise; }
 
 	// accel bias
 	const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2
 	Vector3f getAccelBiasVariance() const { return getStateVariance<State::accel_bias>(); } // get the accelerometer bias variance in m/s**2
-	float getAccelBiasLimit() const { return _params.acc_bias_lim; }
+	float getAccelBiasLimit() const { return _params.ekf2_abl_lim; }
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	const Vector3f &getMagEarthField() const { return _state.mag_I; }
@@ -762,9 +762,18 @@ private:
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
 	void controlRangeHaglFusion(const imuSample &imu_delayed);
+
+
+	void fuseRangeAsHeightSource();
+	void fuseRangeAsHeightAiding();
+
 	bool isConditionalRangeAidSuitable();
-	void stopRngHgtFusion();
-	void stopRngTerrFusion();
+	bool rangeAidConditionsPassed();
+
+
+	void stopRangeAltitudeFusion();
+	void stopRangeTerrainFusion();
+
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -825,7 +825,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 		inertial_dead_reckoning = false;
 
 	} else {
-		if (!_control_status.flags.in_air && (_params.flow_ctrl == 1)
+		if (!_control_status.flags.in_air && (_params.ekf2_of_ctrl == 1)
 		    && isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
 		   ) {
 			// currently landed, but optical flow aiding should be possible once in air
@@ -851,8 +851,8 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 		_control_status.flags.wind_dead_reckoning = false;
 
 		if (!_control_status.flags.in_air && _control_status.flags.fixed_wing
-		    && (_params.beta_fusion_enabled == 1)
-		    && (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
+		    && (_params.ekf2_fuse_beta == 1)
+		    && (_params.ekf2_arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
 		   ) {
 			// currently landed, but air data aiding should be possible once in air
 			aiding_expected_in_air = true;
@@ -877,7 +877,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 	}
 
 	if (inertial_dead_reckoning) {
-		if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
+		if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.ekf2_noaid_tout)) {
 			// deadreckon time exceeded
 			if (!_horizontal_deadreckon_time_exceeded) {
 				ECL_WARN("horizontal dead reckon time exceeded");
@@ -903,7 +903,7 @@ void Ekf::updateVerticalDeadReckoningStatus()
 		_time_last_v_pos_aiding = _time_last_hgt_fuse;
 		_vertical_position_deadreckon_time_exceeded = false;
 
-	} else if (isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) {
+	} else if (isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.ekf2_noaid_tout)) {
 		_vertical_position_deadreckon_time_exceeded = true;
 	}
 
@@ -911,7 +911,7 @@ void Ekf::updateVerticalDeadReckoningStatus()
 		_time_last_v_vel_aiding = _time_last_ver_vel_fuse;
 		_vertical_velocity_deadreckon_time_exceeded = false;
 
-	} else if (isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max)
+	} else if (isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.ekf2_noaid_tout)
 		   && _vertical_position_deadreckon_time_exceeded) {
 
 		_vertical_velocity_deadreckon_time_exceeded = true;
@@ -950,7 +950,7 @@ void Ekf::updateGroundEffect()
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
 			float height = getHagl();
-			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
+			_control_status.flags.gnd_effect = (height < _params.ekf2_gnd_max_hgt);
 
 		} else
 #endif // CONFIG_EKF2_TERRAIN
@@ -975,7 +975,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 	{
 		const Vector3f gyro_corrected = imu_delayed.delta_ang / imu_delayed.delta_ang_dt - _state.gyro_bias;
 
-		const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
+		const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.ekf2_abl_tau), 0.f, 1.f);
 		const float beta = 1.f - alpha;
 
 		_ang_rate_magnitude_filt = fmaxf(gyro_corrected.norm(), beta * _ang_rate_magnitude_filt);
@@ -984,15 +984,15 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 	{
 		const Vector3f accel_corrected = imu_delayed.delta_vel / imu_delayed.delta_vel_dt - _state.accel_bias;
 
-		const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
+		const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.ekf2_abl_tau), 0.f, 1.f);
 		const float beta = 1.f - alpha;
 
 		_accel_magnitude_filt = fmaxf(accel_corrected.norm(), beta * _accel_magnitude_filt);
 	}
 
 
-	const bool is_manoeuvre_level_high = (_ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim)
-					     || (_accel_magnitude_filt > _params.acc_bias_learn_acc_lim);
+	const bool is_manoeuvre_level_high = (_ang_rate_magnitude_filt > _params.ekf2_abl_gyrlim)
+					     || (_accel_magnitude_filt > _params.ekf2_abl_acclim);
 
 
 	// gyro bias inhibit

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -97,7 +97,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 		imuSample imu_downsampled = _imu_down_sampler.getDownSampledImuAndTriggerReset();
 
 		// as a precaution constrain the integration delta time to prevent numerical problems
-		const float filter_update_period_s = _params.filter_update_interval_us * 1e-6f;
+		const float filter_update_period_s = _params.ekf2_predict_us * 1e-6f;
 		const float imu_min_dt = 0.5f * filter_update_period_s;
 		const float imu_max_dt = 2.0f * filter_update_period_s;
 
@@ -139,7 +139,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 	}
 
 	const int64_t time_us = mag_sample.time_us
-				- static_cast<int64_t>(_params.mag_delay_ms * 1000)
+				- static_cast<int64_t>(_params.ekf2_mag_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	// limit data rate to prevent data being lost
@@ -178,7 +178,7 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 	}
 
 	const int64_t time_us = gnss_sample.time_us
-				- static_cast<int64_t>(_params.gps_delay_ms * 1000)
+				- static_cast<int64_t>(_params.ekf2_gps_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {
@@ -225,7 +225,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 	}
 
 	const int64_t time_us = baro_sample.time_us
-				- static_cast<int64_t>(_params.baro_delay_ms * 1000)
+				- static_cast<int64_t>(_params.ekf2_baro_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	// limit data rate to prevent data being lost
@@ -264,7 +264,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 	}
 
 	const int64_t time_us = airspeed_sample.time_us
-				- static_cast<int64_t>(_params.airspeed_delay_ms * 1000)
+				- static_cast<int64_t>(_params.ekf2_asp_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	// limit data rate to prevent data being lost
@@ -341,7 +341,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 	}
 
 	const int64_t time_us = flow.time_us
-				- static_cast<int64_t>(_params.flow_delay_ms * 1000)
+				- static_cast<int64_t>(_params.ekf2_of_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	// limit data rate to prevent data being lost
@@ -380,7 +380,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 
 	// calculate the system time-stamp for the mid point of the integration period
 	const int64_t time_us = evdata.time_us
-				- static_cast<int64_t>(_params.ev_delay_ms * 1000)
+				- static_cast<int64_t>(_params.ekf2_ev_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	// limit data rate to prevent data being lost
@@ -419,7 +419,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 	}
 
 	const int64_t time_us = auxvel_sample.time_us
-				- static_cast<int64_t>(_params.auxvel_delay_ms * 1000)
+				- static_cast<int64_t>(_params.ekf2_avel_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	// limit data rate to prevent data being lost
@@ -477,7 +477,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
 	// sufficient samples have been collected
-	if (_params.drag_ctrl > 0) {
+	if (_params.ekf2_drag_ctrl > 0) {
 
 		// Allocate the required buffer size if not previously done
 		if (_drag_buffer == nullptr) {
@@ -538,15 +538,15 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
-	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
+	const float filter_update_period_ms = _params.ekf2_predict_us / 1000.f;
 
 	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
-	_imu_buffer_length = math::max(2, (int)ceilf(_params.delay_max_ms / filter_update_period_ms));
+	_imu_buffer_length = math::max(2, (int)ceilf(_params.ekf2_delay_max / filter_update_period_ms));
 
 	// set the observation buffer length to handle the minimum time of arrival between observations in combination
 	// with the worst case delay from current time to ekf fusion time
 	// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
-	const float ekf_delay_ms = _params.delay_max_ms * 1.5f;
+	const float ekf_delay_ms = _params.ekf2_delay_max * 1.5f;
 	_obs_buffer_length = roundf(ekf_delay_ms / filter_update_period_ms);
 
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)

src/modules/ekf2/EKF/estimator_interface.h

@@ -120,8 +120,6 @@ public:
 	{
 		_range_sensor.setLimits(min_distance, max_distance);
 	}
-
-	const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -201,7 +199,7 @@ public:
 	void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
 
 	// set flag if static pressure rise due to ground effect is expected
-	// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
+	// use _params.ekf2_gnd_eff_dz to adjust for expected rise in static pressure
 	// flag will clear after GNDEFFECT_TIMEOUT uSec
 	void set_gnd_effect()
 	{
@@ -262,10 +260,10 @@ public:
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
-	// At the next startup, set param.mag_declination_deg to the value saved
+	// At the next startup, set param.ekf2_mag_decl to the value saved
 	bool get_mag_decl_deg(float &val) const
 	{
-		if (PX4_ISFINITE(_wmm_declination_rad) && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
+		if (PX4_ISFINITE(_wmm_declination_rad) && (_params.ekf2_decl_type & GeoDeclinationMask::SAVE_GEO_DECL)) {
 			val = math::degrees(_wmm_declination_rad);
 			return true;
 
@@ -404,15 +402,15 @@ protected:
 	gnssSample _gps_sample_delayed{};
 
 	uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
-	GnssChecks _gnss_checks{_params.gps_check_mask,
-			   _params.req_nsats,
-			   _params.req_pdop,
-			   _params.req_hacc,
-			   _params.req_vacc,
-			   _params.req_sacc,
-			   _params.req_hdrift,
-			   _params.req_vdrift,
-			   _params.velocity_limit,
+	GnssChecks _gnss_checks{_params.ekf2_gps_check,
+			   _params.ekf2_req_nsats,
+			   _params.ekf2_req_pdop,
+			   _params.ekf2_req_eph,
+			   _params.ekf2_req_epv,
+			   _params.ekf2_req_sacc,
+			   _params.ekf2_req_hdrift,
+			   _params.ekf2_req_vdrift,
+			   _params.ekf2_vel_lim,
 			   _min_gps_health_time_us,
 			   _control_status};
 
@@ -508,6 +506,6 @@ protected:
 
 	void printBufferAllocationFailed(const char *buffer_name);
 
-	ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
+	ImuDownSampler _imu_down_sampler{_params.ekf2_predict_us};
 };
 #endif // !EKF_ESTIMATOR_INTERFACE_H

src/modules/ekf2/EKF/position_fusion.cpp

@@ -253,5 +253,5 @@ void Ekf::resetHorizontalPositionToLastKnown()
 
 	// Used when falling back to non-aiding mode of operation
 	resetHorizontalPositionTo(_last_known_gpos.latitude_deg(), _last_known_gpos.longitude_deg(),
-				  sq(_params.pos_noaid_noise));
+				  sq(_params.ekf2_noaid_noise));
 }

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -721,6 +721,37 @@ def compute_gravity_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
+def range_validation_filter_h() -> sf.Matrix:
+
+    state = Values(
+        z=sf.Symbol("z"),
+        terrain=sf.Symbol("terrain")
+    )
+    dist_bottom = state["z"] - state["terrain"]
+
+    H = sf.M21()
+    H[:, 0] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False).transpose()
+
+    return H
+
+def range_validation_filter_P_init(
+        z_var: sf.Scalar,
+        dist_bottom_var: sf.Scalar
+    ) -> sf.Matrix:
+
+    H = range_validation_filter_h().T
+    # enforce terrain to match the measurement
+    K = sf.V2(0, 1/H[1])
+    R = sf.V1(dist_bottom_var)
+
+    # dist_bottom = z - terrain
+    P = sf.M22.diag([z_var, z_var + dist_bottom_var])
+    I = sf.M22.eye()
+    # Joseph form
+    P = (I - K * H) * P * (I - K * H).T + K * R * K.T
+
+    return P
+
 print("Derive EKF2 equations...")
 generate_px4_function(predict_covariance, output_names=None)
 
@@ -752,5 +783,7 @@ generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_va
 generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
 generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
 generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
+generate_px4_function(range_validation_filter_h, output_names=None)
+generate_px4_function(range_validation_filter_P_init, output_names=None)
 
 generate_px4_state(State, tangent_idx)

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h

@@ -0,0 +1,46 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: range_validation_filter_p_init
+ *
+ * Args:
+ *     z_var: Scalar
+ *     dist_bottom_var: Scalar
+ *
+ * Outputs:
+ *     res: Matrix22
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 2, 2> RangeValidationFilterPInit(const Scalar z_var,
+                                                       const Scalar dist_bottom_var) {
+  // Total ops: 1
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 2, 2> _res;
+
+  _res(0, 0) = z_var;
+  _res(1, 0) = z_var;
+  _res(0, 1) = z_var;
+  _res(1, 1) = dist_bottom_var + z_var;
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h

@@ -0,0 +1,41 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: range_validation_filter_h
+ *
+ * Args:
+ *
+ * Outputs:
+ *     res: Matrix21
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 2, 1> RangeValidationFilterH() {
+  // Total ops: 0
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 2, 1> _res;
+
+  _res(0, 0) = 1;
+  _res(1, 0) = -1;
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/terrain_control.cpp

@@ -54,7 +54,6 @@ void Ekf::controlTerrainFakeFusion()
 	// If we are on ground, store the local position and time to use as a reference
 	if (!_control_status.flags.in_air) {
 		_last_on_ground_posD = -_gpos.altitude();
-		_control_status.flags.rng_fault = false;
 
 	} else if (!_control_status_prev.flags.in_air) {
 		// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff

src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp

@@ -207,13 +207,13 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an
 	}
 
 	// Gyro bias estimation
-	constexpr float gyro_bias_limit = 0.05f;
+	constexpr float ekf2_gyr_b_limit = 0.05f;
 	const float spin_rate = ang_rate.length();
 
 	if (spin_rate < math::radians(10.f)) {
 		_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * delta_ang_dt);
 		_ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias,
-						       -gyro_bias_limit, gyro_bias_limit);
+						       -ekf2_gyr_b_limit, ekf2_gyr_b_limit);
 	}
 
 	// delta angle from previous to current frame

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -82,7 +82,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 			// constrain the innovation to the maximum set by the gate
 			// we need to delay this forced fusion to avoid starting it
 			// immediately after touchdown, when the drone is still armed
-			const float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
+			const float gate_sigma = math::max(_params.ekf2_hdg_gate, 1.f);
 			const float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
 			aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
 

src/modules/ekf2/EKF2.cpp

@@ -63,87 +63,87 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
 #endif // CONFIG_EKF2_WIND
 	_params(_ekf.getParamHandle()),
-	_param_ekf2_predict_us(_params->filter_update_interval_us),
-	_param_ekf2_delay_max(_params->delay_max_ms),
+	_param_ekf2_predict_us(_params->ekf2_predict_us),
+	_param_ekf2_delay_max(_params->ekf2_delay_max),
 	_param_ekf2_imu_ctrl(_params->imu_ctrl),
-	_param_ekf2_vel_lim(_params->velocity_limit),
+	_param_ekf2_vel_lim(_params->ekf2_vel_lim),
 #if defined(CONFIG_EKF2_AUXVEL)
-	_param_ekf2_avel_delay(_params->auxvel_delay_ms),
+	_param_ekf2_avel_delay(_params->ekf2_avel_delay),
 #endif // CONFIG_EKF2_AUXVEL
-	_param_ekf2_gyr_noise(_params->gyro_noise),
-	_param_ekf2_acc_noise(_params->accel_noise),
-	_param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
-	_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
+	_param_ekf2_gyr_noise(_params->ekf2_gyr_noise),
+	_param_ekf2_acc_noise(_params->ekf2_acc_noise),
+	_param_ekf2_gyr_b_noise(_params->ekf2_gyr_b_noise),
+	_param_ekf2_acc_b_noise(_params->ekf2_acc_b_noise),
 #if defined(CONFIG_EKF2_WIND)
-	_param_ekf2_wind_nsd(_params->wind_vel_nsd),
+	_param_ekf2_wind_nsd(_params->ekf2_wind_nsd),
 #endif // CONFIG_EKF2_WIND
-	_param_ekf2_noaid_noise(_params->pos_noaid_noise),
+	_param_ekf2_noaid_noise(_params->ekf2_noaid_noise),
 #if defined(CONFIG_EKF2_GNSS)
-	_param_ekf2_gps_ctrl(_params->gnss_ctrl),
-	_param_ekf2_gps_delay(_params->gps_delay_ms),
+	_param_ekf2_gps_ctrl(_params->ekf2_gps_ctrl),
+	_param_ekf2_gps_delay(_params->ekf2_gps_delay),
 	_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
 	_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
 	_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
-	_param_ekf2_gps_v_noise(_params->gps_vel_noise),
-	_param_ekf2_gps_p_noise(_params->gps_pos_noise),
-	_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
-	_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
-	_param_ekf2_gps_check(_params->gps_check_mask),
-	_param_ekf2_req_eph(_params->req_hacc),
-	_param_ekf2_req_epv(_params->req_vacc),
-	_param_ekf2_req_sacc(_params->req_sacc),
-	_param_ekf2_req_nsats(_params->req_nsats),
-	_param_ekf2_req_pdop(_params->req_pdop),
-	_param_ekf2_req_hdrift(_params->req_hdrift),
-	_param_ekf2_req_vdrift(_params->req_vdrift),
-	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default),
+	_param_ekf2_gps_v_noise(_params->ekf2_gps_v_noise),
+	_param_ekf2_gps_p_noise(_params->ekf2_gps_p_noise),
+	_param_ekf2_gps_p_gate(_params->ekf2_gps_p_gate),
+	_param_ekf2_gps_v_gate(_params->ekf2_gps_v_gate),
+	_param_ekf2_gps_check(_params->ekf2_gps_check),
+	_param_ekf2_req_eph(_params->ekf2_req_eph),
+	_param_ekf2_req_epv(_params->ekf2_req_epv),
+	_param_ekf2_req_sacc(_params->ekf2_req_sacc),
+	_param_ekf2_req_nsats(_params->ekf2_req_nsats),
+	_param_ekf2_req_pdop(_params->ekf2_req_pdop),
+	_param_ekf2_req_hdrift(_params->ekf2_req_hdrift),
+	_param_ekf2_req_vdrift(_params->ekf2_req_vdrift),
+	_param_ekf2_gsf_tas(_params->ekf2_gsf_tas),
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_BAROMETER)
-	_param_ekf2_baro_ctrl(_params->baro_ctrl),
-	_param_ekf2_baro_delay(_params->baro_delay_ms),
-	_param_ekf2_baro_noise(_params->baro_noise),
-	_param_ekf2_baro_gate(_params->baro_innov_gate),
-	_param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone),
-	_param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
+	_param_ekf2_baro_ctrl(_params->ekf2_baro_ctrl),
+	_param_ekf2_baro_delay(_params->ekf2_baro_delay),
+	_param_ekf2_baro_noise(_params->ekf2_baro_noise),
+	_param_ekf2_baro_gate(_params->ekf2_baro_gate),
+	_param_ekf2_gnd_eff_dz(_params->ekf2_gnd_eff_dz),
+	_param_ekf2_gnd_max_hgt(_params->ekf2_gnd_max_hgt),
 # if defined(CONFIG_EKF2_BARO_COMPENSATION)
-	_param_ekf2_aspd_max(_params->max_correction_airspeed),
-	_param_ekf2_pcoef_xp(_params->static_pressure_coef_xp),
-	_param_ekf2_pcoef_xn(_params->static_pressure_coef_xn),
-	_param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
-	_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
-	_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
+	_param_ekf2_aspd_max(_params->ekf2_aspd_max),
+	_param_ekf2_pcoef_xp(_params->ekf2_pcoef_xp),
+	_param_ekf2_pcoef_xn(_params->ekf2_pcoef_xn),
+	_param_ekf2_pcoef_yp(_params->ekf2_pcoef_yp),
+	_param_ekf2_pcoef_yn(_params->ekf2_pcoef_yn),
+	_param_ekf2_pcoef_z(_params->ekf2_pcoef_z),
 # endif // CONFIG_EKF2_BARO_COMPENSATION
 #endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_AIRSPEED)
-	_param_ekf2_asp_delay(_params->airspeed_delay_ms),
-	_param_ekf2_tas_gate(_params->tas_innov_gate),
-	_param_ekf2_eas_noise(_params->eas_noise),
-	_param_ekf2_arsp_thr(_params->arsp_thr),
+	_param_ekf2_asp_delay(_params->ekf2_asp_delay),
+	_param_ekf2_tas_gate(_params->ekf2_tas_gate),
+	_param_ekf2_eas_noise(_params->ekf2_eas_noise),
+	_param_ekf2_arsp_thr(_params->ekf2_arsp_thr),
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_SIDESLIP)
-	_param_ekf2_beta_gate(_params->beta_innov_gate),
-	_param_ekf2_beta_noise(_params->beta_noise),
-	_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
+	_param_ekf2_beta_gate(_params->ekf2_beta_gate),
+	_param_ekf2_beta_noise(_params->ekf2_beta_noise),
+	_param_ekf2_fuse_beta(_params->ekf2_fuse_beta),
 #endif // CONFIG_EKF2_SIDESLIP
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	_param_ekf2_mag_delay(_params->mag_delay_ms),
-	_param_ekf2_mag_e_noise(_params->mage_p_noise),
-	_param_ekf2_mag_b_noise(_params->magb_p_noise),
-	_param_ekf2_head_noise(_params->mag_heading_noise),
-	_param_ekf2_mag_noise(_params->mag_noise),
-	_param_ekf2_mag_decl(_params->mag_declination_deg),
-	_param_ekf2_hdg_gate(_params->heading_innov_gate),
-	_param_ekf2_mag_gate(_params->mag_innov_gate),
-	_param_ekf2_decl_type(_params->mag_declination_source),
-	_param_ekf2_mag_type(_params->mag_fusion_type),
-	_param_ekf2_mag_acclim(_params->mag_acc_gate),
-	_param_ekf2_mag_check(_params->mag_check),
-	_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
-	_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
-	_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
+	_param_ekf2_mag_delay(_params->ekf2_mag_delay),
+	_param_ekf2_mag_e_noise(_params->ekf2_mag_e_noise),
+	_param_ekf2_mag_b_noise(_params->ekf2_mag_b_noise),
+	_param_ekf2_head_noise(_params->ekf2_head_noise),
+	_param_ekf2_mag_noise(_params->ekf2_mag_noise),
+	_param_ekf2_mag_decl(_params->ekf2_mag_decl),
+	_param_ekf2_hdg_gate(_params->ekf2_hdg_gate),
+	_param_ekf2_mag_gate(_params->ekf2_mag_gate),
+	_param_ekf2_decl_type(_params->ekf2_decl_type),
+	_param_ekf2_mag_type(_params->ekf2_mag_type),
+	_param_ekf2_mag_acclim(_params->ekf2_mag_acclim),
+	_param_ekf2_mag_check(_params->ekf2_mag_check),
+	_param_ekf2_mag_chk_str(_params->ekf2_mag_chk_str),
+	_param_ekf2_mag_chk_inc(_params->ekf2_mag_chk_inc),
+	_param_ekf2_synt_mag_z(_params->ekf2_synt_mag_z),
 #endif // CONFIG_EKF2_MAGNETOMETER
 	_param_ekf2_hgt_ref(_params->height_sensor_ref),
-	_param_ekf2_noaid_tout(_params->valid_timeout_max),
+	_param_ekf2_noaid_tout(_params->ekf2_noaid_tout),
 #if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
 	_param_ekf2_min_rng(_params->rng_gnd_clearance),
 #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
@@ -155,66 +155,65 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_ctrl(_params->rng_ctrl),
 	_param_ekf2_rng_delay(_params->range_delay_ms),
 	_param_ekf2_rng_noise(_params->range_noise),
-	_param_ekf2_rng_sfe(_params->range_noise_scaler),
+	_param_ekf2_rng_sfe(_params->ekf2_rng_sfe),
 	_param_ekf2_rng_gate(_params->range_innov_gate),
-	_param_ekf2_rng_pitch(_params->rng_sens_pitch),
+	_param_ekf2_rng_pitch(_params->ekf2_rng_pitch),
 	_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
 	_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
 	_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
-	_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
 	_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
 	_param_ekf2_rng_fog(_params->rng_fog),
-	_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
-	_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
-	_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
+	_param_ekf2_rng_pos_x(_params->ekf2_rng_pos_x),
+	_param_ekf2_rng_pos_y(_params->ekf2_rng_pos_y),
+	_param_ekf2_rng_pos_z(_params->ekf2_rng_pos_z),
 #endif // CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_param_ekf2_ev_delay(_params->ev_delay_ms),
-	_param_ekf2_ev_ctrl(_params->ev_ctrl),
-	_param_ekf2_ev_qmin(_params->ev_quality_minimum),
-	_param_ekf2_evp_noise(_params->ev_pos_noise),
-	_param_ekf2_evv_noise(_params->ev_vel_noise),
-	_param_ekf2_eva_noise(_params->ev_att_noise),
-	_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
-	_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
+	_param_ekf2_ev_delay(_params->ekf2_ev_delay),
+	_param_ekf2_ev_ctrl(_params->ekf2_ev_ctrl),
+	_param_ekf2_ev_qmin(_params->ekf2_ev_qmin),
+	_param_ekf2_evp_noise(_params->ekf2_evp_noise),
+	_param_ekf2_evv_noise(_params->ekf2_evv_noise),
+	_param_ekf2_eva_noise(_params->ekf2_eva_noise),
+	_param_ekf2_evv_gate(_params->ekf2_evv_gate),
+	_param_ekf2_evp_gate(_params->ekf2_evp_gate),
 	_param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
 	_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
 	_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_param_ekf2_of_ctrl(_params->flow_ctrl),
-	_param_ekf2_of_gyr_src(_params->flow_gyro_src),
-	_param_ekf2_of_delay(_params->flow_delay_ms),
-	_param_ekf2_of_n_min(_params->flow_noise),
-	_param_ekf2_of_n_max(_params->flow_noise_qual_min),
-	_param_ekf2_of_qmin(_params->flow_qual_min),
-	_param_ekf2_of_qmin_gnd(_params->flow_qual_min_gnd),
-	_param_ekf2_of_gate(_params->flow_innov_gate),
+	_param_ekf2_of_ctrl(_params->ekf2_of_ctrl),
+	_param_ekf2_of_gyr_src(_params->ekf2_of_gyr_src),
+	_param_ekf2_of_delay(_params->ekf2_of_delay),
+	_param_ekf2_of_n_min(_params->ekf2_of_n_min),
+	_param_ekf2_of_n_max(_params->ekf2_of_n_max),
+	_param_ekf2_of_qmin(_params->ekf2_of_qmin),
+	_param_ekf2_of_qmin_gnd(_params->ekf2_of_qmin_gnd),
+	_param_ekf2_of_gate(_params->ekf2_of_gate),
 	_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
 	_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
 	_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 #if defined(CONFIG_EKF2_DRAG_FUSION)
-	_param_ekf2_drag_ctrl(_params->drag_ctrl),
-	_param_ekf2_drag_noise(_params->drag_noise),
-	_param_ekf2_bcoef_x(_params->bcoef_x),
-	_param_ekf2_bcoef_y(_params->bcoef_y),
-	_param_ekf2_mcoef(_params->mcoef),
+	_param_ekf2_drag_ctrl(_params->ekf2_drag_ctrl),
+	_param_ekf2_drag_noise(_params->ekf2_drag_noise),
+	_param_ekf2_bcoef_x(_params->ekf2_bcoef_x),
+	_param_ekf2_bcoef_y(_params->ekf2_bcoef_y),
+	_param_ekf2_mcoef(_params->ekf2_mcoef),
 #endif // CONFIG_EKF2_DRAG_FUSION
 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
-	_param_ekf2_grav_noise(_params->gravity_noise),
+	_param_ekf2_grav_noise(_params->ekf2_grav_noise),
 #endif // CONFIG_EKF2_GRAVITY_FUSION
 	_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
 	_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
 	_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
-	_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
-	_param_ekf2_abias_init(_params->switch_on_accel_bias),
-	_param_ekf2_angerr_init(_params->initial_tilt_err),
-	_param_ekf2_abl_lim(_params->acc_bias_lim),
-	_param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim),
-	_param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
-	_param_ekf2_abl_tau(_params->acc_bias_learn_tc),
-	_param_ekf2_gyr_b_lim(_params->gyro_bias_lim)
+	_param_ekf2_gbias_init(_params->ekf2_gbias_init),
+	_param_ekf2_abias_init(_params->ekf2_abias_init),
+	_param_ekf2_angerr_init(_params->ekf2_angerr_init),
+	_param_ekf2_abl_lim(_params->ekf2_abl_lim),
+	_param_ekf2_abl_acclim(_params->ekf2_abl_acclim),
+	_param_ekf2_abl_gyrlim(_params->ekf2_abl_gyrlim),
+	_param_ekf2_abl_tau(_params->ekf2_abl_tau),
+	_param_ekf2_gyr_b_lim(_params->ekf2_gyr_b_lim)
 {
 	AdvertiseTopics();
 }
@@ -1601,8 +1600,16 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
+
+	// TODO: this makes dist_bottom a function of the Terrain State estimate, which is not
+	// exactly correct. Terrain State can diverge from the distance_sensor.current_distance
+	// when fusion is disabled. This causes Terrain Hold to drift due to dist_bottom diverging.
+	// Ultimately the logic for rangefinder fusion needs to be improved, and when distance_sensor
+	// is not fused into the Terrain State estimate we should not use dist_bottom for Altitude Lock.
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
+	// TODO: review, we should use sensor variance not terrain
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
+
 	_ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter);
 
 	lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE;
@@ -1790,7 +1797,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 #if defined(CONFIG_EKF2_GNSS)
 	// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
 	// the GPS Fix bit, which is always checked)
-	status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1);
+	status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->ekf2_gps_check << 1) | 1);
 #endif // CONFIG_EKF2_GNSS
 
 	status.control_mode_flags = _ekf.control_status().value;
@@ -1910,14 +1917,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_mag_fault             = _ekf.control_status_flags().mag_fault;
 		status_flags.cs_fuse_aspd             = _ekf.control_status_flags().fuse_aspd;
 		status_flags.cs_gnd_effect            = _ekf.control_status_flags().gnd_effect;
-		status_flags.cs_rng_stuck             = _ekf.control_status_flags().rng_stuck;
 		status_flags.cs_gnss_yaw               = _ekf.control_status_flags().gnss_yaw;
 		status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
 		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
 		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
 		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
 		status_flags.cs_gnss_yaw_fault         = _ekf.control_status_flags().gnss_yaw_fault;
-		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
 		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
@@ -2599,7 +2604,7 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
 
 #if defined(CONFIG_EKF2_SIDESLIP)
 
-			if (vehicle_status.is_vtol_tailsitter && _params->beta_fusion_enabled) {
+			if (vehicle_status.is_vtol_tailsitter && _params->ekf2_fuse_beta) {
 				PX4_WARN("Disable EKF beta fusion as unsupported for tailsitter");
 				_param_ekf2_fuse_beta.set(0);
 				_param_ekf2_fuse_beta.commit_no_notification();

src/modules/ekf2/EKF2.hpp

@@ -536,7 +536,7 @@ private:
 		(ParamFloat<px4::params::EKF2_REQ_GPS_H>)     _param_ekf2_req_gps_h,
 
 		// Used by EKF-GSF experimental yaw estimator
-		(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas_default,
+		(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas,
 		(ParamFloat<px4::params::EKF2_GPS_YAW_OFF>) _param_ekf2_gps_yaw_off,
 #endif // CONFIG_EKF2_GNSS
 
@@ -593,7 +593,7 @@ private:
 		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check,
 		(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
 		(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
-		(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synthetic_mag_z,
+		(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synt_mag_z,
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 		(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref,    ///< selects the primary source for height data
@@ -619,7 +619,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
 		(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
 		(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>) _param_ekf2_rng_a_igate,
-		(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
 		(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
 		(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,

src/modules/ekf2/params_gnss.yaml

@@ -77,7 +77,7 @@ parameters:
     EKF2_GPS_POS_X:
       description:
         short: X position of GPS antenna in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        long: Forward (roll) axis with origin relative to vehicle centre of gravity
       type: float
       default: 0.0
       unit: m
@@ -85,7 +85,7 @@ parameters:
     EKF2_GPS_POS_Y:
       description:
         short: Y position of GPS antenna in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        long: Right (pitch) axis with origin relative to vehicle centre of gravity
       type: float
       default: 0.0
       unit: m
@@ -93,7 +93,7 @@ parameters:
     EKF2_GPS_POS_Z:
       description:
         short: Z position of GPS antenna in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        long: Down (yaw) axis with origin relative to vehicle centre of gravity
       type: float
       default: 0.0
       unit: m