diff --git a/panda b/panda index ff1ef85735..045ff88933 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ff1ef85735c053529a840436d33ab5f16840724e +Subproject commit 045ff889333ed92c277c2945c6e12904e96cf80c diff --git a/selfdrive/car/fingerprints.py b/selfdrive/car/fingerprints.py index 1128a31c29..db5784eeb5 100644 --- a/selfdrive/car/fingerprints.py +++ b/selfdrive/car/fingerprints.py @@ -275,9 +275,8 @@ MIGRATION = { "SUBARU FORESTER 2022": SUBARU.SUBARU_FORESTER_2022, "SUBARU OUTBACK 7TH GEN": SUBARU.SUBARU_OUTBACK_2023, "SUBARU ASCENT 2023": SUBARU.SUBARU_ASCENT_2023, - 'TESLA AP1 MODEL S': TESLA.TESLA_AP1_MODELS, - 'TESLA AP2 MODEL S': TESLA.TESLA_AP2_MODELS, - 'TESLA MODEL S RAVEN': TESLA.TESLA_MODELS_RAVEN, + 'TESLA AP3 MODEL 3': TESLA.TESLA_AP3_MODEL3, + 'TESLA AP3 MODEL Y': TESLA.TESLA_AP3_MODELY, "TOYOTA ALPHARD 2020": TOYOTA.TOYOTA_ALPHARD_TSS2, "TOYOTA AVALON 2016": TOYOTA.TOYOTA_AVALON, "TOYOTA AVALON 2019": TOYOTA.TOYOTA_AVALON_2019, diff --git a/selfdrive/car/sunnypilot_carname.json b/selfdrive/car/sunnypilot_carname.json index 9a04ea9f1e..871b609b5e 100644 --- a/selfdrive/car/sunnypilot_carname.json +++ b/selfdrive/car/sunnypilot_carname.json @@ -224,6 +224,8 @@ "Škoda Octavia Scout 2017-19": "SKODA_OCTAVIA_MK3", "Škoda Scala 2020-23": "SKODA_KAMIQ_MK1", "Škoda Superb 2015-22": "SKODA_SUPERB_MK3", + "Tesla AP3 Model 3": "TESLA_AP3_MODEL3", + "Tesla AP3 Model Y": "TESLA_AP3_MODELY", "Toyota Alphard 2019-20": "TOYOTA_ALPHARD_TSS2", "Toyota Alphard Hybrid 2021": "TOYOTA_ALPHARD_TSS2", "Toyota Avalon 2016": "TOYOTA_AVALON", diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index e460111e32..8478f21e25 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.tesla.teslacan import TeslaCAN -from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams +from openpilot.selfdrive.car.tesla.values import DBC, CarControllerParams class CarController(CarControllerBase): @@ -38,25 +38,26 @@ class CarController(CarControllerBase): self.apply_angle_last = apply_angle can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, (self.frame // 2) % 16)) - # Longitudinal control (in sync with stock message, about 40Hz) + # Longitudinal control if self.CP.openpilotLongitudinalControl: + acc_state = CS.das_control["DAS_accState"] target_accel = actuators.accel target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0) max_accel = 0 if target_accel < 0 else target_accel min_accel = 0 if target_accel > 0 else target_accel - while len(CS.das_control_counters) > 0: - can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft())) + counter = CS.das_control["DAS_controlCounter"] + can_sends.append(self.tesla_can.create_longitudinal_commands(acc_state, target_speed, min_accel, max_accel, counter)) # Cancel on user steering override, since there is no steering torque blending if hands_on_fault: pcm_cancel_cmd = True - if self.frame % 10 == 0 and pcm_cancel_cmd: - # Spam every possible counter value, otherwise it might not be accepted - for counter in range(16): - can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter)) - can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter)) + # Sent cancel request only if ACC is enabled + if self.frame % 10 == 0 and pcm_cancel_cmd and CS.acc_enabled: + counter = int(CS.sccm_right_stalk_counter) + can_sends.append(self.tesla_can.right_stalk_press((counter + 1) % 16 , 1)) # half up (cancel acc) + can_sends.append(self.tesla_can.right_stalk_press((counter + 2) % 16, 0)) # to prevent neutral gear warning # TODO: HUD control diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 645ea46014..82a7b5fa77 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -1,8 +1,7 @@ import copy -from collections import deque from cereal import car from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS +from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, BUTTONS, BUTTON_STATES from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine @@ -14,93 +13,100 @@ class CarState(CarStateBase): self.can_define = CANDefine(DBC[CP.carFingerprint]['chassis']) # Needed by carcontroller - self.msg_stw_actn_req = None self.hands_on_level = 0 self.steer_warning = None - self.acc_state = 0 - self.das_control_counters = deque(maxlen=32) + self.acc_enabled = None + self.sccm_right_stalk_counter = None + self.das_control = None - def update(self, cp, cp_cam): + self.buttonStates = BUTTON_STATES.copy() + self.buttonStatesPrev = BUTTON_STATES.copy() + + def update(self, cp, cp_cam, cp_adas): ret = car.CarState.new_message() + self.prev_mads_enabled = self.mads_enabled + self.buttonStatesPrev = self.buttonStates.copy() + # Vehicle speed ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = (ret.vEgo < 0.1) # Gas pedal - ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0 - ret.gasPressed = (ret.gas > 0) + pedal_status = cp.vl["DI_systemStatus"]["DI_accelPedalPos"] + ret.gas = pedal_status / 100.0 + ret.gasPressed = (pedal_status > 0) # Brake pedal ret.brake = 0 - ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1) + ret.brakePressed = cp.vl["IBST_status"]["IBST_driverBrakeApply"] == 2 # Steering wheel - epas_status = cp_cam.vl["EPAS3P_sysStatus"] if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN else cp.vl["EPAS_sysStatus"] + epas_status = cp.vl["EPAS3S_sysStatus"] + self.hands_on_level = epas_status["EPAS3S_handsOnLevel"] + self.steer_warning = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacErrorCode"].get(int(epas_status["EPAS3S_eacErrorCode"]), None) + ret.steeringAngleDeg = -epas_status["EPAS3S_internalSAS"] + ret.steeringRateDeg = -cp_adas.vl["SCCM_steeringAngleSensor"]["SCCM_steeringAngleSpeed"] + ret.steeringTorque = -epas_status["EPAS3S_torsionBarTorque"] - self.hands_on_level = epas_status["EPAS_handsOnLevel"] - self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(epas_status["EPAS_eacErrorCode"]), None) - steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(epas_status["EPAS_eacStatus"]), None) - - ret.steeringAngleDeg = -epas_status["EPAS_internalSAS"] - ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate - ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"] ret.steeringPressed = (self.hands_on_level > 0) - ret.steerFaultPermanent = steer_status == "EAC_FAULT" + ret.steerFaultPermanent = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacStatus"].get(int(epas_status["EPAS3S_eacStatus"]), None) in ["EAC_FAULT"] ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON")) # Cruise state cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None) speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None) - acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")) - - ret.cruiseState.enabled = acc_enabled + self.acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")) + ret.cruiseState.enabled = self.acc_enabled if speed_units == "KPH": ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS elif speed_units == "MPH": ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled) - ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special + ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special # Gear - ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")] + ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_systemStatus"]["DI_gear"].get(int(cp.vl["DI_systemStatus"]["DI_gear"]), "DI_GEAR_INVALID")] # Buttons - buttonEvents = [] + self.buttonStates["accelCruise"] = cp_adas.vl["VCLEFT_switchStatus"]["VCLEFT_swcRightScrollTicks"] in list(range(1, 10)) + self.buttonStates["decelCruise"] = cp_adas.vl["VCLEFT_switchStatus"]["VCLEFT_swcRightScrollTicks"] in list(range(-9, 0)) + self.buttonStates["cancel"] = cp_adas.vl["SCCM_rightStalk"]["SCCM_rightStalkStatus"] in [1, 2] + self.buttonStates["resumeCruise"] = cp_adas.vl["SCCM_rightStalk"]["SCCM_rightStalkStatus"] in [3, 4] + + button_events = [] for button in BUTTONS: - state = (cp.vl[button.can_addr][button.can_msg] in button.values) + state = (cp_adas.vl[button.can_addr][button.can_msg] in button.values) if self.button_states[button.event_type] != state: event = car.CarState.ButtonEvent.new_message() event.type = button.event_type event.pressed = state - buttonEvents.append(event) + button_events.append(event) self.button_states[button.event_type] = state - ret.buttonEvents = buttonEvents + ret.buttonEvents = button_events # Doors - ret.doorOpen = any((self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS) + ret.doorOpen = (cp.vl["UI_warning"]["anyDoorOpen"] == 1) # Blinkers - ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1) - ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1) + ret.leftBlinker = (cp_adas.vl["ID3F5VCFRONT_lighting"]["VCFRONT_indicatorLeftRequest"] != 0) + ret.rightBlinker = (cp_adas.vl["ID3F5VCFRONT_lighting"]["VCFRONT_indicatorRightRequest"] != 0) # Seatbelt - if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: - ret.seatbeltUnlatched = (cp.vl["DriverSeat"]["buckleStatus"] != 1) - else: - ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1) + ret.seatbeltUnlatched = cp.vl["UI_warning"]["buckleStatus"] != 1 - # TODO: blindspot + # Blindspot + ret.leftBlindspot = cp_cam.vl["DAS_status"]["DAS_blindSpotRearLeft"] != 0 + ret.rightBlindspot = cp_cam.vl["DAS_status"]["DAS_blindSpotRearRight"] != 0 # AEB ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1) # Messages needed by carcontroller - self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"]) - self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"] - self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"]) + self.sccm_right_stalk_counter = copy.copy(cp_adas.vl["SCCM_rightStalk"]["SCCM_rightStalkCounter"]) + self.das_control = copy.copy(cp_cam.vl["DAS_control"]) return ret @@ -109,31 +115,33 @@ class CarState(CarStateBase): messages = [ # sig_address, frequency ("ESP_B", 50), - ("DI_torque1", 100), - ("DI_torque2", 100), - ("STW_ANGLHP_STAT", 100), - ("EPAS_sysStatus", 25), + ("DI_systemStatus", 100), + ("IBST_status", 25), ("DI_state", 10), - ("STW_ACTN_RQ", 10), - ("GTW_carState", 10), - ("BrakeMessage", 50), + ("EPAS3S_sysStatus", 100), + ("UI_warning", 10) ] - if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: - messages.append(("DriverSeat", 20)) - else: - messages.append(("SDM1", 10)) - - return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.chassis) + return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.party) @staticmethod def get_cam_can_parser(CP): messages = [ - # sig_address, frequency - ("DAS_control", 40), + ("DAS_control", 25), + ("DAS_status", 2) ] - if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: - messages.append(("EPAS3P_sysStatus", 100)) + return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_party) - return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis) + @staticmethod + def get_adas_can_parser(CP): # Vehicle Can on Model 3 + messages = [ + ("VCLEFT_switchStatus", 20), + ("SCCM_leftStalk", 10), + ("SCCM_rightStalk", 10), + ("SCCM_steeringAngleSensor", 100), + ("DAS_bodyControls", 2), + ("ID3F5VCFRONT_lighting", 10), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CANBUS.vehicle) diff --git a/selfdrive/car/tesla/fingerprints.py b/selfdrive/car/tesla/fingerprints.py index 68c50a62ed..71c8bf710d 100644 --- a/selfdrive/car/tesla/fingerprints.py +++ b/selfdrive/car/tesla/fingerprints.py @@ -4,29 +4,39 @@ from openpilot.selfdrive.car.tesla.values import CAR Ecu = car.CarParams.Ecu FW_VERSIONS = { - CAR.TESLA_AP2_MODELS: { - (Ecu.adas, 0x649, None): [ - b'\x01\x00\x8b\x07\x01\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x11', - ], - (Ecu.electricBrakeBooster, 0x64d, None): [ - b'1037123-00-A', - ], - (Ecu.fwdRadar, 0x671, None): [ - b'\x01\x00W\x00\x00\x00\x07\x00\x00\x00\x00\x08\x01\x00\x00\x00\x07\xff\xfe', - ], + CAR.TESLA_AP3_MODEL3: { (Ecu.eps, 0x730, None): [ - b'\x10#\x01', + b'TeM3_E014p10_0.0.0 (16),E014.17.00', + b'TeMYG4_DCS_Update_0.0.0 (13),E4014.28.1', + b'TeMYG4_DCS_Update_0.0.0 (9),E4014.26.0', + b'TeMYG4_SingleECU_0.0.0 (33),E4S014.27', + ], + (Ecu.engine, 0x606, None): [ + b'\x01\x00\x05\x18A\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x000\x91', + b'\x01\x00\x05\x1cO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\x11', + b'\x01\x00\x05\x1cF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\r', + b'\x01\x00\x05\x1e[\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x05', + b'\x01\x00\x05 D\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xe1', + b'\x01\x00\x05 K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xa4', + b'\x01\x00\x05 N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xa7', + b'\x01\x00\x05 N\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xf2', + b'\x01\x00\x05 [\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xd8', ], }, - CAR.TESLA_MODELS_RAVEN: { - (Ecu.electricBrakeBooster, 0x64d, None): [ - b'1037123-00-A', - ], - (Ecu.fwdRadar, 0x671, None): [ - b'\x01\x00\x99\x02\x01\x00\x10\x00\x00AP8.3.03\x00\x10', - ], + CAR.TESLA_AP3_MODELY: { (Ecu.eps, 0x730, None): [ - b'SX_0.0.0 (99),SR013.7', + b'TeM3_E014p10_0.0.0 (16),Y002.18.00', + b'TeM3_ES014p11_0.0.0 (25),YS002.19.0', + b'TeMYG4_DCS_Update_0.0.0 (13),Y4002.27.1', + b'TeMYG4_DCS_Update_0.0.0 (9),Y4P002.25.0' + ], + (Ecu.engine, 0x606, None): [ + b'\x01\x00\x05\x1cO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\x12', + b'\x01\x00\x05\x1eO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\xe2', + b'\x01\x00\x05\x1e[\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x06', + b'\x01\x00\x05 O\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x9a', + b'\x01\x00\x05 m\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xd5', + b'\x01\x00\x055p\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00;\xc5', ], }, } diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index deb0e00230..07eadbe923 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -1,12 +1,19 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from openpilot.selfdrive.car.tesla.values import CANBUS, CAR -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car.tesla.values import CAR, BUTTON_STATES +from openpilot.selfdrive.car import get_safety_config, create_mads_event from openpilot.selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type + class CarInterface(CarInterfaceBase): + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) + + self.buttonStatesPrev = BUTTON_STATES.copy() + @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "tesla" @@ -14,25 +21,17 @@ class CarInterface(CarInterfaceBase): # There is no safe way to do steer blending with user torque, # so the steering behaves like autopilot. This is not # how openpilot should be, hence dashcamOnly - ret.dashcamOnly = True + # ret.dashcamOnly = True ret.steerControlType = car.CarParams.SteerControlType.angle ret.longitudinalActuatorDelay = 0.5 # s - ret.radarTimeStep = (1.0 / 8) # 8Hz + ret.radarUnavailable = True - # Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus - # If so, we assume that it is connected to the longitudinal harness. - flags = (Panda.FLAG_TESLA_RAVEN if candidate == CAR.TESLA_MODELS_RAVEN else 0) - if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()): - ret.openpilotLongitudinalControl = True + if candidate in [CAR.TESLA_AP3_MODEL3, CAR.TESLA_AP3_MODELY]: + flags = Panda.FLAG_TESLA_MODEL3_Y flags |= Panda.FLAG_TESLA_LONG_CONTROL - ret.safetyConfigs = [ - get_safety_config(car.CarParams.SafetyModel.tesla, flags), - get_safety_config(car.CarParams.SafetyModel.tesla, flags | Panda.FLAG_TESLA_POWERTRAIN), - ] - else: - ret.openpilotLongitudinalControl = False + ret.openpilotLongitudinalControl = True ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, flags)] ret.steerLimitTimer = 1.0 @@ -40,8 +39,62 @@ class CarInterface(CarInterfaceBase): return ret def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + ret = self.CS.update(self.cp, self.cp_cam, self.cp_adas) + self.sp_update_params() - ret.events = self.create_common_events(ret).to_msg() + buttonEvents = [] + + for button in self.CS.buttonStates: + if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: + be = car.CarState.ButtonEvent.new_message() + be.type = button + be.pressed = self.CS.buttonStates[button] + buttonEvents.append(be) + + self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS) + + if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed: + if any(b.type == ButtonType.cancel for b in buttonEvents): + self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) + if self.get_sp_pedal_disengage(ret): + self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) + ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled + + if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed: + if ret.gasPressed and not ret.cruiseState.enabled: + self.CS.accEnabled = False + self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + if self.mads_event_lock: + buttonEvents.append(create_mads_event(self.mads_event_lock)) + self.mads_event_lock = False + else: + if not self.mads_event_lock: + buttonEvents.append(create_mads_event(self.mads_event_lock)) + self.mads_event_lock = True + + ret.buttonEvents = buttonEvents + + events = self.create_common_events(ret, c, pcm_enable=False) + + events, ret = self.create_sp_events(self.CS, ret, events) + + ret.events = events.to_msg() + + # update previous car states + self.buttonStatesPrev = self.CS.buttonStates.copy() return ret diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index ae5077824b..e654bd61fd 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -1,91 +1,4 @@ -#!/usr/bin/env python3 -from cereal import car -from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS from openpilot.selfdrive.car.interfaces import RadarInterfaceBase - class RadarInterface(RadarInterfaceBase): - def __init__(self, CP): - super().__init__(CP) - self.CP = CP - - if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: - messages = [('RadarStatus', 16)] - self.num_points = 40 - self.trigger_msg = 1119 - else: - messages = [('TeslaRadarSguInfo', 10)] - self.num_points = 32 - self.trigger_msg = 878 - - for i in range(self.num_points): - messages.extend([ - (f'RadarPoint{i}_A', 16), - (f'RadarPoint{i}_B', 16), - ]) - - self.rcp = CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar) - self.updated_messages = set() - self.track_id = 0 - - def update(self, can_strings): - if self.rcp is None: - return super().update(None) - - values = self.rcp.update_strings(can_strings) - self.updated_messages.update(values) - - if self.trigger_msg not in self.updated_messages: - return None - - ret = car.RadarData.new_message() - - # Errors - errors = [] - if not self.rcp.can_valid: - errors.append('canError') - - if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: - radar_status = self.rcp.vl['RadarStatus'] - if radar_status['sensorBlocked'] or radar_status['shortTermUnavailable'] or radar_status['vehDynamicsError']: - errors.append('fault') - else: - radar_status = self.rcp.vl['TeslaRadarSguInfo'] - if radar_status['RADC_HWFail'] or radar_status['RADC_SGUFail'] or radar_status['RADC_SensorDirty']: - errors.append('fault') - - ret.errors = errors - - # Radar tracks - for i in range(self.num_points): - msg_a = self.rcp.vl[f'RadarPoint{i}_A'] - msg_b = self.rcp.vl[f'RadarPoint{i}_B'] - - # Make sure msg A and B are together - if msg_a['Index'] != msg_b['Index2']: - continue - - # Check if it's a valid track - if not msg_a['Tracked']: - if i in self.pts: - del self.pts[i] - continue - - # New track! - if i not in self.pts: - self.pts[i] = car.RadarData.RadarPoint.new_message() - self.pts[i].trackId = self.track_id - self.track_id += 1 - - # Parse track data - self.pts[i].dRel = msg_a['LongDist'] - self.pts[i].yRel = msg_a['LatDist'] - self.pts[i].vRel = msg_a['LongSpeed'] - self.pts[i].aRel = msg_a['LongAccel'] - self.pts[i].yvRel = msg_b['LatSpeed'] - self.pts[i].measured = bool(msg_a['Meas']) - - ret.points = list(self.pts.values()) - self.updated_messages.clear() - return ret + pass diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 6bb27b995f..7f68578d21 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -17,6 +17,15 @@ class TeslaCAN: ret += sum(dat) return ret & 0xFF + @staticmethod + def right_stalk_crc(dat): + right_stalk_val = [0x7C, 0xB6, 0xF0, 0x2F, 0x69, 0xA3, 0xDD, 0x1C, 0x56, 0x90, 0xCA, 0x09, 0x43, 0x7D, 0xB7, 0xF1] + cntr = dat[0] & 0xF + crc1_func = crcmod.mkCrcFun(0x12F, initCrc=0x00, xorOut=0xFF, rev=False) + crc1 = crc1_func(dat) & 0xFF + crc2_func = crcmod.mkCrcFun(0x12F, initCrc=crc1, xorOut=0xFF, rev=False) + return crc2_func(bytes([right_stalk_val[cntr]])) & 0xFF + def create_steering_control(self, angle, enabled, counter): values = { "DAS_steeringAngleRequest": -angle, @@ -25,56 +34,11 @@ class TeslaCAN: "DAS_steeringControlCounter": counter, } - data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[2] + data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)[2] values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3]) - return self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values) - - def create_action_request(self, msg_stw_actn_req, cancel, bus, counter): - # We copy this whole message when spamming cancel - values = {s: msg_stw_actn_req[s] for s in [ - "SpdCtrlLvr_Stat", - "VSL_Enbl_Rq", - "SpdCtrlLvrStat_Inv", - "DTR_Dist_Rq", - "TurnIndLvr_Stat", - "HiBmLvr_Stat", - "WprWashSw_Psd", - "WprWash_R_Sw_Posn_V2", - "StW_Lvr_Stat", - "StW_Cond_Flt", - "StW_Cond_Psd", - "HrnSw_Psd", - "StW_Sw00_Psd", - "StW_Sw01_Psd", - "StW_Sw02_Psd", - "StW_Sw03_Psd", - "StW_Sw04_Psd", - "StW_Sw05_Psd", - "StW_Sw06_Psd", - "StW_Sw07_Psd", - "StW_Sw08_Psd", - "StW_Sw09_Psd", - "StW_Sw10_Psd", - "StW_Sw11_Psd", - "StW_Sw12_Psd", - "StW_Sw13_Psd", - "StW_Sw14_Psd", - "StW_Sw15_Psd", - "WprSw6Posn", - "MC_STW_ACTN_RQ", - "CRC_STW_ACTN_RQ", - ]} - - if cancel: - values["SpdCtrlLvr_Stat"] = 1 - values["MC_STW_ACTN_RQ"] = counter - - data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[2] - values["CRC_STW_ACTN_RQ"] = self.crc(data[:7]) - return self.packer.make_can_msg("STW_ACTN_RQ", bus, values) + return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values) def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, cnt): - messages = [] values = { "DAS_setSpeed": speed * CV.MS_TO_KPH, "DAS_accState": acc_state, @@ -86,9 +50,20 @@ class TeslaCAN: "DAS_controlCounter": cnt, "DAS_controlChecksum": 0, } + data = self.packer.make_can_msg("DAS_control", CANBUS.party, values)[2] + values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7]) + return self.packer.make_can_msg("DAS_control", CANBUS.party, values) - for packer, bus in [(self.packer, CANBUS.chassis), (self.pt_packer, CANBUS.powertrain)]: - data = packer.make_can_msg("DAS_control", bus, values)[2] - values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7]) - messages.append(packer.make_can_msg("DAS_control", bus, values)) - return messages + def right_stalk_press(self, counter, position): + values = { + "SCCM_rightStalkCrc": 0, + "SCCM_rightStalkCounter": counter, + "SCCM_rightStalkStatus": position, + "SCCM_rightStalkReserved1": 0, + "SCCM_parkButtonStatus": 0, + "SCCM_rightStalkReserved2": 0, + } + + data = self.pt_packer.make_can_msg("SCCM_rightStalk", CANBUS.vehicle, values)[2] + values["SCCM_rightStalkCrc"] = self.right_stalk_crc(data[1:]) + return self.pt_packer.make_can_msg("SCCM_rightStalk", CANBUS.vehicle, values) diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 0f9cd00f63..90b86aa7bf 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -9,32 +9,22 @@ Ecu = car.CarParams.Ecu Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) + class CAR(Platforms): - TESLA_AP1_MODELS = PlatformConfig( - [CarDocs("Tesla AP1 Model S", "All")], - CarSpecs(mass=2100., wheelbase=2.959, steerRatio=15.0), - dbc_dict('tesla_powertrain', 'tesla_radar_bosch_generated', chassis_dbc='tesla_can') + TESLA_AP3_MODEL3 = PlatformConfig( + [CarDocs("Tesla AP3 Model 3", "All")], + CarSpecs(mass=1899., wheelbase=2.875, steerRatio=12.0), + dbc_dict('tesla_model3_vehicle', None, chassis_dbc='tesla_model3_party') ) - TESLA_AP2_MODELS = PlatformConfig( - [CarDocs("Tesla AP2 Model S", "All")], - TESLA_AP1_MODELS.specs, - TESLA_AP1_MODELS.dbc_dict - ) - TESLA_MODELS_RAVEN = PlatformConfig( - [CarDocs("Tesla Model S Raven", "All")], - TESLA_AP1_MODELS.specs, - dbc_dict('tesla_powertrain', 'tesla_radar_continental_generated', chassis_dbc='tesla_can') + TESLA_AP3_MODELY = PlatformConfig( + [CarDocs("Tesla AP3 Model Y", "All")], + CarSpecs(mass=2072., wheelbase=2.890, steerRatio=12.0), + dbc_dict('tesla_model3_vehicle', None, chassis_dbc='tesla_model3_party') ) + FW_QUERY_CONFIG = FwQueryConfig( requests=[ - Request( - [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], - [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], - whitelist_ecus=[Ecu.eps], - rx_offset=0x08, - bus=0, - ), Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE], @@ -45,23 +35,20 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], - whitelist_ecus=[Ecu.adas, Ecu.electricBrakeBooster, Ecu.fwdRadar], + whitelist_ecus=[Ecu.engine], rx_offset=0x10, - bus=0, + bus=1, + obd_multiplexing=False, ), ] ) -class CANBUS: - # Lateral harness - chassis = 0 - radar = 1 - autopilot_chassis = 2 - # Longitudinal harness - powertrain = 4 - private = 5 - autopilot_powertrain = 6 +class CANBUS: + party = 0 + vehicle = 1 + autopilot_party = 2 + GEAR_MAP = { "DI_GEAR_INVALID": car.CarState.GearShifter.unknown, @@ -72,18 +59,23 @@ GEAR_MAP = { "DI_GEAR_SNA": car.CarState.GearShifter.unknown, } -DOORS = ["DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE"] - -# Make sure the message and addr is also in the CAN parser! BUTTONS = [ - Button(car.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]), - Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]), - Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]), - Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]), - Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]), - Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]), + Button(car.CarState.ButtonEvent.Type.leftBlinker, "SCCM_leftStalk", "SCCM_turnIndicatorStalkStatus", [3, 4]), + Button(car.CarState.ButtonEvent.Type.rightBlinker, "SCCM_leftStalk", "SCCM_turnIndicatorStalkStatus", [1, 2]), + Button(car.CarState.ButtonEvent.Type.accelCruise, "VCLEFT_switchStatus", "VCLEFT_swcRightScrollTicks", list(range(1, 10))), + Button(car.CarState.ButtonEvent.Type.decelCruise, "VCLEFT_switchStatus", "VCLEFT_swcRightScrollTicks", list(range(-9, 0))), + Button(car.CarState.ButtonEvent.Type.cancel, "SCCM_rightStalk", "SCCM_rightStalkStatus", [1, 2]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "SCCM_rightStalk", "SCCM_rightStalkStatus", [3, 4]), ] +BUTTON_STATES = { + "accelCruise": False, + "decelCruise": False, + "cancel": False, + "resumeCruise": False, +} + + class CarControllerParams: ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 1.6, .3]) ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 7.0, 0.8]) diff --git a/selfdrive/car/torque_data/override.toml b/selfdrive/car/torque_data/override.toml index 2949bbb82f..cf56cbcbfc 100644 --- a/selfdrive/car/torque_data/override.toml +++ b/selfdrive/car/torque_data/override.toml @@ -16,9 +16,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "TOYOTA_RAV4_TSS2_2023" = [nan, 3.0, nan] # Tesla has high torque -"TESLA_AP1_MODELS" = [nan, 2.5, nan] -"TESLA_AP2_MODELS" = [nan, 2.5, nan] -"TESLA_MODELS_RAVEN" = [nan, 2.5, nan] +"TESLA_AP3_MODEL3" = [nan, 2.5, nan] +"TESLA_AP3_MODELY" = [nan, 2.5, nan] # Guess "FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan]