Update src/lib/version/px_update_git_header.py

Apply suggestions from code review
Co-authored-by: Daniel Agar <daniel@agar.ca>
2026-06-01 01:30:05 +08:00 · 2024-08-27 16:41:12 +02:00 · 2024-08-27 16:40:11 +02:00 · 2024-08-27 13:21:43 +02:00
156 changed files with 1766 additions and 4105 deletions
@@ -77,7 +77,6 @@ pipeline {
                     "matek_h743-mini_default",
                     "matek_h743-slim_default",
                     "matek_h743_default",
-                     "micoair_h743_default",
                     "modalai_fc-v1_default",
                     "modalai_fc-v2_default",
                     "mro_ctrl-zero-classic_default",
@@ -88,7 +88,7 @@ jobs:
      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
    - name: Upload px4 coredump
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2-preview
      with:
        name: coredump
        path: px4.core
@@ -103,21 +103,21 @@ jobs:

    - name: Upload px4 binary
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2
      with:
        name: binary
        path: build/px4_sitl_default/bin/px4

    - name: Store PX4 log
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2
      with:
        name: px4_log
        path: ~/.ros/log/*/*.ulg

    - name: Store ROS log
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2
      with:
        name: ros_log
        path: ~/.ros/**/rostest-*.log
@@ -83,7 +83,7 @@ jobs:
      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
    - name: Upload px4 coredump
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2-preview
      with:
        name: coredump
        path: px4.core
@@ -98,21 +98,21 @@ jobs:

    - name: Upload px4 binary
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2
      with:
        name: binary
        path: build/px4_sitl_default/bin/px4

    - name: Store PX4 log
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2
      with:
        name: px4_log
        path: ~/.ros/log/*/*.ulg

    - name: Store ROS log
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2
      with:
        name: ros_log
        path: ~/.ros/**/rostest-*.log
@@ -286,11 +286,6 @@ CONFIG:
      buildType: MiniSizeRel
      settings:
        CONFIG: matek_gnss-m9n-f4_default
-    micoair_h743_default:
-      short: micoair_h743
-      buildType: MinSizeRel
-      settings:
-        CONFIG: micoair_h743_default
    modalai_fc-v1_default:
      short: modalai_fc-v1
      buildType: MinSizeRel
@@ -113,7 +113,7 @@ include(px4_parse_function_args)
 include(px4_git)

 execute_process(
-	COMMAND git describe --exclude ext/* --tags --match "v[0-9]*"
+	COMMAND git describe --exclude ext/* --match v[0-9]*.[0-9]*.[0-9]* --tags
 	OUTPUT_VARIABLE PX4_GIT_TAG
 	OUTPUT_STRIP_TRAILING_WHITESPACE
 	RESULTS_VARIABLE GIT_DESCRIBE_RESULT
@@ -340,7 +340,6 @@ bootloaders_update: \
 	matek_h743_bootloader \
 	matek_h743-mini_bootloader \
 	matek_h743-slim_bootloader \
-        micoair_h743_bootloader \
 	modalai_fc-v2_bootloader \
 	mro_ctrl-zero-classic_bootloader \
 	mro_ctrl-zero-h7_bootloader \
@@ -14,19 +14,15 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
 # Rover parameters
 param set-default RD_WHEEL_TRACK 0.3
 param set-default RD_MAN_YAW_SCALE 0.1
+param set-default RD_YAW_RATE_I 0.1
+param set-default RD_YAW_RATE_P 5
 param set-default RD_MAX_ACCEL 6
 param set-default RD_MAX_JERK 30
-param set-default RD_MAX_THR_YAW_R 5
-param set-default RD_YAW_RATE_P 0.1
-param set-default RD_YAW_RATE_I 0
-param set-default RD_YAW_P 5
-param set-default RD_YAW_I 0
-param set-default RD_MAX_SPEED 5
-param set-default RD_MAX_THR_SPD 7
-param set-default RD_SPEED_P 1
-param set-default RD_SPEED_I 0
+param set-default RD_MAX_SPEED 7
+param set-default RD_HEADING_P 5
+param set-default RD_HEADING_I 0.1
 param set-default RD_MAX_YAW_RATE 180
-param set-default RD_MISS_SPD_DEF 5
+param set-default RD_MISS_SPD_DEF 7
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533

@@ -27,8 +27,8 @@ 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_HEADING_P 5
+param set-default RD_HEADING_I 0.1
 param set-default RD_MAX_YAW_RATE 30
 param set-default RD_MISS_SPD_DEF 8
 param set-default RD_TRANS_DRV_TRN 0.349066
@@ -1,10 +0,0 @@
-#!/bin/sh
-#
-# @name Gazebo x500 mono cam
-#
-# @type Quadrotor
-#
-
-PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_mono_cam_down}
-
-. ${R}etc/init.d-posix/airframes/4001_gz_x500
@@ -85,7 +85,6 @@ px4_add_romfs_files(
 	4011_gz_lawnmower
 	4012_gz_rover_ackermann
 	4013_gz_x500_lidar
-	4014_gz_x500_mono_cam_down

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -43,10 +43,6 @@ param set-default CA_SV_CS1_TRQ_P 0.3
 param set-default CA_SV_CS1_TRQ_Y -0.3
 param set-default CA_SV_CS1_TYPE 6

-param set-default FW_AIRSPD_MAX 12
-param set-default FW_AIRSPD_MIN 7
-param set-default FW_AIRSPD_TRIM 10
-
 param set-default HIL_ACT_FUNC1 101
 param set-default HIL_ACT_FUNC2 102
 param set-default HIL_ACT_FUNC5 202
@@ -66,8 +62,6 @@ param set-default CBRK_SUPPLY_CHK 894281
 # - without safety switch
 param set-default CBRK_IO_SAFETY 22027

-param set-default SENS_DPRES_OFF 0.001
-
 param set SIH_T_MAX 2.0
 param set SIH_Q_MAX 0.0165
 param set SIH_MASS 0.2
@@ -81,5 +75,3 @@ param set SIH_L_ROLL 0.145

 # sih as tailsitter
 param set SIH_VEHICLE_TYPE 2
-
-param set-default VT_ARSP_TRANS 6
@@ -1,99 +0,0 @@
-#!/bin/sh
-#
-# @name SIH Tailsitter Duo
-#
-# @type Simulation
-# @class VTOL
-#
-# @output Motor1 motor right
-# @output Motor2 motor left
-# @output Servo1 elevon right
-# @output Servo2 elevon left
-#
-# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
-#
-# @board px4_fmu-v2 exclude
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-param set UAVCAN_ENABLE 0
-param set-default VT_B_TRANS_DUR 5
-param set-default VT_ELEV_MC_LOCK 0
-param set-default VT_TYPE 2
-param set-default MPC_MAN_Y_MAX 60
-param set-default MC_PITCH_P 5
-
-param set-default CA_AIRFRAME 2
-param set-default CA_ROTOR_COUNT 5
-param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR0_PX 0.2
-param set-default CA_ROTOR0_PY 0.2
-param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR1_PX -0.2
-param set-default CA_ROTOR1_PY -0.2
-param set-default CA_ROTOR2_PX 0.2
-param set-default CA_ROTOR2_PY -0.2
-param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.2
-param set-default CA_ROTOR3_PY 0.2
-param set-default CA_ROTOR3_KM -0.05
-
-
-param set-default CA_ROTOR4_PX -0.3
-param set-default CA_ROTOR4_KM 0.05
-param set-default CA_ROTOR4_AX 1
-param set-default CA_ROTOR4_AZ 0
-
-param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_CS0_TRQ_R 0.5
-param set-default CA_SV_CS0_TYPE 2
-param set-default CA_SV_CS1_TRQ_P 1
-param set-default CA_SV_CS1_TYPE 3
-param set-default CA_SV_CS2_TRQ_Y 1
-
-param set HIL_ACT_REV 32
-
-param set-default FW_AIRSPD_MAX 12
-param set-default FW_AIRSPD_MIN 7
-param set-default FW_AIRSPD_TRIM 10
-
-param set-default HIL_ACT_FUNC1 101
-param set-default HIL_ACT_FUNC2 102
-param set-default HIL_ACT_FUNC3 103
-param set-default HIL_ACT_FUNC4 104
-param set-default HIL_ACT_FUNC5 201
-param set-default HIL_ACT_FUNC6 202
-param set-default HIL_ACT_FUNC7 203
-param set-default HIL_ACT_FUNC8 105
-
-param set-default MAV_TYPE 22
-
-
-
-# set SYS_HITL to 2 to start the SIH and avoid sensors startup
-param set-default SYS_HITL 2
-
-# disable some checks to allow to fly:
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-# - without safety switch
-param set-default CBRK_IO_SAFETY 22027
-
-param set-default SENS_DPRES_OFF 0.001
-
-param set SIH_T_MAX 2.0
-param set SIH_Q_MAX 0.0165
-param set SIH_MASS 0.2
-# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
-param set SIH_IXX 0.00354
-param set SIH_IYY 0.000625
-param set SIH_IZZ 0.00300
-param set SIH_IXZ 0
-param set SIH_KDV 0.2
-param set SIH_L_ROLL 0.145
-
-# sih as tailsitter
-param set SIH_VEHICLE_TYPE 3
-
-param set-default VT_ARSP_TRANS 6
@@ -20,27 +20,3 @@ param set-default RBCLW_ADDRESS 128
 param set-default RBCLW_FUNC1 101
 param set-default RBCLW_FUNC2 102
 param set-default RBCLW_REV 1 # reverse right wheels
-
-# Rover parameters
-param set-default RD_WHEEL_TRACK 0.3
-param set-default RD_MAN_YAW_SCALE 1
-param set-default RD_MAX_ACCEL 5
-param set-default RD_MAX_JERK 10
-param set-default RD_MAX_THR_YAW_R 4
-param set-default RD_YAW_RATE_P 0.1
-param set-default RD_YAW_RATE_I 0
-param set-default RD_YAW_P 5
-param set-default RD_YAW_I 0
-param set-default RD_MAX_SPEED 1.8
-param set-default RD_MAX_THR_SPD 2
-param set-default RD_SPEED_P 0.5
-param set-default RD_SPEED_I 0.1
-param set-default RD_MAX_YAW_RATE 300
-param set-default RD_MISS_SPD_DEF 1.8
-param set-default RD_TRANS_DRV_TRN 0.349066
-param set-default RD_TRANS_TRN_DRV 0.174533
-
-# Pure pursuit parameters
-param set-default PP_LOOKAHD_MAX 10
-param set-default PP_LOOKAHD_MIN 1
-param set-default PP_LOOKAHD_GAIN 1
@@ -48,7 +48,6 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
 		1100_rc_quad_x_sih.hil
 		1101_rc_plane_sih.hil
 		1102_tailsitter_duo_sih.hil
-		1103_standard_vtol_sih.hil
 	)
 endif()

@@ -105,13 +105,6 @@ then
 		fi
 	fi

-  if [ -e /fs/microsd/new ]
-  then
-    echo "Updating external autostart files"
-    rm -r $SDCARD_EXT_PATH
-    mv /fs/microsd/new $SDCARD_EXT_PATH
-  fi
-
 	# Check for an update of the ext_autostart folder, and replace the old one with it
 	if [ -e /fs/microsd/ext_autostart_new ]
 	then
@@ -65,7 +65,7 @@ def get_version():

    if os.path.isdir(os.path.join(px4_dir, '.git')):
        # If inside a clone PX4 Firmware repository, get version from "git describe"
-        cmd = 'git describe --exclude ext/* --abbrev=0 --tags'
+        cmd = 'git describe --exclude ext/* --match v[0-9]*.[0-9]*.[0-9]* --abbrev=0 --tags'
        try:
            version = subprocess.check_output(
                cmd, cwd=px4_dir, shell=True).decode().strip()
@@ -225,7 +225,7 @@ if [[ $INSTALL_SIM == "true" ]]; then

 	# Gazebo / Gazebo classic installation
 	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		echo "Gazebo (Harmonic) will be installed"
+		echo "Gazebo (Garden) will be installed"
 		echo "Earlier versions will be removed"
 		# Add Gazebo binary repository
 		sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
@@ -85,6 +85,8 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
@@ -39,6 +39,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -69,6 +70,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -77,6 +79,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -88,6 +91,7 @@ CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
@@ -95,3 +99,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_FAKE_GPS=y
@@ -3,8 +3,6 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_BOARD_TESTING=y
@@ -1,6 +1,4 @@
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_BOARD_CRYPTO=y
 CONFIG_DRIVERS_STUB_KEYSTORE=y
 CONFIG_DRIVERS_SW_CRYPTO=y
@@ -29,6 +29,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -23,27 +23,27 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=n
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=n
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=n
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=n
-CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
-CONFIG_DRIVERS_POWER_MONITOR_INA226=n
+CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_DRIVERS_UAVCAN=n
+CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -377,6 +377,7 @@
 *(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
 *(.text.imxrt_epsubmit)
 *(.text._ZN15PositionControl6updateEf)
+*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE)
 *(.text._ZN23MavlinkStreamScaledIMU24sendEv)
 *(.text._ZN5PX4IO10io_reg_getEhhPtj)
 *(.text.imxrt_dma_send)
@@ -11,7 +11,7 @@ uint8 ACTION_VTOL_TRANSITION_TO_MULTICOPTER = 6
 uint8 ACTION_VTOL_TRANSITION_TO_FIXEDWING = 7

 uint8 source # how the request was triggered
-uint8 SOURCE_STICK_GESTURE = 0
+uint8 SOURCE_RC_STICK_GESTURE = 0
 uint8 SOURCE_RC_SWITCH = 1
 uint8 SOURCE_RC_BUTTON = 2
 uint8 SOURCE_RC_MODE_SLOT = 3
@@ -1,67 +1,67 @@
-uint64 timestamp				# time since system start (microseconds)
-bool connected					# Whether or not a battery is connected, based on a voltage threshold
-float32 voltage_v				# Battery voltage in volts, 0 if unknown
-float32 current_a				# Battery current in amperes, -1 if unknown
-float32 current_average_a			# Battery current average in amperes (for FW average in level flight), -1 if unknown
-float32 discharged_mah				# Discharged amount in mAh, -1 if unknown
-float32 remaining				# From 1 to 0, -1 if unknown
-float32 scale					# Power scaling factor, >= 1, or -1 if unknown
-float32 time_remaining_s			# predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
-float32 temperature				# Temperature of the battery in degrees Celcius, NaN if unknown
-uint8 cell_count				# Number of cells, 0 if unknown
+uint64 timestamp			# time since system start (microseconds)
+bool connected				# Whether or not a battery is connected, based on a voltage threshold
+float32 voltage_v			# Battery voltage in volts, 0 if unknown
+float32 current_a			# Battery current in amperes, -1 if unknown
+float32 current_average_a	# Battery current average in amperes (for FW average in level flight), -1 if unknown
+float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
+float32 remaining			# From 1 to 0, -1 if unknown
+float32 scale				# Power scaling factor, >= 1, or -1 if unknown
+float32 time_remaining_s	# predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
+float32 temperature			# temperature of the battery. NaN if unknown
+uint8 cell_count			# Number of cells, 0 if unknown

 uint8 BATTERY_SOURCE_POWER_MODULE = 0
 uint8 BATTERY_SOURCE_EXTERNAL = 1
 uint8 BATTERY_SOURCE_ESCS = 2
-uint8 source					# Battery source
-uint8 priority					# Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
-uint16 capacity					# actual capacity of the battery
-uint16 cycle_count				# number of discharge cycles the battery has experienced
-uint16 average_time_to_empty			# predicted remaining battery capacity based on the average rate of discharge in min
-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				# state of health. FullChargeCapacity/DesignCapacity, 0-100%.
-uint16 max_error				# max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
+uint8 source				# Battery source
+uint8 priority				# Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
+uint16 capacity				# actual capacity of the battery
+uint16 cycle_count			# number of discharge cycles the battery has experienced
+uint16 average_time_to_empty	# predicted remaining battery capacity based on the average rate of discharge in min
+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		# state of health. FullChargeCapacity/DesignCapacity, 0-100%.
+uint16 max_error			# max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
 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
+uint16 interface_error		# interface error counter

-float32[14] voltage_cell_v			# Battery individual cell voltages, 0 if unknown
-float32 max_cell_voltage_delta			# Max difference between individual cell voltages
+float32[14] voltage_cell_v		# Battery individual cell voltages, 0 if unknown
+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
+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 BATTERY_WARNING_NONE = 0			# no battery low voltage warning active
-uint8 BATTERY_WARNING_LOW = 1			# warning of low voltage
-uint8 BATTERY_WARNING_CRITICAL = 2		# critical voltage, return / abort immediately
-uint8 BATTERY_WARNING_EMERGENCY = 3		# immediate landing required
-uint8 BATTERY_WARNING_FAILED = 4		# the battery has failed completely
-uint8 BATTERY_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 BATTERY_STATE_CHARGING = 7 		# Battery is charging
+uint8 BATTERY_WARNING_NONE = 0		# no battery low voltage warning active
+uint8 BATTERY_WARNING_LOW = 1		# warning of low voltage
+uint8 BATTERY_WARNING_CRITICAL = 2	# critical voltage, return / abort immediately
+uint8 BATTERY_WARNING_EMERGENCY = 3	# immediate landing required
+uint8 BATTERY_WARNING_FAILED = 4	# the battery has failed completely
+uint8 BATTERY_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 BATTERY_STATE_CHARGING = 7 # Battery is charging

-uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 		# Battery has deep discharged
-uint8 BATTERY_FAULT_SPIKES = 1 			# Voltage spikes
-uint8 BATTERY_FAULT_CELL_FAIL= 2 		# One or more cells have failed
-uint8 BATTERY_FAULT_OVER_CURRENT = 3 		# Over-current
-uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 	# Over-temperature
-uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 	# Under-temperature fault
-uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 	# Vehicle voltage is not compatible with battery one
-uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 	# Battery firmware is not compatible with current autopilot firmware
-uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 	# Battery model is not supported by the system
-uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 	# hardware problem
-uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 		# Battery had a problem while arming
-uint8 BATTERY_FAULT_COUNT = 11 			# Counter - keep it as last element!
+uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
+uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes
+uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed
+uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current
+uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature
+uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
+uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one
+uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
+uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
+uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem
+uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming
+uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element!

-uint16 faults					# Smart battery supply status/fault flags (bitmask) for health indication.
-uint8 warning					# Current battery warning
+uint16 faults		# Smart battery supply status/fault flags (bitmask) for health indication.
+uint8 warning		# Current battery warning

 uint8 MAX_INSTANCES = 4

-float32 full_charge_capacity_wh     		# The compensated battery capacity
-float32 remaining_capacity_wh       		# The compensated battery capacity remaining
-uint16 over_discharge_count         		# Number of battery overdischarge
-float32 nominal_voltage             		# Nominal voltage of the battery pack
+float32 full_charge_capacity_wh     # The compensated battery capacity
+float32 remaining_capacity_wh       # The compensated battery capacity remaining
+uint16 over_discharge_count         # Number of battery overdischarge
+float32 nominal_voltage             # Nominal voltage of the battery pack

 float32 internal_resistance_estimate            # [Ohm] Internal resistance per cell estimate
 float32 ocv_estimate                            # [V] Open circuit voltage estimate
@@ -73,7 +73,6 @@ set(msg_files
 	DebugVect.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
-	DistanceSensorModeChangeRequest.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg
@@ -153,10 +152,6 @@ set(msg_files
 	ObstacleDistance.msg
 	OffboardControlMode.msg
 	OnboardComputerStatus.msg
-	OpenDroneIdArmStatus.msg
-	OpenDroneIdOperatorId.msg
-	OpenDroneIdSelfId.msg
-	OpenDroneIdSystem.msg
 	OrbitStatus.msg
 	OrbTest.msg
 	OrbTestLarge.msg
@@ -187,7 +182,6 @@ set(msg_files
 	RoverAckermannGuidanceStatus.msg
 	RoverAckermannStatus.msg
 	RoverDifferentialGuidanceStatus.msg
-	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
 	Rpm.msg
 	RtlStatus.msg
@@ -40,8 +40,3 @@ uint8 ROTATION_UPWARD_FACING   = 24 # MAV_SENSOR_ROTATION_PITCH_90
 uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270

 uint8 ROTATION_CUSTOM          = 100 # MAV_SENSOR_ROTATION_CUSTOM
-
-uint8 mode
-uint8 MODE_UNKNOWN  = 0
-uint8 MODE_ENABLED  = 1
-uint8 MODE_DISABLED = 2
@@ -1,5 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-
-uint8 request_on_off 			# request to disable/enable the distance sensor
-uint8 REQUEST_OFF = 0
-uint8 REQUEST_ON  = 1
@@ -59,6 +59,7 @@ bool fs_bad_airspeed          #  5 - true if fusion of the airspeed has encounte
 bool fs_bad_sideslip          #  6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 bool fs_bad_optflow_x         #  7 - true if fusion of the optical flow X axis has encountered a numerical error
 bool fs_bad_optflow_y         #  8 - true if fusion of the optical flow Y axis has encountered a numerical error
+bool fs_bad_acc_bias          #  9 - true if bad delta velocity bias estimates have been detected
 bool fs_bad_acc_vertical      # 10 - true if bad vertical accelerometer data has been detected
 bool fs_bad_acc_clipping      # 11 - true if delta velocity data contains clipping (asymmetric railing)

@@ -1,3 +0,0 @@
-uint64 timestamp
-uint8 status
-char[50] error
@@ -1,4 +0,0 @@
-uint64 timestamp
-uint8[20] id_or_mac
-uint8 operator_id_type
-char[20] operator_id
@@ -1,4 +0,0 @@
-uint64 timestamp
-uint8[20] id_or_mac
-uint8 description_type
-char[23] description
@@ -1,13 +0,0 @@
-uint64 timestamp
-uint8[20] id_or_mac
-uint8 operator_location_type
-uint8 classification_type
-int32 operator_latitude
-int32 operator_longitude
-uint16 area_count
-uint16 area_radius
-float32 area_ceiling
-float32 area_floor
-uint8 category_eu
-uint8 class_eu
-float32 operator_altitude_geo
@@ -30,8 +30,7 @@ bool loiter_direction_counter_clockwise # loiter direction is clockwise by defau
 float32 loiter_orientation 	# Orientation of the major axis with respect to true north in rad [-pi,pi)
 uint8 	loiter_pattern		# loitern pattern to follow

-float32 acceptance_radius   # horizontal acceptance_radius (meters)
-float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed wing guidance, NAN = let guidance choose (meters)
+float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation

 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)
@@ -1,7 +1,10 @@
 uint64 timestamp # time since system start (microseconds)

+float32 desired_speed 	      # [m/s] Desired forward speed for the rover
 float32 lookahead_distance    # [m] Lookahead distance of pure the pursuit controller
 float32 heading_error_deg 	      # [deg] Heading error of the pure pursuit controller
+float32 pid_heading_integral  # Integral of the PID for the desired yaw rate during missions
+float32 pid_throttle_integral # Integral of the PID for the throttle during missions
 uint8   state_machine         # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED]

 # TOPICS rover_differential_guidance_status
@@ -1,9 +0,0 @@
-uint64 timestamp # time since system start (microseconds)
-
-float32 forward_speed_setpoint 		  # [m/s] Desired forward speed for the rover
-float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover
-float32 yaw_rate_setpoint      		  # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used)
-float32 yaw_rate_setpoint_normalized      # [-1, 1] Normalized yaw rate for the rover
-float32 yaw_setpoint 	       		  # [rad] Desired yaw (heading) for the rover
-
-# TOPICS rover_differential_setpoint
@@ -1,13 +1,8 @@
 uint64 timestamp # time since system start (microseconds)

-float32 actual_speed	         # [m/s] Actual forward speed of the rover
-float32 actual_yaw               # [rad] Actual yaw of the rover
-float32 actual_yaw_rate          # [rad/s] Actual yaw rate of the rover
-float32 desired_yaw_rate         # [rad/s] Yaw rate setpoint for the closed loop yaw rate controller
-float32 forward_speed_normalized # [-1, 1] Normalized forward speed setpoint
-float32 speed_diff_normalized    # [-1, 1] Normalized speed difference setpoint between the left and right motor
-float32 pid_yaw_integral         # Integral of the PID for the closed loop yaw controller
-float32 pid_yaw_rate_integral    # Integral of the PID for the closed loop yaw rate controller
-float32 pid_throttle_integral    # Integral of the PID for the closed loop speed controller
+float32 actual_speed	       # [m/s] Actual forward speed of the rover
+float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate
+float32 actual_yaw_rate_deg_s  # [deg/s] Actual yaw rate of the rover
+float32 pid_yaw_rate_integral  # Integral of the PID for the desired yaw rate controller

 # TOPICS rover_differential_status
@@ -27,4 +27,3 @@ float32 pitch_sp_rad			# Current pitch setpoint [rad]
 float32 throttle_trim			# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions

 float32 underspeed_ratio		# 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0.
-float32 fast_descend_ratio 		#  value indicating if fast descend mode is enabled with ramp up and ramp down [0-1]
@@ -4,9 +4,6 @@ uint64 timestamp_sample                # the timestamp of the raw data (microsec
 float32[2] vel_body                    # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
 float32[2] vel_ne                      # same as vel_body but in local frame (m/s)

-float32[2] vel_body_filtered           # filtered velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
-float32[2] vel_ne_filtered             # filtered same as vel_body_filtered but in local frame (m/s)
-
 float32[2] flow_rate_uncompensated     # integrated optical flow measurement (rad/s)
 float32[2] flow_rate_compensated       # integrated optical flow measurement compensated for angular motion (rad/s)

@@ -12,7 +12,7 @@ uint8 ARMING_STATE_ARMED    = 2
 uint8 latest_arming_reason
 uint8 latest_disarming_reason
 uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0
-uint8 ARM_DISARM_REASON_STICK_GESTURE = 1
+uint8 ARM_DISARM_REASON_RC_STICK = 1
 uint8 ARM_DISARM_REASON_RC_SWITCH = 2
 uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3
 uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4
@@ -55,7 +55,6 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/distance_sensor_mode_change_request.h>

 using namespace time_literals;

@@ -144,11 +143,8 @@ private:
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN};
-	uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)};
-	typeof(px4::msg::DistanceSensorModeChangeRequest::request_on_off) _req_mode{px4::msg::DistanceSensorModeChangeRequest::REQUEST_OFF};
 	bool _restriction{false};
 	bool _auto_restriction{false};
-	bool _prev_restriction{false};
 };

 LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) :
@@ -415,17 +411,6 @@ void LightwareLaser::start()

 int LightwareLaser::updateRestriction()
 {
-	if (_dist_sense_mode_change_sub.updated()) {
-		distance_sensor_mode_change_request_s dist_sense_mode_change;
-
-		if (_dist_sense_mode_change_sub.copy(&dist_sense_mode_change)) {
-			_req_mode = dist_sense_mode_change.request_on_off;
-
-		} else {
-			_req_mode = distance_sensor_mode_change_request_s::REQUEST_OFF;
-		}
-	}
-
 	px4::msg::VehicleStatus vehicle_status;

 	if (_vehicle_status_sub.update(&vehicle_status)) {
@@ -453,7 +438,7 @@ int LightwareLaser::updateRestriction()
 		updateParams();
 	}

-	_prev_restriction = _restriction;
+	bool _prev_restriction{_restriction};

 	switch (_param_sf1xx_mode.get()) {
 	case 0: // Sensor disabled
@@ -466,7 +451,7 @@ int LightwareLaser::updateRestriction()
 		break;

 	case 2:
-		_restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::REQUEST_ON;
+		_restriction = _auto_restriction;
 		break;
 	}

@@ -513,8 +498,6 @@ void LightwareLaser::RunImpl()

 	case State::Running:
 		if (!_restriction) {
-			_px4_rangefinder.set_mode(distance_sensor_s::MODE_ENABLED);
-
 			if (PX4_OK != collect()) {
 				PX4_DEBUG("collection error");

@@ -523,14 +506,6 @@ void LightwareLaser::RunImpl()
 					_consecutive_errors = 0;
 				}
 			}
-
-		} else {
-			_px4_rangefinder.set_mode(distance_sensor_s::MODE_DISABLED);
-
-			if (!_prev_restriction) { // Publish disabled status once
-				_px4_rangefinder.update(hrt_absolute_time(), -1.f, 0);
-			}
-
 		}

 		ScheduleDelayed(_conversion_interval);
@@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
 *
 * @value 0 Disabled
 * @value 1 Enabled
- * @value 2 Enabled in VTOL MC mode, listen to request from system in FW mode
+ * @value 2 Disabled during VTOL fast forward flight
 *
 * @min 0
 * @max 2
@@ -88,7 +88,6 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
 	_battery.setConnected(false);
 	_battery.updateVoltage(0.f);
 	_battery.updateCurrent(0.f);
-	_battery.updateTemperature(0.f);
 	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 }

@@ -307,7 +306,6 @@ INA228::collect()
 	// success = success && (read(INA228_REG_POWER, _power) == PX4_OK);
 	success = success && (read(INA228_REG_CURRENT, _current) == PX4_OK);
 	//success = success && (read(INA228_REG_VSHUNT, _shunt) == PX4_OK);
-	success = success && (read(INA228_REG_DIETEMP, _temperature) == PX4_OK);

 	if (!success) {
 		PX4_DEBUG("error reading from sensor");
@@ -317,7 +315,6 @@ INA228::collect()
 	_battery.setConnected(success);
 	_battery.updateVoltage(static_cast<float>(_bus_voltage * INA228_VSCALE));
 	_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
-	_battery.updateTemperature(static_cast<float>(_temperature * INA228_TSCALE));
 	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());

 	perf_end(_sample_perf);
@@ -384,7 +381,6 @@ INA228::RunImpl()
 		_battery.setConnected(false);
 		_battery.updateVoltage(0.f);
 		_battery.updateCurrent(0.f);
-		_battery.updateTemperature(0.f);
 		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());

 		if (init() != PX4_OK) {
@@ -291,7 +291,6 @@ using namespace time_literals;
 #define INA228_CONST                         13107.2e6f  /* is an internal fixed value used to ensure scaling is maintained properly  */
 #define INA228_SHUNT                         0.0005f   /* Shunt is 500 uOhm */
 #define INA228_VSCALE                        1.95e-04f  /* LSB of voltage is 195.3125 uV/LSB */
-#define INA228_TSCALE                        7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */

 #define swap16(w)                            __builtin_bswap16((w))
 #define swap32(d)                            __builtin_bswap32((d))
@@ -340,7 +339,6 @@ private:
 	int32_t           _bus_voltage{0};
 	int64_t           _power{0};
 	int32_t           _current{0};
-	int16_t           _temperature{0};
 	int32_t           _shunt{0};
 	int16_t           _cal{0};
 	int16_t           _range{INA228_ADCRANGE_HIGH};
@@ -80,7 +80,6 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
 	_battery.setConnected(false);
 	_battery.updateVoltage(0.f);
 	_battery.updateCurrent(0.f);
-	_battery.updateTemperature(0.f);
 	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 }

@@ -225,11 +224,9 @@ int INA238::collect()
 	bool success{true};
 	int16_t bus_voltage{0};
 	int16_t current{0};
-	int16_t temperature{0};

 	success = (RegisterRead(Register::VS_BUS, (uint16_t &)bus_voltage) == PX4_OK);
 	success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK);
-	success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK);

 	if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
 		// check configuration registers periodically or immediately following any failure
@@ -253,7 +250,6 @@ int INA238::collect()
 	_battery.setConnected(success);
 	_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
 	_battery.updateCurrent(static_cast<float>(current * _current_lsb));
-	_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
 	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());

 	perf_end(_sample_perf);
@@ -313,7 +309,6 @@ void INA238::RunImpl()
 		_battery.setConnected(false);
 		_battery.updateVoltage(0.f);
 		_battery.updateCurrent(0.f);
-		_battery.updateTemperature(0.f);
 		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());

 		if (init() != PX4_OK) {
@@ -73,7 +73,6 @@ using namespace ina238;
 #define INA238_DN_MAX                        32768.0f   /* 2^15 */
 #define INA238_CONST                         819.2e6f  /* is an internal fixed value used to ensure scaling is maintained properly  */
 #define INA238_VSCALE                        3.125e-03f  /* LSB of voltage is 3.1255 mV/LSB */
-#define INA238_TSCALE                        7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */

 #define DEFAULT_MAX_CURRENT                  327.68f    /* Amps */
 #define DEFAULT_SHUNT                        0.0003f   /* Shunt is 300 uOhm */
@@ -14,7 +14,6 @@ enum class Register : uint8_t {
 	ADCCONFIG = 0x01, // ADC Configuration Register
 	SHUNT_CAL = 0x02, // Shunt Calibration Register
 	VS_BUS = 0x05,
-	DIETEMP = 0x06,
 	CURRENT = 0x07,
 	MANUFACTURER_ID = 0x3e,
 	DEVICE_ID = 0x3f
@@ -147,8 +147,6 @@ px4_add_module(
 		arming_status.hpp
 		beep.cpp
 		beep.hpp
-		remoteid.cpp
-		remoteid.hpp
 		rgbled.cpp
 		rgbled.hpp
 		safety_state.cpp
@@ -26,10 +26,6 @@ if DRIVERS_UAVCAN
        bool "Include safety state controller"
        default y

-    config UAVCAN_REMOTEID_CONTROLLER
-        bool "Include remote ID controller"
-        default y
-
    config UAVCAN_RGB_CONTROLLER
        bool "Include rgb controller"
        default y
@@ -1,356 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2024 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.
- *
- ****************************************************************************/
-
-#include "remoteid.hpp"
-#include <modules/mavlink/open_drone_id_translations.hpp>
-#include <drivers/drv_hrt.h>
-
-using namespace time_literals;
-
-
-UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) :
-	ModuleParams(nullptr),
-	_timer(node),
-	_node(node),
-	_uavcan_pub_remoteid_basicid(node),
-	_uavcan_pub_remoteid_location(node),
-	_uavcan_pub_remoteid_self_id(node),
-	_uavcan_pub_remoteid_system(node),
-	_uavcan_pub_remoteid_operator_id(node),
-	_uavcan_sub_arm_status(node)
-{
-}
-
-int UavcanRemoteIDController::init()
-{
-	// Setup timer and call back function for periodic updates
-	_timer.setCallback(TimerCbBinder(this, &UavcanRemoteIDController::periodic_update));
-	_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
-
-	int res = _uavcan_sub_arm_status.start(ArmStatusBinder(this, &UavcanRemoteIDController::arm_status_sub_cb));
-
-	if (res < 0) {
-		PX4_WARN("ArmStatus sub failed %i", res);
-		return res;
-	}
-
-	return 0;
-}
-
-void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &)
-{
-	_vehicle_status.update();
-
-	send_basic_id();
-	send_location();
-	send_self_id();
-	send_system();
-	send_operator_id();
-}
-
-void UavcanRemoteIDController::send_basic_id()
-{
-	dronecan::remoteid::BasicID basic_id {};
-	// basic_id.id_or_mac // supposedly only used for drone ID data from other UAs
-	basic_id.id_type = dronecan::remoteid::BasicID::ODID_ID_TYPE_SERIAL_NUMBER;
-	basic_id.ua_type = static_cast<uint8_t>(open_drone_id_translations::odidTypeForMavType(
-			_vehicle_status.get().system_type));
-
-	// uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type
-	// TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format
-
-	char uas_id[20] = {};
-	board_get_px4_guid_formated((char *)(uas_id), sizeof(uas_id));
-	basic_id.uas_id = uas_id;
-
-	_uavcan_pub_remoteid_basicid.broadcast(basic_id);
-}
-
-void UavcanRemoteIDController::send_location()
-{
-	dronecan::remoteid::Location msg {};
-
-	// initialize all fields to unknown
-	msg.status = MAV_ODID_STATUS_UNDECLARED;
-	msg.direction = 36100; // If unknown: 36100 centi-degrees
-	msg.speed_horizontal = 25500; // If unknown: 25500 cm/s
-	msg.speed_vertical = 6300; // If unknown: 6300 cm/s
-	msg.latitude = 0; // If unknown: 0
-	msg.longitude = 0; // If unknown: 0
-	msg.altitude_geodetic = -1000; // If unknown: -1000 m
-	msg.altitude_geodetic = -1000; // If unknown: -1000 m
-	msg.height = -1000; // If unknown: -1000 m
-	msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
-	msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
-	msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
-	msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
-	msg.timestamp = 0xFFFF; // If unknown: 0xFFFF
-	msg.timestamp_accuracy = MAV_ODID_TIME_ACC_UNKNOWN;
-
-	bool updated = false;
-
-	if (_vehicle_land_detected_sub.advertised()) {
-		vehicle_land_detected_s vehicle_land_detected{};
-
-		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
-		    && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 10_s)) {
-			if (vehicle_land_detected.landed) {
-				msg.status = MAV_ODID_STATUS_GROUND;
-
-			} else {
-				msg.status = MAV_ODID_STATUS_AIRBORNE;
-			}
-
-			updated = true;
-		}
-	}
-
-	if (hrt_elapsed_time(&_vehicle_status.get().timestamp) < 10_s) {
-		if (_vehicle_status.get().failsafe && (_vehicle_status.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
-			msg.status = MAV_ODID_STATUS_EMERGENCY;
-			updated = true;
-		}
-	}
-
-	if (_vehicle_gps_position_sub.advertised()) {
-		sensor_gps_s vehicle_gps_position{};
-
-		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)
-		    && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 10_s)) {
-
-			if (vehicle_gps_position.vel_ned_valid) {
-				const matrix::Vector3f vel_ned{vehicle_gps_position.vel_n_m_s, vehicle_gps_position.vel_e_m_s, vehicle_gps_position.vel_d_m_s};
-
-				// direction: calculate GPS course over ground angle
-				const float course = atan2f(vel_ned(1), vel_ned(0));
-				const int course_deg = roundf(math::degrees(matrix::wrap_2pi(course)));
-				msg.direction = math::constrain(100 * course_deg, 0, 35999); // 0 - 35999 centi-degrees
-
-				// speed_horizontal: If speed is larger than 25425 cm/s, use 25425 cm/s.
-				const int speed_horizontal_cm_s = matrix::Vector2f(vel_ned).length() * 100.f;
-				msg.speed_horizontal = math::constrain(speed_horizontal_cm_s, 0, 25425);
-
-				// speed_vertical: Up is positive, If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.
-				const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f);
-				msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200);
-
-				msg.speed_accuracy = open_drone_id_translations::odidSpeedAccForVariance(vehicle_gps_position.s_variance_m_s);
-
-				updated = true;
-			}
-
-			if (vehicle_gps_position.fix_type >= 2) {
-				msg.latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));
-				msg.longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));
-
-				// altitude_geodetic
-				if (vehicle_gps_position.fix_type >= 3) {
-					msg.altitude_geodetic = static_cast<float>(round(vehicle_gps_position.altitude_msl_m)); // [m]
-				}
-
-				msg.horizontal_accuracy = open_drone_id_translations::odidHorAccForEph(vehicle_gps_position.eph);
-
-				msg.vertical_accuracy = open_drone_id_translations::odidVerAccForEpv(vehicle_gps_position.epv);
-
-				updated = true;
-			}
-
-			if (vehicle_gps_position.time_utc_usec != 0) {
-				// timestamp: UTC then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000
-				uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000;
-				msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000;
-
-				msg.timestamp_accuracy = open_drone_id_translations::odidTimeForElapsed(hrt_elapsed_time(
-								 &vehicle_gps_position.timestamp));
-
-				updated = true;
-			}
-		}
-	}
-
-	// altitude_barometric: The altitude calculated from the barometric pressue
-	if (_vehicle_air_data_sub.advertised()) {
-		vehicle_air_data_s vehicle_air_data{};
-
-		if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) {
-			msg.altitude_barometric = vehicle_air_data.baro_alt_meter;
-			msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; // We just don't without calibration.
-			updated = true;
-		}
-	}
-
-	// height: The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference
-	if (_home_position_sub.advertised() && _vehicle_local_position_sub.updated()) {
-		home_position_s home_position{};
-		vehicle_local_position_s vehicle_local_position{};
-
-		if (_home_position_sub.copy(&home_position)
-		    && _vehicle_local_position_sub.copy(&vehicle_local_position)
-		    && (hrt_elapsed_time(&vehicle_local_position.timestamp) < 1_s)
-		   ) {
-
-			if (home_position.valid_alt && vehicle_local_position.z_valid && vehicle_local_position.z_global) {
-				float altitude = (-vehicle_local_position.z + vehicle_local_position.ref_alt);
-
-				msg.height = altitude - home_position.alt;
-				msg.height_reference = MAV_ODID_HEIGHT_REF_OVER_TAKEOFF;
-				updated = true;
-			}
-		}
-	}
-
-	if (updated) {
-		_uavcan_pub_remoteid_location.broadcast(msg);
-	}
-}
-
-void UavcanRemoteIDController::send_system()
-{
-	open_drone_id_system_s system;
-
-	if (_open_drone_id_system.advertised() && _open_drone_id_system.copy(&system)) {
-
-		// Use what ground station sends us.
-
-		dronecan::remoteid::System msg {};
-		msg.timestamp = system.timestamp;
-
-		for (unsigned i = 0; i < sizeof(system.id_or_mac); ++i) {
-			msg.id_or_mac.push_back(system.id_or_mac[i]);
-		}
-
-		msg.operator_location_type = system.operator_location_type;
-		msg.classification_type = system.classification_type;
-		msg.operator_latitude = system.operator_latitude;
-		msg.operator_longitude = system.operator_longitude;
-		msg.area_count = system.area_count;
-		msg.area_radius = system.area_radius;
-		msg.area_ceiling = system.area_ceiling;
-		msg.area_floor = system.area_floor;
-		msg.category_eu = system.category_eu;
-		msg.class_eu = system.class_eu;
-		msg.operator_altitude_geo = system.operator_altitude_geo;
-
-		_uavcan_pub_remoteid_system.broadcast(msg);
-
-	} else {
-		// And otherwise, send our home/takeoff location.
-
-		sensor_gps_s vehicle_gps_position;
-		home_position_s home_position;
-
-		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position) && _home_position_sub.copy(&home_position)) {
-			if (vehicle_gps_position.fix_type >= 3
-			    && home_position.valid_alt && home_position.valid_hpos) {
-
-				dronecan::remoteid::System msg {};
-
-				// msg.id_or_mac // Only used for drone ID data received from other UAs.
-				msg.operator_location_type = MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF;
-				msg.classification_type = MAV_ODID_CLASSIFICATION_TYPE_UNDECLARED;
-				msg.operator_latitude = home_position.lat * 1e7;
-				msg.operator_longitude = home_position.lon * 1e7;
-				msg.area_count = 1;
-				msg.area_radius = 0;
-				msg.area_ceiling = -1000;
-				msg.area_floor = -1000;
-				msg.category_eu = MAV_ODID_CATEGORY_EU_UNDECLARED;
-				msg.class_eu = MAV_ODID_CLASS_EU_UNDECLARED;
-				float wgs84_amsl_offset = vehicle_gps_position.altitude_ellipsoid_m - vehicle_gps_position.altitude_msl_m;
-				msg.operator_altitude_geo = home_position.alt + wgs84_amsl_offset;
-
-				// timestamp: 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.
-				static uint64_t utc_offset_s = 1'546'300'800; // UTC seconds since 00:00:00 01/01/2019
-				msg.timestamp = vehicle_gps_position.time_utc_usec / 1e6 - utc_offset_s;
-
-				_uavcan_pub_remoteid_system.broadcast(msg);
-			}
-		}
-	}
-}
-
-void UavcanRemoteIDController::send_self_id()
-{
-	open_drone_id_self_id_s self_id;
-
-	if (_open_drone_id_self_id.copy(&self_id)) {
-
-		dronecan::remoteid::SelfID msg {};
-
-		for (unsigned i = 0; i < sizeof(self_id.id_or_mac); ++i) {
-			msg.id_or_mac.push_back(self_id.id_or_mac[i]);
-		}
-
-		msg.description_type = self_id.description_type;
-
-		for (unsigned i = 0; i < sizeof(self_id.description); ++i) {
-			msg.description.push_back(self_id.description[i]);
-		}
-
-		_uavcan_pub_remoteid_self_id.broadcast(msg);
-	}
-}
-
-void UavcanRemoteIDController::send_operator_id()
-{
-	open_drone_id_operator_id_s operator_id;
-
-	if (_open_drone_id_operator_id.copy(&operator_id)) {
-
-		dronecan::remoteid::OperatorID msg {};
-
-		for (unsigned i = 0; i < sizeof(operator_id.id_or_mac); ++i) {
-			msg.id_or_mac.push_back(operator_id.id_or_mac[i]);
-		}
-
-		msg.operator_id_type = operator_id.operator_id_type;
-
-		for (unsigned i = 0; i < sizeof(operator_id.operator_id); ++i) {
-			msg.operator_id.push_back(operator_id.operator_id[i]);
-		}
-
-		_uavcan_pub_remoteid_operator_id.broadcast(msg);
-	}
-}
-
-void
-UavcanRemoteIDController::arm_status_sub_cb(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &msg)
-{
-	open_drone_id_arm_status_s arm_status{};
-
-	arm_status.timestamp = hrt_absolute_time();
-	arm_status.status = msg.status;
-	memcpy(arm_status.error, msg.error.c_str(), sizeof(arm_status.error));
-
-	_open_drone_id_arm_status_pub.publish(arm_status);
-}
@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2024 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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <uORB/Subscription.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/topics/sensor_gps.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/vehicle_air_data.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/open_drone_id_operator_id.h>
-#include <uORB/topics/open_drone_id_arm_status.h>
-#include <uORB/topics/open_drone_id_self_id.h>
-#include <uORB/topics/open_drone_id_system.h>
-
-#include <uavcan/uavcan.hpp>
-#include <dronecan/remoteid/BasicID.hpp>
-#include <dronecan/remoteid/Location.hpp>
-#include <dronecan/remoteid/SelfID.hpp>
-#include <dronecan/remoteid/System.hpp>
-#include <dronecan/remoteid/ArmStatus.hpp>
-#include <dronecan/remoteid/OperatorID.hpp>
-
-#include <px4_platform_common/module_params.h>
-
-class UavcanRemoteIDController : public ModuleParams
-{
-public:
-	UavcanRemoteIDController(uavcan::INode &node);
-	~UavcanRemoteIDController() = default;
-
-	int init();
-
-private:
-	typedef uavcan::MethodBinder<UavcanRemoteIDController *, void (UavcanRemoteIDController::*)(const uavcan::TimerEvent &)>
-	TimerCbBinder;
-
-	static constexpr unsigned MAX_RATE_HZ = 1;
-	uavcan::TimerEventForwarder<TimerCbBinder> _timer;
-
-	void periodic_update(const uavcan::TimerEvent &);
-
-	void send_basic_id();
-	void send_location();
-	void send_self_id();
-	void send_system();
-	void send_operator_id();
-
-	void arm_status_sub_cb(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &msg);
-
-	uavcan::INode &_node;
-
-	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
-	uORB::SubscriptionData<vehicle_status_s> _vehicle_status{ORB_ID(vehicle_status)};
-	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
-	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
-	uORB::Subscription _open_drone_id_operator_id{ORB_ID(open_drone_id_operator_id)};
-	uORB::Subscription _open_drone_id_self_id{ORB_ID(open_drone_id_self_id)};
-	uORB::Subscription _open_drone_id_system{ORB_ID(open_drone_id_system)};
-	uORB::Publication<open_drone_id_arm_status_s> _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)};
-
-	uavcan::Publisher<dronecan::remoteid::BasicID> _uavcan_pub_remoteid_basicid;
-	uavcan::Publisher<dronecan::remoteid::Location> _uavcan_pub_remoteid_location;
-	uavcan::Publisher<dronecan::remoteid::SelfID> _uavcan_pub_remoteid_self_id;
-	uavcan::Publisher<dronecan::remoteid::System> _uavcan_pub_remoteid_system;
-	uavcan::Publisher<dronecan::remoteid::OperatorID> _uavcan_pub_remoteid_operator_id;
-
-	using ArmStatusBinder = uavcan::MethodBinder < UavcanRemoteIDController *,
-	      void (UavcanRemoteIDController::*)(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &) >;
-
-	uavcan::Subscriber<dronecan::remoteid::ArmStatus, ArmStatusBinder> _uavcan_sub_arm_status;
-};
@@ -51,7 +51,7 @@ public:
 	int init();

 private:
-	// Max update rate to avoid excessive bus traffic
+	// Max update rate to avoid exessive bus traffic
 	static constexpr unsigned MAX_RATE_HZ = 20;

 	void periodic_update(const uavcan::TimerEvent &);
@@ -96,9 +96,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 #if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER)
 	_safety_state_controller(_node),
 #endif
-#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
-	_remoteid_controller(_node),
-#endif
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 	_rgbled_controller(_node),
 #endif
@@ -561,16 +558,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		return ret;
 	}

-#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
-	ret = _remoteid_controller.init();
-
-	if (ret < 0) {
-		return ret;
-	}
-
-#endif
-
-
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 	ret = _rgbled_controller.init();

@@ -68,10 +68,6 @@

 #include "logmessage.hpp"

-#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
-#include "remoteid.hpp"
-#endif
-
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 #include "rgbled.hpp"
 #endif
@@ -273,9 +269,6 @@ private:
 #if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER)
 	UavcanSafetyState         	_safety_state_controller;
 #endif
-#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
-	UavcanRemoteIDController _remoteid_controller;
-#endif
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 	UavcanRGBController             _rgbled_controller;
 #endif
@@ -106,11 +106,6 @@ void Battery::updateCurrent(const float current_a)
 	_current_a = current_a;
 }

