From b1e2f219c9523736e20378247cfe03f1936d9baf Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Sat, 18 Sep 2021 22:21:03 -0700 Subject: [PATCH] Clip radar acceleration for mpc stability old-commit-hash: a1fcedda21d1ff7d53bd647dc4359c5e5941bf47 --- selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py b/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py index 6c9064e6d9..809e5eac58 100644 --- a/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py +++ b/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py @@ -4,6 +4,7 @@ import math import numpy as np from common.realtime import sec_since_boot +from common.numpy_fast import clip from selfdrive.swaglog import cloudlog from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG, CONTROL_N @@ -211,7 +212,7 @@ class LeadMpc(): if lead is not None and lead.status: x_lead = lead.dRel v_lead = max(0.0, lead.vLead) - a_lead = lead.aLeadK + a_lead = clip(lead.aLeadK, -5.0, 5.0) # MPC will not converge if immidiate crash is expected # Clip lead distance to what is still possible to brake for