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.
This commit is contained in:
bresch
2026-03-17 16:25:24 +01:00
committed by Mathieu Bresciani
parent 24bbf5efd9
commit f90b159401
4 changed files with 17 additions and 9 deletions
+10
View File
@@ -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<int32_t>(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; return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
} }
+2 -3
View File
@@ -2638,8 +2638,7 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
flags.in_air = !vehicle_land_detected.landed; flags.in_air = !vehicle_land_detected.landed;
flags.gnd_effect = vehicle_land_detected.in_ground_effect; 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_pos_lock.get() && !flags.in_air && _ekf.isGlobalHorizontalPositionValid();
flags.constant_pos = _param_ekf2_engine_wrm.get() && !flags.in_air && armed;
} }
launch_detection_status_s launch_detection_status; launch_detection_status_s launch_detection_status;
@@ -2647,7 +2646,7 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
if (_launch_detection_status_sub.copy(&launch_detection_status) if (_launch_detection_status_sub.copy(&launch_detection_status)
&& (ekf2_timestamps.timestamp < launch_detection_status.timestamp + 3_s)) { && (ekf2_timestamps.timestamp < launch_detection_status.timestamp + 3_s)) {
flags.constant_pos = (launch_detection_status.launch_detection_state == flags.constant_pos |= (launch_detection_status.launch_detection_state ==
launch_detection_status_s::STATE_WAITING_FOR_LAUNCH); launch_detection_status_s::STATE_WAITING_FOR_LAUNCH);
} }
+1 -1
View File
@@ -487,7 +487,7 @@ private:
(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max, (ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl, (ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
(ParamExtFloat<px4::params::EKF2_VEL_LIM>) _param_ekf2_vel_lim, (ParamExtFloat<px4::params::EKF2_VEL_LIM>) _param_ekf2_vel_lim,
(ParamBool<px4::params::EKF2_ENGINE_WRM>) _param_ekf2_engine_wrm, (ParamBool<px4::params::EKF2_POS_LOCK>) _param_ekf2_pos_lock,
#if defined(CONFIG_EKF2_AUXVEL) #if defined(CONFIG_EKF2_AUXVEL)
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>) (ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
+3 -4
View File
@@ -186,10 +186,9 @@ parameters:
unit: m/s unit: m/s
decimal: 1 decimal: 1
EKF2_ENGINE_WRM: EKF2_POS_LOCK:
description: description:
short: Enable constant position fusion during engine warmup short: Enable constant position fusion while on ground
long: When enabled, constant position fusion is enabled when the vehicle is landed and armed. 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.
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 type: boolean
default: 0 default: 0