-void Battery::updateTemperature(const float temperature_c)
-{
-	_temperature_c = temperature_c;
-}
-
 void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 {
 	// Require minimum voltage otherwise override connected status
@@ -154,7 +149,7 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.remaining = _state_of_charge;
 	battery_status.scale = _scale;
 	battery_status.time_remaining_s = computeRemainingTime(_current_a);
-	battery_status.temperature = _temperature_c;
+	battery_status.temperature = NAN;
 	battery_status.cell_count = _params.n_cells;
 	battery_status.connected = _connected;
 	battery_status.source = _source;
@@ -91,7 +91,6 @@ public:
 	void setStateOfCharge(const float soc) { _state_of_charge = soc; _external_state_of_charge = true; }
 	void updateVoltage(const float voltage_v);
 	void updateCurrent(const float current_a);
-	void updateTemperature(const float temperature_c);

 	/**
 	 * Update state of charge calculations
@@ -169,7 +168,6 @@ private:
 	float _current_a{-1};
 	AlphaFilter<float>
 	_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
-	float _temperature_c{NAN};
 	float _discharged_mah{0.f};
 	float _discharged_mah_loop{0.f};
 	float _state_of_charge_volt_based{-1.f}; // [0,1]
@@ -39,8 +39,7 @@ PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_or
 {
 	set_device_id(device_id);
 	set_orientation(device_orientation);
-	set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
-	set_mode(distance_sensor_s::MODE_UNKNOWN);
+	set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); // Default to type LASER
 }

 PX4Rangefinder::~PX4Rangefinder()
@@ -60,8 +60,6 @@ public:

 	void set_orientation(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);

-	void set_mode(const uint8_t mode) { _distance_sensor_pub.get().mode = mode; }
-
 	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);

 	int get_instance() { return _distance_sensor_pub.get_instance(); };
@@ -447,12 +447,12 @@
                            "description": "Transition to standby"
                        },
                        "1": {
-                            "name": "stick_gesture",
-                            "description": "Stick gesture"
+                            "name": "rc_stick",
+                            "description": "RC"
                        },
                        "2": {
                            "name": "rc_switch",
-                            "description": "RC switch"
+                            "description": "RC (switch)"
                        },
                        "3": {
                            "name": "command_internal",
@@ -116,39 +116,6 @@ public:
 		return self;
 	}

-	template<size_t MM, size_t NN>
-	Matrix<Type, P, Q> operator-(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
-	{
-		return Matrix<Type, P, Q> {*this} - other;
-	}
-
-
-	Matrix<Type, P, Q> operator-(const Matrix<Type, P, Q> &other)
-	{
-		return Matrix<Type, P, Q> {*this} - other;
-	}
-
-	Matrix<Type, P, Q> operator-(const Type &other)
-	{
-		return Matrix<Type, P, Q> {*this} - other;
-	}
-
-	template<size_t MM, size_t NN>
-	Matrix<Type, P, Q> operator+(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
-	{
-		return Matrix<Type, P, Q> {*this} + other;
-	}
-
-	Matrix<Type, P, Q> operator+(const Matrix<Type, P, Q> &other)
-	{
-		return Matrix<Type, P, Q> {*this} + other;
-	}
-
-	Matrix<Type, P, Q> operator+(const Type &other)
-	{
-		return Matrix<Type, P, Q> {*this} + other;
-	}
-
 	// allow assigning vectors to a slice that are in the axis
 	template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
 	SliceT<MatrixT, Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
@@ -255,19 +222,29 @@ public:
 		return self;
 	}

-	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &scalar)
+	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &other)
 	{
-		return operator*=(Type(1) / scalar);
+		return operator*=(Type(1) / other);
 	}

-	Matrix<Type, P, Q> operator*(const Type &scalar) const
+	Matrix<Type, P, Q> operator*(const Type &other) const
 	{
-		return Matrix<Type, P, Q> {*this} * scalar;
+		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
+		Matrix<Type, P, Q> res;
+
+		for (size_t i = 0; i < P; i++) {
+			for (size_t j = 0; j < Q; j++) {
+				res(i, j) = self(i, j) * other;
+			}
+		}
+
+		return res;
 	}

-	Matrix<Type, P, Q> operator/(const Type &scalar) const
+	Matrix<Type, P, Q> operator/(const Type &other) const
 	{
-		return (*this) * (1 / scalar);
+		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
+		return self * (Type(1) / other);
 	}

 	template<size_t R, size_t S>
@@ -317,7 +317,6 @@ public:
 	}
 };

-using SquareMatrix2f = SquareMatrix<float, 2>;
 using SquareMatrix3f = SquareMatrix<float, 3>;
 using SquareMatrix3d = SquareMatrix<double, 3>;

@@ -102,6 +102,7 @@ public:

 };

+
 using Vector2f = Vector2<float>;
 using Vector2d = Vector2<double>;

@@ -262,79 +262,6 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
-TEST(MatrixSliceTest, SliceAdditions)
-{
-	float data[9] = {0, 2, 3,
-			 4, 5, 6,
-			 7, 8, 10
-			};
-	SquareMatrix3f A{data};
-
-	float operand_data [4] = {2, 1,
-				  -3, -1
-				 };
-	const SquareMatrix2f operand(operand_data);
-
-	// 2x2 Slice + 2x2 Matrix
-	SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) + operand;
-	float res_1_check_data[4] = {6, 6,
-				     4, 7
-				    };
-	EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
-
-	// 2x1 Slice + 2x1 Slice
-	Vector2f res_2 = A.slice<2, 1>(1, 1) + operand.slice<2, 1>(0, 0);
-	EXPECT_EQ(res_2, Vector2f(7, 5));
-
-	// 3x3 Slice + Scalar
-	SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) + (-1);
-	float res_3_check_data[9] = {-1, 1, 2,
-				     3, 4, 5,
-				     6, 7, 9
-				    };
-	EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
-
-	// 3x1 Slice + 3 Vector
-	Vector3f res_4 = A.col(1) + Vector3f(1, -2, 3);
-	EXPECT_EQ(res_4, Vector3f(3, 3, 11));
-
-}
-TEST(MatrixSliceTest, SliceSubtractions)
-{
-	float data[9] = {0, 2, 3,
-			 4, 5, 6,
-			 7, 8, 10
-			};
-	SquareMatrix3f A{data};
-
-	float operand_data[4] = {2, 1,
-				 -3, -1
-				};
-	const SquareMatrix2f operand(operand_data);
-
-	// 2x2 Slice - 2x2 Matrix
-	SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) - operand;
-	float res_1_check_data[4] = {2, 4,
-				     10, 9
-				    };
-	EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
-
-	// 2x1 Slice - 2x1 Slice
-	Vector2f res_2 = A.slice<2, 1>(1, 1) - operand.slice<2, 1>(0, 0);
-	EXPECT_EQ(res_2, Vector2f(3, 11));
-
-	// 3x3 Slice - Scalar
-	SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) - (-1);
-	float res_3_check_data[9] = {1, 3, 4,
-				     5, 6, 7,
-				     8, 9, 11
-				    };
-	EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
-
-	// 3x1 Slice - 3 Vector
-	Vector3f res_4 = A.col(1) - Vector3f(1, -2, 3);
-	EXPECT_EQ(res_4, Vector3f(1, 7, 5));
-}

 TEST(MatrixSliceTest, XYAssignmentTest)
 {
@@ -80,13 +80,4 @@ TEST(MatrixVector3Test, Vector3)
 	Vector3f m2(3.1f, 4.1f, 5.1f);
 	EXPECT_EQ(m2, m1 + 2.1f);
 	EXPECT_EQ(m2 - 2.1f, m1);
-
-	// Test Addition and Subtraction of Slices
-	Vector3f v1(3, 13, 0);
-	Vector3f v2(42, 6, 256);
-
-	EXPECT_EQ(v1.xy() - v2.xy(), Vector2f(-39, 7));
-	EXPECT_EQ(v1.xy() + v2.xy(), Vector2f(45, 19));
-	EXPECT_EQ(v1.xy() + 2.f, Vector2f(5, 15));
-	EXPECT_EQ(v1.xy() - 2.f, Vector2f(1, 11));
 }
@@ -43,4 +43,3 @@ target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

 px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS motion_planning)
 px4_add_unit_gtest(SRC PositionSmoothingTest.cpp LINKLIBS motion_planning)
-px4_add_unit_gtest(SRC HeadingSmoothingTest.cpp LINKLIBS motion_planning)
@@ -35,7 +35,7 @@

 HeadingSmoothing::HeadingSmoothing()
 {
-	_velocity_smoothing.setMaxVel(M_TWOPI_F); // "velocity" is heading. 2Pi limit is needed for correct angle wrapping
+	_velocity_smoothing.setMaxVel(M_PI_F); // smoothed "velocity" is heading [-pi, pi]
 }

 void HeadingSmoothing::reset(const float heading, const float heading_rate)
@@ -1,147 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2024 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.
- *
- ****************************************************************************/
-
-/**
- * Test code for the Heading Smoothing library
- * Run exclusively this test with the command "make tests TESTFILTER=HeadingSmoothing"
- */
-
-#include "mathlib/math/Limits.hpp"
-#include <gtest/gtest.h>
-
-#include <motion_planning/HeadingSmoothing.hpp>
-
-using namespace matrix;
-
-class HeadingSmoothingTest : public ::testing::Test
-{
-public:
-	HeadingSmoothingTest()
-	{
-		_smoothing.setMaxHeadingRate(15.f);
-		_smoothing.setMaxHeadingAccel(1.f);
-	}
-
-	HeadingSmoothing _smoothing;
-	float _dt{.1f};
-};
-
-TEST_F(HeadingSmoothingTest, convergence)
-{
-	const float heading_current = math::radians(0.f);
-	const float heading_target = math::radians(5.f);
-	_smoothing.reset(heading_current, 0.f);
-
-	const int nb_steps = ceilf(1.f / _dt);
-
-	for (int i = 0; i < nb_steps; i++) {
-		_smoothing.update(heading_target, _dt);
-	}
-
-	const float heading = _smoothing.getSmoothedHeading();
-	const float heading_rate = _smoothing.getSmoothedHeadingRate();
-	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
-	EXPECT_EQ(heading_rate, 0.f);
-}
-
-TEST_F(HeadingSmoothingTest, zero_crossing)
-{
-	const float heading_current = math::radians(-95.f);
-	const float heading_target = math::radians(5.f);
-	_smoothing.reset(heading_current, 0.f);
-
-	const int nb_steps = ceilf(4.f / _dt);
-
-	for (int i = 0; i < nb_steps; i++) {
-		_smoothing.update(heading_target, _dt);
-	}
-
-	const float heading = _smoothing.getSmoothedHeading();
-	const float heading_rate = _smoothing.getSmoothedHeadingRate();
-	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
-	EXPECT_EQ(heading_rate, 0.f);
-}
-
-TEST_F(HeadingSmoothingTest, wrap_pi)
-{
-	const float heading_current = math::radians(-170.f);
-	const float heading_target = math::radians(170.f);
-	_smoothing.reset(heading_current, 0.f);
-
-	const int nb_steps = ceilf(2.f / _dt);
-
-	for (int i = 0; i < nb_steps; i++) {
-		_smoothing.update(heading_target, _dt);
-	}
-
-	const float heading = _smoothing.getSmoothedHeading();
-	const float heading_rate = _smoothing.getSmoothedHeadingRate();
-	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
-	EXPECT_EQ(heading_rate, 0.f);
-}
-
-TEST_F(HeadingSmoothingTest, positive_rate)
-{
-	const float max_heading_rate = .15f;
-	_smoothing.setMaxHeadingRate(max_heading_rate);
-	const float heading_current = math::radians(-170.f);
-	const float heading_target = math::radians(-20.f);
-	_smoothing.reset(heading_current, 0.f);
-
-	const int nb_steps = ceilf(2.f / _dt);
-
-	for (int i = 0; i < nb_steps; i++) {
-		_smoothing.update(heading_target, _dt);
-	}
-
-	const float heading_rate = _smoothing.getSmoothedHeadingRate();
-	EXPECT_EQ(heading_rate, max_heading_rate);
-}
-
-TEST_F(HeadingSmoothingTest, negative_rate)
-{
-	const float max_heading_rate = 0.15;
-	_smoothing.setMaxHeadingRate(max_heading_rate);
-	const float heading_current = math::radians(-20.f);
-	const float heading_target = math::radians(-140.f);
-	_smoothing.reset(heading_current, 0.f);
-
-	const int nb_steps = ceilf(2.f / _dt);
-
-	for (int i = 0; i < nb_steps; i++) {
-		_smoothing.update(heading_target, _dt);
-	}
-
-	const float heading_rate = _smoothing.getSmoothedHeadingRate();
-	EXPECT_EQ(heading_rate, -max_heading_rate);
-}
@@ -78,16 +78,18 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2

 	const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned;
 	const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero();
-	_distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u;
-	_curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos;
+	const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) *
+			prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
+	const Vector2f curr_pos_to_path = distance_on_line_segment -
+					  prev_wp_to_curr_pos; // Shortest vector from the current position to the path

