From fce7f62c32c79b2e43b59e0f8dd26f6b212ee8c1 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 15 Aug 2026 02:57:09 -0700 Subject: [PATCH] fix(long): preserve smooth braking through the final stop --- .../selfdrive/controls/lib/longcontrol.py | 7 +- .../selfdrive/controls/lib/longcontrol.py | 53 +++ .../controls/lib/tests/test_longcontrol_sp.py | 392 ++++++++++++++++++ 3 files changed, 450 insertions(+), 2 deletions(-) create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index e3b612e79f..28a9ed743e 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.common.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -39,8 +40,9 @@ def long_control_state_trans(CP_SP, active, long_control_state, return long_control_state -class LongControl: +class LongControl(LongControlSP): def __init__(self, CP, CP_SP): + LongControlSP.__init__(self) self.CP = CP self.CP_SP = CP_SP self.long_control_state = LongCtrlState.off @@ -60,6 +62,7 @@ class LongControl: self.long_control_state = long_control_state_trans(self.CP_SP, active, self.long_control_state, should_stop, CS.brakePressed, CS.cruiseState.standstill) + LongControlSP.update_state(self, self.long_control_state == LongCtrlState.stopping) if self.long_control_state == LongCtrlState.off: self.reset() output_accel = 0. @@ -69,7 +72,7 @@ class LongControl: if output_accel > self.CP.stopAccel: output_accel = min(output_accel, 0.0) # TODO: can we just go straight to stopAccel? - output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop + output_accel -= LongControlSP.stopping_decel_rate(self, CS, a_target) * DT_CTRL self.reset() else: # LongCtrlState.pid diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py b/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py new file mode 100644 index 0000000000..e20d3858e5 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py @@ -0,0 +1,53 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +import math +from typing import cast + +STOPPING_DISTANCE = 0.75 +STOPPING_TIME = 2.5 +STOPPING_ACCEL_TOLERANCE = 0.1 +STOPPING_SPEED_TOLERANCE = 0.05 +STOPPING_SETTLE_FRAMES = 30 + + +class LongControlSP: + def __init__(self): + self._stopping_settle_frames: int | None = None + + def update_state(self, stopping: bool) -> None: + if not stopping: + self._stopping_settle_frames = None + + def stopping_decel_rate(self, CS, a_target: float) -> float: + if not all(math.isfinite(value) for value in (self.last_output_accel, a_target, CS.vEgo, CS.aEgo)): + return 1.0 + can_hold = self.last_output_accel <= 0.0 and a_target >= self.last_output_accel + terminal_speed = (0.0 <= CS.vEgo <= STOPPING_SPEED_TOLERANCE + or CS.standstill and abs(CS.vEgo) <= STOPPING_SPEED_TOLERANCE) + if self.last_output_accel > 0.0 or CS.vEgo < 0.0 and not terminal_speed: + return 1.0 + if terminal_speed and self._stopping_settle_frames is None: + if not can_hold or self.last_output_accel > -STOPPING_ACCEL_TOLERANCE or CS.aEgo >= -STOPPING_ACCEL_TOLERANCE: + return 1.0 + self._stopping_settle_frames = 0 + + time_decel = 0.0 if self._stopping_settle_frames is not None else CS.vEgo / STOPPING_TIME + required_decel = max(time_decel, CS.vEgo ** 2 / (2.0 * STOPPING_DISTANCE), 1e-3) + adequacy = min(max(-CS.aEgo / required_decel, 0.0), 1.0) + if not terminal_speed and self._stopping_settle_frames is None and can_hold and adequacy >= 1.0: + self._stopping_settle_frames = 0 + + motion_need = 1.0 - adequacy ** 2 + planner_need = min(max((self.last_output_accel - a_target) / max(required_decel, STOPPING_ACCEL_TOLERANCE), 0.0), 1.0) + terminal_need = 0.0 + if terminal_speed or self._stopping_settle_frames not in (None, 0): + settle_frames = cast(int, self._stopping_settle_frames) + self._stopping_settle_frames = min(settle_frames + 1, STOPPING_SETTLE_FRAMES) + terminal_need = (self._stopping_settle_frames / STOPPING_SETTLE_FRAMES) ** 2 + + return max(motion_need, planner_need, terminal_need) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py new file mode 100644 index 0000000000..a81ccf1f6a --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py @@ -0,0 +1,392 @@ +import numpy as np +from unittest import mock + +from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs +from openpilot.common.parameterized import parameterized +from openpilot.common.test import OpenpilotTestCase +from opendbc.car.car_helpers import interfaces +from opendbc.car.gm.values import CAR as GM +from opendbc.car.honda.values import CAR as HONDA +from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.rivian.values import CAR as RIVIAN +from opendbc.car.toyota.values import CAR as TOYOTA +from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN +from openpilot.selfdrive.controls.lib.drive_helpers import should_stop +from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState +from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import STOPPING_SETTLE_FRAMES, STOPPING_SPEED_TOLERANCE +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP + + +STOP_ACCEL_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1, RIVIAN.RIVIAN_R1) +SETTLE_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1) +ROUTE_STOP_ONSETS = ( + (0.280, -0.290, -0.220, -0.220), + (0.290, -0.497, -0.270, -0.302), + (0.464, -0.223, -0.264, -0.292), + (0.467, -0.582, -0.316, -0.359), + (0.530, -0.311, -0.309, -0.333), + (0.581, -0.467, -0.312, -0.352), + (0.398, -0.557, -0.311, -0.348), + (0.517, -0.290, -0.301, -0.327), + (0.312, -0.420, -0.271, -0.304), + (0.474, -0.509, -0.303, -0.347), + (0.241, -0.554, -0.573, -0.617), + (0.292, -0.154, -0.302, -0.326), +) + + +def get_car_params(candidate): + fingerprint = gen_empty_fingerprint() + interface = interfaces[candidate] + CP = interface.get_params(candidate, fingerprint, [], True, False, False) + return CP, interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False) + + +def make_car_state(v_ego=0.2, a_ego=0.0, standstill=False) -> structs.CarState: + state = structs.CarState(vEgo=float(v_ego), aEgo=float(a_ego), standstill=standstill) + state.cruiseState.standstill = standstill + return state + + +def make_control(candidate, initial_accel=-0.33): + CP, CP_SP = get_car_params(candidate) + control = LongControl(CP, CP_SP) + control.long_control_state = LongCtrlState.pid + control.last_output_accel = initial_accel + return CP, control + + +def stock_stopping_output(output_accel, stop_accel): + return min(output_accel, 0.0) - DT_CTRL if output_accel > stop_accel else output_accel + + +class TestLongControlSP(OpenpilotTestCase): + def test_stop_threshold_matches_the_shared_helper(self): + assert should_stop(0.29, 0.0) + assert not should_stop(0.3, 0.0) + assert not should_stop(0.29, 0.1) + + @parameterized.expand(ROUTE_STOP_ONSETS, names=("v_ego", "a_ego", "a_target", "initial_accel")) + def test_logged_stop_onsets_hold_the_existing_brake(self, v_ego, a_ego, a_target, initial_accel): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel) + output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0)) + assert control.long_control_state == LongCtrlState.stopping + self.assertAlmostEqual(output, initial_accel) + + def test_glide_hold_survives_a_soft_deceleration_sample(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166) + samples = ((0.388, -0.201, -0.164), (0.330, -0.120, -0.140), (0.283, -0.0675, -0.120)) + outputs = [control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0)) for v_ego, a_ego, a_target in samples] + + np.testing.assert_allclose(outputs, [-0.166] * len(samples), rtol=1e-6, atol=1e-12) + + def test_glide_response_reaches_the_stock_rate_when_deceleration_stops(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166) + control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0)) + output = control.update(True, make_car_state(0.330, -0.01), -0.140, True, (-3.5, 2.0)) + + assert -0.176 < output < -0.175 + + def test_glide_response_increases_with_stopping_distance_error(self): + _, nominal = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166) + _, distance_error = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166) + for control in (nominal, distance_error): + control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0)) + nominal_output = nominal.update(True, make_car_state(0.330, -0.050), -0.140, True, (-3.5, 2.0)) + distance_error_output = distance_error.update(True, make_car_state(0.400, -0.050), -0.140, True, (-3.5, 2.0)) + + assert -0.176 < distance_error_output < nominal_output + + @parameterized.expand(((1.0, 0.0), (0.75, 0.4375), (0.5, 0.75), (0.0, 1.0)), names=("decel_fraction", "expected_rate")) + def test_stopping_rate_scales_with_realized_deceleration(self, decel_fraction, expected_rate): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + output = control.update(True, make_car_state(0.3, -0.12 * decel_fraction), 0.0, True, (-3.5, 2.0)) + + self.assertAlmostEqual((-0.33 - output) / DT_CTRL, expected_rate, delta=1e-6) + + def test_stopping_rate_scales_with_planner_demand(self): + _, gentle = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + _, urgent = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + + gentle_output = gentle.update(True, make_car_state(0.3, -0.12), -0.34, True, (-3.5, 2.0)) + urgent_output = urgent.update(True, make_car_state(0.3, -0.12), -1.0, True, (-3.5, 2.0)) + + assert -0.331 < gentle_output < -0.33 + self.assertAlmostEqual(urgent_output, -0.34) + + def test_glide_hold_yields_to_stronger_planner_braking(self): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166) + control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0)) + output = control.update(True, make_car_state(0.330, -0.120), -1.0, True, (-3.5, 2.0)) + + self.assertAlmostEqual(output, stock_stopping_output(-0.166, CP.stopAccel)) + + @parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",)) + def test_urgent_braking_matches_the_stock_ramp(self, candidate): + CP, control = make_control(candidate) + CS = make_car_state(0.8, -0.1) + output = control.last_output_accel + + for _ in range(round(1.0 / DT_CTRL)): + output = control.update(True, CS, -3.0, True, (-3.5, 2.0)) + + expected = -0.33 + for _ in range(round(1.0 / DT_CTRL)): + expected = stock_stopping_output(expected, CP.stopAccel) + self.assertAlmostEqual(output, expected) + + @parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",)) + def test_stronger_planner_brake_matches_the_stock_ramp(self, candidate): + CP, control = make_control(candidate) + outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0, True, (-3.5, 2.0)) for _ in range(10)] + expected = [] + output = -0.33 + for _ in range(10): + output = stock_stopping_output(output, CP.stopAccel) + expected.append(output) + np.testing.assert_allclose(outputs, expected, rtol=1e-6, atol=1e-12) + + @parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",)) + def test_insufficient_deceleration_uses_most_of_the_stock_ramp(self, candidate): + CP, control = make_control(candidate) + output = control.update(True, make_car_state(0.6, -0.1), -0.1, True, (-3.5, 2.0)) + if -0.33 > CP.stopAccel: + assert -0.34 < output < -0.338 + else: + self.assertAlmostEqual(output, -0.33) + + def test_deceleration_noise_cannot_release_the_brake(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + outputs = [control.update(True, make_car_state(0.3, -0.3 if frame % 2 else 0.0), -0.1, True, (-3.5, 2.0)) for frame in range(40)] + assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True)) + + def test_planner_noise_cannot_release_the_brake(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0 if frame % 2 else -0.1, True, (-3.5, 2.0)) for frame in range(40)] + assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True)) + + @parameterized.expand( + ( + (float("nan"), -0.3, -0.1), + (0.3, float("nan"), -0.1), + (0.3, -0.3, float("nan")), + (float("inf"), -0.3, -0.1), + (0.3, -float("inf"), -0.1), + (0.3, -0.3, float("inf")), + ), + names=("v_ego", "a_ego", "a_target"), + ) + def test_invalid_state_uses_the_stock_ramp(self, v_ego, a_ego, a_target): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0)) + self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel)) + + @parameterized.expand( + ( + (0.24, 0.0, -0.49, 0.15, 0.0), + (0.53, -0.31, -0.49, 0.35, 0.1), + (0.24, 0.0, 0.0, 0.15, 0.0), + (0.464, -0.223, 0.0, 0.25, 0.05), + (0.53, -0.31, 0.0, 0.35, 0.1), + (0.24, 0.0, 0.49, 0.15, 0.0), + (0.53, -0.31, 0.49, 0.25, 0.05), + (0.6, -0.3, 0.49, 0.35, 0.1), + (0.6, -0.3, 0.49, 0.5, 0.1), + ), + names=("speed", "initial_accel", "grade_accel", "actuator_lag", "actuator_delay"), + ) + def test_smooth_stop_distance_is_bounded(self, speed, initial_accel, grade_accel, actuator_lag, actuator_delay): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel) + applied_accel = initial_accel + delay = [initial_accel] * round(actuator_delay / DT_CTRL) + distance = 0.0 + outputs = [] + + for _ in range(round(4.0 / DT_CTRL)): + command = control.update(True, make_car_state(speed, applied_accel), -0.1, True, (-3.5, 2.0)) + outputs.append(command) + delayed_command = command + if delay: + delay.append(command) + delayed_command = delay.pop(0) + applied_accel += DT_CTRL / actuator_lag * (delayed_command + grade_accel - applied_accel) + speed = max(0.0, speed + applied_accel * DT_CTRL) + distance += speed * DT_CTRL + if speed == 0.0: + break + + assert speed == 0.0 + assert distance < 1.0 + assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True)) + + @parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",)) + def test_standstill_uses_the_stock_ramp(self, candidate): + CP, control = make_control(candidate) + control.long_control_state = LongCtrlState.off + CS = make_car_state(0.0, 0.0, standstill=True) + outputs = [control.update(True, CS, 0.0, False, (-3.5, 2.0)) for _ in range(round(2.0 / DT_CTRL))] + expected = -0.33 + for _ in range(round(2.0 / DT_CTRL)): + expected = stock_stopping_output(expected, CP.stopAccel) + self.assertAlmostEqual(outputs[0], stock_stopping_output(-0.33, CP.stopAccel)) + self.assertAlmostEqual(outputs[-1], expected) + + @parameterized.expand(SETTLE_VEHICLES, names=("candidate",)) + def test_final_stop_builds_brake_smoothly_while_vehicle_settles(self, candidate): + _, control = make_control(candidate) + control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0)) + outputs = [control.update(True, make_car_state(0.0006, a_ego, standstill=True), -0.032, True, (-3.5, 2.0)) for a_ego in (-1.098, -0.950, -0.609, -0.286)] + changes = -np.diff([-0.33, *outputs]) + assert np.all(changes > 0.0) + assert np.all(np.diff(changes) > 0.0) + assert changes[-1] < 0.001 + + @parameterized.expand((-0.09, 0.0, 0.1), names=("a_ego",)) + def test_settled_vehicle_uses_the_stock_hold_ramp(self, a_ego): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + output = control.update(True, make_car_state(0.0, a_ego, standstill=True), -0.1, True, (-3.5, 2.0)) + self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel)) + + @parameterized.expand(SETTLE_VEHICLES, names=("candidate",)) + def test_direct_terminal_entry_builds_brake_smoothly(self, candidate): + _, control = make_control(candidate) + CS = make_car_state(0.0006, -0.3, standstill=True) + outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(4)] + + rates = -np.diff([-0.33, *outputs]) / DT_CTRL + np.testing.assert_allclose(rates, [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, 5)], rtol=1e-6, atol=1e-12) + + def test_direct_terminal_entry_keeps_urgent_stock_braking(self): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), -1.0, True, (-3.5, 2.0)) + + self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel)) + + @parameterized.expand((0.0, -0.05), names=("initial_accel",)) + def test_direct_terminal_entry_first_builds_meaningful_brake(self, initial_accel): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel) + output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), 0.0, True, (-3.5, 2.0)) + + self.assertAlmostEqual(output, stock_stopping_output(initial_accel, CP.stopAccel)) + + @parameterized.expand(SETTLE_VEHICLES, names=("candidate",)) + def test_final_settling_ramp_is_bounded(self, candidate): + _, control = make_control(candidate) + control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0)) + CS = make_car_state(0.0, -0.3, standstill=True) + outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)] + + rates = -np.diff([-0.33, *outputs]) / DT_CTRL + expected = [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)] + [1.0] + np.testing.assert_allclose(rates, expected, rtol=1e-6, atol=1e-12) + + @parameterized.expand(((0.6, -0.1, False), (0.0, 0.0, True)), names=("v_ego", "a_ego", "standstill")) + def test_stopping_never_releases_a_stronger_command(self, v_ego, a_ego, standstill): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -3.0) + output = control.update(True, make_car_state(v_ego, a_ego, standstill), 0.0, True, (-3.5, 2.0)) + self.assertAlmostEqual(output, -3.0) + + def test_reported_standstill_while_moving_can_hold_the_brake(self): + _, control = make_control(GM.CHEVROLET_BOLT_EUV) + control.long_control_state = LongCtrlState.off + output = control.update(True, make_car_state(0.3, -0.3, standstill=True), -0.1, False, (-3.5, 2.0)) + self.assertAlmostEqual(output, -0.33) + + def test_stopping_removes_positive_acceleration_immediately(self): + _, control = make_control(HYUNDAI.HYUNDAI_SONATA, 0.2) + output = control.update(True, make_car_state(0.2, -0.2), -0.1, True, (-3.5, 2.0)) + self.assertAlmostEqual(output, -DT_CTRL) + + def test_rollback_uses_the_stock_ramp(self): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + output = control.update(True, make_car_state(-0.1, 0.1), -0.1, True, (-3.5, 2.0)) + self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel)) + + def test_rollback_after_settling_arms_uses_the_stock_ramp(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0)) + control.update(True, make_car_state(0.01, -0.3), -0.1, True, (-3.5, 2.0)) + previous = control.last_output_accel + output = control.update(True, make_car_state(-0.04, -0.3), -0.1, True, (-3.5, 2.0)) + + self.assertAlmostEqual(output, previous - DT_CTRL) + + def test_small_velocity_noise_does_not_trigger_the_stock_rate(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0)) + output = control.update(True, make_car_state(-0.04, -0.3, standstill=True), -0.1, True, (-3.5, 2.0)) + assert -0.331 < output < -0.33 + + def test_terminal_speed_chatter_cannot_extend_settling_ramp(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0)) + outputs = [ + control.update(True, make_car_state(0.049 if frame % 2 == 0 else 0.051, -0.3), -0.1, True, (-3.5, 2.0)) for frame in range(STOPPING_SETTLE_FRAMES + 2) + ] + + rates = -np.diff([-0.33, *outputs]) / DT_CTRL + np.testing.assert_allclose( + rates[:STOPPING_SETTLE_FRAMES], [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)], rtol=1e-6, atol=1e-12 + ) + np.testing.assert_allclose(rates[-2:], [1.0, 1.0], rtol=1e-6, atol=1e-12) + + def test_terminal_speed_plateau_cannot_extend_settling_ramp(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0)) + CS = make_car_state(0.03, -0.3) + outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)] + + rates = -np.diff([-0.33, *outputs]) / DT_CTRL + np.testing.assert_allclose(rates[-2:], [1.0, 1.0], rtol=1e-6, atol=1e-12) + + def test_interrupted_stop_cannot_reuse_settling_hold(self): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0)) + control.update(False, make_car_state(0.0, 0.0, standstill=True), 0.0, False, (-3.5, 2.0)) + output = control.update(True, make_car_state(0.0, -0.3, standstill=True), -0.1, True, (-3.5, 2.0)) + + self.assertAlmostEqual(output, stock_stopping_output(0.0, CP.stopAccel)) + + def test_departure_uses_the_stock_pid_path(self): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + control.long_control_state = LongCtrlState.stopping + output = control.update(True, make_car_state(0.0), 0.6, False, (-3.5, 2.0)) + assert control.long_control_state == LongCtrlState.pid + assert output > 0.0 + + def test_planner_mpc_and_longcontrol_complete_a_smooth_stop(self): + plant = PlantSP( + lead_relevancy=True, + speed=0.6, + distance_lead=3.6, + run_long_control=True, + actuator_model=PRIUS_TSS2_ROUTE_MODEL, + ) + plant.planner.accel_controller.enabled = True + plant.planner.accel_controller.profile = 1 + plant.planner.dec._enabled = False + commands = [] + speeds = [] + states = [] + solver_statuses = [] + + with ( + mock.patch.object(plant.planner.accel_controller, "update_params", return_value=None), + mock.patch.object(plant.planner.dec, "_read_params", return_value=None), + ): + while plant.current_time < 5.0: + result = plant.step(v_lead=0.0, v_cruise=8.0) + commands.append(result["actuator_command"]) + speeds.append(result["speed"]) + states.append(result["long_control_state"]) + solver_statuses.append(plant.planner.mpc.solution_status) + + stopping = states.index(LongCtrlState.stopping) + moving_stop_commands = [ + command for command, state, speed in zip(commands, states, speeds, strict=True) if state == LongCtrlState.stopping and speed > STOPPING_SPEED_TOLERANCE + ] + assert all(current <= previous + 1e-9 for previous, current in zip(commands[stopping:-1], commands[stopping + 1 :], strict=True)) + assert len(moving_stop_commands) > 1 and max(moving_stop_commands) - min(moving_stop_commands) < 1e-9 + assert plant.speed == 0.0 and plant.distance < 1.0 + assert plant.distance_lead - plant.distance > 3.0 + assert all(status == 0 for status in solver_statuses)