added full support for standard vtol in SIH

Signed-off-by: RomanBapst <bapstroman@gmail.com>

.ci/Jenkinsfile-compile

@@ -77,6 +77,7 @@ 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",

.github/workflows/mavros_mission_tests.yml

@@ -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@v2-preview
+      uses: actions/upload-artifact@v4
       with:
         name: coredump
         path: px4.core
@@ -103,21 +103,21 @@ jobs:
 
     - name: Upload px4 binary
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: binary
         path: build/px4_sitl_default/bin/px4
 
     - name: Store PX4 log
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: px4_log
         path: ~/.ros/log/*/*.ulg
 
     - name: Store ROS log
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: ros_log
         path: ~/.ros/**/rostest-*.log

.github/workflows/mavros_offboard_tests.yml

@@ -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@v2-preview
+      uses: actions/upload-artifact@v4
       with:
         name: coredump
         path: px4.core
@@ -98,21 +98,21 @@ jobs:
 
     - name: Upload px4 binary
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: binary
         path: build/px4_sitl_default/bin/px4
 
     - name: Store PX4 log
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: px4_log
         path: ~/.ros/log/*/*.ulg
 
     - name: Store ROS log
       if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
       with:
         name: ros_log
         path: ~/.ros/**/rostest-*.log

.vscode/cmake-variants.yaml

@@ -286,6 +286,11 @@ 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

CMakeLists.txt