-	if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
-		return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0));
+	if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
+		return atan2f(curr_pos_to_path(1), curr_pos_to_path(0));
 	}

-	const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(),
-					  2.f)); // Length of the vector from the endpoint of _distance_on_line_segment to the intersection point
-	const Vector2f prev_wp_to_intersection_point = _distance_on_line_segment + line_extension *
+	const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(),
+					  2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point
+	const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension *
 			prev_wp_to_curr_wp_u;
 	const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos;
 	return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0));
@@ -103,8 +103,6 @@ public:
 				 float vehicle_speed);

 	float getLookaheadDistance() {return _lookahead_distance;};
-	float getCrosstrackError() {return _curr_pos_to_path.norm();};
-	float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();};

 protected:
 	/**
@@ -124,7 +122,5 @@ protected:
 		float lookahead_min{1.f};
 	} _params{};
 private:
-	float _lookahead_distance{0.f}; // Radius of the circle around the vehicle
-	Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
-	Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path
+	float _lookahead_distance{0.f};
 };
@@ -44,11 +44,11 @@

 #include "matrix/Matrix.hpp"
 #include "matrix/Vector2.hpp"
-#include <mathlib/math/Functions.hpp>

 using math::constrain;
 using math::max;
 using math::min;
+using namespace time_literals;

 static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);}

@@ -525,12 +525,16 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec

 	if (1.f - param.fast_descend < FLT_EPSILON) {
 		// During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending
-		throttle_setpoint = param.throttle_min;
+		if (specific_energy_rates.spe_rate.estimate > 0) { // We have a positive altitude rate and are stil climbing
+			throttle_setpoint = param.throttle_trim; // Do not cut off throttle yet
+
+		} else {
+			throttle_setpoint = param.throttle_min;
+		}

 	} else {
 		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
-		throttle_setpoint = (1.f - param.fast_descend) * _calcThrottleControlOutput(limit, ste_rate, param,
-				    flag) + param.fast_descend * param.throttle_min;
+		throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
 	}

 	// Rate limit the throttle demand
@@ -673,11 +677,6 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa
 	_control_param.throttle_min = throttle_min;
 }

-float TECS::calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint)
-{
-	return lerp(eas_to_tas * eas_setpoint, _control_param.tas_max, _fast_descend);
-}
-
 void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
 		      float eas_to_tas)
 {
@@ -700,9 +699,6 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 						.tas_rate = 0.0f};

 	_control.initialize(control_setpoint, control_input, _control_param, _control_flag);
-
-	_fast_descend = 0.f;
-	_enabled_fast_descend_timestamp = 0U;
 }

 void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
@@ -757,7 +753,8 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		TECSControl::Setpoint control_setpoint;
 		control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
 		control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect();
-		control_setpoint.tas_setpoint = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint);
+		control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas *
+						EAS_setpoint;

 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
@@ -768,13 +765,11 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	}

 	_debug_status.control = _control.getDebugOutput();
-	_debug_status.true_airspeed_sp = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint);
 	_debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed;
 	_debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate;
 	_debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt;
 	_debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate;
 	_debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect();
-	_debug_status.fast_descend = _fast_descend;

 	_update_timestamp = now;
 }
@@ -783,22 +778,13 @@ void TECS::_setFastDescend(const float alt_setpoint, const float alt)
 {
 	if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON)
 	    && ((alt_setpoint + _fast_descend_alt_err) < alt)) {
-		auto now = hrt_absolute_time();
-
-		if (_enabled_fast_descend_timestamp == 0U) {
-			_enabled_fast_descend_timestamp = now;
-		}
-
-		_fast_descend = constrain(max(_fast_descend, static_cast<float>(now - _enabled_fast_descend_timestamp) /
-					      static_cast<float>(FAST_DESCEND_RAMP_UP_TIME)), 0.f, 1.f);
+		_fast_descend = 1.f;

 	} else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) {
 		// Were in fast descend, scale it down. up until 5m above target altitude
 		_fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f);
-		_enabled_fast_descend_timestamp = 0U;

 	} else {
 		_fast_descend = 0.f;
-		_enabled_fast_descend_timestamp = 0U;
 	}
 }
@@ -50,8 +50,6 @@
 #include <motion_planning/VelocitySmoothing.hpp>
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>

-using namespace time_literals;
-
 class TECSAirspeedFilter
 {
 public:
@@ -205,8 +203,8 @@ public:
 		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
 		float tas_min;				///< True airspeed demand lower limit [m/s].
 		float tas_max;				///< True airspeed demand upper limit [m/s].
-		float pitch_max;			///< Maximum pitch angle above trim allowed in [rad].
-		float pitch_min;			///< Minimal pitch angle below trim allowed in [rad].
+		float pitch_max;			///< Maximum pitch angle allowed in [rad].
+		float pitch_min;			///< Minimal pitch angle allowed in [rad].
 		float throttle_trim;		///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
 		float throttle_max;			///< Normalized throttle upper limit.
 		float throttle_min;			///< Normalized throttle lower limit.
@@ -322,7 +320,7 @@ public:
 	/**
 	 * @brief Get the pitch setpoint.
 	 *
-	 * @return The commanded pitch angle above trim in [rad].
+	 * @return THe commanded pitch angle in [rad].
 	 */
 	float getPitchSetpoint() const {return _pitch_setpoint;};
 	/**
@@ -480,7 +478,7 @@ private:
 	 * @param seb_rate is the specific energy balance rate in [m²/s³].
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
-	 * @return pitch setpoint angle above trim [rad].
+	 * @return pitch setpoint angle [rad].
 	 */
 	float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
 				      const Flag &flag) const;
@@ -539,7 +537,7 @@ private:

 	// Output
 	DebugOutput _debug_output;				///< Debug output.
-	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint above trim [rad].
+	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [rad].
 	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint [0,1].
 	float _ratio_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
 };
@@ -549,13 +547,11 @@ class TECS
 public:
 	struct DebugOutput {
 		TECSControl::DebugOutput control;
-		float true_airspeed_sp;
 		float true_airspeed_filtered;
 		float true_airspeed_derivative;
 		float altitude_reference;
 		float height_rate_reference;
 		float height_rate_direct;
-		float fast_descend;
 	};
 public:
 	TECS() = default;
@@ -662,17 +658,6 @@ private:
 	void initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max,
 			       float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim);

-	/**
-	 * @brief calculate true airspeed setpoint
-	 *
-	 * Calculate true airspeed setpoint based on input and fast descend ratio
-	 *
-	 * @param eas_to_tas is the eas to tas conversion factor
-	 * @param eas_setpoint is the desired equivalent airspeed setpoint [m/s]
-	 * @return true airspeed setpoint[m/s]
-	 */
-	float calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint);
-
 	/**
 	 * @brief Initialize the control loop
 	 *
@@ -690,11 +675,9 @@ private:
 	float _equivalent_airspeed_max{20.0f};				///< equivalent airspeed demand upper limit (m/sec)
 	float _fast_descend_alt_err{-1.f};	 				///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m].
 	float _fast_descend{0.f};					///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude.
-	hrt_abstime _enabled_fast_descend_timestamp{0U};		///< timestamp at activation of fast descend mode

 	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
 	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)
-	static constexpr hrt_abstime FAST_DESCEND_RAMP_UP_TIME = 2_s; 	///< Ramp up time until fast descend is fully engaged

 	DebugOutput _debug_status{};

@@ -40,7 +40,7 @@ header = """
 if args.git_tag:
    git_tag = args.git_tag
 else:
-    git_describe_cmd = 'git describe --exclude ext/* --always --tags --dirty'
+    git_describe_cmd = 'git describe --exclude ext/* --match v[0-9]*.[0-9]*.[0-9]* --tags --dirty'
    git_tag = subprocess.check_output(git_describe_cmd.split(),
                                  stderr=subprocess.STDOUT).decode('utf-8').strip()

@@ -503,9 +503,9 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
 	switch (calling_reason) {
 	case arm_disarm_reason_t::transition_to_standby: return "";

-	case arm_disarm_reason_t::stick_gesture: return "Stick gesture";
+	case arm_disarm_reason_t::rc_stick: return "RC";

-	case arm_disarm_reason_t::rc_switch: return "RC switch";
+	case arm_disarm_reason_t::rc_switch: return "RC (switch)";

 	case arm_disarm_reason_t::command_internal: return "internal command";

@@ -583,7 +583,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 				return TRANSITION_DENIED;
 			}

-		} else if (calling_reason == arm_disarm_reason_t::stick_gesture
+		} else if (calling_reason == arm_disarm_reason_t::rc_stick
 			   || calling_reason == arm_disarm_reason_t::rc_switch
 			   || calling_reason == arm_disarm_reason_t::rc_button) {

@@ -634,12 +634,12 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 		const bool mc_manual_thrust_mode = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 						   && _vehicle_control_mode.flag_control_manual_enabled
 						   && !_vehicle_control_mode.flag_control_climb_rate_enabled;
-		const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::stick_gesture)
+		const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::rc_stick)
 					     || (calling_reason == arm_disarm_reason_t::rc_switch)
 					     || (calling_reason == arm_disarm_reason_t::rc_button);

 		if (!landed && !(mc_manual_thrust_mode && commanded_by_rc && _param_com_disarm_man.get())) {
-			if (calling_reason != arm_disarm_reason_t::stick_gesture) {
+			if (calling_reason != arm_disarm_reason_t::rc_stick) {
 				mavlink_log_critical(&_mavlink_log_pub, "Disarming denied: not landed\t");
 				events::send(events::ID("commander_disarm_denied_not_landed"),
 				{events::Log::Critical, events::LogInternal::Info},
@@ -887,14 +887,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			}

 			if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
-
-				// Special handling for LAND mode: always allow to switch into it such that if used
-				// as emergency mode it is always available. When triggering it the user generally wants
-				// the vehicle to descend immediately, and if that means to switch to DESCEND it is fine.
-
-				const bool force = desired_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-
-				if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd), false, force)) {
+				if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
 					main_ret = TRANSITION_CHANGED;

 				} else {
@@ -1069,13 +1062,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;

 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
-			// Special handling for LAND mode: always allow to switch into it such that if used
-			// as emergency mode it is always available. When triggering it the user generally wants
-			// the vehicle to descend immediately, and if that means to switch to DESCEND it is fine.
-			const bool force = true;
-
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd), false,
-							force)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
 				mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
 				events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
 					     "Landing at current position");
@@ -1512,7 +1499,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
 	case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
 	case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
-	case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE:
 		/* ignore commands that are handled by other parts of the system */
 		break;

