diff --git a/release/files_common b/release/files_common index 7a56262f15..c31ce0ec6b 100644 --- a/release/files_common +++ b/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/* diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a8b9a61808..b6a02bc923 100755 --- a/selfdrive/controls/controlsd.py +++ b/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: diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py deleted file mode 100644 index 04fed92669..0000000000 --- a/selfdrive/controls/lib/fcw.py +++ /dev/null @@ -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 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d9b1496400..824b6032e5 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index ba87b35dae..339b3c3c5b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 - if self.mpc.new_lead: - 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 + #TODO counter is only needed because radar is glitchy, remove once radar is gone + self.fcw = self.mpc.crash_cnt > 5 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