Mazda opendbc

common/realtime.py

@@ -97,3 +97,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)

opendbc_repo/opendbc/can/dbc.py

@@ -17,6 +17,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:
@@ -35,6 +36,8 @@ class SignalType:
   PSA_CHECKSUM = 12
   VOLKSWAGEN_MLB_CHECKSUM = 13
   PEDAL_CHECKSUM = 14
+  MAZDA2017_CHECKSUM = 15
+  MAZDA2019_CHECKSUM = 16
 
 
 @dataclass
@@ -214,6 +217,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
 
 

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, starpilot_toggles):
     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

opendbc_repo/opendbc/car/mazda/carstate.py

@@ -1,9 +1,10 @@
+import copy
 from cereal import custom
 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
 
@@ -14,16 +15,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
 
-  def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
+    self._prev_steering_angle = 0
+
+  def update(self, can_parsers, starpilot_toggles) -> tuple[structs.CarState, custom.StarPilotCarState]:
+    if self.CP.flags & (MazdaSafetyFlags.GEN2 | MazdaSafetyFlags.GEN3):
+      ret = self.update_gen2(can_parsers)
+      fp_ret = custom.StarPilotCarState.new_message()
+      return ret, fp_ret
     cp = can_parsers[Bus.pt]
     cp_cam = can_parsers[Bus.cam]
+    cp_body = can_parsers[Bus.body]
 
     ret = structs.CarState()
 
@@ -50,9 +68,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"]
@@ -83,35 +115,50 @@ 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})
@@ -122,9 +169,65 @@ class CarState(CarStateBase):
 
     return ret, fp_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], [("CAM_TRAFFIC_SIGNS", 0)], 2),
     }

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
+  }],
 }

opendbc_repo/opendbc/car/mazda/interface.py

@@ -1,34 +1,138 @@
 #!/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
 
     ret.enableBsm = True
 
+    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
+
+    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

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

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

opendbc_repo/opendbc/car/mazda/values.py

@@ -9,20 +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_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
@@ -36,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,
   )
 
 
@@ -79,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:
@@ -87,6 +152,7 @@ class Buttons:
   SET_MINUS = 2
   RESUME = 3
   CANCEL = 4
+  TURN_ON = 5
 
 
 FW_QUERY_CONFIG = FwQueryConfig(
@@ -97,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)