@@ -1629,7 +1615,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)

 	// Silently ignore RC actions during RC calibration
 	if (_vehicle_status.rc_calibration_in_progress
-	    && (action_request.source == action_request_s::SOURCE_STICK_GESTURE
+	    && (action_request.source == action_request_s::SOURCE_RC_STICK_GESTURE
 		|| action_request.source == action_request_s::SOURCE_RC_SWITCH
 		|| action_request.source == action_request_s::SOURCE_RC_BUTTON
 		|| action_request.source == action_request_s::SOURCE_RC_MODE_SLOT)) {
@@ -1637,7 +1623,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 	}

 	switch (action_request.source) {
-	case action_request_s::SOURCE_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::stick_gesture; break;
+	case action_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;

 	case action_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;

@@ -48,8 +48,7 @@ void DistanceSensorChecks::checkAndReport(const Context &context, Report &report

 		if (exists) {
 			distance_sensor_s dist_sens;
-			valid = _distance_sensor_sub[instance].copy(&dist_sens) && ((hrt_elapsed_time(&dist_sens.timestamp) < 1_s)
-					|| (dist_sens.mode == distance_sensor_s::MODE_DISABLED));
+			valid = _distance_sensor_sub[instance].copy(&dist_sens) && hrt_elapsed_time(&dist_sens.timestamp) < 1_s;
 			reporter.setIsPresent(health_component_t::distance_sensor);
 		}

@@ -273,27 +273,12 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 		}

 		if (!context.isArmed() && ekf_gps_check_fail) {
-			NavModes required_groups_gps;
-			events::Log log_level;
+			NavModes required_groups_gps = required_groups;
+			events::Log log_level = events::Log::Error;

-			switch (static_cast<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
-			default:
-
-			/* FALLTHROUGH */
-			case GnssArmingCheck::DenyArming:
-				required_groups_gps = required_groups;
-				log_level = events::Log::Error;
-				break;
-
-			case GnssArmingCheck::WarningOnly:
+			if (_param_com_arm_wo_gps.get()) {
 				required_groups_gps = NavModes::None; // optional
 				log_level = events::Log::Warning;
-				break;
-
-			case GnssArmingCheck::Disabled:
-				required_groups_gps = NavModes::None;
-				log_level = events::Log::Disabled;
-				break;
 			}

 			// Only report the first failure to avoid spamming
@@ -453,20 +438,11 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 			}

 			if (message && reporter.mavlink_log_pub()) {
-				switch (static_cast<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
-				default:
-
-				/* FALLTHROUGH */
-				case GnssArmingCheck::DenyArming:
+				if (!_param_com_arm_wo_gps.get()) {
 					mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
-					break;

-				case GnssArmingCheck::WarningOnly:
-					mavlink_log_warning(reporter.mavlink_log_pub(), message, "");
-					break;
-
-				case GnssArmingCheck::Disabled:
-					break;
+				} else {
+					mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
 				}
 			}
 		}
@@ -59,12 +59,6 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;

 private:
-	enum class GnssArmingCheck : uint8_t {
-		DenyArming = 0,
-		WarningOnly = 1,
-		Disabled = 2
-	};
-
 	void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
 				  NavModes required_groups);
 	void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
@@ -114,7 +108,7 @@ private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
 					(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
-					(ParamInt<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
+					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
 					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
@@ -225,14 +225,11 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
 PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);

 /**
- * GPS preflight check
- *
- * Measures taken when a check defined by EKF2_GPS_CHECK is failing.
+ * Allow arming without GPS
 *
 * @group Commander
- * @value 0 Deny arming
- * @value 1 Warning only
- * @value 2 Disabled
+ * @value 0 Require GPS lock to arm
+ * @value 1 Allow arming without GPS
 */
 PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);

@@ -288,7 +285,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 * @unit s
 * @min 0.0
 * @max 25.0
- * @decimal 1
+ * @decimal 3
 */
 PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);

@@ -338,14 +335,14 @@ PARAM_DEFINE_INT32(COM_QC_ACT, 0);
 * The offboard loss failsafe will only be entered after a timeout,
 * set by COM_OF_LOSS_T in seconds.
 *
- * @value 0 Position mode
- * @value 1 Altitude mode
- * @value 2 Stabilized
- * @value 3 Return mode
- * @value 4 Land mode
- * @value 5 Hold mode
- * @value 6 Terminate
- * @value 7 Disarm
+ * @value  0 Position mode
+ * @value  1 Altitude mode
+ * @value  2 Manual
+ * @value  3 Return mode
+ * @value  4 Land mode
+ * @value  5 Hold mode
+ * @value  6 Terminate
+ * @value  7 Disarm
 * @group Commander
 */
 PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
@@ -453,12 +450,16 @@ PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f);
 PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);

 /**
- * Position mode navigation loss response
+ * Position control navigation loss response.
 *
- * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control in manual Position mode.
+ * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control.
 *
- * @value 0 Altitude mode
- * @value 1 Land mode (descend)
+ * If Altitude/Manual is selected: assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
+ *
+ * If Land/Descend is selected: assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to Descend.
+ *
+ * @value 0 Altitude/Manual
+ * @value 1 Land/Descend
 *
 * @group Commander
 */
@@ -292,7 +292,7 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t
 		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 		break;

-	case offboard_loss_failsafe_mode::Stabilized:
+	case offboard_loss_failsafe_mode::Manual:
 		action = Action::FallbackStab;
 		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
 		break;
@@ -70,7 +70,7 @@ private:
 	enum class offboard_loss_failsafe_mode : int32_t {
 		Position_mode = 0,
 		Altitude_mode = 1,
-		Stabilized = 2,
+		Manual = 2,
 		Return_mode = 3,
 		Land_mode = 4,
 		Hold_mode = 5,
@@ -113,8 +113,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed

 				} else {
 					// Try to initialize using measurement
-					if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
-							sample.epv)) {
+					if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;
@@ -41,6 +41,10 @@

 #include "ekf.h"

+#if defined(CONFIG_EKF2_MAGNETOMETER)
+# include <lib/world_magnetic_model/geo_mag_declination.h>
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 #include <mathlib/mathlib.h>

 // GPS pre-flight check bit locations
@@ -59,27 +63,46 @@ void Ekf::collect_gps(const gnssSample &gps)
 {
 	if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
+		const double lat = gps.lat;
+		const double lon = gps.lon;
+
 		if (!_pos_ref.isInitialized()) {
-			setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
+			_pos_ref.initReference(lat, lon, gps.time_us);
+
+			// if we are already doing aiding, correct for the change in position since the EKF started navigating
+			if (isHorizontalAidingActive()) {
+				double est_lat;
+				double est_lon;
+				_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
+				_pos_ref.initReference(est_lat, est_lon, gps.time_us);
+			}
 		}

 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
 		if (!PX4_ISFINITE(_gps_alt_ref)) {
-			setAltOriginFromCurrentPos(gps.alt, gps.vacc);
+			_gps_alt_ref = gps.alt + _state.pos(2);
 		}

+		_NED_origin_initialised = true;
+
+		// save the horizontal and vertical position uncertainty of the origin
+		_gpos_origin_eph = gps.hacc;
+		_gpos_origin_epv = gps.vacc;
+
 		_information_events.flags.gps_checks_passed = true;
+		ECL_INFO("GPS checks passed");
+	}

-		ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",
-			 _pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());
-
-	} else {
-		// a rough 2D fix is sufficient to lookup earth spin rate
+	if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) {
+		// a rough 2D fix is sufficient to lookup declination
 		const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);

 		if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {
+			updateWmm(gps.lat, gps.lon);
 			_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
 		}
+
+		_wmm_gps_time_last_checked = _time_delayed_us;
 	}
 }

@@ -39,8 +39,6 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>

-#include <lib/world_magnetic_model/geo_mag_declination.h>
-
 #include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>

 void Ekf::controlMagFusion(const imuSample &imu_sample)
@@ -68,9 +66,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			_control_status.flags.mag_fault = false;

 			_state.mag_B.zero();
-			resetMagBiasCov();
-
-			stopMagFusion();
+			resetMagCov();

 			_mag_lpf.reset(mag_sample.mag);
 			_mag_counter = 1;
@@ -80,43 +76,15 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			_mag_counter++;
 		}

-		// check for WMM update periodically or if global origin has changed
-		bool wmm_updated = false;
-
-		if (global_origin().isInitialized()) {
-
-			bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse);
-
-			if (global_origin_valid()
-			    && (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
-			   ) {
-				// position of local NED origin in GPS / WGS84 frame
-				double latitude_deg;
-				double longitude_deg;
-				global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg);
-
-				if (updateWorldMagneticModel(latitude_deg, longitude_deg)) {
-					wmm_updated = true;
-				}
-
-				_wmm_mag_time_last_checked = _time_delayed_us;
-
-			} else if (origin_newer_than_last_mag) {
-				// use global origin to update WMM
-				if (updateWorldMagneticModel(global_origin().getProjectionReferenceLat(),
-							     global_origin().getProjectionReferenceLon())
-				   ) {
-					wmm_updated = true;
-				}
-			}
-		}
-
 		// 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)
-		    && (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
+		    && (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
 		   ) {
-			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);
+			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
+							* Vector3f(_mag_strength_gps, 0, 0);
+
+			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);

 			_control_status.flags.synthetic_mag_z = true;

@@ -172,6 +140,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)

 		checkMagHeadingConsistency(mag_sample);

+		// WMM update can occur on the last epoch, just after mag fusion
+		const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse);
 		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;


@@ -205,14 +175,14 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)

 		// if we are using 3-axis magnetometer fusion, but without external NE aiding,
 		// then the declination must be fused as an observation to prevent long term heading drift
-		const bool no_ne_aiding_or_not_moving = !using_ne_aiding || _control_status.flags.vehicle_at_rest;
-		_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_not_moving;
+		const bool no_ne_aiding_or_pre_takeoff = !using_ne_aiding || !_control_status.flags.in_air;
+		_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_pre_takeoff;

 		if (_control_status.flags.mag) {

 			if (continuing_conditions_passing && _control_status.flags.yaw_align) {

-				if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) {
+				if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
 					aid_src.time_last_fuse = imu_sample.time_us;
@@ -233,35 +203,14 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 					}

 					if (_control_status.flags.mag_dec) {
-
-						// observation variance (rad**2)
-						const float R_DECL = sq(0.5f);
-
-						if ((_params.mag_declination_source & 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);
-
-						} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
-							   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
-							  ) {
-							// using previously saved declination
-							fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states);
-
-						} else {
-							// if there is no aiding coming from an inertial frame we need to fuse some declination
-							// even if we don't know the value, it's better to fuse 0 than nothing
-							float declination_rad = 0.f;
-							fuseDeclination(declination_rad, R_DECL);
-						}
+						fuseDeclination(0.5f);
 					}
 				}

 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);

 				if (is_fusion_failing) {
-					if (no_ne_aiding_or_not_moving) {
+					if (no_ne_aiding_or_pre_takeoff) {
 						ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 						resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
 						aid_src.time_last_fuse = imu_sample.time_us;
@@ -322,9 +271,6 @@ void Ekf::stopMagFusion()
 	if (_control_status.flags.mag) {
 		ECL_INFO("stopping mag fusion");

-		resetMagEarthCov();
-		resetMagBiasCov();
-
 		if (_control_status.flags.yaw_align && (_control_status.flags.mag_3D || _control_status.flags.mag_hdg)) {
 			// reset yaw alignment from mag unless using GNSS aiding
 			const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
@@ -383,36 +329,29 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	const Vector3f mag_I_before_reset = _state.mag_I;
 	const Vector3f mag_B_before_reset = _state.mag_B;

-	static constexpr float kMagEarthMinGauss = 0.01f; // minimum difference in mag earth field strength for reset (Gauss)
+	// reset covariances to default
+	resetMagCov();

 	// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
-	if (_wmm_earth_field_gauss.longerThan(0.f) && _wmm_earth_field_gauss.isAllFinite()) {
+	if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
+
 		// use expected earth field to reset states
+		const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
+						* Vector3f(_mag_strength_gps, 0, 0);

-		// mag_I: reset, skipped if negligible change in state
-		const Vector3f mag_I = _wmm_earth_field_gauss;
-		bool mag_I_reset = false;
-
-		if ((_state.mag_I - mag_I).longerThan(kMagEarthMinGauss)) {
-			_state.mag_I = mag_I;
-			resetMagEarthCov();
-			mag_I_reset = true;
-		}
-
-		// mag_B: reset, skipped if mag_I didn't change
+		// mag_B: reset
 		if (!reset_heading && _control_status.flags.yaw_align) {
-			if (mag_I_reset) {
-				// mag_B: reset using WMM
-				const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-				_state.mag_B = mag - (R_to_body * _wmm_earth_field_gauss);
-				resetMagBiasCov();
-			} // otherwise keep existing mag_B state (!mag_I_reset)
+			// mag_B: reset using WMM
+			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+			_state.mag_B = mag - (R_to_body * mag_earth_pred);

 		} else {
 			_state.mag_B.zero();
-			resetMagBiasCov();
 		}

+		// mag_I: reset, skipped if no change in state and variance good
+		_state.mag_I = mag_earth_pred;
+
 		if (reset_heading) {
 			resetMagHeading(mag);
 		}
@@ -420,32 +359,34 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	} else {
 		// mag_B: reset
 		_state.mag_B.zero();
-		resetMagBiasCov();

-		// Use the magnetometer measurement to reset the heading
+		// Use the magnetometer measurement to reset the field states
 		if (reset_heading) {
 			resetMagHeading(mag);
 		}

-		// mag_I: use the last magnetometer measurement to reset the field states
-		const Vector3f mag_I = _R_to_earth * mag;
-
-		if ((_state.mag_I - mag_I).longerThan(kMagEarthMinGauss)) {
-			_state.mag_I = mag_I;
-			resetMagEarthCov();
-		}
+		// mag_I: use the last magnetometer measurements to reset the field states
+		_state.mag_I = _R_to_earth * mag;
 	}

-	if ((_state.mag_I - mag_I_before_reset).longerThan(0.f)) {
+	if (!mag_I_before_reset.longerThan(0.f)) {
+		ECL_INFO("initializing mag I [%.3f, %.3f, %.3f], mag B [%.3f, %.3f, %.3f]",
+			 (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
+			 (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
+			);
+
+	} else {
 		ECL_INFO("resetting mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
 			 (double)mag_I_before_reset(0), (double)mag_I_before_reset(1), (double)mag_I_before_reset(2),
-			 (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2));
-	}
+			 (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)
+			);

-	if ((_state.mag_B - mag_B_before_reset).longerThan(0.f)) {
-		ECL_INFO("resetting mag B [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
-			 (double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
-			 (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2));
+		if (mag_B_before_reset.longerThan(0.f) || _state.mag_B.longerThan(0.f)) {
+			ECL_INFO("resetting mag B [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
+				 (double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
+				 (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
+				);
+		}
 	}

 	// record the start time for the magnetic field alignment
@@ -510,8 +451,8 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 	_mag_strength = mag_sample.length();

 	if (_params.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 (PX4_ISFINITE(_mag_strength_gps)) {
+			if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
 				_control_status.flags.mag_field_disturbed = true;
 				is_check_failing = true;
 			}
@@ -534,9 +475,9 @@ bool Ekf::checkMagField(const Vector3f &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 (PX4_ISFINITE(_wmm_inclination_rad)) {
+		if (PX4_ISFINITE(_mag_inclination_gps)) {
 			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
-			const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);
+			const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps);

 			if (fabsf(inc_error_rad) > inc_tol_rad) {
 				_control_status.flags.mag_field_disturbed = true;
@@ -608,60 +549,13 @@ 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)
-		   && 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)
-		  ) {
-		// using saved mag declination
-		return math::radians(_params.mag_declination_deg);
-	}
-
-	// otherwise unavailable
-	return 0.f;
-}
-
-bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longitude_deg)
-{
-	// set the magnetic field data returned by the geo library using the current GPS position
-	const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg));
-	const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg));
-	const float strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg);
-
-	if (PX4_ISFINITE(declination_rad) && PX4_ISFINITE(inclination_rad) && PX4_ISFINITE(strength_gauss)) {
-
-		const bool declination_changed = (fabsf(declination_rad - _wmm_declination_rad) > math::radians(1.f));
-		const bool inclination_changed = (fabsf(inclination_rad - _wmm_inclination_rad) > math::radians(1.f));
-		const bool strength_changed = (fabsf(strength_gauss - _wmm_field_strength_gauss) > 0.01f);
-
-		if (!PX4_ISFINITE(_wmm_declination_rad)
-		    || !PX4_ISFINITE(_wmm_inclination_rad)
-		    || !PX4_ISFINITE(_wmm_field_strength_gauss)
-		    || !_wmm_earth_field_gauss.longerThan(0.f)
-		    || !_wmm_earth_field_gauss.isAllFinite()
-		    || declination_changed
-		    || inclination_changed
-		    || strength_changed
-		   ) {
-
-			ECL_DEBUG("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)",
-				  (double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad),
-				  (double)latitude_deg, (double)longitude_deg
-				 );
-
-			_wmm_declination_rad = declination_rad;
-			_wmm_inclination_rad = inclination_rad;
-			_wmm_field_strength_gauss = strength_gauss;
-
-			_wmm_earth_field_gauss = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(strength_gauss, 0, 0);
-
-			return true;
+	} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
+		// use parameter value until GPS is available, then use value returned by geo library
+		if (PX4_ISFINITE(_mag_declination_gps)) {
+			return _mag_declination_gps;
 		}
 	}

-	return false;
+	// otherwise use the parameter value
+	return math::radians(_params.mag_declination_deg);
 }
@@ -99,8 +99,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 				resetQuatCov(_params.mag_heading_noise);
 			}

-			resetMagEarthCov();
-			resetMagBiasCov();
+			resetMagCov();

 			return false;
 		}
@@ -151,44 +150,51 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	return false;
 }

-bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states)
+bool Ekf::fuseDeclination(float decl_sigma)
 {
-	VectorState H;
-	float decl_pred;
-	float innovation_variance;
+	float decl_measurement = NAN;

-	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R, FLT_EPSILON,
-			&decl_pred, &innovation_variance, &H);
+	if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+	    && PX4_ISFINITE(_mag_declination_gps)
+	   ) {
+		decl_measurement = _mag_declination_gps;

-	const float innovation = wrap_pi(decl_pred - decl_measurement_rad);
-
-	if (innovation_variance < R) {
-		// variance calculation is badly conditioned
-		_fault_status.flags.bad_mag_decl = true;
-		return false;
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+		  ) {
+		decl_measurement = math::radians(_params.mag_declination_deg);
 	}

-	// Calculate the Kalman gains
-	VectorState Kfusion = P * H / innovation_variance;
+	if (PX4_ISFINITE(decl_measurement)) {

-	if (!update_all_states) {
-		// zero non-mag Kalman gains if not updating all states
+		// observation variance (rad**2)
+		const float R_DECL = sq(decl_sigma);

-		// copy mag_I and mag_B Kalman gains
-		const Vector3f K_mag_I = Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0);
-		const Vector3f K_mag_B = Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0);
+		VectorState H;
+		float decl_pred;
+		float innovation_variance;

-		// zero all Kalman gains, then restore mag
-		Kfusion.setZero();
-		Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = K_mag_I;
-		Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
+		sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance,
+				&H);
+
+		const float innovation = wrap_pi(decl_pred - decl_measurement);
+
+		if (innovation_variance < R_DECL) {
+			// variance calculation is badly conditioned
+			return false;
+		}
+
+		// Calculate the Kalman gains
+		VectorState Kfusion = P * H / innovation_variance;
+
+		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
+
+		_fault_status.flags.bad_mag_decl = !is_fused;
+
+		return is_fused;
 	}

-	const bool is_fused = measurementUpdate(Kfusion, H, R, innovation);
-
-	_fault_status.flags.bad_mag_decl = !is_fused;
-
-	return is_fused;
+	return false;
 }

 float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
@@ -122,16 +122,8 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		const float range = predictFlowRange();
 		_flow_vel_body(0) = -flow_compensated(1) * range;
 		_flow_vel_body(1) =  flow_compensated(0) * range;
+		_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));

-		if (_flow_counter == 0) {
-			_flow_vel_body_lpf.reset(_flow_vel_body);
-			_flow_counter = 1;
-
-		} else {
-
-			_flow_vel_body_lpf.update(_flow_vel_body);
-			_flow_counter++;
-		}

 		// Check if we are in-air and require optical flow to control position drift
 		bool is_flow_required = _control_status.flags.in_air
@@ -152,7 +144,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				&& is_quality_good
 				&& is_magnitude_good
 				&& is_tilt_good
-				&& (_flow_counter > 10)
 				&& (isTerrainEstimateValid() || isHorizontalAidingActive())
 				&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching

@@ -169,7 +160,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
 				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
 					if (is_flow_required && is_quality_good && is_magnitude_good) {
-						resetFlowFusion(flow_sample);
+						resetFlowFusion();

 						if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {
 							resetTerrainToFlow();
@@ -203,7 +194,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				} else {
 					if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
 						ECL_INFO("starting optical flow, resetting");
-						resetFlowFusion(flow_sample);
+						resetFlowFusion();
 						_control_status.flags.opt_flow = true;

 					} else if (_control_status.flags.opt_flow_terrain) {
@@ -222,13 +213,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 	}
 }

-void Ekf::resetFlowFusion(const flowSample &flow_sample)
+void Ekf::resetFlowFusion()
 {
 	ECL_INFO("reset velocity to flow");
 	_information_events.flags.reset_vel_to_flow = true;

-	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample);
-	resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var);
+	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(_flow_sample_delayed);
+	resetHorizontalVelocityTo(_flow_vel_ne, flow_vel_var);

 	// reset position, estimate is relative to initial position in this mode, so we start with zero error
 	if (!_control_status.flags.in_air) {
@@ -282,8 +273,6 @@ void Ekf::stopFlowFusion()

 		_innov_check_fail_status.flags.reject_optflow_X = false;
 		_innov_check_fail_status.flags.reject_optflow_Y = false;
-
-		_flow_counter = 0;
 	}
 }

@@ -54,7 +54,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		_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);

 		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);

@@ -84,9 +84,8 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)

 			if (isTiltOk() && isDataInRange()) {
 				updateStuckCheck();
-				updateFogCheck(getDistBottom(), _sample.time_us);

-				if (!_is_stuck && !_is_blocked) {
+				if (!_is_stuck) {
 					_is_sample_valid = true;
 					_time_last_valid_us = _sample.time_us;
 				}
@@ -147,23 +146,5 @@ void SensorRangeFinder::updateStuckCheck()
 	}
 }

-void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
-{
-	if (_max_fog_dist > 0.f && time_us - _time_last_valid_us < 1e6) {
-
-		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
@@ -44,7 +44,6 @@
 #include "Sensor.hpp"

 #include <matrix/math.hpp>
-#include <lib/mathlib/math/filter/MedianFilter.hpp>

 namespace estimator
 {
@@ -100,8 +99,6 @@ public:
 		_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);
@@ -132,7 +129,6 @@ private:
 	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
 	bool isDataInRange() const;
 	void updateStuckCheck();
-	void updateFogCheck(const float dist_bottom, const uint64_t time_us);

 	rangeSample _sample{};

@@ -176,14 +172,6 @@ private:
 	 */
 	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
@@ -421,7 +421,6 @@ struct parameters {
 	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 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)
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -514,7 +513,7 @@ bool bad_sideslip      :
 		1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 		bool bad_optflow_X     : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
 		bool bad_optflow_Y     : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
-		bool __UNUSED          : 1; ///< 9 -
+		bool bad_acc_bias      : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
 		bool bad_acc_vertical  : 1; ///< 10 - true if bad vertical accelerometer data has been detected
 		bool bad_acc_clipping  : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
 	} flags;
@@ -96,8 +96,7 @@ void Ekf::initialiseCovariance()
 	resetAccelBiasCov();

 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	resetMagEarthCov();
-	resetMagBiasCov();
+	resetMagCov();
 #endif // CONFIG_EKF2_MAGNETOMETER

 #if defined(CONFIG_EKF2_WIND)
@@ -341,17 +340,11 @@ void Ekf::resetAccelBiasCov()
 }

 #if defined(CONFIG_EKF2_MAGNETOMETER)
-void Ekf::resetMagEarthCov()
+void Ekf::resetMagCov()
 {
-	ECL_INFO("reset mag earth covariance");
+	ECL_INFO("reset mag covariance");

 	P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.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));
 }
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -81,7 +81,6 @@ void Ekf::reset()
 	_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;
@@ -115,6 +114,8 @@ void Ekf::reset()

 	_last_known_pos.setZero();

+	_time_acc_bias_check = 0;
+
 #if defined(CONFIG_EKF2_BAROMETER)
 	_baro_counter = 0;
 #endif // CONFIG_EKF2_BAROMETER
@@ -275,100 +276,68 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }

-bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude,
-		const float eph,
-		const float epv, uint64_t timestamp_observation)
+bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
+		uint64_t timestamp_observation)
 {
-	if (!checkLatLonValidity(latitude, longitude)) {
+	if (!_pos_ref.isInitialized()) {
+		ECL_WARN("unable to reset global position, position reference not initialized");
 		return false;
 	}

-	if (!_pos_ref.isInitialized()) {
-		if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
-			return false;
-		}
-
-		if (!PX4_ISFINITE(_gps_alt_ref)) {
-			setAltOriginFromCurrentPos(altitude, epv);
-		}
-
-		return true;
-	}
-
-	Vector3f pos_correction;
+	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);

 	// apply a first order correction using velocity at the delayed time horizon and the delta time
-	if ((timestamp_observation > 0) && local_position_is_valid()) {
+	if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) {

 		timestamp_observation = math::min(_time_latest_us, timestamp_observation);

-		float dt_us;
+		float diff_us = 0.f;

 		if (_time_delayed_us >= timestamp_observation) {
-			dt_us = static_cast<float>(_time_delayed_us - timestamp_observation);
+			diff_us = static_cast<float>(_time_delayed_us - timestamp_observation);

 		} else {
-			dt_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
+			diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
 		}

-		const float dt_s = dt_us * 1e-6f;
-		pos_correction = _state.vel * dt_s;
+		const float dt_s = diff_us * 1e-6f;
+		pos_corrected += _state.vel.xy() * dt_s;
 	}

-	{
-		const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy();
+	const float obs_var = math::max(sq(accuracy), sq(0.01f));

-		const float obs_var = math::max(sq(eph), sq(0.01f));
+	const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
+	const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;

-		const Vector2f innov = Vector2f(_state.pos.xy()) - hpos;
-		const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
+	const float sq_gate = sq(5.f); // magic hardcoded gate
+	const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
+				  sq(innov(1)) / (sq_gate * innov_var(1))};

-		const float sq_gate = sq(5.f); // magic hardcoded gate
-		const float test_ratio = sq(innov(0)) / (sq_gate * innov_var(0)) + sq(innov(1)) / (sq_gate * innov_var(1));
+	const bool innov_rejected = (test_ratio.max() > 1.f);

-		const bool innov_rejected = (test_ratio > 1.f);
+	if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) {
+		// when on ground or accuracy chosen to be very low, we hard reset position
+		// this allows the user to still send hard resets at any time
+		ECL_INFO("reset position to external observation");
+		_information_events.flags.reset_pos_to_ext_obs = true;

-		if (!_control_status.flags.in_air || (eph > 0.f && eph < 1.f) || innov_rejected) {
-			// when on ground or accuracy chosen to be very low, we hard reset position
-			// this allows the user to still send hard resets at any time
-			ECL_INFO("reset position to external observation");
-			_information_events.flags.reset_pos_to_ext_obs = true;
+		resetHorizontalPositionTo(pos_corrected, obs_var);
+		_last_known_pos.xy() = _state.pos.xy();
+		return true;

-			resetHorizontalPositionTo(hpos, obs_var);
-			_last_known_pos.xy() = _state.pos.xy();
-
-		} else {
-			ECL_INFO("fuse external observation as position measurement");
-			fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0);
-			fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1);
-
-			// Use the reset counters to inform the controllers about a potential large position jump
-			// TODO: compute the actual position change
+	} else {
+		if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
+		    && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
+		   ) {
+			ECL_INFO("fused external observation as position measurement");
 			_state_reset_status.reset_count.posNE++;
-			_state_reset_status.posNE_change.zero();
-
 			_time_last_hor_pos_fuse = _time_delayed_us;
 			_last_known_pos.xy() = _state.pos.xy();
+			return true;
 		}
 	}

-	if (checkAltitudeValidity(altitude)) {
-		const float altitude_corrected = altitude - pos_correction(2);
-
-		if (!PX4_ISFINITE(_gps_alt_ref)) {
-			setAltOriginFromCurrentPos(altitude_corrected, epv);
-
-		} else {
-			const float vpos = -(altitude_corrected - _gps_alt_ref);
-			const float obs_var = math::max(sq(epv), sq(0.01f));
-
-			ECL_INFO("reset height to external observation");
-			resetVerticalPositionTo(vpos, obs_var);
-			_last_known_pos(2) = _state.pos(2);
-		}
-	}
-
-	return true;
+	return false;
 }

 void Ekf::updateParameters()
@@ -122,10 +122,7 @@ public:
 	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }

 	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
-	Vector2f getFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFlowVelBody()(0), getFlowVelBody()(1), 0.f)); }
-
-	const Vector2f &getFilteredFlowVelBody() const { return _flow_vel_body_lpf.getState(); }
-	Vector2f getFilteredFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFilteredFlowVelBody()(0), getFilteredFlowVelBody()(1), 0.f)); }
+	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }

 	const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; }
 	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; }
@@ -261,11 +258,8 @@ public:
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
-	bool checkLatLonValidity(double latitude, double longitude);
-	bool checkAltitudeValidity(float altitude);
-	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
-	bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN,
-					      float epv = NAN);
+	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
+	void updateWmm(double lat, double lon);

 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@@ -472,9 +466,6 @@ public:
 #endif // CONFIG_EKF2_GNSS

 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	// set the magnetic field data returned by the geo library using position
-	bool updateWorldMagneticModel(const double latitude_deg, const double longitude_deg);
-
 	const auto &aid_src_mag() const { return _aid_src_mag; }
 #endif // CONFIG_EKF2_MAGNETOMETER

@@ -533,7 +524,7 @@ public:
 		return true;
 	}

