diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a0bab90afa..f65442632a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -35,16 +35,16 @@ RAW_LEAD_SAFETY_TTC = 7.0 RAW_LEAD_SAFETY_DISTANCE = 40.0 STANDSTILL_LEAD_NUDGE_ACCEL = 0.05 STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0 -STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.20 +STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.35 STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5 STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6 -STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 1.5 +STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 0.8 STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08 LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75 LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 -LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.5 -LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.45 -LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.35 +LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 +LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25 +LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2 LEAD_DEPART_ACCEL_HOLD_TIME = 1.2 LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 1.5 LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6 @@ -945,11 +945,8 @@ class LongitudinalPlanner: return False lead_radar = bool(getattr(lead, "radar", False)) - if lead_radar: - return False - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB: + lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) + if not lead_radar and lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB: return False lead_speed = max(float(lead.vLead), 0.0) diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 04dd4fef07..b16327501e 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -102,6 +102,7 @@ class ConditionalExperimentalMode: self.mode_hold_until = 0.0 self.mode_false_since = 0.0 self._prev_ce_status = None + self.close_stopped_lead_since = 0.0 def update(self, v_ego, sm, starpilot_toggles): now = time.monotonic() @@ -157,7 +158,18 @@ class ConditionalExperimentalMode: self.stop_light_detected &= not is_manual_ce_status(self.status_value) self.stop_light_filter.x = 0 + # At standstill behind a close, stopped lead, prefer Chill over CEM. + # Why: CEM is slow to release when the lead pulls away (waits on stop-light + # filter + STOP_LIGHT_DETECTED_HOLD_TIME). Chill reacts to lead departure faster. + STANDSTILL_LEAD_OVERRIDE_MAX_DISTANCE = 15.0 # meters + STANDSTILL_LEAD_OVERRIDE_MAX_SPEED = 1.0 # m/s + # Persistence required before handing off CEM->Chill. Cross-traffic (cars passing + # perpendicular in front) briefly registers as a close, stopped lead and would + # otherwise flap CEM out of EXP. Real queue-mates persist much longer than this. + STANDSTILL_LEAD_OVERRIDE_PERSIST_TIME = 0.5 # seconds + def get_standstill_stop_hold(self, sm): + now = time.monotonic() dash_stop_sign = ( bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) or bool(getattr(sm["starpilotCarState"], "dashboardStopSign", 0) > 0) @@ -168,6 +180,7 @@ class ConditionalExperimentalMode: if pedal_override or not bool(sm["carState"].standstill): self.standstill_stop_reason = None + self.close_stopped_lead_since = 0.0 return False if dash_stop_sign: @@ -179,8 +192,24 @@ class ConditionalExperimentalMode: self.standstill_stop_reason = None if self.standstill_stop_reason == "sign": + self.close_stopped_lead_since = 0.0 return True + lead = getattr(self.starpilot_planner, "lead_one", None) + close_stopped_lead = bool( + lead is not None and + getattr(lead, "status", False) and + float(getattr(lead, "dRel", float("inf"))) < self.STANDSTILL_LEAD_OVERRIDE_MAX_DISTANCE and + float(getattr(lead, "vLead", float("inf"))) < self.STANDSTILL_LEAD_OVERRIDE_MAX_SPEED + ) + if close_stopped_lead: + if self.close_stopped_lead_since == 0.0: + self.close_stopped_lead_since = now + if (now - self.close_stopped_lead_since) >= self.STANDSTILL_LEAD_OVERRIDE_PERSIST_TIME: + return False + else: + self.close_stopped_lead_since = 0.0 + return bool(self.stop_light_detected or force_stop_active or model_stopped) def check_conditions(self, v_ego, sm, starpilot_toggles):