diff --git a/opendbc/car/__init__.py b/opendbc/car/__init__.py index 32f53fcdee..5b24daad3a 100644 --- a/opendbc/car/__init__.py +++ b/opendbc/car/__init__.py @@ -21,6 +21,8 @@ # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. +ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2 + ButtonType = structs.CarState.ButtonEvent.Type AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v']) diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index f2a8974d15..237ba2c26b 100644 --- a/opendbc/car/car.capnp +++ b/opendbc/car/car.capnp @@ -508,7 +508,6 @@ struct CarParams { transmissionType @43 :TransmissionType; carFw @44 :List(CarFw); - radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard radarDelay @74 :Float32; fingerprintSource @49: FingerprintSource; networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network @@ -719,4 +718,5 @@ struct CarParams { maxSteeringAngleDegDEPRECATED @54 :Float32; longitudinalActuatorDelayLowerBoundDEPRECATED @61 :Float32; stoppingControlDEPRECATED @31 :Bool; # Does the car allow full control even at lows speeds when stopping + radarTimeStepDEPRECATED @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard } diff --git a/opendbc/car/chrysler/fingerprints.py b/opendbc/car/chrysler/fingerprints.py index 6f7fdfc708..ba597d70ec 100644 --- a/opendbc/car/chrysler/fingerprints.py +++ b/opendbc/car/chrysler/fingerprints.py @@ -325,6 +325,7 @@ b'68402703AB', b'68402704AB', b'68402708AB', + b'68402714AB', b'68402971AD', b'68454144AD', b'68454145AB', @@ -362,6 +363,7 @@ ], (Ecu.engine, 0x7e0, None): [ b'05035674AB ', + b'68412635AE ', b'68412635AG ', b'68412660AD ', b'68422860AB', @@ -561,6 +563,7 @@ b'68455145AE ', b'68455146AC ', b'68460927AA ', + b'68467909AB ', b'68467915AC ', b'68467916AC ', b'68467936AC ', @@ -611,6 +614,7 @@ b'68445536AB', b'68445537AB', b'68466081AB', + b'68466086AB', b'68466087AB', b'68484466AC', b'68484467AC', diff --git a/opendbc/car/ford/carcontroller.py b/opendbc/car/ford/carcontroller.py index 89728c09ba..85f42b1cfd 100644 --- a/opendbc/car/ford/carcontroller.py +++ b/opendbc/car/ford/carcontroller.py @@ -1,8 +1,9 @@ +import math from opendbc.can.packer import CANPacker -from opendbc.car import apply_std_steer_angle_limits, structs +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, apply_std_steer_angle_limits, structs from opendbc.car.ford import fordcan from opendbc.car.ford.values import CarControllerParams, FordFlags -from opendbc.car.common.numpy_fast import clip +from opendbc.car.common.numpy_fast import clip, interp from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX LongCtrlState = structs.CarControl.Actuators.LongControlState @@ -21,6 +22,13 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) +def apply_creep_compensation(accel: float, v_ego: float) -> float: + creep_accel = interp(v_ego, [1., 3.], [0.6, 0.]) + if accel < 0.: + accel -= creep_accel + return accel + + class CarController(CarControllerBase): def __init__(self, dbc_name, CP): super().__init__(dbc_name, CP) @@ -28,10 +36,14 @@ def __init__(self, dbc_name, CP): self.CAN = fordcan.CanBus(CP) self.apply_curvature_last = 0 + self.accel = 0.0 + self.gas = 0.0 + self.brake_request = False self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False self.lead_distance_bars_last = None + self.distance_bar_frame = 0 def update(self, CC, CS, now_nanos): can_sends = [] @@ -87,14 +99,34 @@ def update(self, CC, CS, now_nanos): ### longitudinal control ### # send acc msg at 50Hz if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: + # Compensate for engine creep at low speed. + # Either the ABS does not account for engine creep, or the correction is very slow + # TODO: verify this applies to EV/hybrid + self.accel = actuators.accel + if CC.longActive: + self.accel = apply_creep_compensation(self.accel, CS.out.vEgo) + self.accel = clip(self.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + # Both gas and accel are in m/s^2, accel is used solely for braking - accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) - gas = accel - if not CC.longActive or gas < CarControllerParams.MIN_GAS: - gas = CarControllerParams.INACTIVE_GAS + self.gas = self.accel + if not CC.longActive or self.gas < CarControllerParams.MIN_GAS: + self.gas = CarControllerParams.INACTIVE_GAS + + # PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits + if len(CC.orientationNED) == 3: + accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY + else: + accel_due_to_pitch = 0.0 + + accel_pitch_compensated = self.accel + accel_due_to_pitch + if accel_pitch_compensated > 0.3 or not CC.longActive: + self.brake_request = False + elif accel_pitch_compensated < 0.0: + self.brake_request = True + stopping = CC.actuators.longControlState == LongCtrlState.stopping # TODO: look into using the actuators packet to send the desired speed - can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=V_CRUISE_MAX)) + can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, self.gas, self.accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX)) ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) @@ -105,10 +137,13 @@ def update(self, CC, CS, now_nanos): # send acc ui msg at 5Hz or if ui state changes if hud_control.leadDistanceBars != self.lead_distance_bars_last: send_ui = True + self.distance_bar_frame = self.frame + if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: + show_distance_bars = self.frame - self.distance_bar_frame < 400 can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, - fcw_alert, CS.out.cruiseState.standstill, hud_control, - CS.acc_tja_status_stock_values)) + fcw_alert, CS.out.cruiseState.standstill, show_distance_bars, + hud_control, CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive @@ -117,6 +152,8 @@ def update(self, CC, CS, now_nanos): new_actuators = actuators.as_builder() new_actuators.curvature = self.apply_curvature_last + new_actuators.accel = self.accel + new_actuators.gas = self.gas self.frame += 1 return new_actuators, can_sends diff --git a/opendbc/car/ford/carstate.py b/opendbc/car/ford/carstate.py index 8b631e8dee..832d4b7321 100644 --- a/opendbc/car/ford/carstate.py +++ b/opendbc/car/ford/carstate.py @@ -76,6 +76,8 @@ def update(self, cp, cp_cam, *_) -> structs.CarState: else: ret.gearShifter = GearShifter.drive + ret.engineRpm = cp.vl["EngVehicleSpThrottle"]["EngAout_N_Actl"] + # safety ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"]) ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"]) diff --git a/opendbc/car/ford/fordcan.py b/opendbc/car/ford/fordcan.py index 61263099c7..2ac68c4427 100644 --- a/opendbc/car/ford/fordcan.py +++ b/opendbc/car/ford/fordcan.py @@ -117,7 +117,7 @@ def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path return packer.make_can_msg("LateralMotionControl2", CAN.main, values) -def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, v_ego_kph: float): +def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, brake_request, v_ego_kph: float): """ Creates a CAN message for the Ford ACC Command. @@ -126,24 +126,27 @@ def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: fl Frequency is 50Hz. """ - decel = accel < 0 and long_active values = { "AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2 "Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes "AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2 + # No observed acceleration seen from this signal alone. During stock system operation, it appears to + # be the raw acceleration request (AccPrpl_A_Rq when positive, AccBrkTot_A_Rq when negative) "AccPrpl_A_Pred": -5.0, # Acceleration request: [-5|5.23] m/s^2 "AccResumEnbl_B_Rq": 1 if long_active else 0, + # No observed acceleration seen from this signal alone "AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h # TODO: we may be able to improve braking response by utilizing pre-charging better - "AccBrkPrchg_B_Rq": 1 if decel else 0, # Pre-charge brake request: 0=No, 1=Yes - "AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active + # When setting these two bits without AccBrkTot_A_Rq, an initial jerk is observed and car may be able to brake temporarily with AccPrpl_A_Rq + "AccBrkPrchg_B_Rq": 1 if brake_request else 0, # Pre-charge brake request: 0=No, 1=Yes + "AccBrkDecel_B_Rq": 1 if brake_request else 0, # Deceleration request: 0=Inactive, 1=Active "AccStopStat_B_Rq": 1 if stopping else 0, } return packer.make_can_msg("ACCDATA", CAN.main, values) def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool, - hud_control, stock_values: dict): + show_distance_bars: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status. @@ -207,7 +210,7 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw values.update({ "AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text "AccMsgTxt_D2_Rq": 0, # ACC text - "AccTGap_B_Dsply": 0, # Show time gap control UI + "AccTGap_B_Dsply": 1 if show_distance_bars else 0, # Show time gap control UI "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator "AccStopMde_B_Dsply": 1 if standstill else 0, "AccWarn_D_Dsply": 0, # ACC warning diff --git a/opendbc/car/ford/interface.py b/opendbc/car/ford/interface.py index fbd027a2dc..dfc64747cb 100644 --- a/opendbc/car/ford/interface.py +++ b/opendbc/car/ford/interface.py @@ -2,7 +2,7 @@ from opendbc.car import get_safety_config, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.ford.fordcan import CanBus -from opendbc.car.ford.values import Ecu, FordFlags +from opendbc.car.ford.values import DBC, Ecu, FordFlags, RADAR from opendbc.car.interfaces import CarInterfaceBase TransmissionType = structs.CarParams.TransmissionType @@ -19,6 +19,11 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime ret.steerActuatorDelay = 0.2 ret.steerLimitTimer = 1.0 + if DBC[candidate]['radar'] == RADAR.DELPHI_MRR: + # average of 33.3 Hz radar timestep / 4 scan modes = 60 ms + # MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms + ret.radarDelay = 0.06 + CAN = CanBus(fingerprint=fingerprint) cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)] if CAN.main >= 4: diff --git a/opendbc/car/ford/radar_interface.py b/opendbc/car/ford/radar_interface.py index efe3bef7d5..b145207e94 100644 --- a/opendbc/car/ford/radar_interface.py +++ b/opendbc/car/ford/radar_interface.py @@ -9,8 +9,12 @@ DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540)) DELPHI_MRR_RADAR_START_ADDR = 0x120 +DELPHI_MRR_RADAR_HEADER_ADDR = 0x174 # MRR_Header_SensorCoverage DELPHI_MRR_RADAR_MSG_COUNT = 64 +DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175} # scan index to detection range (m) +MIN_LONG_RANGE_DIST = 30 # meters + def _create_delphi_esr_radar_can_parser(CP) -> CANParser: msg_n = len(DELPHI_ESR_RADAR_MSGS) @@ -20,11 +24,14 @@ def _create_delphi_esr_radar_can_parser(CP) -> CANParser: def _create_delphi_mrr_radar_can_parser(CP) -> CANParser: - messages = [] + messages = [ + ("MRR_Header_InformationDetections", 33), + ("MRR_Header_SensorCoverage", 33), + ] for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1): msg = f"MRR_Detection_{i:03d}" - messages += [(msg, 20)] + messages += [(msg, 33)] return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar) @@ -36,7 +43,7 @@ def __init__(self, CP): self.updated_messages = set() self.track_id = 0 self.radar = DBC[CP.carFingerprint]['radar'] - if self.radar is None or CP.radarUnavailable: + if CP.radarUnavailable: self.rcp = None elif self.radar == RADAR.DELPHI_ESR: self.rcp = _create_delphi_esr_radar_can_parser(CP) @@ -44,7 +51,7 @@ def __init__(self, CP): self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS} elif self.radar == RADAR.DELPHI_MRR: self.rcp = _create_delphi_mrr_radar_can_parser(CP) - self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT - 1 + self.trigger_msg = DELPHI_MRR_RADAR_HEADER_ADDR else: raise ValueError(f"Unsupported radar: {self.radar}") @@ -62,14 +69,14 @@ def update(self, can_strings): errors = [] if not self.rcp.can_valid: errors.append("canError") - ret.errors = errors if self.radar == RADAR.DELPHI_ESR: self._update_delphi_esr() elif self.radar == RADAR.DELPHI_MRR: - self._update_delphi_mrr() + errors.extend(self._update_delphi_mrr()) ret.points = list(self.pts.values()) + ret.errors = errors self.updated_messages.clear() return ret @@ -103,14 +110,26 @@ def _update_delphi_esr(self): del self.pts[ii] def _update_delphi_mrr(self): + headerScanIndex = int(self.rcp.vl["MRR_Header_InformationDetections"]['CAN_SCAN_INDEX']) & 0b11 + + errors = [] + if DELPHI_MRR_RADAR_RANGE_COVERAGE[headerScanIndex] != int(self.rcp.vl["MRR_Header_SensorCoverage"]["CAN_RANGE_COVERAGE"]): + errors.append("wrongConfig") + for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1): msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"] - # SCAN_INDEX rotates through 0..3 on each message - # treat these as separate points + # SCAN_INDEX rotates through 0..3 on each message for different measurement modes + # Indexes 0 and 2 have a max range of ~40m, 1 and 3 are ~170m (MRR_Header_SensorCoverage->CAN_RANGE_COVERAGE) + # Indexes 0 and 1 have a Doppler coverage of +-71 m/s, 2 and 3 have +-60 m/s + # TODO: can we group into 2 groups? scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"] i = (ii - 1) * 4 + scanIndex + # Throw out old measurements. Very unlikely to happen, but is proper behavior + if scanIndex != headerScanIndex: + continue + if i not in self.pts: self.pts[i] = structs.RadarData.RadarPoint() self.pts[i].trackId = self.track_id @@ -120,9 +139,13 @@ def _update_delphi_mrr(self): valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"]) + # Long range measurement mode is more sensitive and can detect the road surface + dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984] + if scanIndex in (1, 3) and dist < MIN_LONG_RANGE_DIST: + valid = False + if valid: azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964] - dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984] distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984] dRel = cos(azimuth) * dist # m from front of car yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive @@ -141,3 +164,5 @@ def _update_delphi_mrr(self): else: del self.pts[i] + + return errors diff --git a/opendbc/car/gm/interface.py b/opendbc/car/gm/interface.py index 767909bd95..f3953c0eb9 100755 --- a/opendbc/car/gm/interface.py +++ b/opendbc/car/gm/interface.py @@ -138,7 +138,6 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerLimitTimer = 0.4 - ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking if candidate == CAR.CHEVROLET_VOLT: diff --git a/opendbc/car/gm/radar_interface.py b/opendbc/car/gm/radar_interface.py index 78f6a89a94..e79d26533b 100755 --- a/opendbc/car/gm/radar_interface.py +++ b/opendbc/car/gm/radar_interface.py @@ -40,7 +40,6 @@ def __init__(self, CP): self.trigger_msg = LAST_RADAR_MSG self.updated_messages = set() - self.radar_ts = CP.radarTimeStep def update(self, can_strings): if self.rcp is None: diff --git a/opendbc/car/honda/fingerprints.py b/opendbc/car/honda/fingerprints.py index edfd7d00c4..663bf65107 100644 --- a/opendbc/car/honda/fingerprints.py +++ b/opendbc/car/honda/fingerprints.py @@ -439,6 +439,7 @@ b'36161-TPA-E050\x00\x00', b'36161-TPG-A030\x00\x00', b'36161-TPG-A040\x00\x00', + b'36161-TPG-A050\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TMB-H040\x00\x00', diff --git a/opendbc/car/honda/radar_interface.py b/opendbc/car/honda/radar_interface.py index 316838aa5d..97c6664d0e 100755 --- a/opendbc/car/honda/radar_interface.py +++ b/opendbc/car/honda/radar_interface.py @@ -18,7 +18,6 @@ def __init__(self, CP): self.radar_fault = False self.radar_wrong_config = False self.radar_off_can = CP.radarUnavailable - self.radar_ts = CP.radarTimeStep # Nidec if self.radar_off_can: diff --git a/opendbc/car/honda/values.py b/opendbc/car/honda/values.py index 1200f69cca..30f84361c2 100644 --- a/opendbc/car/honda/values.py +++ b/opendbc/car/honda/values.py @@ -159,7 +159,7 @@ class CAR(Platforms): flags=HondaFlags.BOSCH_ALT_BRAKE, ) HONDA_CRV_HYBRID = HondaBoschPlatformConfig( - [HondaCarDocs("Honda CR-V Hybrid 2017-21", min_steer_speed=12. * CV.MPH_TO_MS)], + [HondaCarDocs("Honda CR-V Hybrid 2017-22", min_steer_speed=12. * CV.MPH_TO_MS)], # mass: mean of 4 models in kg, steerRatio: 12.3 is spec end-to-end CarSpecs(mass=1667, wheelbase=2.66, steerRatio=16, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), dbc_dict('honda_accord_2018_can_generated', None), diff --git a/opendbc/car/hyundai/fingerprints.py b/opendbc/car/hyundai/fingerprints.py index 7496e2bff0..68b245300b 100644 --- a/opendbc/car/hyundai/fingerprints.py +++ b/opendbc/car/hyundai/fingerprints.py @@ -231,6 +231,7 @@ (Ecu.abs, 0x7d1, None): [ b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600', b'\xf1\x00TM ESC \x02 102\x18\x07\x01 58910-S2600', + b'\xf1\x00TM ESC \x02 103\x18\x11\x05 58910-S2500', b'\xf1\x00TM ESC \x02 103\x18\x11\x07 58910-S2600', b'\xf1\x00TM ESC \x02 104\x19\x07\x07 58910-S2600', b'\xf1\x00TM ESC \x03 103\x18\x11\x07 58910-S2600', @@ -616,24 +617,24 @@ b'\xf1\x00OS IEB \x03 210 \x02\x14 58520-K4000', b'\xf1\x00OS IEB \x03 211 \x04\x02 58520-K4000', b'\xf1\x00OS IEB \x03 212 \x11\x13 58520-K4000', - b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000', b'\xf1\x00OS IEB \x04 212 \x11\x13 58520-K4000', + b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00OE2 LKAS AT EUR LHD 1.00 1.00 95740-K4200 200', + b'\xf1\x00OSE LKAS AT AUS RHD 1.00 1.00 95740-K4300 W50', b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', b'\xf1\x00OSE LKAS AT EUR RHD 1.00 1.00 95740-K4100 W40', b'\xf1\x00OSE LKAS AT KOR LHD 1.00 1.00 95740-K4100 W40', b'\xf1\x00OSE LKAS AT USA LHD 1.00 1.00 95740-K4100 W40', b'\xf1\x00OSE LKAS AT USA LHD 1.00 1.00 95740-K4300 W50', - b'\xf1\x00OSE LKAS AT AUS RHD 1.00 1.00 95740-K4300 W50', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00OS MDPS C 1.00 1.03 56310/K4550 4OEDC103', b'\xf1\x00OS MDPS C 1.00 1.04 56310-XX000 4OEDC104', + b'\xf1\x00OS MDPS C 1.00 1.04 56310/K4550 4OEDC104', b'\xf1\x00OS MDPS C 1.00 1.04 56310K4000\x00 4OEDC104', b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', - b'\xf1\x00OS MDPS C 1.00 1.04 56310/K4550 4OEDC104', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00OSev SCC F-CUP 1.00 1.00 99110-K4000 ', diff --git a/opendbc/car/interfaces.py b/opendbc/car/interfaces.py index c704f12f4a..f5644a5cff 100644 --- a/opendbc/car/interfaces.py +++ b/opendbc/car/interfaces.py @@ -259,12 +259,11 @@ def __init__(self, CP: structs.CarParams): self.CP = CP self.rcp = None self.pts: dict[int, structs.RadarData.RadarPoint] = {} - self.radar_ts = CP.radarTimeStep self.frame = 0 def update(self, can_packets: list[tuple[int, list[CanData]]]) -> structs.RadarDataT | None: self.frame += 1 - if (self.frame % int(100 * self.radar_ts)) == 0: + if (self.frame % 5) == 0: # 20 Hz is very standard return structs.RadarData() return None diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index e8a988ac29..e1b4003b84 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -1,6 +1,6 @@ import math from opendbc.car import carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ - make_tester_present_msg, rate_limit, structs + make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY from opendbc.car.can_definitions import CanData from opendbc.car.common.numpy_fast import clip from opendbc.car.secoc import add_mac, build_sync_mac @@ -15,8 +15,6 @@ SteerControlType = structs.CarParams.SteerControlType VisualAlert = structs.CarControl.HUDControl.VisualAlert -ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2 - ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame # LKA limits diff --git a/opendbc/car/toyota/fingerprints.py b/opendbc/car/toyota/fingerprints.py index 27a6f11210..24d7ae3655 100644 --- a/opendbc/car/toyota/fingerprints.py +++ b/opendbc/car/toyota/fingerprints.py @@ -394,6 +394,7 @@ (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646FF410200\x00\x00\x00\x008646GF408200\x00\x00\x00\x00', b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00', + b'\x028646FF413000\x00\x00\x00\x008646GF411000\x00\x00\x00\x00', b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00', ], }, @@ -723,11 +724,13 @@ b'\x01896630EE7000\x00\x00\x00\x00', b'\x01896630EF8000\x00\x00\x00\x00', b'\x01896630EG3000\x00\x00\x00\x00', + b'\x01896630EG3100\x00\x00\x00\x00', b'\x01896630EG5000\x00\x00\x00\x00', b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896630EB3200\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', diff --git a/opendbc/car/toyota/radar_interface.py b/opendbc/car/toyota/radar_interface.py index 922dbac278..bfb2df0494 100755 --- a/opendbc/car/toyota/radar_interface.py +++ b/opendbc/car/toyota/radar_interface.py @@ -23,7 +23,6 @@ class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) self.track_id = 0 - self.radar_ts = CP.radarTimeStep if CP.carFingerprint in TSS2_CAR: self.RADAR_A_MSGS = list(range(0x180, 0x190)) diff --git a/opendbc/car/volkswagen/fingerprints.py b/opendbc/car/volkswagen/fingerprints.py index 4711beaf3c..a3bae382ac 100644 --- a/opendbc/car/volkswagen/fingerprints.py +++ b/opendbc/car/volkswagen/fingerprints.py @@ -52,6 +52,7 @@ CAR.VOLKSWAGEN_ATLAS_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8703H906026AA\xf1\x899970', + b'\xf1\x8703H906026AG\xf1\x899971', b'\xf1\x8703H906026AG\xf1\x899973', b'\xf1\x8703H906026AJ\xf1\x890638', b'\xf1\x8703H906026AJ\xf1\x891017', @@ -355,11 +356,13 @@ b'\xf1\x8704E906024L \xf1\x895595', b'\xf1\x8704E906024L \xf1\x899970', b'\xf1\x8704E906027MS\xf1\x896223', + b'\xf1\x8705E906013BN\xf1\x893711', b'\xf1\x8705E906013DB\xf1\x893361', b'\xf1\x875G0906259T \xf1\x890003', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158BQ\xf1\x893545', + b'\xf1\x8709H927158 \xf1\x890515', b'\xf1\x8709S927158BS\xf1\x893642', b'\xf1\x8709S927158BS\xf1\x893694', b'\xf1\x8709S927158CK\xf1\x893770', @@ -387,8 +390,10 @@ b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A00642A1', b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A10A01A1', b'\xf1\x875QN909144B \xf1\x895082\xf1\x82\x0571A10A11A1', + b'\xf1\x875QV907144F \xf1\x891122\xf1\x82\x0001A00701]V', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907567B \xf1\x890534', b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x875Q0907572N \xf1\x890681', b'\xf1\x875Q0907572P \xf1\x890682',