@@ -113,7 +113,7 @@ include(px4_parse_function_args)
 include(px4_git)
 
 execute_process(
-	COMMAND git describe --exclude ext/* --match v[0-9]*.[0-9]*.[0-9]* --tags
+	COMMAND git describe --exclude ext/* --tags --match "v[0-9]*"
 	OUTPUT_VARIABLE PX4_GIT_TAG
 	OUTPUT_STRIP_TRAILING_WHITESPACE
 	RESULTS_VARIABLE GIT_DESCRIBE_RESULT

Makefile

@@ -340,6 +340,7 @@ 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 \

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

@@ -14,15 +14,19 @@ 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_SPEED 7
-param set-default RD_HEADING_P 5
-param set-default RD_HEADING_I 0.1
+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_YAW_RATE 180
-param set-default RD_MISS_SPD_DEF 7
+param set-default RD_MISS_SPD_DEF 5
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 

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

@@ -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_HEADING_P 5
-param set-default RD_HEADING_I 0.1
+param set-default RD_YAW_P 5
+param set-default RD_YAW_I 0.1
 param set-default RD_MAX_YAW_RATE 30
 param set-default RD_MISS_SPD_DEF 8
 param set-default RD_TRANS_DRV_TRN 0.349066

ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_x500_mono_cam_down

@@ -0,0 +1,10 @@
+#!/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

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -85,6 +85,7 @@ 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

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -43,6 +43,10 @@ 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
@@ -62,6 +66,8 @@ 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
@@ -75,3 +81,5 @@ param set SIH_L_ROLL 0.145
 
 # sih as tailsitter
 param set SIH_VEHICLE_TYPE 2
+
+param set-default VT_ARSP_TRANS 6

ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil

@@ -0,0 +1,99 @@
+#!/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

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

@@ -20,3 +20,27 @@ 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

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -48,6 +48,7 @@ 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()
 

ROMFS/px4fmu_common/init.d/rcS

@@ -105,6 +105,13 @@ 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

Tools/px4.py

@@ -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/* --match v[0-9]*.[0-9]*.[0-9]* --abbrev=0 --tags'
+        cmd = 'git describe --exclude ext/* --abbrev=0 --tags'
         try:
             version = subprocess.check_output(
                 cmd, cwd=px4_dir, shell=True).decode().strip()

Tools/setup/ubuntu.sh

@@ -225,7 +225,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 
 	# Gazebo / Gazebo classic installation
 	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		echo "Gazebo (Garden) will be installed"
+		echo "Gazebo (Harmonic) 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

boards/ark/fmu-v6x/default.px4board

@@ -85,8 +85,6 @@ 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

boards/cuav/x7pro/default.px4board

@@ -39,7 +39,6 @@ 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
@@ -70,7 +69,6 @@ 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
@@ -79,7 +77,6 @@ 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
@@ -91,7 +88,6 @@ 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
@@ -99,4 +95,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y

boards/px4/fmu-v4pro/test.px4board

@@ -3,6 +3,8 @@ 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

boards/px4/fmu-v5/cryptotest.px4board

@@ -1,4 +1,6 @@
 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

boards/px4/fmu-v5x/default.px4board

@@ -29,7 +29,6 @@ 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

boards/px4/fmu-v6x/default.px4board

@@ -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=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_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_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
-CONFIG_DRIVERS_POWER_MONITOR_INA226=y
+CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 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=y
+CONFIG_DRIVERS_UAVCAN=n
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y

boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -377,7 +377,6 @@
 *(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
 *(.text.imxrt_epsubmit)
 *(.text._ZN15PositionControl6updateEf)
-*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE)
 *(.text._ZN23MavlinkStreamScaledIMU24sendEv)
 *(.text._ZN5PX4IO10io_reg_getEhhPtj)
 *(.text.imxrt_dma_send)

msg/ActionRequest.msg

@@ -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_RC_STICK_GESTURE = 0
+uint8 SOURCE_STICK_GESTURE = 0
 uint8 SOURCE_RC_SWITCH = 1
 uint8 SOURCE_RC_BUTTON = 2
 uint8 SOURCE_RC_MODE_SLOT = 3

msg/BatteryStatus.msg

@@ -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. 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 in degrees Celcius, 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

msg/CMakeLists.txt

@@ -73,6 +73,7 @@ set(msg_files
 	DebugVect.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
+	DistanceSensorModeChangeRequest.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg
@@ -152,6 +153,10 @@ set(msg_files
 	ObstacleDistance.msg
 	OffboardControlMode.msg
 	OnboardComputerStatus.msg
+	OpenDroneIdArmStatus.msg
+	OpenDroneIdOperatorId.msg
+	OpenDroneIdSelfId.msg
+	OpenDroneIdSystem.msg
 	OrbitStatus.msg
 	OrbTest.msg
 	OrbTestLarge.msg
@@ -182,6 +187,7 @@ set(msg_files
 	RoverAckermannGuidanceStatus.msg
 	RoverAckermannStatus.msg
 	RoverDifferentialGuidanceStatus.msg
+	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
 	Rpm.msg
 	RtlStatus.msg

msg/DistanceSensor.msg

@@ -40,3 +40,8 @@ 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

msg/DistanceSensorModeChangeRequest.msg

@@ -0,0 +1,5 @@
+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

msg/EstimatorStatusFlags.msg

@@ -59,7 +59,6 @@ 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)
 

msg/OpenDroneIdArmStatus.msg

@@ -0,0 +1,3 @@
+uint64 timestamp
+uint8 status
+char[50] error

msg/OpenDroneIdOperatorId.msg

@@ -0,0 +1,4 @@
+uint64 timestamp
+uint8[20] id_or_mac
+uint8 operator_id_type
+char[20] operator_id

msg/OpenDroneIdSelfId.msg

@@ -0,0 +1,4 @@
+uint64 timestamp
+uint8[20] id_or_mac
+uint8 description_type
+char[23] description

msg/OpenDroneIdSystem.msg

@@ -0,0 +1,13 @@
+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

msg/PositionSetpoint.msg

@@ -30,7 +30,8 @@ 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   # navigation acceptance_radius if we're doing waypoint navigation
+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 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)

msg/RoverDifferentialGuidanceStatus.msg

@@ -1,10 +1,7 @@
 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

msg/RoverDifferentialSetpoint.msg

@@ -0,0 +1,9 @@
+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

msg/RoverDifferentialStatus.msg

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

msg/TecsStatus.msg

@@ -27,3 +27,4 @@ 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]

msg/VehicleOpticalFlowVel.msg

@@ -4,6 +4,9 @@ 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)
 

msg/VehicleStatus.msg

@@ -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_RC_STICK = 1
+uint8 ARM_DISARM_REASON_STICK_GESTURE = 1
 uint8 ARM_DISARM_REASON_RC_SWITCH = 2
 uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3
 uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4

src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp

@@ -55,6 +55,7 @@
 #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;
 
@@ -143,8 +144,11 @@ 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) :
@@ -411,6 +415,17 @@ 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)) {
@@ -438,7 +453,7 @@ int LightwareLaser::updateRestriction()
 		updateParams();
 	}
 
-	bool _prev_restriction{_restriction};
+	_prev_restriction = _restriction;
 
 	switch (_param_sf1xx_mode.get()) {
 	case 0: // Sensor disabled
@@ -451,7 +466,7 @@ int LightwareLaser::updateRestriction()
 		break;
 
 	case 2:
-		_restriction = _auto_restriction;
+		_restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::REQUEST_ON;
 		break;
 	}
 
@@ -498,6 +513,8 @@ 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");
 
@@ -506,6 +523,14 @@ 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);

src/drivers/distance_sensor/lightware_laser_i2c/parameters.c

@@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
  *
  * @value 0 Disabled
  * @value 1 Enabled
- * @value 2 Disabled during VTOL fast forward flight
+ * @value 2 Enabled in VTOL MC mode, listen to request from system in FW mode
  *
  * @min 0
  * @max 2

src/drivers/power_monitor/ina228/ina228.cpp

@@ -88,6 +88,7 @@ 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());
 }
 
@@ -306,6 +307,7 @@ 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");
@@ -315,6 +317,7 @@ 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);
@@ -381,6 +384,7 @@ 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) {

src/drivers/power_monitor/ina228/ina228.h

@@ -291,6 +291,7 @@ 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))
@@ -339,6 +340,7 @@ 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};

src/drivers/power_monitor/ina238/ina238.cpp

@@ -80,6 +80,7 @@ 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());
 }
 
@@ -224,9 +225,11 @@ 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
@@ -250,6 +253,7 @@ 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);
@@ -309,6 +313,7 @@ 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) {

src/drivers/power_monitor/ina238/ina238.h

@@ -73,6 +73,7 @@ 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 */

src/drivers/power_monitor/ina238/ina238_registers.hpp

@@ -14,6 +14,7 @@ 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

src/drivers/uavcan/CMakeLists.txt

@@ -147,6 +147,8 @@ px4_add_module(
 		arming_status.hpp
 		beep.cpp
 		beep.hpp
+		remoteid.cpp
+		remoteid.hpp
 		rgbled.cpp
 		rgbled.hpp
 		safety_state.cpp

src/drivers/uavcan/Kconfig

@@ -26,6 +26,10 @@ 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

src/drivers/uavcan/remoteid.cpp

@@ -0,0 +1,356 @@
+/****************************************************************************
+ *
+ *   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);
+}

src/drivers/uavcan/remoteid.hpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   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;
+};

src/drivers/uavcan/rgbled.hpp

@@ -51,7 +51,7 @@ public:
 	int init();
 
 private:
-	// Max update rate to avoid exessive bus traffic
+	// Max update rate to avoid excessive bus traffic
 	static constexpr unsigned MAX_RATE_HZ = 20;
 
 	void periodic_update(const uavcan::TimerEvent &);

src/drivers/uavcan/uavcan_main.cpp

@@ -96,6 +96,9 @@ 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
@@ -558,6 +561,16 @@ 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();
 

src/drivers/uavcan/uavcan_main.hpp

@@ -68,6 +68,10 @@
 
 #include "logmessage.hpp"
 
+#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
+#include "remoteid.hpp"
+#endif
+
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 #include "rgbled.hpp"
 #endif
@@ -269,6 +273,9 @@ 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

src/lib/battery/battery.cpp

@@ -106,6 +106,11 @@ 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
@@ -149,7 +154,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 = NAN;
+	battery_status.temperature = _temperature_c;
 	battery_status.cell_count = _params.n_cells;
 	battery_status.connected = _connected;
 	battery_status.source = _source;

src/lib/battery/battery.h

@@ -91,6 +91,7 @@ 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
@@ -168,6 +169,7 @@ 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]

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -39,7 +39,8 @@ 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); // Default to type LASER
+	set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
+	set_mode(distance_sensor_s::MODE_UNKNOWN);
 }
 
 PX4Rangefinder::~PX4Rangefinder()

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -60,6 +60,8 @@ 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(); };

src/lib/events/enums.json

@@ -447,12 +447,12 @@
                             "description": "Transition to standby"
                         },
                         "1": {
-                            "name": "rc_stick",
-                            "description": "RC"
+                            "name": "stick_gesture",
+                            "description": "Stick gesture"
                         },
                         "2": {
                             "name": "rc_switch",
-                            "description": "RC (switch)"
+                            "description": "RC switch"
                         },
                         "3": {
                             "name": "command_internal",

src/lib/matrix/matrix/Slice.hpp

@@ -116,6 +116,39 @@ 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)
@@ -222,29 +255,19 @@ public:
 		return self;
 	}
 
-	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &other)
+	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &scalar)
 	{
-		return operator*=(Type(1) / other);
+		return operator*=(Type(1) / scalar);
 	}
 
-	Matrix<Type, P, Q> operator*(const Type &other) const
+	Matrix<Type, P, Q> operator*(const Type &scalar) const
 	{
-		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;
+		return Matrix<Type, P, Q> {*this} * scalar;
 	}
 
-	Matrix<Type, P, Q> operator/(const Type &other) const
+	Matrix<Type, P, Q> operator/(const Type &scalar) const
 	{
-		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
-		return self * (Type(1) / other);
+		return (*this) * (1 / scalar);
 	}
 
 	template<size_t R, size_t S>

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -317,6 +317,7 @@ public:
 	}
 };
 
+using SquareMatrix2f = SquareMatrix<float, 2>;
 using SquareMatrix3f = SquareMatrix<float, 3>;
 using SquareMatrix3d = SquareMatrix<double, 3>;
 

src/lib/matrix/matrix/Vector2.hpp

@@ -102,7 +102,6 @@ public:
 
 };
 
-
 using Vector2f = Vector2<float>;
 using Vector2d = Vector2<double>;
 

src/lib/matrix/test/MatrixSliceTest.cpp

@@ -262,6 +262,79 @@ 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)
 {

src/lib/matrix/test/MatrixVector3Test.cpp

@@ -80,4 +80,13 @@ 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));
 }

src/lib/motion_planning/CMakeLists.txt

@@ -43,3 +43,4 @@ 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)

src/lib/motion_planning/HeadingSmoothing.cpp

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

src/lib/motion_planning/HeadingSmoothingTest.cpp

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   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);
+}

src/lib/pure_pursuit/PurePursuit.cpp

@@ -78,18 +78,16 @@ 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();
-	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
+	_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;
 
-	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));

src/lib/pure_pursuit/PurePursuit.hpp

@@ -103,6 +103,8 @@ 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:
 	/**
@@ -122,5 +124,7 @@ protected:
 		float lookahead_min{1.f};
 	} _params{};
 private:
-	float _lookahead_distance{0.f};
+	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
 };

src/lib/tecs/TECS.cpp

@@ -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,16 +525,12 @@ 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
-		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;
-		}
+		throttle_setpoint = param.throttle_min;
 
 	} else {
 		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
-		throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
+		throttle_setpoint = (1.f - param.fast_descend) * _calcThrottleControlOutput(limit, ste_rate, param,
+				    flag) + param.fast_descend * param.throttle_min;
 	}
 
 	// Rate limit the throttle demand
@@ -677,6 +673,11 @@ 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)
 {
@@ -699,6 +700,9 @@ 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,
@@ -753,8 +757,7 @@ 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 = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas *
-						EAS_setpoint;
+		control_setpoint.tas_setpoint = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint);
 
 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
@@ -765,11 +768,13 @@ 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;
 }
@@ -778,13 +783,22 @@ 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)) {
-		_fast_descend = 1.f;
+		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);
 
 	} 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;
 	}
 }

src/lib/tecs/TECS.hpp

@@ -50,6 +50,8 @@
 #include <motion_planning/VelocitySmoothing.hpp>
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
+using namespace time_literals;
+
 class TECSAirspeedFilter
 {
 public:
@@ -203,8 +205,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 allowed in [rad].
-		float pitch_min;			///< Minimal pitch angle allowed in [rad].
+		float pitch_max;			///< Maximum pitch angle above trim allowed in [rad].
+		float pitch_min;			///< Minimal pitch angle below trim 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.
@@ -320,7 +322,7 @@ public:
 	/**
 	 * @brief Get the pitch setpoint.
 	 *
-	 * @return THe commanded pitch angle in [rad].
+	 * @return The commanded pitch angle above trim in [rad].
 	 */
 	float getPitchSetpoint() const {return _pitch_setpoint;};
 	/**
@@ -478,7 +480,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 [rad].
+	 * @return pitch setpoint angle above trim [rad].
 	 */
 	float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
 				      const Flag &flag) const;
@@ -537,7 +539,7 @@ private:
 
 	// Output
 	DebugOutput _debug_output;				///< Debug output.
-	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [rad].
+	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint above trim [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]
 };
