diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py index 65e157bdb..f3ed0fc07 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py @@ -151,8 +151,8 @@ class SmartCruiseControlMap: a = 0.5 * TARGET_JERK b = self.a_ego c = self.v_ego - tv - t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a - t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a + t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / (2 * a) + t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / (2 * a) if not isinstance(t_a, complex) and t_a > 0: t = t_a else: diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py index 3b16c59bb..dc27447c0 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py @@ -4,13 +4,17 @@ 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 json +import math import platform +import pytest + from openpilot.cereal import custom from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import R, SmartCruiseControlMap MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState @@ -55,4 +59,17 @@ class TestSmartCruiseControlMap: self.scc_m.update(True, False, 0., 0., 0.) assert self.scc_m.state == VisionState.enabled + def test_moderate_curve(self): + # Regression: `... / 2 * a` parsed as `(.../2)*a` instead of `.../(2*a)`, + # making max_d ~11x too small so the moderate-curve branch never tripped. + # v_ego=25, a_ego=0, tv=24: fixed max_dā‰ˆ45m vs buggy ā‰ˆ4m at a 40m waypoint. + waypoint_lon_deg = (40.0 / R) * (180.0 / math.pi) + self.mem_params.put("LastGPSPosition", json.dumps({"latitude": 0.0, "longitude": 0.0}), block=True) + self.mem_params.put("MapTargetVelocities", + json.dumps([{"latitude": 0.0, "longitude": waypoint_lon_deg, "velocity": 24.0}]), block=True) + + self.scc_m.update(True, False, 25.0, 0.0, 30.0) + + assert self.scc_m.v_target == pytest.approx(24.0) + # TODO-SP: mock data from modelV2 to test other states