From bcd67b7bada67b10ab58c3a62189840b498c91e3 Mon Sep 17 00:00:00 2001
From: Mahima Yoga <mahima@auterion.com>
Date: Fri, 5 Dec 2025 15:08:37 +0100
Subject: [PATCH] ekf: enable constant position fusion during engine warm-up
 (#26041)

In cold weather, fuel engines need to be warmed up. Currently, this is done by switching to stabilized mode and throttle up. The issue is that when not using GNSS data, the vehicle is not detected as "at rest" due to vibrations and cannot switch into auto/takeoff modes.

If EKF2_ENGINE_WRM is enabled, and if the vehicle is armed and landed, constant position fusion is enabled.
---
 src/modules/ekf2/EKF2.cpp    | 8 +++++++-
 src/modules/ekf2/EKF2.hpp    | 1 +
 src/modules/ekf2/module.yaml | 8 ++++++++
 3 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index c82c447686..2a4a3133ca 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -2614,11 +2614,14 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
 		// vehicle_status
 		vehicle_status_s vehicle_status;
 
+		bool armed = false;
+
 		if (_status_sub.copy(&vehicle_status)
 		    && (ekf2_timestamps.timestamp < vehicle_status.timestamp + 3_s)) {
 
 			// initially set in_air from arming_state (will be overridden if land detector is available)
-			flags.in_air = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			flags.in_air = armed;
 
 			// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
 			flags.is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
@@ -2644,6 +2647,9 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
 			flags.at_rest = vehicle_land_detected.at_rest;
 			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;
 		}
 
 		launch_detection_status_s launch_detection_status;
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index c95e3c3623..4c39ea1188 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -489,6 +489,7 @@ private:
 		(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
 		(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
 		(ParamExtFloat<px4::params::EKF2_VEL_LIM>) _param_ekf2_vel_lim,
+		(ParamBool<px4::params::EKF2_ENGINE_WRM>) _param_ekf2_engine_wrm,
 
 #if defined(CONFIG_EKF2_AUXVEL)
 		(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml
index cd815ed56f..b0462ec5f8 100644
--- a/src/modules/ekf2/module.yaml
+++ b/src/modules/ekf2/module.yaml
@@ -183,3 +183,11 @@ parameters:
       max: 299792458
       unit: m/s
       decimal: 1
+
+    EKF2_ENGINE_WRM:
+      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.
+      type: boolean
+      default: 0