diff --git a/cereal/car.capnp b/cereal/car.capnp index fa33f94d47a4c8..744e457442096e 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -398,6 +398,9 @@ struct CarParams { kiBP @2 :List(Float32); kiV @3 :List(Float32); kf @4 :Float32; + kfV @5 :List(Float32); + kfBP @6 :List(Float32); + } struct LongitudinalPIDTuning { diff --git a/common/op_params.py b/common/op_params.py index 60410f39988e8a..7b2b625701eee4 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -48,7 +48,7 @@ def __init__(self): "All profiles support dynamic follow so you'll get your preferred distance while\n" "retaining the smoothness and safety of dynamic follow!", 'live': True}, 'force_pedal': {'default': False, 'allowed_types': [bool], 'description': "If openpilot isn't recognizing your comma pedal, set this to True", 'live': False}, - 'global_df_mod': {'default': None, 'allowed_types': [type(None), float, int], 'description': 'The modifer for the current distance used by dynamic follow. The range is limited from 0.7 to 1.1\n' + 'global_df_mod': {'default': None, 'allowed_types': [type(None), float, int], 'description': 'The multiplier for the current distance used by dynamic follow. The range is limited from 0.85 to 1.2\n' 'Smaller values will get you closer, larger will get you farther\n' 'This is multiplied by any profile that\'s active. Set to None to disable', 'live': True}, 'hide_auto_df_alerts': {'default': True, 'allowed_types': [bool], 'description': 'Hides the alert that shows what profile the model has chosen'}, @@ -63,11 +63,14 @@ def __init__(self): 'rsa_max_speed': {'default': 24.5, 'allowed_types': [float, int], 'description': 'Speed limit to ignore RSA in m/s', 'live': False}, 'smart_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use Smart Speed for drives above smart_speed_max_vego', 'live': False}, 'smart_speed_max_vego': {'default': 26.8, 'allowed_types': [float, int], 'description': 'Speed limit to ignore Smartspeed in m/s', 'live': False}, + 'spairrowtuning': {'default': False, 'allowed_types': [bool], 'description': 'Better Tuning for Corolla', 'live': False}, 'speed_offset': {'default': 0, 'allowed_types': [float, int], 'description': 'Speed limit offset in m/s', 'live': True}, 'traffic_light_alerts': {'default': False, 'allowed_types': [bool], 'description': "Switch off the traffic light alerts", 'live': False}, 'traffic_lights': {'default': True, 'allowed_types': [bool], 'description': "Should Openpilot stop for traffic lights", 'live': False}, 'traffic_lights_without_direction': {'default': False, 'allowed_types': [bool], 'description': "Should Openpilot stop for traffic lights without a direction specified", 'live': False}, - 'use_car_caching': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use fingerprint caching', 'live': False} + 'use_car_caching': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use fingerprint caching', 'live': False}, + 'min_TR': {'default': None, 'allowed_types': [type(None), float, int], 'description': 'The minimum allowed following distance in seconds. Default is 0.9 seconds.\n' + 'The range is limited from 0.85 to 1.3. Set to None to disable', 'live': True}, } self.params = {} diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 39dfb770cb6995..910dcd3c822db7 100644 Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ diff --git a/models/supercombo.keras b/models/supercombo.keras index d1f6ed7a8766e1..76d549b116742d 100644 Binary files a/models/supercombo.keras and b/models/supercombo.keras differ diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index c6430eddd20b27..3ca19a799274be 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -162,9 +162,10 @@ def fingerprint(logcan, sendcan, has_relay): if frame > frame_fingerprint: # fingerprint done car_fingerprint = candidate_cars[b][0] - elif len(candidate_cars[b]) == 2: # For the RAV4 2019 and Corolla 2020 LE Fingerprint problem + elif len(candidate_cars[b]) < 4: # For the RAV4 2019 and Corolla 2020 LE Fingerprint problem if frame > 180: - car_fingerprint = candidate_cars[b][1] + if any(("TOYOTA COROLLA TSS2 2019" in c) for c in candidate_cars[b]): + car_fingerprint = "TOYOTA COROLLA TSS2 2019" # bail if no cars left or we've been waiting for more than 2s failed = all(len(cc) == 0 for cc in candidate_cars.values()) or frame > 200 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 7e91528b453883..96629a364e062d 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -25,9 +25,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 ret.mass = 2858. + STD_CARGO_KG # kg curb weight Pacifica Hybrid 2017 - ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15,0.30], [0.03,0.05]] - ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kfBP = [[9., 20.], [9., 20.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV, ret.lateralTuning.pid.kfV = [[0.15,0.30], [0.03,0.05], [0.00006]] # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index f57fbcbc629107..dd6bebd1ddc89f 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -24,9 +24,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.wheelbase = 2.85 ret.steerRatio = 14.8 ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this - ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV, ret.lateralTuning.pid.kfV = [[0.01], [0.005], [1. / MAX_ANGLE]] # TODO: tune this # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerLimitTimer = 0.8 ret.steerRateCost = 1.0 diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 634c864cf480d8..ec2dea1d0d1683 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -34,9 +34,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 # not optimized yet # Start with a baseline lateral tuning for all GM vehicles. Override tuning as needed in each model section below. - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] - ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV, ret.lateralTuning.pid.kfV = [[0.2], [0.00], [0.00004]] # full torque for 20 deg at 80mph means 0.00007818594 ret.steerRateCost = 1.0 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index a05fcf15e68594..185d413dea879b 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -157,8 +157,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]] - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] + ret.lateralTuning.pid.kfV = [0.00006] # conservative feed-forward eps_modified = False for fw in car_fw: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 297006909a2017..2ece17a5d34a35 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -29,118 +29,118 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 1. if candidate == CAR.SANTA_FE: - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kfV = [0.00005] ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.766 # Values from optimizer ret.steerRatio = 16.55 # 13.8 is spec end-to-end tire_stiffness_factor = 0.82 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[9., 22.], [9., 22.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] ret.minSteerSpeed = 0. elif candidate == CAR.SONATA: - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kfV = [0.00005] ret.mass = 1513. + STD_CARGO_KG ret.wheelbase = 2.84 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 0. elif candidate == CAR.PALISADE: - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kfV = [0.00005] ret.mass = 1999. + STD_CARGO_KG ret.wheelbase = 2.90 ret.steerRatio = 13.75 * 1.15 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_SORENTO: - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kfV = [0.00005] ret.mass = 1985. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 0. elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]: - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec tire_stiffness_factor = 0.385 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.HYUNDAI_GENESIS: - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kfV = [0.00005] ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] ret.minSteerSpeed = 60 * CV.KPH_TO_MS elif candidate in [CAR.GENESIS_G90, CAR.GENESIS_G80]: ret.mass = 2200 ret.wheelbase = 3.15 ret.steerRatio = 12.069 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kfV = [0.00005] ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_STINGER: - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kfV = [0.00005] ret.mass = 1825. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 0. elif candidate == CAR.KONA: - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec tire_stiffness_factor = 0.385 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.IONIQ: - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec tire_stiffness_factor = 0.385 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.KONA_EV: - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] ret.mass = 1685. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec tire_stiffness_factor = 0.385 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.IONIQ_EV_LTD: - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] ret.mass = 1490. + STD_CARGO_KG #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec tire_stiffness_factor = 0.385 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.KIA_FORTE: - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kfV = [0.00005] ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.centerToFront = ret.wheelbase * 0.4 diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index c2a67b8bfa4aec..b68017d670fbc7 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -30,9 +30,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRateCost = 0.5 ret.steerActuatorDelay = 0.1 - ret.lateralTuning.pid.kf = 0.00006 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.0], [0.0]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.0], [0.0], [0.0]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV, ret.lateralTuning.pid.kfV = [[0.01], [0.005], [0.00006]] ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 54fd83ec293e93..59b9598f4fe826 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -35,9 +35,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 15 ret.steerActuatorDelay = 0.4 # end-to-end angle controller - ret.lateralTuning.pid.kf = 0.00005 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0., 20.], [0., 20.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV, ret.lateralTuning.pid.kfV = [[0.2, 0.3], [0.02, 0.03], [0.00005]] # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 547a34a193bed5..36c40afc412e09 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -6,6 +6,10 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase +from common.op_params import opParams + +op_params = opParams() +spairrowtuning = op_params.get('spairrowtuning', False) GearShifter = car.CarState.GearShifter @@ -42,7 +46,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, if candidate not in [CAR.PRIUS_2019, CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.COROLLA]: # These cars use LQR/INDI ret.lateralTuning.init('pid') - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] if candidate in [CAR.PRIUS, CAR.PRIUS_2019]: stop_and_go = True @@ -125,7 +129,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.5533 ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] elif candidate == CAR.LEXUS_RXH: stop_and_go = True @@ -135,7 +139,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] - ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kfV = [0.00006] # full torque for 10 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RX_TSS2: stop_and_go = True @@ -145,7 +149,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.5533 # not optimized yet ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] - ret.lateralTuning.pid.kf = 0.00007818594 + ret.lateralTuning.pid.kfV = [0.00007818594] elif candidate in [CAR.CHR, CAR.CHRH]: stop_and_go = True @@ -155,7 +159,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] elif candidate in [CAR.CAMRY, CAR.CAMRYH]: stop_and_go = True @@ -165,7 +169,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]: stop_and_go = True @@ -175,7 +179,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.8 ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning - ret.lateralTuning.pid.kf = 0.00012 # community tuning + ret.lateralTuning.pid.kfV = [0.00012] # community tuning elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: stop_and_go = True @@ -185,7 +189,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.8 ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid limited ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning - ret.lateralTuning.pid.kf = 0.00012 # community tuning + ret.lateralTuning.pid.kfV = [0.00012] # community tuning elif candidate == CAR.AVALON: stop_and_go = False @@ -195,7 +199,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] elif candidate == CAR.RAV4_TSS2: stop_and_go = True @@ -207,7 +211,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.longitudinalTuning.kiV = [0.10, 0.10] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13], [0.05]] ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG - ret.lateralTuning.pid.kf = 0.00004 + ret.lateralTuning.pid.kfV = [0.00004] elif candidate == CAR.RAV4H_TSS2: stop_and_go = True @@ -219,7 +223,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.longitudinalTuning.kiV = [0.10, 0.10] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.12], [0.04]] ret.mass = 3800. * CV.LB_TO_KG + STD_CARGO_KG - ret.lateralTuning.pid.kf = 0.00004 + ret.lateralTuning.pid.kfV = [0.00004] elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: stop_and_go = True @@ -227,9 +231,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.wheelbase = 2.63906 ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet - ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.5], [0.1]] - ret.lateralTuning.pid.kf = 0.00007818594 + ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG + ret.lateralTuning.pid.kfV = [0.00007818594] + if spairrowtuning: + ret.steerRatio = 15.33 + tire_stiffness_factor = 0.996 # not optimized yet + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[0.0, 14.0, 26.0], [0.13, 0.39, 0.64]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[0.0, 14.0, 26.0], [0.005, 0.01, 0.1]] + ret.lateralTuning.pid.kfBP, ret.lateralTuning.pid.kfV = [[0.0, 14,0, 26.0], [0.00009, 0.00015, 0.0000781818594]] elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]: stop_and_go = True @@ -239,7 +249,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] - ret.lateralTuning.pid.kf = 0.00007818594 + ret.lateralTuning.pid.kfV = [0.00007818594] elif candidate == CAR.SIENNA: stop_and_go = True @@ -249,7 +259,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.02]] - ret.lateralTuning.pid.kf = 0.00007818594 + ret.lateralTuning.pid.kfV = [0.00007818594] elif candidate in [CAR.LEXUS_IS, CAR.LEXUS_ISH, CAR.LEXUS_RX]: stop_and_go = False @@ -259,7 +269,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] elif candidate == CAR.LEXUS_CTH: stop_and_go = True @@ -269,7 +279,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.517 ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] - ret.lateralTuning.pid.kf = 0.00007 + ret.lateralTuning.pid.kfV = [0.00007] elif candidate == CAR.LEXUS_NXH: stop_and_go = True @@ -279,7 +289,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] elif candidate == CAR.LEXUS_NXH: stop_and_go = True @@ -289,7 +299,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kfV = [0.00006] elif candidate == CAR.LEXUS_UXH_TSS2: stop_and_go = True @@ -299,7 +309,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3500. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] - ret.lateralTuning.pid.kf = 0.00007 + ret.lateralTuning.pid.kfV = [0.00007] ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index e10124ac779b49..58c5a06fab201a 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -105,7 +105,7 @@ class ECU: {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 814: 8, 824: 2, 825: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 861: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 875: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1130: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1681: 8, 1767:4, 1777: 8, 1779: 8, 1792: 8, 1840: 8, 1863:8, 1872: 8, 1904: 8, 1912: 8, 1941: 8, 1949: 8, 1952: 8, 1960: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996:8, 1998: 8, 2004: 8, 2010: 8, 2012: 8, 2015: 8, 2016: 8, 2018: 8, 2024: 8, 2026: 8, 2027: 8, 2029: 8, 2030: 8, 2031: 8} ], CAR.PRIUS_2019: [ - {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2002: 8, 2010: 8} + {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2002: 8, 2010: 8} ], CAR.COROLLA: [ {36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8} @@ -132,7 +132,7 @@ class ECU: {36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 513: 6, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1767: 4, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1808: 8, 1816: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1956: 8, 1961: 8, 1964: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} ], CAR.CAMRYH: [ - {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1872: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8} + {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1872: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8} ], CAR.HIGHLANDER: [ {36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1585: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} @@ -144,13 +144,13 @@ class ECU: {36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8} ], CAR.HIGHLANDERH_TSS2: [ - {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 942: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1263: 8, 1264: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8} + {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 942: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1263: 8, 1264: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8} ], CAR.AVALON: [ {36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8} ], CAR.RAV4_TSS2: [ - {36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} + {36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1872: 8, 1880:8 , 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} ], CAR.COROLLA_TSS2: [ {36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1960: 8, 1981: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8} @@ -248,6 +248,7 @@ class ECU: b'\x028966306R5000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966306R6000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x028966306N8100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152633712\x00\x00\x00\x00\x00\x00', @@ -414,11 +415,13 @@ class ECU: }, CAR.COROLLAH_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x01896630ZJ1000\x00\x00\x00\x00', b'\x018966342M5000\x00\x00\x00\x00', b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00', b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ @@ -452,6 +455,7 @@ class ECU: b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, CAR.HIGHLANDER: { @@ -612,15 +616,18 @@ class ECU: CAR.PRIUS_2019: { (Ecu.engine, 0x700, None): [ b'\x028966347A5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x038966347B6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B47060\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152647470\x00\x00\x00\x00\x00\x00', + b'F152647290\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'881514706000\x00\x00\x00\x00', + b'881514706100\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702300\x00\x00\x00\x00', diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 08ae23741dc638..d7ad1523f34575 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -74,11 +74,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRatio = 15.6 ret.steerRatioRear = 0. - ret.lateralTuning.pid.kf = 0.00006 ret.lateralTuning.pid.kpBP = [0.] ret.lateralTuning.pid.kiBP = [0.] + ret.lateralTuning.pid.kfBP = [0.] ret.lateralTuning.pid.kpV = [0.6] ret.lateralTuning.pid.kiV = [0.2] + ret.lateralTuning.pid.kfV = [0.00006] ret.enableCamera = True # Stock camera detection doesn't apply to VW ret.transmissionType = car.CarParams.TransmissionType.automatic diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 83abd1641ac9b1..c1451a1a753a2b 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -106,7 +106,7 @@ def __gt__(self, alert2): "BRAKE PRESSED", "Longitudinal Control Disabled", AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .4, 2., 0.2), + Priority.LOW, VisualAlert.none, AudibleAlert.none, .4, 2., 0.2), Alert( "trafficSlow", @@ -304,7 +304,7 @@ def __gt__(self, alert2): Alert( "posenetInvalid", "TAKE CONTROL", - "Vision Model Output Uncertain", + "Camera blocked or Device too hot!", AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.), @@ -695,7 +695,7 @@ def __gt__(self, alert2): Alert( "posenetInvalidNoEntry", "openpilot Unavailable", - "Vision Model Output Uncertain", + "Camera blocked or Device too hot!", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 77c7f9235db7c9..254531edad5755 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -8,7 +8,7 @@ from selfdrive.config import Conversions as CV from common.travis_checker import travis -from selfdrive.controls.lib.dynamic_follow.auto_df import load_weights, predict +from selfdrive.controls.lib.dynamic_follow.auto_df import predict from selfdrive.controls.lib.dynamic_follow.df_manager import dfManager from selfdrive.controls.lib.dynamic_follow.support import LeadData, CarData, dfData, dfProfiles @@ -19,7 +19,6 @@ def __init__(self, mpc_id): self.op_params = opParams() self.df_profiles = dfProfiles() self.df_manager = dfManager(self.op_params) - load_weights() if not travis and mpc_id == 1: self.pm = messaging_arne.PubMaster(['dynamicFollowData']) @@ -28,10 +27,10 @@ def __init__(self, mpc_id): # Model variables mpc_rate = 1 / 20. - self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 139.52000427246094]} + self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625]} self.predict_rate = 1 / 4. - self.skip_every = round(0.2 / mpc_rate) - self.model_input_len = round(35 / mpc_rate) # int: model input time + self.skip_every = round(0.25 / mpc_rate) + self.model_input_len = round(45 / mpc_rate) # int: model input time # Dynamic follow variables self.default_TR = 1.8 @@ -40,6 +39,9 @@ def __init__(self, mpc_id): self.v_ego_retention = 2.5 self.v_rel_retention = 1.5 + self.sng_TR = 1.8 # reacceleration stop and go TR + self.sng_speed = 18.0 * CV.MPH_TO_MS + self._setup_changing_variables() def _setup_changing_variables(self): @@ -55,6 +57,7 @@ def _setup_changing_variables(self): self.last_cost = 0.0 self.last_predict_time = 0.0 self.auto_df_model_data = [] + self._get_live_params() # so they're defined just in case def update(self, CS, libmpc): self._get_live_params() @@ -117,6 +120,7 @@ def _store_df_data(self): # Store data for auto-df model self.auto_df_model_data.append([self._norm(self.car_data.v_ego, 'v_ego'), self._norm(self.lead_data.v_lead, 'v_lead'), + self._norm(self.lead_data.a_lead, 'a_lead'), self._norm(self.lead_data.x_lead, 'x_lead')]) while len(self.auto_df_model_data) > self.model_input_len: del self.auto_df_model_data[0] @@ -202,12 +206,27 @@ def _calculate_relative_accel_new(self): return calc_mod return None - def global_profile_mod(self, TR, profile_mod_pos, profile_mod_neg): - if self.global_df_mod is not None: # only apply when not in sng - TR *= self.global_df_mod - profile_mod_pos *= (1 - self.global_df_mod) + 1 - profile_mod_neg *= self.global_df_mod - return TR, profile_mod_pos, profile_mod_neg + def global_profile_mod(self, profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist): + """ + This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod + It also intelligently adjusts the profile mods at each breakpoint based on the change in TR + """ + if self.global_df_mod is None: + return profile_mod_pos, profile_mod_neg, y_dist + global_df_mod = 1 - self.global_df_mod + + # Calculate new TRs + speeds = [0, self.sng_speed, 18, x_vel[-1]] # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)] + mods = [0, 0.1, 0.7, 1] # how much to limit global_df_mod at each speed, 1 is full effect + y_dist_new = [y - (y * global_df_mod * np.interp(x, speeds, mods)) for x, y in zip(x_vel, y_dist)] + + # Calculate how to change profile mods based on change in TR + # eg. if df mod is 0.7, then increase positive mod and decrease negative mod + calc_profile_mods = [(np.interp(mod_x, x_vel, y_dist) - np.interp(mod_x, x_vel, y_dist_new) + 1) for mod_x in profile_mod_x] + profile_mod_pos = [mod_pos * mod for mod_pos, mod in zip(profile_mod_pos, calc_profile_mods)] + profile_mod_neg = [mod_neg * ((1 - mod) + 1) for mod_neg, mod in zip(profile_mod_neg, calc_profile_mods)] + + return profile_mod_pos, profile_mod_neg, y_dist_new def _get_TR(self): x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities @@ -236,24 +255,23 @@ def _get_TR(self): else: raise Exception('Unknown profile type: {}'.format(df_profile)) + # Global df mod + profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist) + # Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics profile_mod_pos = interp(self.car_data.v_ego, profile_mod_x, profile_mod_pos) profile_mod_neg = interp(self.car_data.v_ego, profile_mod_x, profile_mod_neg) - sng_TR = 1.8 # reacceleration stop and go TR - sng_speed = 18.0 * CV.MPH_TO_MS - - if self.car_data.v_ego > sng_speed: # keep sng distance until we're above sng speed again + if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again self.sng = False - if (self.car_data.v_ego >= sng_speed or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego) and not self.sng: + if (self.car_data.v_ego >= self.sng_speed or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego) and not self.sng: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease TR = interp(self.car_data.v_ego, x_vel, y_dist) - TR, profile_mod_pos, profile_mod_neg = self.global_profile_mod(TR, profile_mod_pos, profile_mod_neg) # only within normal driving conditions else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating self.sng = True - x = [sng_speed * 0.7, sng_speed] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating - y = [sng_TR, interp(sng_speed, x_vel, y_dist)] + x = [self.sng_speed * 0.7, self.sng_speed] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating + y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)] TR = interp(self.car_data.v_ego, x, y) TR_mods = [] @@ -272,7 +290,7 @@ def _get_TR(self): if self.lead_data.v_lead - deadzone > self.car_data.v_ego: TR_mods.append(rel_accel_mod) - x = [sng_speed / 5.0, sng_speed] # as we approach 0, apply x% more distance + x = [self.sng_speed / 5.0, self.sng_speed] # as we approach 0, apply x% more distance y = [1.05, 1.0] profile_mod_pos *= interp(self.car_data.v_ego, x, y) # but only for currently positive mods @@ -283,7 +301,8 @@ def _get_TR(self): x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph y = [1.0, .75, .65] # reduce TR when changing lanes TR *= interp(self.car_data.v_ego, x, y) - return clip(TR, 0.9, 2.7) + + return float(clip(TR, self.min_TR, 2.7)) def update_lead(self, v_lead=None, a_lead=None, x_lead=None, status=False, new_lead=False): self.lead_data.v_lead = v_lead @@ -304,4 +323,10 @@ def _update_car(self, CS): def _get_live_params(self): self.global_df_mod = self.op_params.get('global_df_mod', None) if self.global_df_mod is not None: - self.global_df_mod = np.clip(self.global_df_mod, 0.7, 1.1) + self.global_df_mod = np.clip(self.global_df_mod, 0.85, 1.2) + + self.min_TR = self.op_params.get('min_TR', None) + if self.min_TR is not None: + self.min_TR = clip(self.min_TR, 0.85, 1.3) + else: + self.min_TR = 0.9 # default diff --git a/selfdrive/controls/lib/dynamic_follow/auto_df.py b/selfdrive/controls/lib/dynamic_follow/auto_df.py index eb7214e687c0a8..ae36240867acad 100644 --- a/selfdrive/controls/lib/dynamic_follow/auto_df.py +++ b/selfdrive/controls/lib/dynamic_follow/auto_df.py @@ -2,13 +2,11 @@ Generated using Konverter: https://github.com/ShaneSmiskol/Konverter """ +from common.basedir import BASEDIR import numpy as np - -def load_weights(): - global w, b - wb = np.load('/data/openpilot/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz', allow_pickle=True) - w, b = wb['wb'] +wb = np.load(BASEDIR + '/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz', allow_pickle=True) +w, b = wb['wb'] def softmax(x): return np.exp(x) / np.sum(np.exp(x), axis=0) @@ -19,7 +17,5 @@ def predict(x): l1 = np.dot(l0, w[1]) + b[1] l1 = np.maximum(0, l1) l2 = np.dot(l1, w[2]) + b[2] - l2 = np.maximum(0, l2) - l3 = np.dot(l2, w[3]) + b[3] - l3 = softmax(l3) - return l3 + l2 = softmax(l2) + return l2 diff --git a/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz b/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz index 865f3cdc610285..da367c5e6886d2 100644 Binary files a/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz and b/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz differ diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 7d477bc9ceec50..d16aa63837522e 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -10,11 +10,11 @@ def __init__(self, CP): self.scale = CP.lateralTuning.lqr.scale self.ki = CP.lateralTuning.lqr.ki - self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2)) - self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1)) - self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2)) - self.K = np.array(CP.lateralTuning.lqr.k).reshape((1,2)) - self.L = np.array(CP.lateralTuning.lqr.l).reshape((2,1)) + self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) + self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) + self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) + self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) + self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) self.dc_gain = CP.lateralTuning.lqr.dcGain self.x_hat = np.array([[0], [0]]) @@ -23,9 +23,6 @@ def __init__(self, CP): self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer - self.accel_limit = 0.1 # 100x degrees/sec**2 - self.angle_rate_des = 0.0 # degrees/sec, rate dynamically limited by accel_limit - self.angle_steers_des = 0.0 self.reset() @@ -50,22 +47,11 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, v_ego) - if (self.output_steer > 0.0) and (angle_steers > 0.0): - factor = -0.4 - elif (self.output_steer < 0.0) and (angle_steers < 0.0): - factor = -0.4 - else: - factor = 0.2 - torque_scale = (1-factor*min(abs(angle_steers)/100,1.0))*(0.45 + v_ego / 60.0)**2 # Scale actuator model with speed + torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed - if not steer_override and abs(angle_steers) < 10 and v_ego > 10: - self.angle_rate_des = min(self.angle_rate_des + self.accel_limit * v_ego, max(self.angle_rate_des - self.accel_limit * v_ego, path_plan.angleSteers - path_plan.angleOffset - self.angle_steers_des)) - self.angle_steers_des = self.angle_steers_des + self.angle_rate_des - else: - self.angle_rate_des = angle_steers_rate / 100. - self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset # Subtract offset. Zero angle should correspond to zero torque + self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset angle_steers -= path_plan.angleOffset # Update Kalman filter @@ -77,8 +63,6 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste lqr_log.active = False lqr_output = 0. self.reset() - self.angle_rate_des = angle_steers_rate / 100. - self.angle_steers_des = angle_steers else: lqr_log.active = True @@ -94,8 +78,8 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste i = self.i_lqr + self.ki * self.i_rate * error control = lqr_output + i - if ((error >= 0 and (control <= steers_max or i < 0.0)) or \ - (error <= 0 and (control >= -steers_max or i > 0.0))): + if (error >= 0 and (control <= steers_max or i < 0.0)) or \ + (error <= 0 and (control >= -steers_max or i > 0.0)): self.i_lqr = i self.output_steer = lqr_output + self.i_lqr diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 6a59565509ce8d..d73a8c17b31886 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,4 +1,4 @@ -from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.pid import PIControllerk_f from selfdrive.controls.lib.drive_helpers import get_steer_max from cereal import car from cereal import log @@ -6,9 +6,9 @@ class LatControlPID(): def __init__(self, CP): - self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), + self.pid = PIControllerk_f((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer) + (CP.lateralTuning.pid.kfBP, CP.lateralTuning.pid.kfV), pos_limit=1.0, sat_limit=CP.steerLimitTimer) self.angle_steers_des = 0. def reset(self): diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index a0c59304814777..71e69e89f13dce 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -189,5 +189,86 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri self.last_setpoint = setpoint + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control + +class PIControllerk_f: + def __init__(self, k_p, k_i, k_f, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + self._k_p = k_p # proportional gain + self._k_i = k_i # integral gain + self._k_f = k_f # feedforward gain + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.sat_count_rate = 1.0 / rate + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.sat_limit = sat_limit + self.convert = convert + + self.reset() + + @property + def k_p(self): + return interp(self.speed, self._k_p[0], self._k_p[1]) + + @property + def k_i(self): + return interp(self.speed, self._k_i[0], self._k_i[1]) + + @property + def k_f(self): + return interp(self.speed, self._k_f[0], self._k_f[1]) + + def _check_saturation(self, control, check_saturation, error): + saturated = (control < self.neg_limit) or (control > self.pos_limit) + + if saturated and check_saturation and abs(error) > 0.1: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.f = 0.0 + self.sat_count = 0.0 + self.saturated = False + self.control = 0 + + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): + self.speed = speed + + error = float(apply_deadzone(setpoint - measurement, deadzone)) + self.p = error * self.k_p + self.f = feedforward * self.k_f + + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + i = self.i + error * self.k_i * self.i_rate + control = self.p + self.f + i + + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + # Update when changing i will move the control away from the limits + # or when i will move towards the sign of the error + if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ + (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ + not freeze_integrator: + self.i = i + + control = self.p + self.f + self.i + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + self.saturated = self._check_saturation(control, check_saturation, error) + self.control = clip(control, self.neg_limit, self.pos_limit) return self.control diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 0cfcb36eed8a87..1cb386f324ea01 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -303,10 +303,10 @@ def thermald_thread(): if max_cpu_temp > 107. or bat_temp >= 63.: # onroad not allowed thermal_status = ThermalStatus.danger - elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C + elif max_comp_temp > 96.0 or bat_temp > 60.: # CPU throttling starts around ~90C # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) - elif max_cpu_temp > 87.5: + elif max_cpu_temp > 94.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 80.0: