diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py index 66f1b947a8..551a786c76 100644 --- a/selfdrive/car/body/values.py +++ b/selfdrive/car/body/values.py @@ -17,6 +17,9 @@ class CarControllerParams: LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower STEER_THRESHOLD = 1.0 + def __init__(self, CP): + pass + class CAR: BODY = "COMMA BODY" diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 5a2d90c64c..8bb8d916c1 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -45,7 +45,7 @@ class CarController: self.hud_count += 1 # steering - if self.frame % 2 == 0: + if self.frame % self.params.STEER_STEP == 0: # TODO: can we make this more sane? why is it different for all the cars? lkas_control_bit = self.lkas_control_bit_prev diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 3b3fc6e558..dd901d87ab 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -30,6 +30,7 @@ class CAR: class CarControllerParams: def __init__(self, CP): + self.STEER_STEP = 2 # 50 Hz self.STEER_ERROR_MAX = 80 if CP.carFingerprint in RAM_HD: self.STEER_DELTA_UP = 14 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 592d8586ca..b3c619859e 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -63,7 +63,7 @@ class CarController: apply_angle = CS.out.steeringAngleDeg # send steering commands at 20Hz - if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: + if (self.frame % CarControllerParams.STEER_STEP) == 0: lca_rq = 1 if CC.latActive else 0 # use LatCtlPath_An_Actl to actuate steering diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 5820b5c9fd..3495976d99 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -17,7 +17,7 @@ AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_p class CarControllerParams: # Messages: Lane_Assist_Data1, LateralMotionControl - LKAS_STEER_STEP = 5 + STEER_STEP = 5 # Message: IPMA_Data LKAS_UI_STEP = 100 # Message: ACCDATA_3 @@ -29,6 +29,9 @@ class CarControllerParams: RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + def __init__(self, CP): + pass + class RADAR: DELPHI_ESR = 'ford_fusion_2018_adas' diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 21ede171e0..40c4595f13 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -49,6 +49,9 @@ class CarControllerParams: GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] BRAKE_LOOKUP_V = [MAX_BRAKE, 0.] + def __init__(self, CP): + pass + class CAR: HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 4225826c1d..b5f349cf9b 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -18,11 +18,12 @@ class CarControllerParams: def __init__(self, CP): self.STEER_DELTA_UP = 3 - self.STEER_DELTA_DOWN = 7 + self.STEER_DELTA_DOWN = 4 self.STEER_DRIVER_ALLOWANCE = 50 self.STEER_DRIVER_MULTIPLIER = 2 self.STEER_DRIVER_FACTOR = 1 self.STEER_THRESHOLD = 150 + self.STEER_STEP = 1 # 100 Hz if CP.carFingerprint in CANFD_CAR: self.STEER_MAX = 270 diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 9befad4d0b..f4826d2785 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -20,6 +20,10 @@ class CarControllerParams: STEER_DRIVER_MULTIPLIER = 1 # weight driver torque STEER_DRIVER_FACTOR = 1 # from dbc STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + STEER_STEP = 1 # 100 Hz + + def __init__(self, CP): + pass class CAR: diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 09bd7ca838..0736bf95cd 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -18,6 +18,9 @@ class CarControllerParams: LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower STEER_THRESHOLD = 1.0 + def __init__(self, CP): + pass + class CAR: XTRAIL = "NISSAN X-TRAIL 2017" diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 296169587a..1fc6f418a5 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -75,3 +75,6 @@ class CarControllerParams: JERK_LIMIT_MAX = 8 JERK_LIMIT_MIN = -8 ACCEL_TO_SPEED_MULTIPLIER = 3 + + def __init__(self, CP): + pass diff --git a/selfdrive/car/tests/test_lateral_limits.py b/selfdrive/car/tests/test_lateral_limits.py index c8609acc5c..8a070cc1bb 100755 --- a/selfdrive/car/tests/test_lateral_limits.py +++ b/selfdrive/car/tests/test_lateral_limits.py @@ -1,8 +1,10 @@ #!/usr/bin/env python3 +import argparse import random import unittest from parameterized import parameterized, parameterized_class -from selfdrive.car.hyundai.values import CarControllerParams +# from selfdrive.car.hyundai.values import CarControllerParams +import importlib from cereal import car @@ -25,17 +27,19 @@ jerks = defaultdict(dict) class TestLateralLimits(unittest.TestCase): @classmethod def setUpClass(cls): - # TODO: remove these once it works with all platforms - if not cls.car_model.startswith(('KIA', 'HYUNDAI', 'GENESIS')): - raise unittest.SkipTest - - # if not cls.car_model.startswith('KIA EV6'): - # raise unittest.SkipTest - CarInterface, _, _ = interfaces[cls.car_model] CP = CarInterface.get_params(cls.car_model) - # TODO: just make all CCPs take CarParams + + if CP.dashcamOnly: + raise unittest.SkipTest("Platform is behind dashcamOnly") + + # TODO: test these + if CP.carName in ("honda", "nissan", "body"): + raise unittest.SkipTest("No steering safety") + + CarControllerParams = importlib.import_module(f'selfdrive.car.{CP.carName}.values').CarControllerParams cls.control_params = CarControllerParams(CP) + cls.torque_params = get_torque_params(cls.car_model) @classmethod @@ -47,19 +51,28 @@ class TestLateralLimits(unittest.TestCase): def _calc_jerk(self): # TODO: some cars don't send at 100hz, put steer rate/step into CCP to calculate this properly - time_to_max = self.control_params.STEER_MAX / self.control_params.STEER_DELTA_UP / 100. - time_to_min = self.control_params.STEER_MAX / self.control_params.STEER_DELTA_DOWN / 100. - # TODO: fix this - max_lat_accel = (self.torque_params['LAT_ACCEL_FACTOR'] + self.torque_params['MAX_LAT_ACCEL_MEASURED']) / 2. + steer_step = self.control_params.STEER_STEP + time_to_max = self.control_params.STEER_MAX / self.control_params.STEER_DELTA_UP / 100. * steer_step + time_to_min = self.control_params.STEER_MAX / self.control_params.STEER_DELTA_DOWN / 100. * steer_step + max_lat_accel = self.torque_params['LAT_ACCEL_FACTOR'] return max_lat_accel / time_to_max, max_lat_accel / time_to_min - def test_something(self): + def test_jerk_limits(self): up_jerk, down_jerk = self._calc_jerk() jerks[self.car_model] = {"up_jerk": up_jerk, "down_jerk": down_jerk} - # self.assertLess(up_jerk, 2.0) - # self.assertLess(down_jerk, 2.5) + self.assertLess(up_jerk, 2.5) + # self.assertGreater(down_jerk, 2.0) if __name__ == "__main__": - unittest.main() + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("--print-jerks", action="store_true", help="Print theoretical max lateral jerk values for all platforms") + + args = parser.parse_args() + + if args.print_jerks: + # TODO: would be nice to support this + pass + else: + unittest.main() diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 94fbdc8bf2..7be2795a7a 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -21,6 +21,8 @@ class CarControllerParams: STEER_MAX = 1500 STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + STEER_STEP = 1 + def __init__(self, CP): if CP.lateralTuning.which == 'torque': self.STEER_DELTA_UP = 15 # 1.0s time to peak torque diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 5624c3dd5f..9e2d1ea5e7 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -30,7 +30,7 @@ class CarController: # **** Steering Controls ************************************************ # - if self.frame % self.CCP.HCA_STEP == 0: + if self.frame % self.CCP.STEER_STEP == 0: # Logic to avoid HCA state 4 "refused": # * Don't steer unless HCA is in state 3 "ready" or 5 "active" # * Don't steer at standstill @@ -49,14 +49,14 @@ class CarController: self.hcaEnabledFrameCount = 0 else: self.hcaEnabledFrameCount += 1 - if self.hcaEnabledFrameCount >= 118 * (100 / self.CCP.HCA_STEP): # 118s + if self.hcaEnabledFrameCount >= 118 * (100 / self.CCP.STEER_STEP): # 118s hcaEnabled = False self.hcaEnabledFrameCount = 0 else: hcaEnabled = True if self.apply_steer_last == apply_steer: self.hcaSameTorqueCount += 1 - if self.hcaSameTorqueCount > 1.9 * (100 / self.CCP.HCA_STEP): # 1.9s + if self.hcaSameTorqueCount > 1.9 * (100 / self.CCP.STEER_STEP): # 1.9s apply_steer -= (1, -1)[apply_steer < 0] self.hcaSameTorqueCount = 0 else: diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 05994c0100..c071c603a8 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -18,7 +18,7 @@ Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) class CarControllerParams: - HCA_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz + STEER_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz