diff --git a/common/realtime.py b/common/realtime.py index 57926b4c4..7f7e9fca8 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -94,3 +94,88 @@ class Ratekeeper: self._frame += 1 self._remaining = remaining return lagged + +class DurationTimer: + def __init__(self, duration=0, step=DT_CTRL) -> None: + self.step = step + self.duration = duration + self.was_reset = False + self.timer = 0 + self.min = float("-inf") # type: float + self.max = float("inf") # type: float + + def tick_obj(self) -> None: + self.timer += self.step + # reset on overflow + self.timer = 0 if (self.timer == (self.max or self.min)) else self.timer + + def reset(self) -> None: + """Resets this objects timer""" + self.timer = 0 + self.was_reset = True + + def active(self) -> bool: + """Returns true if time since last reset is less than duration""" + return bool(round(self.timer,2) < self.duration) + + def adjust(self, duration) -> None: + """Adjusts the duration of the timer""" + self.duration = duration + + def once_after_reset(self) -> bool: + """Returns true only one time after calling reset()""" + ret = self.was_reset + self.was_reset = False + return ret + + @staticmethod + def interval_obj(rate, frame) -> bool: + if frame % rate == 0: # Highlighting shows "frame" in white + return True + return False + +class ModelTimer(DurationTimer): + frame: int = 0 + objects: list = [] + def __init__(self, duration=0) -> None: + self.step = DT_MDL + super().__init__(duration, self.step) + self.__class__.objects.append(self) + + @classmethod + def tick(cls) -> None: + cls.frame += 1 + for obj in cls.objects: + ModelTimer.tick_obj(obj) + + @classmethod + def reset_all(cls) -> None: + for obj in cls.objects: + obj.reset() + + @classmethod + def interval(cls, rate) -> bool: + return ModelTimer.interval_obj(rate, cls.frame) + +class ControlsTimer(DurationTimer): + frame = 0 + objects = [] # type: list[DurationTimer] + def __init__(self, duration=0) -> None: + self.step = DT_CTRL + super().__init__(duration=duration, step=self.step) + self.__class__.objects.append(self) + + @classmethod + def tick(cls) -> None: + cls.frame += 1 + for obj in cls.objects: + ControlsTimer.tick_obj(obj) + + @classmethod + def reset_all(cls) -> None: + for obj in cls.objects: + obj.reset() + + @classmethod + def interval(cls, rate) -> bool: + return ControlsTimer.interval_obj(rate, cls.frame) \ No newline at end of file diff --git a/opendbc_repo/opendbc/can/dbc.py b/opendbc_repo/opendbc/can/dbc.py index 658bbdab3..2d226db30 100644 --- a/opendbc_repo/opendbc/can/dbc.py +++ b/opendbc_repo/opendbc/can/dbc.py @@ -15,6 +15,7 @@ from opendbc.car.volkswagen.mqbcan import volkswagen_mqb_meb_checksum, xor_check from opendbc.car.tesla.teslacan import tesla_checksum from opendbc.car.body.bodycan import body_checksum from opendbc.car.psa.psacan import psa_checksum +from opendbc.car.mazda.mazdacan import mazda2017_checksum, mazda2019_checksum class SignalType: @@ -31,6 +32,8 @@ class SignalType: FCA_GIORGIO_CHECKSUM = 10 TESLA_CHECKSUM = 11 PSA_CHECKSUM = 12 + MAZDA2017_CHECKSUM = 13 + MAZDA2019_CHECKSUM = 14 @dataclass @@ -200,6 +203,12 @@ def get_checksum_state(dbc_name: str) -> ChecksumState | None: return ChecksumState(8, -1, 0, -1, True, SignalType.TESLA_CHECKSUM, tesla_checksum, tesla_setup_signal) elif dbc_name.startswith("psa_"): return ChecksumState(4, 4, 7, 3, False, SignalType.PSA_CHECKSUM, psa_checksum) + elif dbc_name.startswith("mazda_2023"): + return ChecksumState(8, 8, 0, 0, True, SignalType.MAZDA2019_CHECKSUM, mazda2019_checksum) + elif dbc_name.startswith("mazda_2019"): + return ChecksumState(8, 8, 0, 0, True, SignalType.MAZDA2019_CHECKSUM, mazda2019_checksum) + elif dbc_name.startswith("mazda_2017"): + return ChecksumState(8, -1, 0, -1, True, SignalType.MAZDA2017_CHECKSUM, mazda2017_checksum) return None diff --git a/opendbc_repo/opendbc/car/mazda/carcontroller.py b/opendbc_repo/opendbc/car/mazda/carcontroller.py index 453f3ee20..e2c7d1cd5 100644 --- a/opendbc_repo/opendbc/car/mazda/carcontroller.py +++ b/opendbc_repo/opendbc/car/mazda/carcontroller.py @@ -3,63 +3,127 @@ from opendbc.car import Bus, structs from opendbc.car.lateral import apply_driver_steer_torque_limits from opendbc.car.interfaces import CarControllerBase from opendbc.car.mazda import mazdacan -from opendbc.car.mazda.values import CarControllerParams, Buttons +from opendbc.car.mazda.values import CarControllerParams, Buttons, MazdaSafetyFlags +from openpilot.common.realtime import ControlsTimer as Timer, DT_CTRL +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.params import Params VisualAlert = structs.CarControl.HUDControl.VisualAlert - +LongCtrlState = structs.CarControl.Actuators.LongControlState class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) self.apply_torque_last = 0 + self.ti_apply_torque_last = 0 self.packer = CANPacker(dbc_names[Bus.pt]) self.brake_counter = 0 + self.ccp = CarControllerParams(CP) + self.hold_timer = Timer(6.0) + self.hold_delay = Timer(.5) # delay before we start holding as to not hit the brakes too hard + self.resume_timer = Timer(0.5) + self.cancel_delay = Timer(0.07) # 70ms delay to try to avoid a race condition with stock system + def update(self, CC, CS, now_nanos): can_sends = [] apply_torque = 0 + ti_apply_torque = 0 if CC.latActive: # calculate steer and also set limits due to driver torque - new_torque = int(round(CC.actuators.torque * CarControllerParams.STEER_MAX)) + new_torque = int(round(CC.actuators.torque * self.ccp.STEER_MAX)) apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, - CS.out.steeringTorque, CarControllerParams) - - if CC.cruiseControl.cancel: - # If brake is pressed, let us wait >70ms before trying to disable crz to avoid - # a race condition with the stock system, where the second cancel from openpilot - # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to - # read 3 messages and most likely sync state before we attempt cancel. - self.brake_counter = self.brake_counter + 1 - if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): - # Cancel Stock ACC if it's enabled while OP is disengaged - # Send at a rate of 10hz until we sync with stock ACC state - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL)) - else: - self.brake_counter = 0 - if CC.cruiseControl.resume and self.frame % 5 == 0: - # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds - # Send Resume button when planner wants car to move - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME)) + CS.out.steeringTorque, self.ccp) + if self.CP.flags & MazdaSafetyFlags.TORQUE_INTERCEPTOR: + if CS.ti_lkas_allowed: + ti_new_torque = int(round(CC.actuators.steer * self.ccp.STEER_MAX)) + ti_apply_torque = apply_driver_steer_torque_limits(ti_new_torque, self.ti_apply_steer_last, + CS.out.steeringTorque, self.ccp) self.apply_torque_last = apply_torque + self.ti_apply_torque_last = ti_apply_torque + + if self.CP.flags & MazdaSafetyFlags.GEN1: + if CC.cruiseControl.cancel: + # If brake is pressed, let us wait >70ms before trying to disable crz to avoid + # a race condition with the stock system, where the second cancel from openpilot + # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to + # read 3 messages and most likely sync state before we attempt cancel. + self.brake_counter = self.brake_counter + 1 + if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): + # Cancel Stock ACC if it's enabled while OP is disengaged + # Send at a rate of 10hz until we sync with stock ACC state + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL)) + else: + self.brake_counter = 0 + if CC.cruiseControl.resume and self.frame % 5 == 0: + # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds + # Send Resume button when planner wants car to move + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME)) + + # send HUD alerts + if self.frame % 50 == 0: + ldw = CC.hudControl.visualAlert == VisualAlert.ldw + steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired + # TODO: find a way to silence audible warnings so we can add more hud alerts + steer_required = steer_required and CS.lkas_allowed_speed + can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) + + if self.CP.openpilotLongitudinalControl: + hold = False + if CS.out.standstill: + hold = self.hold_timer.active() + else: + self.hold_timer.reset() + + raw_acc_output = CC.actuators.accel * 1150 + raw_acc_output = max(-1000, min(raw_acc_output, 1000)) + CS.crz_info["ACCEL_CMD"] = raw_acc_output + + if self.frame % 2 == 0: + can_sends.extend(mazdacan.create_radar_command(self.packer, self.frame, CC.longActive, CS, hold)) + + elif self.CP.flags & MazdaSafetyFlags.GEN2: + if CC.longActive and self.CP.openpilotLongitudinalControl: + CS.acc["ACCEL_CMD"] = (CC.actuators.accel * 200) + 2000 + + resume = False + hold = False + if Timer.interval(2): # send ACC command at 50hz + """ + Without this hold/resum logic, the car will only stop momentarily. + It will then start creeping forward again. This logic allows the car to + apply the electric brake to hold the car. The hold delay also fixes a + bug with the stock ACC where it sometimes will apply the brakes too early + when coming to a stop. + """ + if CS.out.standstill: # if we're stopped + if not self.hold_delay.active(): # and we have been stopped for more than hold_delay duration. This prevents a hard brake if we aren't fully stopped. + if ((CC.cruiseControl.resume and CC.actuators.longControlState != LongCtrlState.stopping) or + CC.cruiseControl.override or CS.out.gasPressed or + (CC.actuators.longControlState == LongCtrlState.starting) or CS.acc["RESUME"]): # if we are resuming or overriding, we want to release the brake + self.resume_timer.reset() # reset the resume timer so its active + else: # otherwise we're holding + hold = self.hold_timer.active() # hold for 6s. This allows the electric brake to hold the car. + + else: # if we're moving + self.hold_timer.reset() # reset the hold timer so its active when we stop + self.hold_delay.reset() # reset the hold delay + + resume = self.resume_timer.active() # stay on for 0.5s to release the brake. This allows the car to move. + can_sends.append(mazdacan.create_acc_cmd(self.packer, CS.acc, hold, resume)) - # send HUD alerts - if self.frame % 50 == 0: - ldw = CC.hudControl.visualAlert == VisualAlert.ldw - steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired - # TODO: find a way to silence audible warnings so we can add more hud alerts - steer_required = steer_required and CS.lkas_allowed_speed - can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) # send steering command - can_sends.append(mazdacan.create_steering_control(self.packer, self.CP, + can_sends.extend(mazdacan.create_steering_control(self.packer, self.CP, self.frame, apply_torque, CS.cam_lkas)) new_actuators = CC.actuators.as_builder() - new_actuators.torque = apply_torque / CarControllerParams.STEER_MAX + new_actuators.torque = apply_torque / self.ccp.STEER_MAX new_actuators.torqueOutputCan = apply_torque self.frame += 1 - return new_actuators, can_sends + Timer.tick() + return new_actuators, can_sends \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/mazda/carstate.py b/opendbc_repo/opendbc/car/mazda/carstate.py index 074a13c26..fee4751d8 100644 --- a/opendbc_repo/opendbc/car/mazda/carstate.py +++ b/opendbc_repo/opendbc/car/mazda/carstate.py @@ -1,8 +1,9 @@ +import copy from opendbc.can import CANDefine, CANParser -from opendbc.car import Bus, create_button_events, structs +from opendbc.car import Bus, create_button_events, structs, DT_CTRL from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase -from opendbc.car.mazda.values import DBC, LKAS_LIMITS +from opendbc.car.mazda.values import DBC, LKAS_LIMITS, MazdaSafetyFlags, TI_STATE, CarControllerParams ButtonType = structs.CarState.ButtonEvent.Type @@ -13,16 +14,33 @@ class CarState(CarStateBase): can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.shifter_values = can_define.dv["GEAR"]["GEAR"] + if CP.flags & MazdaSafetyFlags.MANUAL_TRANSMISSION: + self.shifter_values = can_define.dv["MANUAL_GEAR"]["GEAR"] self.crz_btns_counter = 0 self.acc_active_last = False self.lkas_allowed_speed = False + self.cam_lkas = 0 + self.params = CarControllerParams(CP) self.distance_button = 0 + self.ti_ramp_down = False + self.ti_version = 1 + self.ti_state = TI_STATE.RUN + self.ti_violation = 0 + self.ti_error = 0 + self.ti_lkas_allowed = False + + self._prev_steering_angle = 0 + def update(self, can_parsers) -> structs.CarState: + if self.CP.flags & (MazdaSafetyFlags.GEN2 | MazdaSafetyFlags.GEN3): + return self.update_gen2(can_parsers) + cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] + cp_body = can_parsers[Bus.body] ret = structs.CarState() @@ -49,9 +67,23 @@ class CarState(CarStateBase): ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) + if self.CP.flags & MazdaSafetyFlags.TORQUE_INTERCEPTOR: + ret.steeringTorque = cp_body.vl["TI_FEEDBACK"]["TI_TORQUE_SENSOR"] + + self.ti_version = cp_body.vl["TI_FEEDBACK"]["VERSION_NUMBER"] + self.ti_state = cp_body.vl["TI_FEEDBACK"]["STATE"] # DISCOVER = 0, OFF = 1, DRIVER_OVER = 2, RUN=3 + self.ti_violation = cp_body.vl["TI_FEEDBACK"]["VIOL"] # 0 = no violation + self.ti_error = cp_body.vl["TI_FEEDBACK"]["ERROR"] # 0 = no error + if self.ti_version > 1: + self.ti_ramp_down = (cp_body.vl["TI_FEEDBACK"]["RAMP_DOWN"] == 1) + + ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.TI_STEER_THRESHOLD + self.ti_lkas_allowed = not self.ti_ramp_down and self.ti_state == TI_STATE.RUN + else: + ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] + ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD + ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] - ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] - ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]["STEER_TORQUE_MOTOR"] ret.steeringRateDeg = cp.vl["STEER_RATE"]["STEER_ANGLE_RATE"] @@ -82,44 +114,115 @@ class CarState(CarStateBase): # TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on # it should be used for carState.cruiseState.nonAdaptive instead - ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1 - ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1 - ret.cruiseState.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1 + ret.cruiseState.standstill = ret.standstill ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS + if self.CP.flags & MazdaSafetyFlags.RADAR_INTERCEPTOR: + self.crz_info = copy.copy(cp_cam.vl["CRZ_INFO"]) + self.crz_cntr = copy.copy(cp_cam.vl["CRZ_CTRL"]) + self.cp_cam = cp_cam + ret.cruiseState.enabled = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1 + ret.cruiseState.available = cp.vl["PEDALS"]["CRZ_AVAILABLE"] == 1 + elif self.CP.flags & MazdaSafetyFlags.NO_MRCC: + ret.cruiseState.enabled = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1 + ret.cruiseState.available = cp.vl["PEDALS"]["CRZ_AVAILABLE"] == 1 + else: + ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1 + ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1 + # stock lkas should be on # TODO: is this needed? - ret.invalidLkasSetting = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 - - if ret.cruiseState.enabled: - if not self.lkas_allowed_speed and self.acc_active_last: - self.low_speed_alert = True - else: - self.low_speed_alert = False - ret.lowSpeedAlert = self.low_speed_alert + if not self.CP.flags & MazdaSafetyFlags.TORQUE_INTERCEPTOR: + if ret.cruiseState.enabled: + if not self.lkas_allowed_speed and self.acc_active_last: + self.low_speed_alert = True + else: + self.low_speed_alert = False + ret.lowSpeedAlert = self.low_speed_alert # Check if LKAS is disabled due to lack of driver torque when all other states indicate # it should be enabled (steer lockout). Don't warn until we actually get lkas active # and lose it again, i.e, after initial lkas activation - ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked + ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked and not self.ti_lkas_allowed self.acc_active_last = ret.cruiseState.enabled self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] # camera signals - self.cam_lkas = cp_cam.vl["CAM_LKAS"] - self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] - ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 + if not self.CP.flags & MazdaSafetyFlags.NO_FSC: + ret.invalidLkasSetting = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 + self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 if not self.CP.flags & MazdaSafetyFlags.TORQUE_INTERCEPTOR else False + self.cam_lkas = cp_cam.vl["CAM_LKAS"] + self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] + ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 if not self.CP.flags & MazdaSafetyFlags.TORQUE_INTERCEPTOR else False + self.cp_cam = cp_cam + self.cp = cp # TODO: add button types for inc and dec ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) return ret + def update_gen2(self, can_parsers) -> structs.CarState: + cp = can_parsers[Bus.pt] + cp_cam = can_parsers[Bus.cam] + cp_body = can_parsers[Bus.body] + ret = structs.CarState() + + self.parse_wheel_speeds(ret, + cp_cam.vl["WHEEL_SPEEDS"]["FL"], + cp_cam.vl["WHEEL_SPEEDS"]["FR"], + cp_cam.vl["WHEEL_SPEEDS"]["RL"], + cp_cam.vl["WHEEL_SPEEDS"]["RR"], + ) + + ret.steeringTorque = cp_body.vl["TI_FEEDBACK"]["STEER_TORQUE_SENSOR"] + ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_DRIVER_ALLOWANCE + + if self.CP.flags & MazdaSafetyFlags.MANUAL_TRANSMISSION: + can_gear = int(cp_cam.vl["MANUAL_GEAR"]["GEAR"]) + else: + can_gear = int(cp_cam.vl["GEAR"]["GEAR"]) + + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(100, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, + cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) + + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + ret.gasPressed = cp_cam.vl["ENGINE_DATA"]["PEDAL_GAS"] > 0 + ret.seatbeltUnlatched = False # Cruise will not engage if seatbelt is unlatched (handled by car) + ret.doorOpen = False # Cruise will not engage if door is open (handled by car) + ret.brakePressed = cp.vl["BRAKE_PEDAL"]["BRAKE_PRESSED"] == 1 + ret.brake = .1 + ret.steerFaultPermanent = False # TODO locate signal. Car shows light on dash if there is a fault + ret.steerFaultTemporary = False # TODO locate signal. Car shows light on dash if there is a fault + + unit_conversion = CV.MPH_TO_MS if cp.vl["SYSTEM_SETTINGS"]["IMPERIAL_UNIT"] else CV.KPH_TO_MS + ret.standstill = cp_cam.vl["SPEED"]["SPEED"] * unit_conversion < 0.1 + if self.CP.flags & MazdaSafetyFlags.GEN2: + ret.steeringAngleDeg = cp_cam.vl["STEER"]["STEER_ANGLE"] + ret.cruiseState.speed = cp.vl["CRUZE_STATE"]["CRZ_SPEED"] * unit_conversion + ret.cruiseState.enabled = (cp.vl["CRUZE_STATE"]["CRZ_STATE"] >= 2) + ret.cruiseState.available = (cp.vl["CRUZE_STATE"]["CRZ_STATE"] != 0) + else: + ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] + ret.cruiseState.speed = cp_body.vl["CRUZE_STATE"]["CRZ_SPEED"] * unit_conversion + ret.cruiseState.enabled = (cp_body.vl["CRUZE_STATE"]["CRZ_STATE"] >= 3) + ret.cruiseState.available = (cp_body.vl["CRUZE_STATE"]["CRZ_STATE"] >= 2) + ret.steeringRateDeg = (ret.steeringAngleDeg - self._prev_steering_angle) / DT_CTRL + self._prev_steering_angle = ret.steeringAngleDeg + ret.cruiseState.standstill = ret.standstill if not self.CP.openpilotLongitudinalControl else False + + self.cp = cp + self.cp_cam = cp_cam + self.acc = copy.copy(cp.vl["ACC"]) + + return ret + @staticmethod def get_can_parsers(CP): return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0), + Bus.body: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2), - } + } \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/mazda/fingerprints.py b/opendbc_repo/opendbc/car/mazda/fingerprints.py index 5569e28c7..266ac9361 100644 --- a/opendbc_repo/opendbc/car/mazda/fingerprints.py +++ b/opendbc_repo/opendbc/car/mazda/fingerprints.py @@ -280,4 +280,102 @@ FW_VERSIONS = { b'PXM7-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, + CAR.MAZDA_3_2019: { + (Ecu.eps, 0x730, None): [ + b'BDGF-3216X-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-3216X-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + #b'BDGF-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-3216X-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PA2J-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + #b'PX06-188K2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX06-188K2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX08-188K2-L\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4W-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'BDTS-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + #b'B0N2-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B0N2-67XK2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'BFVV-4300F-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BHCB-4300F-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-4300F-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-4300F-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BFVV-4300F-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'BDGF-67WK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BDGF-67WK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + #b'BDGF-67WK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'DFR5-67WK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PAM6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + #b'PX01-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX03-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4K-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + CAR.MAZDA_CX_30: { + (Ecu.eps, 0x730, None): [ + b'DFR5-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BDGF-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX06-188K2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4N-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'BDTS-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B0N2-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'DEJW-4300F-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-4300F-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'BDGF-67WK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BDGF-67WK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PX01-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4N-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + + CAR.MAZDA_CX_50: { + (Ecu.eps, 0x730, None): [ + b'VA40-3216Y-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX06-188K2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX08-188K2-L\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4W-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'VA45-67XK2-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'VA40-4300F-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'VA40-4300F-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'VA40-67WK2-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + #b'PX01-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX03-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4K-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, +} + +FINGERPRINTS = { + CAR.MAZDA_CX_30_2023: [{ + 80: 8, 119: 8, 126: 8, 128: 8, 131: 8, 135: 8, 136: 8, 137: 8, 143: 4, 145: 8, 150: 8, 153: 8, 154: 8, 155: 8, 156: 8, 159: 8, 192: 8, 253: 8, 254: 8, 304: 8, 357: 8, 358: 8, 379: 8, 512: 8, 514: 8, 518: 8, 529: 8, 531: 8, 533: 8, 542: 8, 543: 8, 546: 8, 547: 8, 552: 8, 570: 8, 608: 8, 640: 8, 641: 8, 736: 8, 830: 8, 832: 8, 864: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 871: 8, 872: 8, 873: 8, 874: 8, 875: 8, 876: 8, 877: 8, 878: 8, 879: 8, 880: 8, 881: 8, 882: 8, 883: 8, 884: 8, 885: 8, 886: 8, 887: 8, 888: 8, 960: 8, 979: 8, 1010: 8, 1016: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1034: 8, 1044: 8, 1045: 8, 1056: 8, 1058: 8, 1060: 8, 1066: 8, 1067: 8, 1078: 8, 1084: 8, 1086: 8, 1087: 8, 1088: 8, 1093: 8, 1100: 8, 1120: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1157: 8, 1163: 8, 1167: 8, 1203: 8, 1242: 8, 1243: 8, 1244: 8, 1260: 8, 1264: 8, 1266: 8, 1267: 8, 1274: 8, 1275: 8, 1305: 8, 1306: 8, 1307: 8, 1344: 8, 1409: 8, 1430: 8, 1440: 8 + }], } diff --git a/opendbc_repo/opendbc/car/mazda/interface.py b/opendbc_repo/opendbc/car/mazda/interface.py index 814846e85..efd69e3c7 100755 --- a/opendbc_repo/opendbc/car/mazda/interface.py +++ b/opendbc_repo/opendbc/car/mazda/interface.py @@ -1,32 +1,137 @@ #!/usr/bin/env python3 +from math import exp, fabs +import numpy as np + from opendbc.car import get_safety_config, structs from opendbc.car.common.conversions import Conversions as CV -from opendbc.car.interfaces import CarInterfaceBase +from opendbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType from opendbc.car.mazda.carcontroller import CarController from opendbc.car.mazda.carstate import CarState -from opendbc.car.mazda.values import CAR, LKAS_LIMITS +from opendbc.car.mazda.values import CAR, LKAS_LIMITS, MazdaSafetyFlags, MazdaSafetyFlags, GEN1, GEN2, GEN3 +from openpilot.common.params import Params +NON_LINEAR_TORQUE_PARAMS = { + CAR.MAZDA_3_2019: (3.650, 1.0, 0.13, 0.3605), + CAR.MAZDA_CX_30: (2.082, 1.444, 0.1, 0.238), + CAR.MAZDA_CX_30_2023: (5.5, 0.79999, 0.18244, 0.38763), + CAR.MAZDA_CX_50: (3.8818, 0.6873, 0.0999, 0.3605), +} class CarInterface(CarInterfaceBase): CarState = CarState CarController = CarController + def get_lataccel_torque_siglin(self) -> float: + + def torque_from_lateral_accel_siglin_func(lateral_acceleration: float) -> float: + # The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves + # An important thing to consider is that the slope at 0 should be > 0 (ideally >1) + # This has big effect on the stability about 0 (noise when going straight) + non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint) + assert non_linear_torque_params, "The params are not defined" + a, b, c, _ = non_linear_torque_params + sig_input = a * lateral_acceleration + sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5) + steer_torque = (sig * b) + (lateral_acceleration * c) + return float(steer_torque) + + lataccel_values = np.arange(-8.0, 8.0, 0.01) + torque_values = [torque_from_lateral_accel_siglin_func(x) for x in lataccel_values] + print(torque_values) + assert min(torque_values) < -1 and max(torque_values) > 1, "The torque values should cover the range [-1, 1]" + return torque_values, lataccel_values + + def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: + if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS: + torque_values, lataccel_values = self.get_lataccel_torque_siglin() + + def torque_from_lateral_accel_siglin(lateral_acceleration: float, torque_params: structs.CarParams.LateralTorqueTuning): + return np.interp(lateral_acceleration, lataccel_values, torque_values) + return torque_from_lateral_accel_siglin + else: + return self.torque_from_lateral_accel_linear + + def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType: + if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS: + torque_values, lataccel_values = self.get_lataccel_torque_siglin() + + def lateral_accel_from_torque_siglin(torque: float, torque_params: structs.CarParams.LateralTorqueTuning): + return np.interp(torque, torque_values, lataccel_values) + return lateral_accel_from_torque_siglin + else: + return self.lateral_accel_from_torque_linear + + @staticmethod def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams: ret.brand = "mazda" ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.mazda)] + p = Params() + ret.radarUnavailable = True - ret.dashcamOnly = candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021) + ret.dashcamOnly = False ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.8 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - if candidate not in (CAR.MAZDA_CX5_2022,): + if candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_3_2019, CAR.MAZDA_CX_30, CAR.MAZDA_CX_50, CAR.MAZDA_3_2023, CAR.MAZDA_CX_30_2023) and not ret.flags & MazdaSafetyFlags.TORQUE_INTERCEPTOR: ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS ret.centerToFront = ret.wheelbase * 0.41 - return ret + if p.get_bool("ManualTransmission"): + ret.flags |= MazdaSafetyFlags.MANUAL_TRANSMISSION.value + ret.transmissionType = structs.CarParams.TransmissionType.manual + else: + ret.transmissionType = structs.CarParams.TransmissionType.automatic + + if candidate in GEN1: + ret.safetyConfigs[0].safetyParam |= MazdaSafetyFlags.GEN1.value + if p.get_bool("TorqueInterceptorEnabled"): # Torque Interceptor Installed + ret.flags |= MazdaSafetyFlags.TORQUE_INTERCEPTOR.value + ret.safetyConfigs[0].safetyParam |= MazdaSafetyFlags.TORQUE_INTERCEPTOR.value + if p.get_bool("RadarInterceptorEnabled"): # Radar Interceptor Installed + ret.flags |= MazdaSafetyFlags.RADAR_INTERCEPTOR.value + ret.safetyConfigs[0].safetyParam |= MazdaSafetyFlags.RADAR_INTERCEPTOR.value + ret.alphaLongitudinalAvailable = alpha_long + ret.openpilotLongitudinalControl = True + ret.radarUnavailable = False + ret.startingState = True + ret.longitudinalTuning.kpBP = [0., 5., 30.] + ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7] + ret.longitudinalTuning.kiBP = [0., 5., 20., 30.] + ret.longitudinalTuning.kiV = [0.36, 0.23, 0.17, 0.1] + if p.get_bool("NoMRCC"): # No Mazda Radar Cruise Control; Missing CRZ_CTRL signal + ret.flags |= MazdaSafetyFlags.NO_MRCC.value + ret.safetyConfigs[0].safetyParam |= MazdaSafetyFlags.NO_MRCC.value + if p.get_bool("NoFSC"): # No Front Sensing Camera + ret.flags |= MazdaSafetyFlags.NO_FSC.value + ret.safetyConfigs[0].safetyParam |= MazdaSafetyFlags.NO_FSC.value + + ret.steerActuatorDelay = 0.1 + ret.enableBsm = True + + if candidate in GEN2: + ret.safetyConfigs[0].safetyParam |= MazdaSafetyFlags.GEN2.value + ret.alphaLongitudinalAvailable = alpha_long + ret.openpilotLongitudinalControl = True + ret.stopAccel = -.5 + ret.vEgoStarting = .2 + ret.longitudinalActuatorDelay = 0.35 # gas is 0.25s and brake looks like 0.5 + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [0.0, 0.0, 0.0] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.1, 0.1] + ret.startingState = True + ret.steerActuatorDelay = 0.335 + + if candidate in GEN3: + ret.safetyConfigs[0].safetyParam |= MazdaSafetyFlags.GEN3.value + ret.alphaLongitudinalAvailable = False + ret.openpilotLongitudinalControl = False + if p.get_bool("ManualTransmission"): + ret.flags |= MazdaSafetyFlags.MANUAL_TRANSMISSION.value + return ret \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/mazda/mazdacan.py b/opendbc_repo/opendbc/car/mazda/mazdacan.py index adccdd642..782434d58 100644 --- a/opendbc_repo/opendbc/car/mazda/mazdacan.py +++ b/opendbc_repo/opendbc/car/mazda/mazdacan.py @@ -1,66 +1,83 @@ -from opendbc.car.mazda.values import Buttons, MazdaFlags - +from opendbc.car.mazda.values import Buttons, MazdaSafetyFlags +from numpy import clip def create_steering_control(packer, CP, frame, apply_torque, lkas): + msgs = [] + if CP.flags & MazdaSafetyFlags.GEN1: + if not CP.flags & MazdaSafetyFlags.NO_FSC: + tmp = apply_torque + 2048 - tmp = apply_torque + 2048 + lo = tmp & 0xFF + hi = tmp >> 8 - lo = tmp & 0xFF - hi = tmp >> 8 + # copy values from camera + b1 = int(lkas["BIT_1"]) + er1 = int(lkas["ERR_BIT_1"]) + lnv = 0 + ldw = 0 + er2 = int(lkas["ERR_BIT_2"]) - # copy values from camera - b1 = int(lkas["BIT_1"]) - er1 = int(lkas["ERR_BIT_1"]) - lnv = 0 - ldw = 0 - er2 = int(lkas["ERR_BIT_2"]) + # Some older models do have these, newer models don't. + # Either way, they all work just fine if set to zero. + steering_angle = 0 + b2 = 0 - # Some older models do have these, newer models don't. - # Either way, they all work just fine if set to zero. - steering_angle = 0 - b2 = 0 + tmp = steering_angle + 2048 + ahi = tmp >> 10 + amd = (tmp & 0x3FF) >> 2 + amd = (amd >> 4) | (( amd & 0xF) << 4) + alo = (tmp & 0x3) << 2 - tmp = steering_angle + 2048 - ahi = tmp >> 10 - amd = (tmp & 0x3FF) >> 2 - amd = (amd >> 4) | ((amd & 0xF) << 4) - alo = (tmp & 0x3) << 2 + ctr = frame % 16 + # bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ] + csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5) - ctr = frame % 16 - # bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ] - csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - (er2 << 4) - (b1 << 5) + # bytes [ 5 ] [ 6 ] [ 7 ] + csum = csum - ahi - amd - alo - b2 - # bytes [ 5 ] [ 6 ] [ 7 ] - csum = csum - ahi - amd - alo - b2 + if ahi == 1: + csum = csum + 15 - if ahi == 1: - csum = csum + 15 + if csum < 0: + if csum < -256: + csum = csum + 512 + else: + csum = csum + 256 - if csum < 0: - if csum < -256: - csum = csum + 512 - else: - csum = csum + 256 + csum = csum % 256 - csum = csum % 256 + values = { + "LKAS_REQUEST": apply_torque, + "CTR": ctr, + "ERR_BIT_1": er1, + "LINE_NOT_VISIBLE" : lnv, + "LDW": ldw, + "BIT_1": b1, + "ERR_BIT_2": er2, + "STEERING_ANGLE": steering_angle, + "ANGLE_ENABLED": b2, + "CHKSUM": csum + } + msgs.append(packer.make_can_msg("CAM_LKAS", 0, values)) - values = {} - if CP.flags & MazdaFlags.GEN1: + if CP.flags & MazdaSafetyFlags.TORQUE_INTERCEPTOR: + values = { + "LKAS_REQUEST" : apply_torque, + "CHKSUM" : apply_torque, + "KEY" : 3294744160 + } + msgs.append(packer.make_can_msg("CAM_LKAS2", 1, values)) + + elif CP.flags & (MazdaSafetyFlags.GEN2 | MazdaSafetyFlags.GEN3) : + bus = 1 + sig_name = "EPS_LKAS" values = { "LKAS_REQUEST": apply_torque, - "CTR": ctr, - "ERR_BIT_1": er1, - "LINE_NOT_VISIBLE": lnv, - "LDW": ldw, - "BIT_1": b1, - "ERR_BIT_2": er2, - "STEERING_ANGLE": steering_angle, - "ANGLE_ENABLED": b2, - "CHKSUM": csum + "STEER_FEEL": 10000, } + msgs.append(packer.make_can_msg(sig_name, bus, values)) - return packer.make_can_msg("CAM_LKAS", 0, values) - + return msgs def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool): values = {s: cam_msg[s] for s in [ @@ -93,7 +110,7 @@ def create_button_cmd(packer, CP, counter, button): can = int(button == Buttons.CANCEL) res = int(button == Buttons.RESUME) - if CP.flags & MazdaFlags.GEN1: + if CP.flags & MazdaSafetyFlags.GEN1: values = { "CAN_OFF": can, "CAN_OFF_INV": (can + 1) % 2, @@ -126,3 +143,96 @@ def create_button_cmd(packer, CP, counter, button): } return packer.make_can_msg("CRZ_BTNS", 0, values) + +STATIC_DATA_21B = [0x01FFE000, 0x00000000] +STATIC_DATA_361 = [0xFFF7FEFE, 0x1FC] +STATIC_DATA_362 = [0xFFF7FEFE, 0x1FC] +STATIC_DATA_363 = [0xFFF7FEFE, 0x1FC0000] +STATIC_DATA_364 = [0xFFF7FEFE, 0x1FC0000] +STATIC_DATA_365 = [0xFFF7FE7F, 0xFBFF3FC] +STATIC_DATA_366 = [0xFFF7FE7F, 0xFBFF3FC] +static_data_list = [STATIC_DATA_361, STATIC_DATA_362, STATIC_DATA_363, STATIC_DATA_364, STATIC_DATA_365, STATIC_DATA_366] + +# GEN1 radar interceptor +def create_radar_command(packer, frame, active, CS, hold): + #accel = 0 + ret = [] + crz_ctrl = CS.crz_cntr + crz_info = CS.crz_info + + # if CC.longActive: # this is set true in longcontrol.py + # accel = CC.actuators.accel * 1150 + # accel = accel if accel < 1000 else 1000 + # else: + # accel = int(crz_info["ACCEL_CMD"]) + + crz_info["ACC_ACTIVE"] = active + crz_info["ACC_SET_ALLOWED"] = int(bool(int(CS.cp.vl["GEAR"]["GEAR"]) & 4)) # we can set ACC_SET_ALLOWED bit when in drive. Allows crz to be set from 1kmh. + crz_info["CRZ_ENDED"] = 0 # this should keep acc on down to 5km/h on my 2018 M3 + #crz_info["ACCEL_CMD"] = accel + crz_info["STOPPING_MAYBE"] = hold + crz_info["STOPPING_MAYBE2"] = hold + + crz_ctrl["CRZ_ACTIVE"] = active + crz_ctrl["ACC_ACTIVE_2"] = active + crz_ctrl["DISABLE_TIMER_1"] = 0 + crz_ctrl["DISABLE_TIMER_2"] = 0 + + ret.append(packer.make_can_msg("CRZ_INFO", 0, crz_info)) + ret.append(packer.make_can_msg("CRZ_CTRL", 0, crz_ctrl)) + # convert steering angle to radar units and clip to range + steer_angle = (CS.out.steeringAngleDeg *-17.4) + 2048 + + if (frame % 10 == 0): + for i, addr in enumerate(range(361,367)): + addr_name = f"RADAR_{addr}" + msg = CS.cp_cam.vl[addr_name] + values = { + "MSGS_1" : static_data_list[i][0], + "MSGS_2" : static_data_list[i][1], + "CTR" : int(msg["CTR"]) #frame % 16 + } + if addr == 361: + values.update({ + "INVERSE_SPEED" : int(CS.out.vEgo * -4.4), + "BIT" : 1, + }) + if addr == 362: + values.update({ + "CLIPPED_STEER_ANGLE" : int(clip(steer_angle, 0, 4092)), + }) + ret.append(packer.make_can_msg(addr_name, 0, values)) + + return ret + +# GEN2 new mazdas +def create_acc_cmd(packer, values, hold, resume): + msg_name = "ACC" + bus = 2 + + if (values["ACC_ENABLED"]): + values["HOLD"] = hold + values["RESUME"] = resume + else: + pass + + return packer.make_can_msg(msg_name, bus, values) + +def mazda2019_checksum(address: int, sig, d: bytearray) -> int: + checksum = 0 + if address == 0x220: + checksum = 0x2a + if address == 0x249: + checksum = 0x53 + # Simple sum over the payload, except for the byte where the checksum lives. + for i in range(7): + checksum += d[i] + return checksum % 256 + +def mazda2017_checksum(address: int, sig, d: bytearray) -> int: + sum_val = 0 + if d[5] & 0x5: + sum_val = 0xFC + for i in range(len(d) - 1): + sum_val += d[i] + return (~sum_val) & 0xFF \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/mazda/radar_interface.py b/opendbc_repo/opendbc/car/mazda/radar_interface.py new file mode 100644 index 000000000..9f870c8a5 --- /dev/null +++ b/opendbc_repo/opendbc/car/mazda/radar_interface.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +import math + +from opendbc.can.parser import CANParser +from opendbc.car import Bus, structs +from opendbc.car.interfaces import RadarInterfaceBase +from opendbc.car.mazda.values import DBC, MazdaSafetyFlags + +def get_radar_can_parser(CP): + if Bus.radar not in DBC[CP.carFingerprint]: + return None + if not CP.flags & MazdaSafetyFlags.RADAR_INTERCEPTOR: + return None + + messages = [(f"RADAR_TRACK_{addr}", 10) for addr in range(361,367)] + return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 2) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.updated_messages = set() + self.track_id = 0 + + self.radar_off_can = CP.radarUnavailable + self.rcp = get_radar_can_parser(CP) + + def update(self, can_strings): + if self.radar_off_can or (self.rcp is None): + return super().update(None) + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + rr = self._update(self.updated_messages) + self.updated_messages.clear() + + return rr + + def _update(self, updated_messages): + ret = structs.RadarData() + if self.rcp is None: + return ret + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + for addr in range(361,367): + msg = self.rcp.vl[f"RADAR_TRACK_{addr}"] + if addr not in self.pts: + self.pts[addr] = structs.RadarData.RadarPoint() + self.pts[addr].trackId = self.track_id + self.track_id += 1 + valid = (msg['DIST_OBJ'] != 4095) and (msg['ANG_OBJ'] != 2046) and (msg['RELV_OBJ'] != -16) + if valid: + azimuth = math.radians(msg['ANG_OBJ']/64) + self.pts[addr].measured = True + self.pts[addr].dRel = msg['DIST_OBJ']/16 + self.pts[addr].yRel = -math.sin(azimuth) * msg['DIST_OBJ']/16 + self.pts[addr].vRel = msg['RELV_OBJ']/16 + self.pts[addr].aRel = float('nan') + self.pts[addr].yvRel = float('nan') + else: + del self.pts[addr] + ret.points = list(self.pts.values()) + return ret \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/mazda/values.py b/opendbc_repo/opendbc/car/mazda/values.py index e85b352b8..0e3a92f66 100644 --- a/opendbc_repo/opendbc/car/mazda/values.py +++ b/opendbc_repo/opendbc/car/mazda/values.py @@ -9,21 +9,39 @@ from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = CarParams.Ecu - -# Steer torque limits - class CarControllerParams: - STEER_MAX = 800 # theoretical max_steer 2047 - STEER_DELTA_UP = 10 # torque increase per refresh - STEER_DELTA_DOWN = 25 # torque decrease per refresh - STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting - STEER_DRIVER_MULTIPLIER = 1 # weight driver torque - STEER_DRIVER_FACTOR = 1 # from dbc - STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor - STEER_STEP = 1 # 100 Hz - def __init__(self, CP): - pass + self.STEER_STEP = 1 # 100 Hz + if CP.flags & MazdaSafetyFlags.GEN1: + self.STEER_MAX = 800 # theoretical max_steer 2047 + self.STEER_DELTA_UP = 10 # torque increase per refresh + self.STEER_DELTA_DOWN = 25 # torque decrease per refresh + self.STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting + self.STEER_DRIVER_MULTIPLIER = 40 # weight driver torque + self.STEER_DRIVER_FACTOR = 1 # from dbc + self.STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + + self.TI_STEER_MAX = 600 # theoretical max_steer 2047 + self.TI_STEER_DELTA_UP = 6 # torque increase per refresh + self.TI_STEER_DELTA_DOWN = 15 # torque decrease per refresh + self.TI_STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting + self.TI_STEER_DRIVER_MULTIPLIER = 40 # weight driver torque + self.TI_STEER_DRIVER_FACTOR = 1 # from dbc + self.TI_STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + if CP.flags & (MazdaSafetyFlags.GEN2 | MazdaSafetyFlags.GEN3): + self.STEER_MAX = 8000 + self.STEER_DELTA_UP = 45 # torque increase per refresh + self.STEER_DELTA_DOWN = 80 # torque decrease per refresh + self.STEER_DRIVER_ALLOWANCE = 1400 # allowed driver torque before start limiting + self.STEER_DRIVER_MULTIPLIER = 5 # weight driver torque + self.STEER_DRIVER_FACTOR = 1 # from dbc + self.STEER_ERROR_MAX = 3500 # max delta between torque cmd and torque motor + +class TI_STATE: + DISCOVER = 0 + OFF = 1 + DRIVER_OVER = 2 + RUN = 3 @dataclass @@ -37,42 +55,85 @@ class MazdaCarSpecs(CarSpecs): tireStiffnessFactor: float = 0.7 # not optimized yet -class MazdaFlags(IntFlag): - # Static flags - # Gen 1 hardware: same CAN messages and same camera +class MazdaSafetyFlags(IntFlag): + # Safety flags GEN1 = 1 + GEN2 = 2 + GEN3 = 4 + TORQUE_INTERCEPTOR = 8 + RADAR_INTERCEPTOR = 16 + NO_FSC = 32 + NO_MRCC = 64 + MANUAL_TRANSMISSION = 128 @dataclass class MazdaPlatformConfig(PlatformConfig): dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'mazda_2017'}) - flags: int = MazdaFlags.GEN1 + def init(self): + if self.flags & MazdaSafetyFlags.GEN2: + self.dbc_dict = {Bus.pt: 'mazda_2019'} + elif self.flags & MazdaSafetyFlags.GEN1 and self.flags & MazdaSafetyFlags.RADAR_INTERCEPTOR: + self.dbc_dict = {Bus.pt: 'mazda_2017', Bus.radar: 'mazda_radar'} + elif self.flags & MazdaSafetyFlags.GEN3: + self.dbc_dict = {Bus.pt: 'mazda_2023'} class CAR(Platforms): MAZDA_CX5 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-5 2017-21")], - MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5) + MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5), + flags=MazdaSafetyFlags.GEN1, ) MAZDA_CX9 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-9 2016-20")], - MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6) + MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6), + flags=MazdaSafetyFlags.GEN1, ) MAZDA_3 = MazdaPlatformConfig( [MazdaCarDocs("Mazda 3 2017-18")], - MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0) + MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0), + flags=MazdaSafetyFlags.GEN1, ) MAZDA_6 = MazdaPlatformConfig( [MazdaCarDocs("Mazda 6 2017-20")], - MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5) + MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5), + flags=MazdaSafetyFlags.GEN1, ) MAZDA_CX9_2021 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-9 2021-23", video="https://youtu.be/dA3duO4a0O4")], - MAZDA_CX9.specs + MAZDA_CX9.specs, + flags=MazdaSafetyFlags.GEN1, ) MAZDA_CX5_2022 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-5 2022-25")], MAZDA_CX5.specs, + flags=MazdaSafetyFlags.GEN1, + ) + MAZDA_3_2019 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda 3 2019-24")], + MazdaCarSpecs(mass=3000 * CV.LB_TO_KG, wheelbase=2.725, steerRatio=18.8), + flags=MazdaSafetyFlags.GEN2, + ) + MAZDA_CX_30 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda CX-30 2019-24")], + MazdaCarSpecs(mass=3375 * CV.LB_TO_KG, wheelbase=2.814, steerRatio=15.5), + flags=MazdaSafetyFlags.GEN2, + ) + MAZDA_CX_50 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda CX-50 2022-24")], + MazdaCarSpecs(mass=3375 * CV.LB_TO_KG, wheelbase=2.814, steerRatio=15.5), + flags=MazdaSafetyFlags.GEN2, + ) + MAZDA_3_2023 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda 3 2024-26")], + MazdaCarSpecs(mass=3000 * CV.LB_TO_KG, wheelbase=2.725, steerRatio=18.8), + flags=MazdaSafetyFlags.GEN3, + ) + MAZDA_CX_30_2023 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda CX-30 23-26")], + MazdaCarSpecs(mass=3375 * CV.LB_TO_KG, wheelbase=2.814, steerRatio=15.5), + flags=MazdaSafetyFlags.GEN3, ) @@ -80,6 +141,9 @@ class LKAS_LIMITS: STEER_THRESHOLD = 15 DISABLE_SPEED = 45 # kph ENABLE_SPEED = 52 # kph + TI_STEER_THRESHOLD = 6 + TI_DISABLE_SPEED = 0 # kph + TI_ENABLE_SPEED = 0 # kph class Buttons: @@ -88,6 +152,7 @@ class Buttons: SET_MINUS = 2 RESUME = 3 CANCEL = 4 + TURN_ON = 5 FW_QUERY_CONFIG = FwQueryConfig( @@ -98,7 +163,21 @@ FW_QUERY_CONFIG = FwQueryConfig( [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], bus=0, ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine], + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + bus=0, + whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire], + ) ], ) DBC = CAR.create_dbc_map() +GEN1 = CAR.with_flags(MazdaSafetyFlags.GEN1) +GEN2 = CAR.with_flags(MazdaSafetyFlags.GEN2) +GEN3 = CAR.with_flags(MazdaSafetyFlags.GEN3) \ No newline at end of file