-	bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
+	bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
 			uint64_t timestamp_observation);

 	/**
@@ -612,6 +603,8 @@ private:

 	Vector3f _last_known_pos{};		///< last known local position vector (m)

+	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
+
 	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s

 	Dcmf _R_to_earth{};	///< transformation matrix from body frame to earth frame from last EKF prediction
@@ -639,12 +632,10 @@ private:

 	// optical flow processing
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
+	Vector2f _flow_vel_body{};	///< velocity from corrected flow measurement (body frame)(m/s)
+	Vector2f _flow_vel_ne{};		///< velocity from corrected flow measurement (local frame) (m/s)
 	Vector3f _ref_body_rate{};

-	Vector2f _flow_vel_body{};                      ///< velocity from corrected flow measurement (body frame)(m/s)
-	AlphaFilter<Vector2f> _flow_vel_body_lpf{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s)
-	uint32_t _flow_counter{0};                      ///< number of flow samples read for initialization
-
 	Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
 #endif // CONFIG_EKF2_OPTICAL_FLOW

@@ -767,12 +758,6 @@ private:
 		P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
 	}

-	bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
-	bool setAltOrigin(float altitude, float epv = NAN);
-
-	bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN);
-	bool setAltOriginFromCurrentPos(float altitude, float epv = NAN);
-
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
@@ -785,8 +770,8 @@ private:
 		     bool update_all_states = false, bool update_tilt = false);

 	// fuse magnetometer declination measurement
-	//  R: declination observation variance (rad**2)
-	bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false);
+	// argument passed in is the declination uncertainty in radians
+	bool fuseDeclination(float decl_sigma);

 #endif // CONFIG_EKF2_MAGNETOMETER

@@ -886,7 +871,7 @@ private:
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion(const imuSample &imu_delayed);
-	void resetFlowFusion(const flowSample &flow_sample);
+	void resetFlowFusion();
 	void stopFlowFusion();

 	void updateOnGroundMotionForOpticalFlowChecks();
@@ -1092,6 +1077,7 @@ private:
 	void stopAuxVelFusion();
 #endif // CONFIG_EKF2_AUXVEL

+	void checkVerticalAccelerationBias(const imuSample &imu_delayed);
 	void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
 	Likelihood estimateInertialNavFallingLikelihood() const;

@@ -1120,8 +1106,7 @@ private:
 	void resetQuatCov(const Vector3f &rot_var_ned);

 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	void resetMagEarthCov();
-	void resetMagBiasCov();
+	void resetMagCov();
 #endif // CONFIG_EKF2_MAGNETOMETER

 #if defined(CONFIG_EKF2_WIND)
@@ -72,151 +72,97 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
 	return _NED_origin_initialised;
 }

-bool Ekf::checkLatLonValidity(const double latitude, const double longitude)
-{
-	const bool lat_valid = (PX4_ISFINITE(latitude) && (abs(latitude) <= 90));
-	const bool lon_valid = (PX4_ISFINITE(longitude) && (abs(longitude) <= 180));
-
-	return (lat_valid && lon_valid);
-}
-
-bool Ekf::checkAltitudeValidity(const float altitude)
-{
-	// sanity check valid altitude anywhere between the Mariana Trench and edge of Space
-	return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f)));
-}
-
 bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
 			     const float epv)
 {
-	if (!setLatLonOrigin(latitude, longitude, eph)) {
-		return false;
-	}
+	// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
+	if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
+	    && PX4_ISFINITE(longitude) && (abs(longitude) <= 180)
+	    && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f)
+	   ) {
+		bool current_pos_available = false;
+		double current_lat = static_cast<double>(NAN);
+		double current_lon = static_cast<double>(NAN);

-	// altitude is optional
-	setAltOrigin(altitude, epv);
+		// if we are already doing aiding, correct for the change in position since the EKF started navigating
+		if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
+			_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
+			current_pos_available = true;
+		}

-	return true;
-}
+		const float gps_alt_ref_prev = _gps_alt_ref;

-bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
-{
-	if (!checkLatLonValidity(latitude, longitude)) {
-		return false;
-	}
+		// reinitialize map projection to latitude, longitude, altitude, and reset position
+		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
+		_gps_alt_ref = altitude;

-	bool current_pos_available = false;
-	double current_lat = static_cast<double>(NAN);
-	double current_lon = static_cast<double>(NAN);
+		updateWmm(current_lat, current_lon);

-	// if we are already doing aiding, correct for the change in position since the EKF started navigating
-	if (_pos_ref.isInitialized() && local_position_is_valid()) {
-		_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
-		current_pos_available = true;
-	}
-
-	// reinitialize map projection to latitude, longitude, altitude, and reset position
-	_pos_ref.initReference(latitude, longitude, _time_delayed_us);
-
-	if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
 		_gpos_origin_eph = eph;
-	}
-
-	_NED_origin_initialised = true;
-
-	if (current_pos_available) {
-		// reset horizontal position if we already have a global origin
-		Vector2f position = _pos_ref.project(current_lat, current_lon);
-		resetHorizontalPositionTo(position);
-	}
-
-	return true;
-}
-
-bool Ekf::setAltOrigin(const float altitude, const float epv)
-{
-	if (!checkAltitudeValidity(altitude)) {
-		return false;
-	}
-
-	const float gps_alt_ref_prev = _gps_alt_ref;
-	_gps_alt_ref = altitude;
-
-	if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
 		_gpos_origin_epv = epv;
-	}

-	if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) {
-		// determine current z
-		const float z_prev = _state.pos(2);
-		const float current_alt = -z_prev + gps_alt_ref_prev;
+		_NED_origin_initialised = true;
+
+		if (current_pos_available) {
+			// reset horizontal position if we already have a global origin
+			Vector2f position = _pos_ref.project(current_lat, current_lon);
+			resetHorizontalPositionTo(position);
+		}
+
+		if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
+			// determine current z
+			const float z_prev = _state.pos(2);
+			const float current_alt = -z_prev + gps_alt_ref_prev;
 #if defined(CONFIG_EKF2_GNSS)
-		const float gps_hgt_bias = _gps_hgt_b_est.getBias();
+			const float gps_hgt_bias = _gps_hgt_b_est.getBias();
 #endif // CONFIG_EKF2_GNSS
-		resetVerticalPositionTo(_gps_alt_ref - current_alt);
-		ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
-			  (double)_state.pos(2));
+			resetVerticalPositionTo(_gps_alt_ref - current_alt);
+			ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
+				  (double)_state.pos(2));
 #if defined(CONFIG_EKF2_GNSS)
-		// adjust existing GPS height bias
-		_gps_hgt_b_est.setBias(gps_hgt_bias);
+			// adjust existing GPS height bias
+			_gps_hgt_b_est.setBias(gps_hgt_bias);
 #endif // CONFIG_EKF2_GNSS
+		}
+
+		return true;
 	}

-	return true;
+	return false;
 }

-bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude,
-		const float eph, const float epv)
+void Ekf::updateWmm(const double lat, const double lon)
 {
-	if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
-		return false;
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+
+	// set the magnetic field data returned by the geo library using the current GPS position
+	const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon));
+	const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon));
+	const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
+
+	if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
+
+		const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
+		const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
+
+		if ((_wmm_gps_time_last_set == 0)
+		    || !PX4_ISFINITE(_mag_declination_gps)
+		    || !PX4_ISFINITE(_mag_inclination_gps)
+		    || !PX4_ISFINITE(_mag_strength_gps)
+		    || mag_declination_changed
+		    || mag_inclination_changed
+		   ) {
+			_mag_declination_gps = mag_declination_gps;
+			_mag_inclination_gps = mag_inclination_gps;
+			_mag_strength_gps = mag_strength_gps;
+
+			_wmm_gps_time_last_set = _time_delayed_us;
+		}
 	}

-	// altitude is optional
-	setAltOriginFromCurrentPos(altitude, epv);
-
-	return true;
+#endif // CONFIG_EKF2_MAGNETOMETER
 }

-bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph)
-{
-	if (!checkLatLonValidity(latitude, longitude)) {
-		return false;
-	}
-
-	_pos_ref.initReference(latitude, longitude, _time_delayed_us);
-
-	// if we are already doing aiding, correct for the change in position since the EKF started navigating
-	if (local_position_is_valid()) {
-		double est_lat;
-		double est_lon;
-		_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
-		_pos_ref.initReference(est_lat, est_lon, _time_delayed_us);
-	}
-
-	if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
-		_gpos_origin_eph = eph;
-	}
-
-	_NED_origin_initialised = true;
-
-	return true;
-}
-
-bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv)
-{
-	if (!checkAltitudeValidity(altitude)) {
-		return false;
-	}
-
-	_gps_alt_ref = altitude + _state.pos(2);
-
-	if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
-		_gpos_origin_epv = epv;
-	}
-
-	return true;
-}

 void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
@@ -250,7 +250,7 @@ public:
 	bool get_mag_decl_deg(float &val) const
 	{
 		if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
-			val = math::degrees(_wmm_declination_rad);
+			val = math::degrees(_mag_declination_gps);
 			return true;

 		} else {
@@ -261,7 +261,7 @@ public:
 	bool get_mag_inc_deg(float &val) const
 	{
 		if (_NED_origin_initialised) {
-			val = math::degrees(_wmm_inclination_rad);
+			val = math::degrees(_mag_inclination_gps);
 			return true;

 		} else {
@@ -272,9 +272,9 @@ public:
 	void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const
 	{
 		inc_deg = math::degrees(_mag_inclination);
-		inc_ref_deg = math::degrees(_wmm_inclination_rad);
+		inc_ref_deg = math::degrees(_mag_inclination_gps);
 		strength_gs = _mag_strength;
-		strength_ref_gs = _wmm_field_strength_gauss;
+		strength_ref_gs = _mag_strength_gps;
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER

@@ -451,14 +451,13 @@ protected:
 	// allocate data buffers and initialize interface variables
 	bool initialise_interface(uint64_t timestamp);

+	uint64_t _wmm_gps_time_last_checked{0};  // time WMM last checked
+	uint64_t _wmm_gps_time_last_set{0};      // time WMM last set
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control
-
-	float _wmm_declination_rad{NAN};        // magnetic declination returned by the geo library using the last valid GPS position (rad)
-	float _wmm_inclination_rad{NAN};        // magnetic inclination returned by the geo library using the last valid GPS position (rad)
-	float _wmm_field_strength_gauss{NAN};   // magnetic strength returned by the geo library using the last valid GPS position (Gauss)
-
-	Vector3f _wmm_earth_field_gauss{};      // expected magnetic field vector from the last valid GPS position (Gauss)
+	float _mag_declination_gps {NAN};        // magnetic declination returned by the geo library using the last valid GPS position (rad)
+	float _mag_inclination_gps{NAN};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
+	float _mag_strength_gps{NAN};	          // magnetic strength returned by the geo library using the last valid GPS position (T)

 	float _mag_inclination{NAN};
 	float _mag_strength{NAN};
@@ -39,6 +39,7 @@

 void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 {
+	checkVerticalAccelerationBias(imu_delayed);
 	checkVerticalAccelerationHealth(imu_delayed);

 #if defined(CONFIG_EKF2_BAROMETER)
@@ -119,6 +120,106 @@ void Ekf::checkHeightSensorRefFallback()
 	}
 }

+void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed)
+{
+	// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
+	// calculate accel bias term aligned with the gravity vector
+	const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
+	const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg;
+	const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));
+
+	// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
+	bool bad_acc_bias = false;
+
+	if (fabsf(down_dvel_bias) > dVel_bias_lim) {
+
+		bool bad_vz = false;
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+
+		if (_control_status.flags.ev_hgt) {
+			if (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.f) {
+				bad_vz = true;
+			}
+		}
+
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+#if defined(CONFIG_EKF2_GNSS)
+
+		if (_control_status.flags.gps) {
+			if (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.f) {
+				bad_vz = true;
+			}
+		}
+
+#endif // CONFIG_EKF2_GNSS
+
+		if (bad_vz) {
+#if defined(CONFIG_EKF2_BAROMETER)
+
+			if (_control_status.flags.baro_hgt) {
+				if (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.f) {
+					bad_acc_bias = true;
+				}
+			}
+
+#endif // CONFIG_EKF2_BAROMETER
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+
+			if (_control_status.flags.ev_hgt) {
+				if (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.f) {
+					bad_acc_bias = true;
+				}
+			}
+
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+#if defined(CONFIG_EKF2_GNSS)
+
+			if (_control_status.flags.gps_hgt) {
+				if (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.f) {
+					bad_acc_bias = true;
+				}
+			}
+
+#endif // CONFIG_EKF2_GNSS
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+
+			if (_control_status.flags.rng_hgt) {
+				if (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.f) {
+					bad_acc_bias = true;
+				}
+			}
+
+#endif // CONFIG_EKF2_RANGE_FINDER
+		}
+	}
+
+	// record the pass/fail
+	if (!bad_acc_bias) {
+		_fault_status.flags.bad_acc_bias = false;
+		_time_acc_bias_check = _time_delayed_us;
+
+	} else {
+		_fault_status.flags.bad_acc_bias = true;
+	}
+
+	// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
+	// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
+	if (_fault_status.flags.bad_acc_bias && isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) {
+
+		resetAccelBiasCov();
+
+		_time_acc_bias_check = imu_delayed.time_us;
+
+		_fault_status.flags.bad_acc_bias = false;
+		ECL_WARN("invalid accel bias - covariance reset");
+	}
+}
+
 void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed)
 {
 	// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
@@ -91,9 +91,6 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
 		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
 	}

-	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, P(State::pos.idx, State::pos.idx));
-	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, P(State::pos.idx + 1, State::pos.idx + 1));
-
 	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);

 	// record the state change
@@ -120,8 +117,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
 	}

-	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, P(State::pos.idx + 2, State::pos.idx + 2));
-
 	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);

 	// record the state change
@@ -162,7 +162,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_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)),
@@ -546,8 +545,9 @@ void EKF2::Run()
 						accuracy = vehicle_command.param3;
 					}

-					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, vehicle_command.param7,
-							accuracy, accuracy, timestamp_observation)
+					// TODO add check for lat and long validity
+					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
+							accuracy, timestamp_observation)
 					   ) {
 						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

@@ -571,12 +571,7 @@ void EKF2::Run()
 				const float wind_direction_accuracy_rad = math::radians(vehicle_command.param4);
 				_ekf.resetWindToExternalObservation(vehicle_command.param1, wind_direction_rad, vehicle_command.param2,
 								    wind_direction_accuracy_rad);
-				command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-#else
-				command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 #endif // CONFIG_EKF2_WIND
-				command_ack.timestamp = hrt_absolute_time();
-				_vehicle_command_ack_pub.publish(command_ack);
 			}
 		}
 	}
@@ -1943,6 +1938,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.fs_bad_sideslip          = _ekf.fault_status_flags().bad_sideslip;
 		status_flags.fs_bad_optflow_x         = _ekf.fault_status_flags().bad_optflow_X;
 		status_flags.fs_bad_optflow_y         = _ekf.fault_status_flags().bad_optflow_Y;
+		status_flags.fs_bad_acc_bias          = _ekf.fault_status_flags().bad_acc_bias;
 		status_flags.fs_bad_acc_vertical      = _ekf.fault_status_flags().bad_acc_vertical;
 		status_flags.fs_bad_acc_clipping      = _ekf.fault_status_flags().bad_acc_clipping;

@@ -2031,9 +2027,6 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 		_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
 		_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);

-		_ekf.getFilteredFlowVelBody().copyTo(flow_vel.vel_body_filtered);
-		_ekf.getFilteredFlowVelNE().copyTo(flow_vel.vel_ne_filtered);
-
 		_ekf.getFlowUncompensated().copyTo(flow_vel.flow_rate_uncompensated);
 		_ekf.getFlowCompensated().copyTo(flow_vel.flow_rate_compensated);

@@ -2671,6 +2664,7 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 	const bool bias_valid = (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::AccelBias))
 				&& _ekf.control_status_flags().tilt_align
 				&& (_ekf.fault_status().value == 0)
+				&& !_ekf.fault_status_flags().bad_acc_bias
 				&& !_ekf.fault_status_flags().bad_acc_clipping
 				&& !_ekf.fault_status_flags().bad_acc_vertical;

@@ -623,7 +623,6 @@ private:
 		(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,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z,
@@ -150,16 +150,3 @@ parameters:
      default: 0.0
      unit: m
      decimal: 3
-    EKF2_RNG_FOG:
-      description:
-        short: Maximum distance at which the range finder could detect fog (m)
-        long: Limit for fog detection. If the range finder measures a distance greater
-          than this value, the measurement is considered to not be blocked by fog or rain.
-          If there's a jump from larger than RNG_FOG to smaller than EKF2_RNG_FOG, the
-          measurement may gets rejected. 0 - disabled
-      type: float
-      default: 1.0
-      min: 0.0
-      max: 20.0
-      unit: m
-      decimal: 1
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
KonradRudin	31ced4647c	Update src/lib/version/px_update_git_header.py	2024-08-27 16:41:12 +02:00
KonradRudin	516414ee3b	Apply suggestions from code review Co-authored-by: Daniel Agar <daniel@agar.ca>	2024-08-27 16:40:11 +02:00
Konrad	0fee107e79	git_tag: make sure to search for last valid tage beginning with vx.y.z only	2024-08-27 13:21:43 +02:00