Change radar-based FCW to model-based FCW (#22379)

* just use MPC for fcw checking

* thats already bad

* model FCW is always good

* better fcw

* should be good for now

* comment

* linting

* cleaner

* unused
old-commit-hash: a8b4249ebc

release/files_common

@@ -235,7 +235,6 @@ selfdrive/controls/lib/pid.py
 selfdrive/controls/lib/longitudinal_planner.py
 selfdrive/controls/lib/radar_helpers.py
 selfdrive/controls/lib/vehicle_model.py
-selfdrive/controls/lib/fcw.py
 
 selfdrive/controls/lib/cluster/*
 

selfdrive/controls/controlsd.py

@@ -270,7 +270,9 @@ class Controls:
       self.events.add(EventName.deviceFalling)
     if log.PandaState.FaultType.relayMalfunction in self.sm['pandaState'].faults:
       self.events.add(EventName.relayMalfunction)
-    if self.sm['longitudinalPlan'].fcw or (self.enabled and self.sm['modelV2'].meta.hardBrakePredicted):
+    planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
+    model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed
+    if planner_fcw or model_fcw:
       self.events.add(EventName.fcw)
 
     if TICI:

selfdrive/controls/lib/fcw.py

@@ -1,76 +0,0 @@
-import math
-from collections import defaultdict
-
-from common.numpy_fast import interp
-
-_FCW_A_ACT_V = [-3., -2.]
-_FCW_A_ACT_BP = [0., 30.]
-
-
-class FCWChecker():
-  def __init__(self):
-    self.reset_lead(0.0)
-    self.common_counters = defaultdict(lambda: 0)
-
-  def reset_lead(self, cur_time):
-    self.last_fcw_a = 0.0
-    self.v_lead_max = 0.0
-    self.lead_seen_t = cur_time
-    self.last_fcw_time = 0.0
-    self.last_min_a = 0.0
-
-    self.counters = defaultdict(lambda: 0)
-
-  @staticmethod
-  def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
-    max_ttc = 5.0
-
-    v_rel = v_ego - v_lead
-    a_rel = a_ego - a_lead
-
-    # assuming that closing gap ARel comes from lead vehicle decel,
-    # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
-    # This helps underweighting ARel when v_lead is close to zero.
-    t_decel = 2.
-    a_rel = min(a_rel, v_lead / t_decel)
-
-    # delta of the quadratic equation to solve for ttc
-    delta = v_rel**2 + 2 * x_lead * a_rel
-
-    # assign an arbitrary high ttc value if there is no solution to ttc
-    if delta < 0.1 or (math.sqrt(delta) + v_rel < 0.1):
-      ttc = max_ttc
-    else:
-      ttc = min(2 * x_lead / (math.sqrt(delta) + v_rel), max_ttc)
-    return ttc
-
-  def update(self, mpc_solution_a, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
-
-    self.last_min_a = min(mpc_solution_a)
-    self.v_lead_max = max(self.v_lead_max, v_lead)
-
-    self.common_counters['blinkers'] = self.common_counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
-    self.common_counters['v_ego'] = self.common_counters['v_ego'] + 1 if v_ego > 5.0 else 0
-
-    if (fcw_lead > 0.99):
-      ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
-      self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
-      self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
-      self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
-      self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
-      self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
-      self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
-
-      a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
-      a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
-
-      future_fcw_allowed = all(c >= 10 for c in self.counters.values())
-      future_fcw_allowed = future_fcw_allowed and all(c >= 10 for c in self.common_counters.values())
-      future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
-
-      if future_fcw and (self.last_fcw_time + 5.0 < cur_time):
-        self.last_fcw_time = cur_time
-        self.last_fcw_a = self.last_min_a
-        return True
-
-    return False

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -30,7 +30,7 @@ X_EGO_E2E_COST = 100.
 A_EGO_COST = .1
 J_EGO_COST = .2
 DANGER_ZONE_COST = 1e3
-CRASH_DISTANCE = 1.5
+CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
 T_IDXS = np.array(T_IDXS_LST)
 
@@ -190,7 +190,7 @@ class LongitudinalMpc():
     self.status = False
     self.new_lead = False
     self.prev_lead_status = False
-    self.crashing = False
+    self.crash_cnt = 0.0
     self.prev_lead_x = 10
     self.solution_status = 0
     self.x0 = np.zeros(X_DIM)
@@ -259,7 +259,6 @@ class LongitudinalMpc():
       min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
       if x_lead < min_x_lead:
         x_lead = min_x_lead
-        self.crashing = True
 
       if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
         v_lead = 0.0
@@ -289,7 +288,6 @@ class LongitudinalMpc():
 
   def update(self, carstate, radarstate, v_cruise):
     v_ego = self.x0[1]
-    self.crashing = False
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)
@@ -319,8 +317,11 @@ class LongitudinalMpc():
     self.params[:,2] = np.min(x_obstacles, axis=1)
 
     self.run()
-    self.crashing = self.crashing or np.sum(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) > 0
+    if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
-
+            radarstate.leadOne.modelProb > 0.9):
+      self.crash_cnt += 1
+    else:
+      self.crash_cnt = 0
 
   def update_with_xva(self, x, v, a):
     self.yref[:,1] = x

selfdrive/controls/lib/longitudinal_planner.py

@@ -6,10 +6,8 @@ from common.numpy_fast import interp
 import cereal.messaging as messaging
 from cereal import log
 from common.realtime import DT_MDL
-from common.realtime import sec_since_boot
 from selfdrive.modeld.constants import T_IDXS
 from selfdrive.config import Conversions as CV
-from selfdrive.controls.lib.fcw import FCWChecker
 from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
@@ -49,7 +47,6 @@ class Planner():
     self.mpc = LongitudinalMpc()
 
     self.fcw = False
-    self.fcw_checker = FCWChecker()
 
     self.v_desired = init_v
     self.a_desired = init_a
@@ -63,7 +60,6 @@ class Planner():
 
 
   def update(self, sm, CP):
-    cur_time = sec_since_boot()
     v_ego = sm['carState'].vEgo
     a_ego = sm['carState'].aEgo
 
@@ -102,18 +98,10 @@ class Planner():
     self.a_desired_trajectory = self.mpc.a_solution[:CONTROL_N]
     self.j_desired_trajectory = self.mpc.j_solution[:CONTROL_N]
 
-    # determine fcw
+    #TODO counter is only needed because radar is glitchy, remove once radar is gone
-    if self.mpc.new_lead:
+    self.fcw = self.mpc.crash_cnt > 5
-      self.fcw_checker.reset_lead(cur_time)
-    blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
-    self.fcw = self.fcw_checker.update(self.mpc.x_sol[:,2], cur_time,
-                                       sm['controlsState'].active,
-                                       v_ego, sm['carState'].aEgo,
-                                       self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
-                                       self.lead_1.yRel, self.lead_1.vLat,
-                                       self.lead_1.fcw, blinkers) and not sm['carState'].brakePressed
     if self.fcw:
-      cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
+      cloudlog.info("FCW triggered")
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired