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) _param_ekf2_delay_max, (ParamExtInt) _param_ekf2_imu_ctrl, (ParamExtFloat) _param_ekf2_vel_lim, + (ParamBool) _param_ekf2_engine_wrm, #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) 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