diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index 3271476de6..1ae996c89f 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/opendbc_repo/opendbc/safety/safety.h @@ -603,8 +603,9 @@ void pcm_cruise_check(bool cruise_engaged) { void speed_mismatch_check(const float speed_2) { // Disable controls if speeds from two sources are too far apart. // For safety modes that use speed to adjust torque or angle limits - const float MAX_SPEED_DELTA = 2.0; // m/s - bool is_invalid_speed = SAFETY_ABS(speed_2 - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > MAX_SPEED_DELTA; + // compared in vehicle_speed's integer units, so the limit is exact + const int MAX_SPEED_DELTA = 2 * (int)VEHICLE_SPEED_FACTOR; // 2 m/s + bool is_invalid_speed = SAFETY_ABS(ROUND(speed_2 * VEHICLE_SPEED_FACTOR) - vehicle_speed.values[0]) > MAX_SPEED_DELTA; if (is_invalid_speed) { controls_allowed = false; } diff --git a/opendbc_repo/opendbc/safety/tests/common.py b/opendbc_repo/opendbc/safety/tests/common.py index 7a3d71868c..711beb1153 100644 --- a/opendbc_repo/opendbc/safety/tests/common.py +++ b/opendbc_repo/opendbc/safety/tests/common.py @@ -35,6 +35,12 @@ def round_speed(v): return round(v * VEHICLE_SPEED_FACTOR) / VEHICLE_SPEED_FACTOR +def speeds_within_delta(speed, speed_2): + # safety compares the speeds in integer VEHICLE_SPEED_FACTOR units + delta = away_round(speed * VEHICLE_SPEED_FACTOR) - away_round(speed_2 * VEHICLE_SPEED_FACTOR) + return abs(delta) <= MAX_SPEED_DELTA * VEHICLE_SPEED_FACTOR + + def make_msg(bus, addr, length=8, dat=None): if dat is None: dat = b'\x00' * length @@ -1218,7 +1224,7 @@ class CarSafetyTest(SafetyTest): self.safety.set_controls_allowed(True) self._rx(self._speed_msg_2(speed_2)) - within_delta = abs(speed - speed_2) <= MAX_SPEED_DELTA + within_delta = speeds_within_delta(speed, speed_2) self.assertEqual(self.safety.get_controls_allowed(), within_delta) def test_safety_tick(self): diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py old mode 100755 new mode 100644 index 62684f0635..46190c06d4 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -185,7 +185,7 @@ class TestFordSafetyBase(common.CarSafetyTest): self.safety.set_controls_allowed(True) self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0)) - within_delta = abs(speed - speed_2) <= common.MAX_SPEED_DELTA + within_delta = common.speeds_within_delta(speed, speed_2) self.assertEqual(self.safety.get_controls_allowed(), within_delta) def test_rx_hook(self): diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla.py b/opendbc_repo/opendbc/safety/tests/test_tesla.py old mode 100755 new mode 100644 index ca9040f5bd..6344400a0c --- a/opendbc_repo/opendbc/safety/tests/test_tesla.py +++ b/opendbc_repo/opendbc/safety/tests/test_tesla.py @@ -10,7 +10,7 @@ from opendbc.car.structs import CarParams from opendbc.car.vehicle_model import VehicleModel from opendbc.can import CANDefine import opendbc.safety.tests.common as common -from opendbc.safety.tests.common import MAX_SPEED_DELTA, MAX_WRONG_COUNTERS, away_round, round_speed +from opendbc.safety.tests.common import MAX_WRONG_COUNTERS, away_round, round_speed, speeds_within_delta MSG_DAS_steeringControl = 0x488 MSG_APS_eacMonitor = 0x27d @@ -183,7 +183,7 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest, self.safety.set_controls_allowed(True) self.assertTrue(self._rx(self._speed_msg_2(speed_2))) - within_delta = abs(speed - speed_2) <= MAX_SPEED_DELTA + within_delta = speeds_within_delta(speed, speed_2) self.assertEqual(self.safety.get_controls_allowed(), within_delta) # Test ESP_B quality flag diff --git a/opendbc_repo/opendbc/safety/tests/test_volkswagen_meb.py b/opendbc_repo/opendbc/safety/tests/test_volkswagen_meb.py index 6521b80a39..68de2982bc 100644 --- a/opendbc_repo/opendbc/safety/tests/test_volkswagen_meb.py +++ b/opendbc_repo/opendbc/safety/tests/test_volkswagen_meb.py @@ -224,7 +224,7 @@ class TestVolkswagenMebSafetyBase(common.CarSafetyTest, common.CurvatureSteering self.safety.set_controls_allowed(True) self._tx(self._curvature_cmd_msg(0, steer_req=True)) - within_delta = abs(speed - speed_2) <= common.MAX_SPEED_DELTA + within_delta = common.speeds_within_delta(speed, speed_2) self.assertEqual(self.safety.get_controls_allowed(), within_delta) def test_curvature_violation(self):