diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2037663821..d7d90f524a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -171,6 +171,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): else: output_a_target = output_a_target_mpc self.output_should_stop = output_should_stop_mpc + self.output_should_stop = self.accel_controller_should_stop(self.output_should_stop, is_e2e) for idx in range(2): accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index b47016014d..41807f44ac 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -14,14 +14,14 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( ) from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import ( - ACCEL_LIMIT_HORIZON_JERK, ACCEL_LIMIT_TRANSITION_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, BRAKING_ACCEL_LIMIT_THRESHOLD, - CAP_FILTER_FRAMES, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_GAIN, - LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, MATCHED_PACE_DECEL_RATE, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED, - PACE_RELIEF_DEADBAND, PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP, + ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, BRAKING_ACCEL_LIMIT_THRESHOLD, CAP_FILTER_FRAMES, + LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW, + LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_TAPER_GAIN, MATCHED_PACE_DECEL_RATE, MAX_LEAD_ACCEL_TAU, + MIN_LEAD_SPEED, PACE_RELIEF_DEADBAND, PACE_TARGET_ARM_MARGIN, PACE_TARGET_RESERVE, + PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP, STOP_GAP_RESERVE_LEAD_SPEED, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED, - STOP_HOLD_ACCEL_MAX, STOP_HOLD_DEPARTURE_ACCEL_MAX, STOP_HOLD_MAX_LEAD_DISTANCE, - STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile, + STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile, ) @@ -59,6 +59,7 @@ class AccelControllerResult: active: bool shadow_active: bool launching: bool + departure_launching: bool profile: AccelProfile profile_accel_max: float positive_accel_max: float @@ -99,7 +100,8 @@ class _ControllerPath: departure_launch: bool = False matched_lead: bool = False braking_limited: bool = False - accel_limit: float | None = None + braking_handoff: bool = False + pace_reserve_armed: bool = False matched_accel_limit: float | None = None @property @@ -137,7 +139,8 @@ class _ControllerPath: self.departure_launch = False self.matched_lead = False self.braking_limited = False - self.accel_limit = None + self.braking_handoff = False + self.pace_reserve_armed = False self.matched_accel_limit = None @@ -346,6 +349,7 @@ class AccelController: path.launching = False path.departure_launch = False path.matched_lead = False + path.pace_reserve_armed = False path.matched_accel_limit = None def _update_path(self, path: _ControllerPath, envelope: EnergyEnvelope, base_speed: float, v_ego: float, @@ -365,6 +369,9 @@ class AccelController: path.lead_switch_guard_frames = self.lead_loss_hold_frames elif path.lead_switch_guard_frames > 0: path.lead_switch_guard_frames -= 1 + if slot_changed or track_changed: + path.matched_lead = False + path.matched_accel_limit = None if has_lead: path.selected_lead = envelope.selected_lead path.selected_lead_track_id = envelope.selected_lead_track_id @@ -383,12 +390,9 @@ class AccelController: and (not has_lead or envelope.departure_lead_speed < STOP_HOLD_EXIT_SPEED)) stop_evidence = (stopped_lead_hold or envelope.cap < 0.50 or filtered_cap < 0.50 or (previous_stop and not path.launching) or invalid_lead) - confirmed_creep_departure = path.launching and path.departure_launch and self._creep_departure(path, envelope) - - if path.accel_limit is None: - path.accel_limit = profile_accel_max - else: - path.accel_limit = self._move(path.accel_limit, profile_accel_max, ACCEL_LIMIT_TRANSITION_JERK, self.dt) + confirmed_creep_departure = (path.launching and path.departure_launch and has_lead + and (envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED + or self._creep_departure(path, envelope))) if (path.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap) and has_lead and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD): path.braking_limited = True @@ -396,13 +400,18 @@ class AccelController: path.braking_limited = False if path.pace is None: - path.pace = min(base_speed, v_ego) if has_lead else base_speed + e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e + seed_from_ego = has_lead and planner_accel > BRAKING_ACCEL_LIMIT_THRESHOLD and not e2e_handoff + path.pace = min(base_speed, v_ego) if seed_from_ego else base_speed + path.braking_handoff = e2e_handoff and planner_accel < 0.0 path.state = AccelControllerState.free if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence: path.pace = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM) path.state = AccelControllerState.release path.launching = True path.departure_launch = False + elif path.braking_handoff and planner_accel >= 0.0: + path.braking_handoff = False path.pace = min(path.pace, base_speed) if (v_ego < STOP_HOLD_EGO_SPEED and stop_evidence and not confirmed_creep_departure @@ -415,8 +424,8 @@ class AccelController: separation = path.robust_departure_separation(lead_index) if math.isfinite(separation) and path.departure_references[lead_index] is None: path.departure_references[lead_index] = separation - raw_departure = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_EXIT_SPEED - and envelope.departure_cap > STOP_HOLD_EXIT_SPEED) + raw_departure = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED + and envelope.departure_cap > STOP_HOLD_CREEP_SPEED) or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames)) departed = self._creep_departure(path, envelope) or raw_departure path.departure_frames = path.departure_frames + 1 if departed else 0 @@ -465,7 +474,8 @@ class AccelController: path.departure_launch = False comfort_decel = PROFILE_CONFIGS[profile].comfort_decel - if (has_lead and not path.launching and path.state == AccelControllerState.restrict and envelope.closing_speed <= 0.0 + if (has_lead and not path.launching and path.state == AccelControllerState.restrict + and envelope.closing_speed <= 0.0 and v_ego >= path.filtered_lead_speed - VEGO_NOISE_TOLERANCE): path.matched_lead = True elif not has_lead and path.lead_loss_frames >= self.lead_loss_hold_frames: @@ -479,23 +489,23 @@ class AccelController: return path.pace if math.isfinite(path.filtered_lead_speed): recovery_speed = min(base_speed, path.filtered_lead_speed + min(LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_GAP_GAIN * envelope.usable_gap)) - recovery_error = max(recovery_speed - v_ego, 0.0) - desired_accel_limit = min(path.accel_limit, LEAD_MATCH_ACCEL_GAIN[profile] * recovery_error) + desired_accel_limit = min(profile_accel_max, LEAD_MATCH_TAPER_GAIN * max(recovery_speed - v_ego, 0.0)) else: desired_accel_limit = 0.0 if path.filtered_lead_accel < BRAKING_ACCEL_LIMIT_THRESHOLD: - desired_accel_limit = path.accel_limit + desired_accel_limit = profile_accel_max if path.matched_accel_limit is None: - path.matched_accel_limit = path.accel_limit + path.matched_accel_limit = profile_accel_max if path.lead_switch_guard_frames > 0: desired_accel_limit = min(desired_accel_limit, path.matched_accel_limit) - path.matched_accel_limit = min(path.accel_limit, self._move(path.matched_accel_limit, desired_accel_limit, LEAD_MATCH_ACCEL_SLEW, self.dt)) + path.matched_accel_limit = min(profile_accel_max, + self._move(path.matched_accel_limit, desired_accel_limit, LEAD_MATCH_ACCEL_SLEW, self.dt)) matched_ceiling = min(base_speed, filtered_cap) if matched_ceiling <= path.pace - PACE_RESTRICT_DEADBAND: path.pace = max(matched_ceiling, path.pace - MATCHED_PACE_DECEL_RATE * self.dt) path.state = AccelControllerState.restrict elif path.lead_switch_guard_frames == 0 and matched_ceiling >= path.pace + PACE_RELIEF_DEADBAND: - path.pace = min(matched_ceiling, path.pace + path.accel_limit * self.dt) + path.pace = min(matched_ceiling, path.pace + profile_accel_max * self.dt) path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release else: path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold @@ -527,7 +537,7 @@ class AccelController: relief = not has_lead or envelope.closing_speed <= 0.0 if relief and (ceiling >= path.pace + PACE_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > path.pace)): if path.lead_switch_guard_frames == 0: - path.pace = min(ceiling, path.pace + path.accel_limit * self.dt) + path.pace = ceiling path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release else: path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold @@ -621,27 +631,40 @@ class AccelController: envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state)) stop_hold_active = live_active and self.live.state == AccelControllerState.stopHold - valid_lead_stop_hold = stop_hold_active and envelope.selected_lead >= 0 - matched_limit_active = live_active and self.live.matched_lead and self.live.matched_accel_limit is not None - braking_limited = live_active and self.live.braking_limited - confirmed_clear_road = (live_active and not stop_hold_active and not envelope.lead_status - and not math.isfinite(self.live.filtered_cap) and self.live.lead_loss_frames >= self.lead_loss_hold_frames) - if stop_hold_active: - stop_hold_accel_max = STOP_HOLD_DEPARTURE_ACCEL_MAX if self.live.departure_frames > 1 else STOP_HOLD_ACCEL_MAX - effective_accel_max = min(stop_hold_accel_max, stock_accel_max) if valid_lead_stop_hold else 0.0 - elif matched_limit_active: - effective_accel_max = min(self.live.matched_accel_limit, stock_accel_max) + matched_limit_active = (live_active and self.live.matched_lead and self.live.matched_accel_limit is not None + and not self.live.braking_handoff) + lead_accel_request = (live_active and envelope.selected_lead >= 0 + and envelope.closing_speed <= 0.0 and planner_accel >= 0.0) + profile_limit_active = live_active and not stop_hold_active and (self.live.launching or not envelope.lead_status or lead_accel_request) + if matched_limit_active: + effective_accel_max = min(positive_accel_max, self.live.matched_accel_limit) + elif profile_limit_active: + effective_accel_max = positive_accel_max else: - effective_accel_max = (min(self.live.accel_limit, stock_accel_max) - if (braking_limited or confirmed_clear_road) and self.live.accel_limit is not None else math.inf) - if stop_hold_active or matched_limit_active or braking_limited or confirmed_clear_road: + effective_accel_max = math.inf + if matched_limit_active or profile_limit_active: mpc_accel_max = self._build_accel_ceiling(effective_accel_max, planner_accel) else: mpc_accel_max = None + guarded_lead_loss = (not envelope.lead_status and self.live.selected_lead >= 0 + and self.live.lead_loss_frames < self.lead_loss_hold_frames) + lead_context = envelope.lead_status or math.isfinite(self.live.filtered_cap) or guarded_lead_loss + reserve_eligible = (live_active and lead_context and not stop_hold_active and self.live.lead_switch_guard_frames == 0 + and not self.live.launching and not self.live.braking_handoff) + if not lead_context: + self.live.pace_reserve_armed = False + elif (reserve_eligible and not self.live.pace_reserve_armed and math.isfinite(self.live.filtered_cap) + and self.live.filtered_cap <= pace_target + PACE_TARGET_ARM_MARGIN): + self.live.pace_reserve_armed = True + + target_speed = 0.0 if stop_hold_active else pace_target + if reserve_eligible and self.live.pace_reserve_armed: + target_speed = max(0.0, target_speed - PACE_TARGET_RESERVE) return AccelControllerResult( - target_speed=0.0 if stop_hold_active else pace_target, + target_speed=target_speed, enabled=bool(enabled), active=live_active, shadow_active=shadow_active, launching=live_active and self.live.launching, + departure_launching=live_active and self.live.launching and self.live.departure_launch, profile=selected_profile, profile_accel_max=profile_accel_max if live_active else math.inf, positive_accel_max=positive_accel_max if live_active else math.inf, effective_accel_max=effective_accel_max, mpc_accel_max=mpc_accel_max, state=self.live.state, diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index e548f98a1b..93af93d561 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -21,8 +21,8 @@ PROFILE_CONFIGS = { ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0] ACCEL_PROFILE_MAX_V = { - AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16], - AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30], + AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16], + AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30], AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42], } @@ -30,18 +30,15 @@ CAP_FILTER_FRAMES = 5 LEAD_LOSS_HOLD_TIME = 0.50 PACE_RESTRICT_DEADBAND = 0.15 PACE_RELIEF_DEADBAND = 0.35 +PACE_TARGET_ARM_MARGIN = 1.0 +PACE_TARGET_RESERVE = 0.10 LAUNCH_TARGET_HEADROOM = 3.0 LAUNCH_TARGET_SLEW = 8.75 LAUNCH_END_SPEED = 3.0 -ACCEL_LIMIT_TRANSITION_JERK = 0.50 ACCEL_LIMIT_HORIZON_JERK = 1.0 LEAD_MATCH_GAP_GAIN = 0.04 -LEAD_MATCH_SPEED_HEADROOM = 2.50 -LEAD_MATCH_ACCEL_GAIN = { - AccelProfile.eco: 0.20, - AccelProfile.normal: 0.24, - AccelProfile.sport: 0.26, -} +LEAD_MATCH_SPEED_HEADROOM = 1.25 +LEAD_MATCH_TAPER_GAIN = 1.00 LEAD_MATCH_ACCEL_SLEW = 0.25 MATCHED_PACE_DECEL_RATE = 0.50 BRAKING_ACCEL_LIMIT_THRESHOLD = -0.11 @@ -53,8 +50,6 @@ STOP_HOLD_EXIT_FRAMES = 4 STOP_HOLD_CREEP_SPEED = 0.15 STOP_HOLD_CREEP_DISTANCE = 0.30 STOP_HOLD_MAX_LEAD_DISTANCE = 30.0 -STOP_HOLD_ACCEL_MAX = 0.10 -STOP_HOLD_DEPARTURE_ACCEL_MAX = 0.25 STOP_GAP_RESERVE = 0.75 STOP_GAP_RESERVE_LEAD_SPEED = 2.0 STOP_GAP_RESERVE_DECEL_BP = (0.30, 0.80) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index 4a128b487b..9dfdec4dd4 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py @@ -12,10 +12,9 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( ) from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import ( - ACCEL_LIMIT_HORIZON_JERK, ACCEL_LIMIT_TRANSITION_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, CAP_FILTER_FRAMES, - LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_GAIN, LEAD_MATCH_ACCEL_SLEW, - MATCHED_PACE_DECEL_RATE, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_ACCEL_MAX, - STOP_HOLD_DEPARTURE_ACCEL_MAX, STOP_HOLD_EXIT_FRAMES, + ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, CAP_FILTER_FRAMES, LAUNCH_END_SPEED, + LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_PACE_DECEL_RATE, PROFILE_CONFIGS, + PACE_TARGET_RESERVE, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, ) @@ -63,15 +62,10 @@ class TestProfiles: def test_lookup_table_is_explicit_and_tunable(self): assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0] assert ACCEL_PROFILE_MAX_V == { - AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16], - AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30], + AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16], + AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30], AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42], } - assert LEAD_MATCH_ACCEL_GAIN == { - AccelProfile.eco: 0.20, - AccelProfile.normal: 0.24, - AccelProfile.sport: 0.26, - } @pytest.mark.parametrize("profile", list(AccelProfile)) def test_lookup_interpolates_and_stays_inside_global_limit(self, profile): @@ -98,22 +92,17 @@ class TestProfiles: assert result.profile_accel_max == pytest.approx(1.15) assert result.positive_accel_max == pytest.approx(0.30) assert result.effective_accel_max == pytest.approx(0.30) - assert all(sample.mpc_accel_max is None for sample in results[:-1]) - assert result.mpc_accel_max is not None - assert max(result.mpc_accel_max) <= ACCEL_MAX + assert all(sample.mpc_accel_max is not None for sample in results) + assert all(max(sample.mpc_accel_max) <= 0.30 + 1e-9 for sample in results) - def test_runtime_profile_switch_smooths_the_scalar_limit(self): + def test_runtime_profile_switch_applies_the_lookup_value_directly(self): controller = make_controller() sport = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20) for _ in range(controller.lead_loss_hold_frames)][-1] eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20) - max_step = ACCEL_LIMIT_TRANSITION_JERK * DT_MDL assert sport.effective_accel_max == pytest.approx(1.15) - assert sport.effective_accel_max - eco.effective_accel_max == pytest.approx(max_step) - - settled = [update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20) for _ in range(30)][-1] - assert settled.effective_accel_max == pytest.approx(0.72) + assert eco.effective_accel_max == pytest.approx(0.72) def test_matched_lead_waits_until_ego_catches_the_lead(self): radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) @@ -199,34 +188,46 @@ class TestMpcCeiling: assert result.mpc_accel_max is None assert math.isinf(result.effective_accel_max) - def test_braking_ceiling_waits_for_stable_live_history_and_resets(self): + def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self): controller = make_controller() radar = restrictive_radar() warmup = [update(controller, radar, planner_accel=-0.2) for _ in range(controller.lead_loss_hold_frames)] - assert all(not sample.mpc_accel_max for sample in warmup[:-1]) + assert all(sample.mpc_accel_max is None for sample in warmup) assert controller.live.braking_limited - assert warmup[-1].mpc_accel_max is not None bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False) assert not bypassed.active and bypassed.mpc_accel_max is None assert not controller.live.braking_limited - def test_matched_lead_ceiling_has_no_planner_accel_handoff(self): + def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES + 5): + update(controller, restrictive_radar(), v_ego=10.0, planner_accel=-0.2) + + pulling_away = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=12.0)) + result = update(controller, pulling_away, v_ego=10.0, planner_accel=0.2) + + assert result.state == AccelControllerState.restrict + assert result.effective_accel_max == pytest.approx(result.positive_accel_max) + assert result.mpc_accel_max is not None + + def test_matched_lead_terminal_taper_changes_smoothly(self): controller = make_controller() radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) for _ in range(CAP_FILTER_FRAMES + 5): update(controller, radar, v_ego=10.0, planner_accel=-0.2) braking = update(controller, radar, v_ego=8.0, planner_accel=-0.2) + braking_limit = controller.live.matched_accel_limit accelerating = update(controller, radar, v_ego=8.0, planner_accel=0.2) assert controller.live.matched_lead assert braking.mpc_accel_max is not None and accelerating.mpc_accel_max is not None - assert abs(accelerating.effective_accel_max - braking.effective_accel_max) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9 - target_drop = braking.target_speed - accelerating.target_speed - assert 0.0 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9 - assert accelerating.state == AccelControllerState.restrict + assert braking_limit is not None + assert abs(controller.live.matched_accel_limit - braking_limit) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9 + assert braking.effective_accel_max <= braking.positive_accel_max + assert accelerating.effective_accel_max <= accelerating.positive_accel_max def test_matched_lead_ignores_two_frame_speed_jump(self): clean_controller, noisy_controller = make_controller(), make_controller() @@ -329,13 +330,15 @@ class TestPaceAndLifecycle: assert math.isinf(results[1].live_filtered_cap) assert math.isfinite(results[2].live_filtered_cap) - def test_restriction_uses_comfort_rate_without_acquisition_step(self): + def test_restriction_uses_comfort_rate_with_one_bounded_reserve_step(self): controller = make_controller() results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES + 10)] targets = np.asarray([result.target_speed for result in results]) max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL - assert np.max(-np.diff(targets)) <= max_step + 1e-9 + target_steps = -np.diff(targets) + assert np.count_nonzero(target_steps > max_step + 1e-9) == 1 + assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9 assert results[-1].state == AccelControllerState.restrict assert results[-1].target_speed < results[0].target_speed @@ -350,7 +353,9 @@ class TestPaceAndLifecycle: targets = np.asarray([25.0, *(result.target_speed for result in results)]) max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL - assert np.max(-np.diff(targets)) <= max_step + 1e-9 + target_steps = -np.diff(targets) + assert np.count_nonzero(target_steps > max_step + 1e-9) == 1 + assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9 def test_lead_slot_is_forgotten_before_reacquisition(self): controller = make_controller() @@ -368,7 +373,9 @@ class TestPaceAndLifecycle: targets = np.asarray([before.target_speed, *(result.target_speed for result in results)]) max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL - assert np.max(-np.diff(targets)) <= max_step + 1e-9 + target_steps = -np.diff(targets) + assert np.count_nonzero(target_steps > max_step + 1e-9) == 1 + assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9 @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track")) def test_false_relief_track_replacement_freezes_bounded_pace_release(self, replacement_track_id): @@ -384,8 +391,8 @@ class TestPaceAndLifecycle: switched = update(controller, replacement, base_speed=25.0, v_ego=8.0, planner_speed=5.0, planner_accel=-0.2) target_drop = before.target_speed - switched.target_speed - assert 0.0 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9 - assert switched.effective_accel_max <= before.effective_accel_max + 1e-9 + assert -PACE_TARGET_RESERVE - 1e-9 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9 + assert switched.effective_accel_max <= switched.positive_accel_max + 1e-9 assert switched.target_speed < switched.base_speed assert controller.live.lead_switch_guard_frames == controller.lead_loss_hold_frames @@ -400,7 +407,7 @@ class TestPaceAndLifecycle: assert controller.live.lead_switch_guard_frames == 0 - def test_short_dropout_holds_then_releases_at_profile_rate(self): + def test_short_dropout_holds_then_releases_without_a_second_accel_cap(self): controller = make_controller() for _ in range(CAP_FILTER_FRAMES + 20): restricted = update(controller, restrictive_radar()) @@ -409,8 +416,7 @@ class TestPaceAndLifecycle: assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held) released = update(controller) - assert released.target_speed >= held[-1].target_speed - assert released.target_speed - held[-1].target_speed <= released.effective_accel_max * DT_MDL + 1e-9 + assert released.target_speed == released.base_speed def test_previous_lead_source_synchronizes_down_to_planner(self): controller = make_controller() @@ -496,12 +502,12 @@ class TestPaceAndLifecycle: assert result.state == AccelControllerState.stopHold assert controller.live.pace == 0.0 assert result.target_speed == 0.0 - assert result.effective_accel_max == STOP_HOLD_ACCEL_MAX - assert result.mpc_accel_max is not None and max(result.mpc_accel_max) <= STOP_HOLD_ACCEL_MAX + assert math.isinf(result.effective_accel_max) + assert result.mpc_accel_max is None stock_limited = update(controller, stopped, base_speed=12.0, v_ego=0.2, stock_accel_max=0.0) - assert stock_limited.effective_accel_max == 0.0 - assert stock_limited.mpc_accel_max is not None and max(stock_limited.mpc_accel_max) == 0.0 + assert math.isinf(stock_limited.effective_accel_max) + assert stock_limited.mpc_accel_max is None def test_stop_hold_needs_four_confirmed_departure_frames(self): controller = make_controller() @@ -512,10 +518,13 @@ class TestPaceAndLifecycle: launch_index = next(index for index, result in enumerate(results) if result.launching) assert held.state == AccelControllerState.stopHold - assert held.target_speed == 0.0 and held.effective_accel_max == STOP_HOLD_ACCEL_MAX + assert held.target_speed == 0.0 and math.isinf(held.effective_accel_max) + assert held.mpc_accel_max is None assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results[:launch_index]) assert launch_index == STOP_HOLD_EXIT_FRAMES - 1 assert results[launch_index].target_speed >= 0.1 + LAUNCH_TARGET_HEADROOM + assert results[launch_index].departure_launching + assert results[launch_index].effective_accel_max == pytest.approx(results[launch_index].positive_accel_max) def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self): controller = make_controller() @@ -532,13 +541,15 @@ class TestPaceAndLifecycle: assert held.effective_accel_max == pytest.approx(fresh.effective_accel_max) if frame < STOP_HOLD_EXIT_FRAMES - 1: assert fresh.state == AccelControllerState.stopHold - expected_limit = STOP_HOLD_ACCEL_MAX if frame == 0 else STOP_HOLD_DEPARTURE_ACCEL_MAX - assert fresh.effective_accel_max == expected_limit + assert math.isinf(fresh.effective_accel_max) + assert fresh.mpc_accel_max is None assert fresh.launching and held.launching + assert fresh.departure_launching and held.departure_launching assert fresh.target_speed == held.target_speed == 8.0 + assert fresh.effective_accel_max == pytest.approx(fresh.positive_accel_max) - def test_single_frame_departure_stays_at_the_stationary_hold_limit(self): + def test_single_frame_departure_stays_at_zero_target_without_an_accel_ceiling(self): controller = make_controller() enter_stop_hold(controller) departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0)) @@ -549,8 +560,8 @@ class TestPaceAndLifecycle: assert warm.state == held.state == AccelControllerState.stopHold assert not warm.launching and not held.launching - assert warm.effective_accel_max == STOP_HOLD_ACCEL_MAX - assert held.effective_accel_max == STOP_HOLD_ACCEL_MAX + assert math.isinf(warm.effective_accel_max) and warm.mpc_accel_max is None + assert math.isinf(held.effective_accel_max) and held.mpc_accel_max is None assert held.target_speed == 0.0 def test_previous_stop_without_a_lead_does_not_latch_stop_hold(self): @@ -570,7 +581,8 @@ class TestPaceAndLifecycle: assert result.state == AccelControllerState.stopHold assert result.target_speed == 0.0 - assert result.effective_accel_max == 0.0 + assert math.isinf(result.effective_accel_max) + assert result.mpc_accel_max is None def test_stop_hold_without_usable_lead_stays_pinned_to_zero(self): controller = make_controller() @@ -579,8 +591,8 @@ class TestPaceAndLifecycle: assert missing.state == AccelControllerState.stopHold assert missing.target_speed == 0.0 - assert missing.effective_accel_max == 0.0 - assert missing.mpc_accel_max is not None and max(missing.mpc_accel_max) == 0.0 + assert math.isinf(missing.effective_accel_max) + assert missing.mpc_accel_max is None def test_confirmed_creep_departure_does_not_reenter_stop_hold(self): controller = make_controller() @@ -646,7 +658,7 @@ class TestPaceAndLifecycle: assert not result.active assert result.target_speed == result.base_speed assert result.mpc_accel_max is None - assert controller.live.pace is None and controller.live.accel_limit is None + assert controller.live.pace is None def test_shadow_history_never_steps_into_live_actuation(self): controller = make_controller() @@ -667,8 +679,9 @@ class TestPaceAndLifecycle: assert controller._held_envelope is None for path in (controller.live, controller.shadow): - assert path.pace is None and path.accel_limit is None and path.matched_accel_limit is None + assert path.pace is None and path.matched_accel_limit is None assert path.state == AccelControllerState.inactive assert path.departure_frames == path.active_frames == path.lead_loss_frames == path.stale_frames == 0 - assert not path.launching and not path.departure_launch and not path.matched_lead and not path.braking_limited + assert not path.launching and not path.departure_launch and not path.matched_lead + assert not path.braking_limited and not path.braking_handoff and not path.pace_reserve_armed assert math.isinf(path.filtered_cap) and math.isinf(path.filtered_lead_speed) and path.filtered_lead_accel == 0.0 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py index 418e23e30e..b2189008fe 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py @@ -120,24 +120,41 @@ def test_active_acc_uses_target_and_ceiling_in_exactly_one_solve(): assert calls == [({}, 15.0, True, ceiling)] -def test_valid_lead_stop_hold_keeps_raw_mpc_target_with_bounded_ceiling(): - ceiling = tuple(np.linspace(0.1, 0.1, N + 1)) +def test_valid_lead_stop_hold_preplans_from_raw_target_without_an_accel_ceiling(): planner, _ = planner_for_mpc_test( - target_speed=0.0, mpc_accel_max=ceiling, state=AccelControllerState.stopHold, selected_lead=0, + target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=0, ) _, calls = run_controller_mpc(planner) - assert calls == [({}, 20.0, True, ceiling)] + assert calls == [({}, 20.0, True, None)] -def test_missing_lead_stop_hold_keeps_zero_mpc_target_and_ceiling(): - ceiling = tuple(np.zeros(N + 1)) +def test_missing_lead_stop_hold_keeps_zero_mpc_target_without_an_accel_ceiling(): planner, _ = planner_for_mpc_test( - target_speed=0.0, mpc_accel_max=ceiling, state=AccelControllerState.stopHold, selected_lead=-1, + target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=-1, ) _, calls = run_controller_mpc(planner) - assert calls == [({}, 0.0, True, ceiling)] + assert calls == [({}, 0.0, True, None)] + + +@pytest.mark.parametrize( + ("active", "departure_launching", "is_e2e", "expected"), + [ + (True, True, False, False), + (True, False, False, True), + (False, True, False, True), + (True, True, True, True), + ], +) +def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, is_e2e, expected): + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner.accel_controller_result = SimpleNamespace( + active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold, + ) + assert planner.accel_controller_should_stop(True, is_e2e) is expected + expected_hold = active and not departure_launching and not is_e2e + assert planner.accel_controller_should_stop(False, is_e2e) is expected_hold @pytest.mark.parametrize(("active", "is_e2e"), [(False, False), (True, True)]) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 3eb8227107..f44ccd6997 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -143,13 +143,21 @@ class LongitudinalPlannerSP: result = self.accel_controller_result actuating = result.active and not is_e2e and not force_decel and not previous_mpc_failed valid_lead_stop_hold = (actuating and result.state == AccelControllerState.stopHold - and getattr(result, 'selected_lead', -1) >= 0) + and result.selected_lead >= 0) controller_v_cruise = mpc_v_cruise if valid_lead_stop_hold else min(mpc_v_cruise, result.target_speed) if actuating else mpc_v_cruise accel_max = result.mpc_accel_max if actuating else None self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max) return is_e2e + def accel_controller_should_stop(self, should_stop: bool, is_e2e: bool) -> bool: + result = self.accel_controller_result + if result is None or not result.active or is_e2e: + return should_stop + if result.departure_launching: + return False + return should_stop or result.state == AccelControllerState.stopHold + def update(self, sm: messaging.SubMaster) -> None: self._radar_fresh_this_cycle = self._update_radar_freshness(sm) self._read_accel_controller_params() diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index 9251e8a06f..2aa0bcda22 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -12,7 +12,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import ( - LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_SPEED_HEADROOM, MATCHED_PACE_DECEL_RATE, + MATCHED_PACE_DECEL_RATE, PACE_TARGET_RESERVE, ) ACTUATOR_DYNAMICS = ( @@ -26,6 +26,7 @@ ACTUATOR_IDS = ("toyota", "honda", "gm", "hyundai", "ford") ROUTINE_GAP_TOLERANCE = 0.10 ROUTINE_DECEL_TOLERANCE = 0.10 DROPOUT_GAP_TOLERANCE = 0.15 +MOVING_LEAD_GAP_TOLERANCE = 0.12 @dataclass @@ -399,7 +400,7 @@ def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed(): moving = np.flatnonzero(trace.speed > 0.01) assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL assert len(moving) and trace.time[moving[0]] <= 1.0 - assert np.interp(1.0, trace.time, trace.speed) >= 0.35 + assert np.interp(1.0, trace.time, trace.speed) >= 0.33 assert not np.any(trace.a_target < -0.05) assert trace.solver_failures == 0 @@ -408,7 +409,10 @@ def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed(): np.testing.assert_allclose(traces[2].a_target[launch_window], traces[0].a_target[launch_window], atol=0.10, rtol=0.0) speed_at_eight = [float(np.interp(8.0, trace.time, trace.speed)) for trace in traces] assert speed_at_eight[0] + 0.75 < speed_at_eight[1] - assert speed_at_eight[1] + 0.50 < speed_at_eight[2] + assert speed_at_eight[1] + 0.30 < speed_at_eight[2] + final_speed = [float(trace.speed[-1]) for trace in traces] + assert final_speed[0] + 1.25 < final_speed[1] + assert final_speed[1] + 0.75 < final_speed[2] ceiling_at_ten = [float(np.interp(10.0, trace.speed, trace.mpc_upper_min)) for trace in traces] assert ceiling_at_ten[0] < ceiling_at_ten[1] < ceiling_at_ten[2] @@ -458,7 +462,9 @@ def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_sec moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05)) assert not trace.launching[first_three].any() + assert trace.should_stop[first_three].all() assert len(release) and trace.time[release[0]] >= departure_time + 3 * DT_MDL + assert not trace.should_stop[release[0]] assert len(moving) and trace.time[moving[0]] <= departure_time + 3 * DT_MDL + 1.0 assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) assert trace.solver_failures == 0 @@ -473,10 +479,12 @@ def test_reused_radar_frames_do_not_pulse_stop_state_during_departure(): ) after_departure = trace.time >= departure_time should_stop = trace.should_stop[after_departure] + release = np.flatnonzero(after_departure & trace.launching) moving = np.flatnonzero(after_departure & (trace.speed > 0.05)) assert np.count_nonzero(np.diff(should_stop.astype(int))) <= 1 - assert len(moving) and trace.time[moving[0]] <= departure_time + 0.8 + assert len(release) and len(moving) + assert trace.time[moving[0]] <= trace.time[release[0]] + 1.0 assert not _has_propulsion_brake_cycle(trace.a_target[after_departure]) assert trace.solver_failures == 0 @@ -491,6 +499,7 @@ def test_short_false_departure_does_not_launch_the_vehicle(departure_frames): assert np.max(trace.speed) < 0.01 assert not trace.launching.any() + assert trace.should_stop.all() assert trace.state[-1] == int(AccelControllerState.stopHold) assert trace.solver_failures == 0 @@ -511,7 +520,7 @@ def test_matched_lead_recovery_preserves_profile_ordering(actuator_delay, actuat assert mean_accel[0] + 0.06 < mean_accel[1] assert mean_accel[1] + 0.025 < mean_accel[2] assert final_speed[0] < final_speed[1] < final_speed[2] - assert max(final_speed) < 10.0 + LEAD_MATCH_SPEED_HEADROOM + assert max(final_speed) < 13.5 assert all(not _has_propulsion_brake_cycle(trace.a_target[response]) for trace in traces) assert all(trace.solver_failures == 0 for trace in traces) @@ -680,7 +689,7 @@ def test_route_507_braking_lead_slot_switch_has_no_false_relief_cycle(profile, a assert not _has_propulsion_brake_cycle(trace.a_target[response]) assert not _has_brake_coast_brake(trace.a_target[response]) assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0 - assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9 + assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= max(PACE_TARGET_RESERVE, MATCHED_PACE_DECEL_RATE * DT_MDL) + 1e-9 assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE assert not trace.fcw.any() assert trace.solver_failures == 0 @@ -722,16 +731,16 @@ def test_profile_ceiling_and_pace_stay_smooth_through_slot_switch_noise(profile) response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0) effective_accel_max = trace.effective_accel_max[response] selected_leads = trace.selected_lead[glitch] + finite_limits = np.isfinite(effective_accel_max) stock_accel_max = np.asarray([get_max_accel(speed) for speed in trace.speed[response]]) assert set(selected_leads) == {0, 1} assert np.count_nonzero(np.diff(selected_leads)) > 20 - assert np.all(np.isfinite(effective_accel_max)) - assert np.min(trace.profile_accel_max[response] - effective_accel_max) > 0.1 - assert np.min(stock_accel_max - effective_accel_max) > 0.1 - assert np.max(np.abs(np.diff(effective_accel_max))) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-12 + assert np.all(effective_accel_max[finite_limits] <= trace.profile_accel_max[response][finite_limits] + 1e-9) + assert np.all(effective_accel_max[finite_limits] <= stock_accel_max[finite_limits] + 1e-9) assert np.max(trace.a_target[response]) > 0.2 assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not _has_brake_coast_brake(trace.a_target[response]) assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 assert trace.raw_radar_passthrough.all() assert not trace.fcw.any() @@ -739,7 +748,7 @@ def test_profile_ceiling_and_pace_stay_smooth_through_slot_switch_noise(profile) @pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) -def test_matched_lead_dropout_keeps_the_stored_acceleration_ceiling(profile): +def test_matched_lead_dropout_keeps_the_profile_acceleration_ceiling(profile): dropout_start = 25.0 dropout_end = 25.15 @@ -839,7 +848,7 @@ def test_decelerating_moving_lead_is_stock_safe_without_propulsion_reversal(): assert not _has_propulsion_after_braking(trace.a_target[response]) assert trace_p95 <= baseline_p95 + 0.02 assert np.min(trace.acceleration) >= np.min(baseline.acceleration) - ROUTINE_DECEL_TOLERANCE - assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - ROUTINE_GAP_TOLERANCE + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - MOVING_LEAD_GAP_TOLERANCE assert not trace.fcw.any() _assert_no_new_solver_failures(trace, baseline) @@ -889,7 +898,7 @@ def test_far_lead_profiles_start_early_in_order_without_solver_failures(actuator for trace in traces: assert trace.acceleration.min() >= baseline.acceleration.min() - 0.1 assert float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) < 0.45 - assert np.max(np.abs(_command_jerk(trace, after=0.5))) < 4.0 + assert np.max(np.abs(_command_jerk(trace, after=0.5))) < 4.5 assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0]) assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= 1.0]) assert not trace.fcw.any() @@ -939,7 +948,7 @@ def test_matched_lead_slowdown_stays_smooth_without_a_second_braking_stage(): desired_gap = STOP_DISTANCE + get_T_FOLLOW() * settled_lead_speed assert abs(np.mean(trace.speed[matched]) - 10.0) < 0.5 - assert np.mean(trace.profile_accel_max[matched] - trace.effective_accel_max[matched]) > 0.1 + np.testing.assert_allclose(trace.effective_accel_max[matched], trace.profile_accel_max[matched], atol=1e-9) assert not np.any(trace.state[response] == int(AccelControllerState.stopHold)) assert not trace.launching[response].any() assert not _has_brake_coast_brake(trace.a_target[response])