diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 3edec4f45..dbea79348 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -134,9 +134,6 @@ class LongControl: def update_mpc_mode(self, experimental_mode): new_mode = 'blended' if experimental_mode else 'acc' - if self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc': - self.mode_transition_timer = 0.0 - if new_mode != self.current_mode: self.prev_mode = self.current_mode self.transitioning = True @@ -320,6 +317,9 @@ class LongControl: freeze_integrator = self.vehicle_tuning.get_integrator_freeze( self.last_output_accel, a_target, error, CS.vEgo, accel_limits, ) + leaving_experimental = self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc' + if leaving_experimental: + freeze_integrator = True raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, freeze_integrator=freeze_integrator) raw_output_accel = self._cap_positive_output_on_negative_target(raw_output_accel, a_target, error, CS) @@ -337,7 +337,13 @@ class LongControl: raw_output_accel, CS.vEgo, should_stop, leads, ) - if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': + if leaving_experimental: + if raw_output_accel > self.last_output_accel: + progress = min(1.0, self.mode_transition_timer / max(self.mode_transition_duration, 1e-3)) + output_accel = self.last_output_accel + (raw_output_accel - self.last_output_accel) * progress + else: + output_accel = raw_output_accel + elif self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': if raw_output_accel < 0 and raw_output_accel < self.last_output_accel: progress = min(1.0, self.mode_transition_timer / self.mode_transition_duration) # Soften transition at low urgency, but keep sharp for high decel diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0cf199b6d..b615199f2 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -291,6 +291,10 @@ EXPERIMENTAL_RELEASE_ACCEL_MAX_LATERAL_OFFSET = 1.5 EXPERIMENTAL_RELEASE_ACCEL_MIN_HEADWAY_MARGIN = 0.0 EXPERIMENTAL_RELEASE_ACCEL_MIN_DELTA_A = 0.12 EXPERIMENTAL_RELEASE_ACCEL_STEP = 0.06 +# Last few mph below CESpeed/CESpeedLead: mix MPC back in so experimental +# cannot crawl into the breakpoint and then snap to ACC. +EXPERIMENTAL_SPEED_HANDOFF_BAND = 5.0 * CV.MPH_TO_MS +EXPERIMENTAL_HANDOFF_KEEP_E2E_BRAKE = -0.15 MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 20.0 TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0 TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95 @@ -1776,6 +1780,38 @@ class LongitudinalPlanner: self.experimental_release_accel_until = 0.0 self.prev_experimental_mode = bool(experimental_mode) + def get_experimental_speed_handoff_weight(self, v_ego, experimental_mode, following_lead, + starpilot_toggles, hold_experimental): + if not experimental_mode or hold_experimental: + return 0.0 + + limit_key = "conditional_limit_lead" if following_lead else "conditional_limit" + limit = float(getattr(starpilot_toggles, limit_key, 0.0) or 0.0) + if limit <= 1.0: + return 0.0 + + return float(np.clip( + (float(v_ego) - (limit - EXPERIMENTAL_SPEED_HANDOFF_BAND)) / EXPERIMENTAL_SPEED_HANDOFF_BAND, + 0.0, + 1.0, + )) + + @staticmethod + def is_cem_following_lead(tracking_lead, d_rel, t_follow, v_ego): + # Same inputs as StarPilotFollowing.following_lead / CEM: published + # trackingLead and tFollow, plus leadOne.dRel inside 2*t_follow*v_ego. + return bool(tracking_lead and float(d_rel) < (float(t_follow) * 2.0) * float(v_ego)) + + @staticmethod + def apply_experimental_speed_handoff(output_a_target, output_a_target_mpc, output_a_target_e2e, speed_handoff): + if speed_handoff <= 0.0: + return output_a_target + # Keep a real E2E brake. Only mix MPC back in when experimental is crawling + # or matching ACC, not when it is already asking for more deceleration. + if output_a_target_e2e < min(output_a_target_mpc, EXPERIMENTAL_HANDOFF_KEEP_E2E_BRAKE): + return output_a_target + return (1.0 - speed_handoff) * output_a_target + speed_handoff * output_a_target_mpc + def get_experimental_release_accel_target(self, lead, v_ego, base_t_follow, prev_output_a_target, output_a_target, release_active): @@ -2404,6 +2440,26 @@ class LongitudinalPlanner: else: output_a_target = min(output_a_target_mpc, output_a_target_e2e) output_should_stop = output_should_stop_e2e or output_should_stop_mpc + cem_following_lead = self.is_cem_following_lead( + tracking_lead, + self.lead_one.dRel, + sm['starpilotPlan'].tFollow, + scene_v_ego, + ) + speed_handoff = self.get_experimental_speed_handoff_weight( + scene_v_ego, + experimental_mode, + cem_following_lead, + starpilot_toggles, + bool( + output_should_stop_e2e or + getattr(sm['starpilotPlan'], 'forcingStop', False) or + getattr(sm['starpilotPlan'], 'redLight', False) + ), + ) + output_a_target = self.apply_experimental_speed_handoff( + output_a_target, output_a_target_mpc, output_a_target_e2e, speed_handoff, + ) else: output_a_target, output_should_stop = get_accel_from_plan( self.v_desired_trajectory, self.a_desired_trajectory, diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 93eeebf67..8f50702f2 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -8,6 +8,7 @@ import openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes as vehicle_tun from opendbc.car.gm.values import CAR, GMFlags from opendbc.car.subaru.values import CAR as SUBARU_CAR from opendbc.car.toyota.values import CAR as TOYOTA_CAR +from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.longcontrol import ( LongControl, LongCtrlState, @@ -1531,3 +1532,50 @@ def test_gm_stock_truck_update_gradually_releases_stale_brake_integral(): ) assert -0.66 < output_accel < -0.44 + + +def test_leaving_experimental_slews_positive_accel(): + CP = make_longcontrol_cp() + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.pid + lc.experimental_mode = True + lc.current_mode = "blended" + lc.prev_mode = "acc" + lc.last_output_accel = 0.05 + CS = car.CarState.new_message(vEgo=20.0, aEgo=0.05, brakePressed=False) + CS.cruiseState.standstill = False + + lc.experimental_mode = False + output_accel = lc.update( + active=True, + CS=CS, + a_target=1.5, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + ) + + assert lc.current_mode == "acc" + assert lc.transitioning + assert output_accel > 0.05 + assert output_accel < 0.25 + + +def test_leaving_experimental_does_not_reset_mode_transition_timer(): + CP = make_longcontrol_cp() + lc = LongControl(CP) + lc.current_mode = "blended" + + lc.update_mpc_mode(False) + first = lc.mode_transition_timer + lc.update_mpc_mode(False) + + assert lc.current_mode == "acc" + assert lc.transitioning + assert first == pytest.approx(DT_CTRL) + assert lc.mode_transition_timer == pytest.approx(2.0 * DT_CTRL) + + for _ in range(int(lc.mode_transition_duration / DT_CTRL)): + lc.update_mpc_mode(False) + + assert not lc.transitioning diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index f19c2d760..cb65d8997 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -7,6 +7,7 @@ import numpy as np import pytest from cereal import log +from openpilot.common.constants import CV from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR from opendbc.car.gm.values import CAR as GM_CAR, GMFlags @@ -653,6 +654,8 @@ def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False): model_version=model_version, vEgoStopping=0.5, radar_takeoffs=radar_takeoffs, + conditional_limit=0.0, + conditional_limit_lead=0.0, ) @@ -3694,6 +3697,47 @@ def test_experimental_release_accel_transition_damps_moving_lead_handoff(): assert target == pytest.approx(0.09) +def test_experimental_speed_handoff_weight_ramps_into_cespeed(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP) + toggles = make_toggles() + limit = 35.0 * CV.MPH_TO_MS + band = longitudinal_planner_module.EXPERIMENTAL_SPEED_HANDOFF_BAND + toggles.conditional_limit = limit + + assert planner.get_experimental_speed_handoff_weight(limit - band - 1.0, True, False, toggles, False) == 0.0 + assert planner.get_experimental_speed_handoff_weight(limit, True, False, toggles, False) == pytest.approx(1.0) + assert planner.get_experimental_speed_handoff_weight(limit - 0.5 * band, True, False, toggles, False) == pytest.approx(0.5) + assert planner.get_experimental_speed_handoff_weight(limit, True, False, toggles, True) == 0.0 + assert planner.get_experimental_speed_handoff_weight(limit, False, False, toggles, False) == 0.0 + + +def test_experimental_speed_handoff_uses_lead_limit_when_following(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP) + toggles = make_toggles() + toggles.conditional_limit = 55.0 * CV.MPH_TO_MS + toggles.conditional_limit_lead = 20.0 * CV.MPH_TO_MS + + assert planner.get_experimental_speed_handoff_weight(20.0 * CV.MPH_TO_MS, True, True, toggles, False) == pytest.approx(1.0) + assert planner.get_experimental_speed_handoff_weight(20.0 * CV.MPH_TO_MS, True, False, toggles, False) == 0.0 + + +def test_experimental_speed_handoff_keeps_stronger_e2e_brake(): + kept = LongitudinalPlanner.apply_experimental_speed_handoff(-0.50, 0.20, -0.50, 1.0) + blended = LongitudinalPlanner.apply_experimental_speed_handoff(0.02, 0.40, 0.02, 0.5) + + assert kept == pytest.approx(-0.50) + assert blended == pytest.approx(0.21) + + +def test_experimental_speed_handoff_following_lead_matches_cem_window(): + # Distant radar-active lead is not CEM following_lead. + assert LongitudinalPlanner.is_cem_following_lead(True, 40.0, 1.5, 20.0) + assert not LongitudinalPlanner.is_cem_following_lead(True, 80.0, 1.5, 20.0) + assert not LongitudinalPlanner.is_cem_following_lead(False, 10.0, 1.5, 20.0) + + def test_experimental_release_accel_transition_does_not_mask_stopped_lead(): v_ego = 23.96 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/starpilot/controls/lib/starpilot_following.py b/starpilot/controls/lib/starpilot_following.py index 7d8d6ea8f..ea96a5599 100644 --- a/starpilot/controls/lib/starpilot_following.py +++ b/starpilot/controls/lib/starpilot_following.py @@ -101,7 +101,6 @@ class StarPilotFollowing: self.danger_jerk = self.base_danger_jerk self.speed_jerk = self.base_speed_jerk - self.following_lead = self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego self.slower_lead = False if self.starpilot_planner.starpilot_weather.weather_id != 0: @@ -109,6 +108,9 @@ class StarPilotFollowing: self.update_lane_change_gap(long_control_active, v_ego, sm, starpilot_toggles) + # After t_follow adjustments so CEM and the published tFollow use the same window. + self.following_lead = self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego + self.disable_throttle = False if self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.status: lead_distance = self.starpilot_planner.lead_one.dRel