@@ -547,11 +549,13 @@ 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;
@@ -658,6 +662,17 @@ 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
 	 *
@@ -675,9 +690,11 @@ 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{};
 

src/lib/version/px_update_git_header.py

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

src/modules/commander/Commander.cpp

@@ -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::rc_stick: return "RC";
+	case arm_disarm_reason_t::stick_gesture: return "Stick gesture";
 
-	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::rc_stick
+		} else if (calling_reason == arm_disarm_reason_t::stick_gesture
 			   || 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::rc_stick)
+		const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::stick_gesture)
 					     || (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::rc_stick) {
+			if (calling_reason != arm_disarm_reason_t::stick_gesture) {
 				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,7 +887,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			}
 
 			if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
-				if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
+
+				// 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)) {
 					main_ret = TRANSITION_CHANGED;
 
 				} else {
@@ -1062,7 +1069,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
+			// 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)) {
 				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");
@@ -1499,6 +1512,7 @@ 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;
 
@@ -1615,7 +1629,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_RC_STICK_GESTURE
+	    && (action_request.source == action_request_s::SOURCE_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)) {
@@ -1623,7 +1637,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 	}
 
 	switch (action_request.source) {
-	case action_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;
+	case action_request_s::SOURCE_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::stick_gesture; break;
 
 	case action_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;
 

src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp

@@ -48,7 +48,8 @@ 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;
+			valid = _distance_sensor_sub[instance].copy(&dist_sens) && ((hrt_elapsed_time(&dist_sens.timestamp) < 1_s)
+					|| (dist_sens.mode == distance_sensor_s::MODE_DISABLED));
 			reporter.setIsPresent(health_component_t::distance_sensor);
 		}
 

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -273,12 +273,27 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 		}
 
 		if (!context.isArmed() && ekf_gps_check_fail) {
-			NavModes required_groups_gps = required_groups;
-			events::Log log_level = events::Log::Error;
+			NavModes required_groups_gps;
+			events::Log log_level;
 
-			if (_param_com_arm_wo_gps.get()) {
+			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:
 				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
@@ -438,11 +453,20 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 			}
 
 			if (message && reporter.mavlink_log_pub()) {
-				if (!_param_com_arm_wo_gps.get()) {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+				switch (static_cast<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
+				default:
 
-				} else {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+				/* FALLTHROUGH */
+				case GnssArmingCheck::DenyArming:
+					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;
 				}
 			}
 		}

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -59,6 +59,12 @@ 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);
@@ -108,7 +114,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,
-					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
+					(ParamInt<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,

src/modules/commander/commander_params.c

@@ -225,11 +225,14 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
 PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);
 
 /**
- * Allow arming without GPS
+ * GPS preflight check
+ *
+ * Measures taken when a check defined by EKF2_GPS_CHECK is failing.
  *
  * @group Commander
- * @value 0 Require GPS lock to arm
- * @value 1 Allow arming without GPS
+ * @value 0 Deny arming
+ * @value 1 Warning only
+ * @value 2 Disabled
  */
 PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
 
