From f90b159401d3959c806fe5aadda6927b71a92d41 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 17 Mar 2026 16:25:24 +0100 Subject: [PATCH] feat(ekf2): generalize engine warmup mode This is not only needed for engine warmup but in general, when the vehicle is static on the ground, relying on fixed position to maintain a valid global position estimate before takeoff, even when bumping it or starting the engine. --- src/lib/parameters/param_translation.cpp | 10 ++++++++++ src/modules/ekf2/EKF2.cpp | 7 +++---- src/modules/ekf2/EKF2.hpp | 2 +- src/modules/ekf2/module.yaml | 7 +++---- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 9e8bdbb974..d1266fdf17 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -223,5 +223,15 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node) } } + // 2026-03-19: translate EKF2_ENGINE_WRM to EKF2_POS_LOCK + { + if (strcmp("EKF2_ENGINE_WRM", node->name) == 0) { + int32_t delay_ms = static_cast(node->d); + param_set(param_find("EKF2_POS_LOCK"), &delay_ms); + PX4_INFO("migrating %s -> %s", "EKF2_ENGINE_WRM", "EKF2_POS_LOCK"); + return param_modify_on_import_ret::PARAM_MODIFIED; + } + } + return param_modify_on_import_ret::PARAM_NOT_MODIFIED; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 90c2d29487..84227fab6a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2638,8 +2638,7 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) flags.in_air = !vehicle_land_detected.landed; flags.gnd_effect = vehicle_land_detected.in_ground_effect; - // Enable constant position fusion for engine warmup when landed and armed - flags.constant_pos = _param_ekf2_engine_wrm.get() && !flags.in_air && armed; + flags.constant_pos = _param_ekf2_pos_lock.get() && !flags.in_air && _ekf.isGlobalHorizontalPositionValid(); } launch_detection_status_s launch_detection_status; @@ -2647,8 +2646,8 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) if (_launch_detection_status_sub.copy(&launch_detection_status) && (ekf2_timestamps.timestamp < launch_detection_status.timestamp + 3_s)) { - flags.constant_pos = (launch_detection_status.launch_detection_state == - launch_detection_status_s::STATE_WAITING_FOR_LAUNCH); + flags.constant_pos |= (launch_detection_status.launch_detection_state == + launch_detection_status_s::STATE_WAITING_FOR_LAUNCH); } _ekf.setSystemFlagData(flags); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 1e4e9f7b2b..36d7763995 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -487,7 +487,7 @@ private: (ParamExtFloat) _param_ekf2_delay_max, (ParamExtInt) _param_ekf2_imu_ctrl, (ParamExtFloat) _param_ekf2_vel_lim, - (ParamBool) _param_ekf2_engine_wrm, + (ParamBool) _param_ekf2_pos_lock, #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index edd89e8457..763ceab9c8 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -186,10 +186,9 @@ parameters: unit: m/s decimal: 1 - EKF2_ENGINE_WRM: + EKF2_POS_LOCK: description: - short: Enable constant position fusion during engine warmup - long: When enabled, constant position fusion is enabled when the vehicle is landed and armed. - This is intended for IC engine warmup (e.g., fuel engines on catapult) to allow mode transitions to auto/takeoff despite vibrations from running engines. + short: Enable constant position fusion while on ground + long: When enabled, constant position fusion is enabled when the vehicle is landeded if position has been initialized but has currently no vel/pos aiding. type: boolean default: 0