From c1a85d37e8b5a4c5453ae7a82bcae60802a8f88b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 20 May 2025 11:54:27 +0200 Subject: [PATCH] latlongcontrol: set lateral acc to 0 if NAN Signed-off-by: Silvan Fuhrer --- .../FwLateralLongitudinalControl.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 953795b2f3..3d618fe4de 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -271,6 +271,10 @@ void FwLateralLongitudinalControl::Run() _lat_control_sp.lateral_acceleration; } + if (!PX4_ISFINITE(lateral_accel_sp)) { + lateral_accel_sp = 0.f; // mitigation if no valid setpoint is received: 0 lateral acceleration + } + lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp); lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_configuration.lateral_accel_max, _lateral_configuration.lateral_accel_max);