@@ -285,7 +288,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
  * @unit s
  * @min 0.0
  * @max 25.0
- * @decimal 3
+ * @decimal 1
  */
 PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);
 
@@ -335,14 +338,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 Manual
- * @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 Stabilized
+ * @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);
@@ -450,16 +453,12 @@ PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f);
 PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
 
 /**
- * Position control navigation loss response.
+ * Position mode navigation loss response
  *
- * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control.
+ * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control in manual Position mode.
  *
- * 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
+ * @value 0 Altitude mode
+ * @value 1 Land mode (descend)
  *
  * @group Commander
  */

src/modules/commander/failsafe/failsafe.cpp

@@ -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::Manual:
+	case offboard_loss_failsafe_mode::Stabilized:
 		action = Action::FallbackStab;
 		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
 		break;

src/modules/commander/failsafe/failsafe.h

@@ -70,7 +70,7 @@ private:
 	enum class offboard_loss_failsafe_mode : int32_t {
 		Position_mode = 0,
 		Altitude_mode = 1,
-		Manual = 2,
+		Stabilized = 2,
 		Return_mode = 3,
 		Land_mode = 4,
 		Hold_mode = 5,

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

@@ -113,7 +113,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				} else {
 					// Try to initialize using measurement
-					if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) {
+					if (ekf.setEkfGlobalOriginFromCurrentPos(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;

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

@@ -41,10 +41,6 @@
 
 #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
@@ -63,46 +59,27 @@ 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()) {
-			_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);
-			}
+			setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
 		}
 
 		// 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)) {
-			_gps_alt_ref = gps.alt + _state.pos(2);
+			setAltOriginFromCurrentPos(gps.alt, gps.vacc);
 		}
 
-		_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");
-	}
 
-	if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) {
-		// a rough 2D fix is sufficient to lookup declination
+		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
 		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;
 	}
 }
 

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

@@ -39,6 +39,8 @@
 #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)
@@ -66,7 +68,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			_control_status.flags.mag_fault = false;
 
 			_state.mag_B.zero();
-			resetMagCov();
+			resetMagBiasCov();
+
+			stopMagFusion();
 
 			_mag_lpf.reset(mag_sample.mag);
 			_mag_counter = 1;
@@ -76,15 +80,43 @@ 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)
-		    && (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
+		    && (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
 		   ) {
-			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);
+			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);
 
 			_control_status.flags.synthetic_mag_z = true;
 
@@ -140,8 +172,6 @@ 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;
 
 
@@ -175,14 +205,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_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;
+		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;
 
 		if (_control_status.flags.mag) {
 
 			if (continuing_conditions_passing && _control_status.flags.yaw_align) {
 
-				if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
+				if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) {
 					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;
@@ -203,14 +233,35 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 					}
 
 					if (_control_status.flags.mag_dec) {
-						fuseDeclination(0.5f);
+
+						// 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);
+						}
 					}
 				}
 
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
 
 				if (is_fusion_failing) {
-					if (no_ne_aiding_or_pre_takeoff) {
+					if (no_ne_aiding_or_not_moving) {
 						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;
@@ -271,6 +322,9 @@ 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;
@@ -329,29 +383,36 @@ 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;
 
-	// reset covariances to default
-	resetMagCov();
+	static constexpr float kMagEarthMinGauss = 0.01f; // minimum difference in mag earth field strength for reset (Gauss)
 
 	// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
-	if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
-
+	if (_wmm_earth_field_gauss.longerThan(0.f) && _wmm_earth_field_gauss.isAllFinite()) {
 		// 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_B: reset
+		// 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
 		if (!reset_heading && _control_status.flags.yaw_align) {
-			// mag_B: reset using WMM
-			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-			_state.mag_B = mag - (R_to_body * mag_earth_pred);
+			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)
 
 		} 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);
 		}
@@ -359,34 +420,32 @@ 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 field states
+		// Use the magnetometer measurement to reset the heading
 		if (reset_heading) {
 			resetMagHeading(mag);
 		}
 
-		// mag_I: use the last magnetometer measurements to reset the field states
-		_state.mag_I = _R_to_earth * 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();
+		}
 	}
 
-	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 {
+	if ((_state.mag_I - mag_I_before_reset).longerThan(0.f)) {
 		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 (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)
-				);
-		}
+	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));
 	}
 
 	// record the start time for the magnetic field alignment
@@ -451,8 +510,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(_mag_strength_gps)) {
-			if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
+		if (PX4_ISFINITE(_wmm_field_strength_gauss)) {
+			if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) {
 				_control_status.flags.mag_field_disturbed = true;
 				is_check_failing = true;
 			}
@@ -475,9 +534,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(_mag_inclination_gps)) {
+		if (PX4_ISFINITE(_wmm_inclination_rad)) {
 			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
-			const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps);
+			const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);
 
 			if (fabsf(inc_error_rad) > inc_tol_rad) {
 				_control_status.flags.mag_field_disturbed = true;
@@ -549,13 +608,60 @@ 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) {
-		// use parameter value until GPS is available, then use value returned by geo library
-		if (PX4_ISFINITE(_mag_declination_gps)) {
-			return _mag_declination_gps;
+	} 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;
 		}
 	}
 
-	// otherwise use the parameter value
-	return math::radians(_params.mag_declination_deg);
+	return false;
 }

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

@@ -99,7 +99,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 				resetQuatCov(_params.mag_heading_noise);
 			}
 
-			resetMagCov();
+			resetMagEarthCov();
+			resetMagBiasCov();
 
 			return false;
 		}
@@ -150,51 +151,44 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	return false;
 }
 
-bool Ekf::fuseDeclination(float decl_sigma)
+bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states)
 {
-	float decl_measurement = NAN;
+	VectorState H;
+	float decl_pred;
+	float innovation_variance;
 
-	if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
-	    && PX4_ISFINITE(_mag_declination_gps)
-	   ) {
-		decl_measurement = _mag_declination_gps;
+	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R, FLT_EPSILON,
+			&decl_pred, &innovation_variance, &H);
 
-	} 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);
+	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;
 	}
 
-	if (PX4_ISFINITE(decl_measurement)) {
+	// Calculate the Kalman gains
+	VectorState Kfusion = P * H / innovation_variance;
 
-		// observation variance (rad**2)
-		const float R_DECL = sq(decl_sigma);
+	if (!update_all_states) {
+		// zero non-mag Kalman gains if not updating all states
 
-		VectorState H;
-		float decl_pred;
-		float innovation_variance;
+		// 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);
 
-		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;
+		// 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;
 	}
 
-	return false;
+	const bool is_fused = measurementUpdate(Kfusion, H, R, innovation);
+
+	_fault_status.flags.bad_mag_decl = !is_fused;
+
+	return is_fused;
 }
 
 float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)

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

@@ -122,8 +122,16 @@ 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
@@ -144,6 +152,7 @@ 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
 
@@ -160,7 +169,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();
+						resetFlowFusion(flow_sample);
 
 						if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {
 							resetTerrainToFlow();
@@ -194,7 +203,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				} else {
 					if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
 						ECL_INFO("starting optical flow, resetting");
-						resetFlowFusion();
+						resetFlowFusion(flow_sample);
 						_control_status.flags.opt_flow = true;
 
 					} else if (_control_status.flags.opt_flow_terrain) {
@@ -213,13 +222,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 	}
 }
 
-void Ekf::resetFlowFusion()
+void Ekf::resetFlowFusion(const flowSample &flow_sample)
 {
 	ECL_INFO("reset velocity to flow");
 	_information_events.flags.reset_vel_to_flow = true;
 
-	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(_flow_sample_delayed);
-	resetHorizontalVelocityTo(_flow_vel_ne, flow_vel_var);
+	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample);
+	resetHorizontalVelocityTo(getFilteredFlowVelNE(), 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) {
@@ -273,6 +282,8 @@ void Ekf::stopFlowFusion()
 
 		_innov_check_fail_status.flags.reject_optflow_X = false;
 		_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+		_flow_counter = 0;
 	}
 }
 

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

@@ -54,6 +54,7 @@ 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);
 

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

@@ -84,8 +84,9 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 
 			if (isTiltOk() && isDataInRange()) {
 				updateStuckCheck();
+				updateFogCheck(getDistBottom(), _sample.time_us);
 
-				if (!_is_stuck) {
+				if (!_is_stuck && !_is_blocked) {
 					_is_sample_valid = true;
 					_time_last_valid_us = _sample.time_us;
 				}
@@ -146,5 +147,23 @@ 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

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

@@ -44,6 +44,7 @@
 #include "Sensor.hpp"
 
 #include <matrix/math.hpp>
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
 
 namespace estimator
 {
@@ -99,6 +100,8 @@ 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);
@@ -129,6 +132,7 @@ 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{};
 
@@ -172,6 +176,14 @@ 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

src/modules/ekf2/EKF/common.h

@@ -421,6 +421,7 @@ 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
@@ -513,7 +514,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 bad_acc_bias      : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
+		bool __UNUSED          : 1; ///< 9 -
 		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;

src/modules/ekf2/EKF/covariance.cpp

@@ -96,7 +96,8 @@ void Ekf::initialiseCovariance()
 	resetAccelBiasCov();
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	resetMagCov();
+	resetMagEarthCov();
+	resetMagBiasCov();
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_WIND)
@@ -340,11 +341,17 @@ void Ekf::resetAccelBiasCov()
 }
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-void Ekf::resetMagCov()
+void Ekf::resetMagEarthCov()
 {
-	ECL_INFO("reset mag covariance");
+	ECL_INFO("reset mag earth 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

src/modules/ekf2/EKF/ekf.cpp

@@ -81,6 +81,7 @@ 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;
@@ -114,8 +115,6 @@ void Ekf::reset()
 
 	_last_known_pos.setZero();
 
-	_time_acc_bias_check = 0;
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	_baro_counter = 0;
 #endif // CONFIG_EKF2_BAROMETER
@@ -276,68 +275,100 @@ 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(double lat_deg, double lon_deg, float accuracy,
-		uint64_t timestamp_observation)
+bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude,
+		const float eph,
+		const float epv, uint64_t timestamp_observation)
 {
-	if (!_pos_ref.isInitialized()) {
-		ECL_WARN("unable to reset global position, position reference not initialized");
+	if (!checkLatLonValidity(latitude, longitude)) {
 		return false;
 	}
 
-	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);
+	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;
 
 	// apply a first order correction using velocity at the delayed time horizon and the delta time
-	if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) {
+	if ((timestamp_observation > 0) && local_position_is_valid()) {
 
 		timestamp_observation = math::min(_time_latest_us, timestamp_observation);
 
-		float diff_us = 0.f;
+		float dt_us;
 
 		if (_time_delayed_us >= timestamp_observation) {
-			diff_us = static_cast<float>(_time_delayed_us - timestamp_observation);
+			dt_us = static_cast<float>(_time_delayed_us - timestamp_observation);
 
 		} else {
-			diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
+			dt_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
 		}
 
-		const float dt_s = diff_us * 1e-6f;
-		pos_corrected += _state.vel.xy() * dt_s;
+		const float dt_s = dt_us * 1e-6f;
+		pos_correction = _state.vel * dt_s;
 	}
 
-	const float obs_var = math::max(sq(accuracy), sq(0.01f));
+	{
+		const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy();
 
-	const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
-	const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
+		const float obs_var = math::max(sq(eph), sq(0.01f));
 
-	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 Vector2f innov = Vector2f(_state.pos.xy()) - hpos;
+		const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
 
-	const bool innov_rejected = (test_ratio.max() > 1.f);
+		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));
 
-	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;
+		const bool innov_rejected = (test_ratio > 1.f);
 
-		resetHorizontalPositionTo(pos_corrected, obs_var);
-		_last_known_pos.xy() = _state.pos.xy();
-		return 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;
 
-	} 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");
+			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
 			_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;
 		}
 	}
 
-	return false;
+	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;
 }
 
 void Ekf::updateParameters()

src/modules/ekf2/EKF/ekf.h

@@ -122,7 +122,10 @@ public:
 	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
 
 	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
-	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
+	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 &getFlowCompensated() const { return _flow_rate_compensated; }
 	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; }
@@ -258,8 +261,11 @@ 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 setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
-	void updateWmm(double lat, double lon);
+	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);
 
 	// 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;
@@ -466,6 +472,9 @@ 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
 
@@ -524,7 +533,7 @@ public:
 		return true;
 	}
 
-	bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
+	bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
 			uint64_t timestamp_observation);
 
 	/**
@@ -603,8 +612,6 @@ 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
@@ -632,10 +639,12 @@ 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
 
@@ -758,6 +767,12 @@ 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;
@@ -770,8 +785,8 @@ private:
 		     bool update_all_states = false, bool update_tilt = false);
 
 	// fuse magnetometer declination measurement
-	// argument passed in is the declination uncertainty in radians
-	bool fuseDeclination(float decl_sigma);
+	//  R: declination observation variance (rad**2)
+	bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false);
 
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -871,7 +886,7 @@ private:
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion(const imuSample &imu_delayed);
-	void resetFlowFusion();
+	void resetFlowFusion(const flowSample &flow_sample);
 	void stopFlowFusion();
 
 	void updateOnGroundMotionForOpticalFlowChecks();
@@ -1077,7 +1092,6 @@ private:
 	void stopAuxVelFusion();
 #endif // CONFIG_EKF2_AUXVEL
 
-	void checkVerticalAccelerationBias(const imuSample &imu_delayed);
 	void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
 	Likelihood estimateInertialNavFallingLikelihood() const;
 
@@ -1106,7 +1120,8 @@ private:
 	void resetQuatCov(const Vector3f &rot_var_ned);
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	void resetMagCov();
+	void resetMagEarthCov();
+	void resetMagBiasCov();
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_WIND)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -72,97 +72,151 @@ 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)
 {
-	// 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);
-
-		// 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;
-		}
-
-		const float gps_alt_ref_prev = _gps_alt_ref;
-
-		// reinitialize map projection to latitude, longitude, altitude, and reset position
-		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
-		_gps_alt_ref = altitude;
-
-		updateWmm(current_lat, current_lon);
-
-		_gpos_origin_eph = eph;
-		_gpos_origin_epv = epv;
-
-		_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();
-#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));
-#if defined(CONFIG_EKF2_GNSS)
-			// adjust existing GPS height bias
-			_gps_hgt_b_est.setBias(gps_hgt_bias);
-#endif // CONFIG_EKF2_GNSS
-		}
-
-		return true;
+	if (!setLatLonOrigin(latitude, longitude, eph)) {
+		return false;
 	}
 
-	return false;
+	// altitude is optional
+	setAltOrigin(altitude, epv);
+
+	return true;
 }
 
-void Ekf::updateWmm(const double lat, const double lon)
+bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
 {
-#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;
-		}
+	if (!checkLatLonValidity(latitude, longitude)) {
+		return false;
 	}
 
-#endif // CONFIG_EKF2_MAGNETOMETER
+	bool current_pos_available = false;
+	double current_lat = static_cast<double>(NAN);
+	double current_lon = static_cast<double>(NAN);
+
+	// 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;
+#if defined(CONFIG_EKF2_GNSS)
+		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));
+#if defined(CONFIG_EKF2_GNSS)
+		// adjust existing GPS height bias
+		_gps_hgt_b_est.setBias(gps_hgt_bias);
+#endif // CONFIG_EKF2_GNSS
+	}
+
+	return true;
+}
+
+bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude,
+		const float eph, const float epv)
+{
+	if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
+		return false;
+	}
+
+	// altitude is optional
+	setAltOriginFromCurrentPos(altitude, epv);
+
+	return true;
+}
+
+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
 {

src/modules/ekf2/EKF/estimator_interface.h

@@ -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(_mag_declination_gps);
+			val = math::degrees(_wmm_declination_rad);
 			return true;
 
 		} else {
@@ -261,7 +261,7 @@ public:
 	bool get_mag_inc_deg(float &val) const
 	{
 		if (_NED_origin_initialised) {
-			val = math::degrees(_mag_inclination_gps);
+			val = math::degrees(_wmm_inclination_rad);
 			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(_mag_inclination_gps);
+		inc_ref_deg = math::degrees(_wmm_inclination_rad);
 		strength_gs = _mag_strength;
-		strength_ref_gs = _mag_strength_gps;
+		strength_ref_gs = _wmm_field_strength_gauss;
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -451,13 +451,14 @@ 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)
-	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)
+	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_inclination{NAN};
 	float _mag_strength{NAN};

src/modules/ekf2/EKF/height_control.cpp

@@ -39,7 +39,6 @@
 
 void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 {
-	checkVerticalAccelerationBias(imu_delayed);
 	checkVerticalAccelerationHealth(imu_delayed);
 
 #if defined(CONFIG_EKF2_BAROMETER)
@@ -120,106 +119,6 @@ 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

src/modules/ekf2/EKF/velocity_fusion.cpp

@@ -91,6 +91,9 @@ 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
@@ -117,6 +120,8 @@ 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

src/modules/ekf2/EKF2.cpp

@@ -162,6 +162,7 @@ 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)),
@@ -545,9 +546,8 @@ void EKF2::Run()
 						accuracy = vehicle_command.param3;
 					}
 
-					// TODO add check for lat and long validity
-					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
-							accuracy, timestamp_observation)
+					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, vehicle_command.param7,
+							accuracy, accuracy, timestamp_observation)
 					   ) {
 						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -571,7 +571,12 @@ 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);
 			}
 		}
 	}
@@ -1938,7 +1943,6 @@ 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;
 
@@ -2027,6 +2031,9 @@ 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);
 
@@ -2664,7 +2671,6 @@ 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;
 

src/modules/ekf2/EKF2.hpp

@@ -623,6 +623,7 @@ 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,

src/modules/ekf2/params_range_finder.yaml

@@ -150,3 +150,16 @@ 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