mazda part1

common/params.cc

@@ -561,6 +561,12 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"WheelIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"WheelSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"WheelToDownload", PERSISTENT},
+
+    {"RecordRoad", PERSISTENT},
+    {"TorqueInterceptorEnabled", PERSISTENT},
+    {"RadarInterceptorEnabled", PERSISTENT},
+    {"NoMRCC", PERSISTENT},
+    {"NoFSC", PERSISTENT},
 };
 
 } // namespace

common/realtime.py

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

selfdrive/car/__init__.py

@@ -103,6 +103,28 @@ def apply_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_tor
 
   return int(round(float(apply_torque)))
 
+#alternate settings when using torque interceptor. May or may not be useful to some users/branches.
+def apply_ti_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS):
+
+  # limits due to driver torque
+  driver_max_torque = LIMITS.TI_STEER_MAX + (LIMITS.TI_STEER_DRIVER_ALLOWANCE +
+                                             driver_torque * LIMITS.TI_STEER_DRIVER_FACTOR) * LIMITS.TI_STEER_DRIVER_MULTIPLIER
+  driver_min_torque = -LIMITS.TI_STEER_MAX + (-LIMITS.TI_STEER_DRIVER_ALLOWANCE + driver_torque *
+                                              LIMITS.TI_STEER_DRIVER_FACTOR) * LIMITS.TI_STEER_DRIVER_MULTIPLIER
+  max_steer_allowed = max(min(LIMITS.TI_STEER_MAX, driver_max_torque), 0)
+  min_steer_allowed = min(max(-LIMITS.TI_STEER_MAX, driver_min_torque), 0)
+  apply_torque = clip(apply_torque, min_steer_allowed, max_steer_allowed)
+
+  # slow rate if steer torque increases in magnitude
+  if apply_torque_last > 0:
+    apply_torque = clip(apply_torque, max(apply_torque_last - LIMITS.TI_STEER_DELTA_DOWN, -LIMITS.TI_STEER_DELTA_UP),
+                        apply_torque_last + LIMITS.TI_STEER_DELTA_UP)
+  else:
+    apply_torque = clip(apply_torque, apply_torque_last - LIMITS.TI_STEER_DELTA_UP,
+                        min(apply_torque_last + LIMITS.TI_STEER_DELTA_DOWN, LIMITS.TI_STEER_DELTA_UP))
+
+  return int(round(float(apply_torque)))
+
 
 def apply_dist_to_meas_limits(val, val_last, val_meas,
                               STEER_DELTA_UP, STEER_DELTA_DOWN,

selfdrive/car/mazda/carcontroller.py

@@ -1,31 +1,45 @@
 from cereal import car
 from opendbc.can.packer import CANPacker
-from openpilot.selfdrive.car import apply_driver_steer_torque_limits
+from openpilot.selfdrive.car import apply_driver_steer_torque_limits, apply_ti_steer_torque_limits
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.mazda import mazdacan
-from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
+from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons, MazdaFlags
+from openpilot.common.realtime import ControlsTimer as Timer
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
 class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
     self.apply_steer_last = 0
+    self.ti_apply_steer_last = 0
     self.packer = CANPacker(dbc_name)
     self.brake_counter = 0
     self.frame = 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, frogpilot_toggles):
     can_sends = []
 
     apply_steer = 0
+    ti_apply_steer = 0
 
     if CC.latActive:
       # calculate steer and also set limits due to driver torque
-      new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX))
+      new_steer = int(round(CC.actuators.steer * self.ccp.STEER_MAX))
       apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last,
-                                                     CS.out.steeringTorque, CarControllerParams)
+                                                     CS.out.steeringTorque, self.ccp)
+      if self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR:
+        if CS.ti_lkas_allowed:
+          ti_new_steer = int(round(CC.actuators.steer * self.ccp.TI_STEER_MAX))
+          ti_apply_steer = apply_ti_steer_torque_limits(ti_new_steer, self.ti_apply_steer_last,
+                                                    CS.out.steeringTorque, self.ccp)
 
     if CC.cruiseControl.cancel:
       # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
@@ -45,22 +59,59 @@ class CarController(CarControllerBase):
         can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
 
     self.apply_steer_last = apply_steer
+    self.ti_apply_steer_last = ti_apply_steer
+    if self.CP.flags & MazdaFlags.GEN1:
+      # 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 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.flags & MazdaFlags.RADAR_INTERCEPTOR:
+        hold = False
+        if CS.out.standstill:
+          hold = self.hold_timer.active()
+        else:
+          self.hold_timer.reset()
+        if self.frame % 2 == 0:
+          can_sends.extend(mazdacan.create_radar_command(self.packer, self.frame, CC, CS, hold))
+
+    else:
+      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 or CC.cruiseControl.override or CS.out.gasPressed or
+                CC.actuators.longControlState == LongCtrlState.starting): # and we want to resume
+              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, CC.actuators.accel, hold, resume))
 
     # 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_steer, CS.cam_lkas))
 
     new_actuators = CC.actuators.as_builder()
-    new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
+    new_actuators.steer = apply_steer / self.ccp.STEER_MAX
     new_actuators.steerOutputCan = apply_steer
 
     self.frame += 1
+    Timer.tick()
     return new_actuators, can_sends

selfdrive/car/mazda/carstate.py

@@ -1,9 +1,10 @@
+import copy
 from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import CarStateBase
-from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags
+from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, TI_STATE, CAR, CarControllerParams
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -17,11 +18,26 @@ class CarState(CarStateBase):
     self.low_speed_alert = False
     self.lkas_allowed_speed = False
     self.lkas_disabled = False
+    self.cam_lkas = 0
+    self.params = CarControllerParams(CP)
 
     self.prev_distance_button = 0
     self.distance_button = 0
 
-  def update(self, cp, cp_cam, frogpilot_toggles):
+    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.update = self.update_gen1
+    if CP.flags & MazdaFlags.GEN1:
+      self.update = self.update_gen1
+    if CP.flags & MazdaFlags.GEN2:
+      self.update = self.update_gen2
+
+  def update_gen1(self, cp, cp_cam, cp_body, frogpilot_variables):
 
     ret = car.CarState.new_message()
     fp_ret = custom.FrogPilotCarState.new_message()
@@ -51,6 +67,22 @@ 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 & MazdaFlags.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
@@ -85,16 +117,25 @@ 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.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
+    if self.CP.flags & MazdaFlags.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 & MazdaFlags.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
 
-    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
+    if self.CP.carFingerprint not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021):
+      ret.steerFaultTemporary = cp.vl["STEER_RATE"]["HANDS_OFF_5_SECONDS"] == 1
+    else:
+      ret.steerFaultTemporary = False
 
     # 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
@@ -106,10 +147,58 @@ class CarState(CarStateBase):
     self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
 
     # camera signals
-    self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
+    self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 if not self.CP.flags & MazdaFlags.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
+    ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 if not self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR else False
+    self.cp_cam = cp_cam
+    self.cp = cp
+    return ret, fp_ret
+
+  def update_gen2(self, cp, cp_cam, cp_body, frogpilot_variables):
+    ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
+
+    ret.wheelSpeeds = self.get_wheel_speeds(
+        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.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
+    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # Doesn't match cluster speed exactly
+
+    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.steeringAngleDeg = cp_cam.vl["STEER"]["STEER_ANGLE"]
+
+    ret.steeringTorque = cp_body.vl["EPS_FEEDBACK"]["STEER_TORQUE_SENSOR"]
+    can_gear = int(cp_cam.vl["GEAR"]["GEAR"])
+    ret.gas = cp_cam.vl["ENGINE_DATA"]["PEDAL_GAS"]
+
+    unit_conversion = CV.MPH_TO_MS if cp.vl["SYSTEM_SETTINGS"]["IMPERIAL_UNIT"] else CV.KPH_TO_MS
+
+    ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_DRIVER_ALLOWANCE
+    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
+    ret.gasPressed = ret.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_PEDAL_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
+
+    ret.standstill = cp_cam.vl["SPEED"]["SPEED"] * unit_conversion == 0.0
+    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)
+    ret.cruiseState.standstill = ret.standstill
+
+    self.cp = cp
+    self.cp_cam = cp_cam
+    self.acc = copy.copy(cp.vl["ACC"])
 
     # FrogPilot CarState functions
     self.lkas_previously_enabled = self.lkas_enabled
@@ -117,21 +206,38 @@ class CarState(CarStateBase):
 
     return ret, fp_ret
 
+  @staticmethod
+  def get_ti_messages(CP):
+    messages = []
+    if CP.flags & (MazdaFlags.TORQUE_INTERCEPTOR | MazdaFlags.GEN1):
+      messages += [
+        ("TI_FEEDBACK", 50),
+      ]
+    elif CP.flags & MazdaFlags.GEN2:
+      messages += [
+        ("EPS_FEEDBACK", 50),
+        ("EPS_FEEDBACK2", 50),
+        ("EPS_FEEDBACK3", 50),
+      ]
+    return messages
+
   @staticmethod
   def get_can_parser(CP):
-    messages = [
-      # sig_address, frequency
-      ("BLINK_INFO", 10),
-      ("STEER", 67),
-      ("STEER_RATE", 83),
-      ("STEER_TORQUE", 83),
-      ("WHEEL_SPEEDS", 100),
-    ]
+    messages = []
+    if not (CP.flags & MazdaFlags.GEN2):
+      messages += [
+        # sig_address, frequency
+        ("BLINK_INFO", 10),
+        ("STEER", 67),
+        ("STEER_RATE", 83),
+        ("STEER_TORQUE", 83),
+        ("WHEEL_SPEEDS", 100),
+      ]
 
     if CP.flags & MazdaFlags.GEN1:
+      messages += CarState.get_ti_messages(CP)
       messages += [
         ("ENGINE_DATA", 100),
-        ("CRZ_CTRL", 50),
         ("CRZ_EVENTS", 50),
         ("CRZ_BTNS", 10),
         ("PEDALS", 50),
@@ -142,6 +248,21 @@ class CarState(CarStateBase):
         ("BSM", 10),
       ]
 
+      if not (CP.flags & MazdaFlags.RADAR_INTERCEPTOR) and not (CP.flags & MazdaFlags.NO_MRCC):
+        messages += [
+          ("CRZ_CTRL", 50),
+        ]
+
+    if CP.flags & MazdaFlags.GEN2:
+      messages += [
+        ("BRAKE_PEDAL", 20),
+        ("CRUZE_STATE", 10),
+        ("BLINK_INFO", 10),
+        ("ACC", 50),
+        ("CRZ_BTNS", 10),
+        ("SYSTEM_SETTINGS", 10),
+      ]
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
@@ -150,9 +271,34 @@ class CarState(CarStateBase):
 
     if CP.flags & MazdaFlags.GEN1:
       messages += [
-        # sig_address, frequency
+        #  address, frequency
         ("CAM_LANEINFO", 2),
         ("CAM_LKAS", 16),
       ]
 
+      if CP.flags & MazdaFlags.RADAR_INTERCEPTOR:
+        messages += [
+          ("CRZ_INFO", 50),
+          ("CRZ_CTRL", 50),
+        ]
+        for addr in range(361,367):
+          msg = f"RADAR_{addr}"
+          messages += [
+            (msg,10),
+          ]
+
+    if CP.flags & MazdaFlags.GEN2:
+      messages += [
+        ("ENGINE_DATA", 100),
+        ("STEER_TORQUE", 100),
+        ("GEAR", 40),
+        ("WHEEL_SPEEDS", 100),
+        ("STEER", 100),
+        ("SPEED", 50),
+      ]
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
+
+  @staticmethod
+  def get_body_can_parser(CP):
+    return CANParser(DBC[CP.carFingerprint]["pt"], CarState.get_ti_messages(CP), 1)

selfdrive/car/mazda/fingerprints.py

@@ -262,4 +262,94 @@ FW_VERSIONS = {
       b'PXM7-21PS1-B\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',
+    ],
+    (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',
+    ],
+  },
+
+  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',
+    ],
+  },
 }

selfdrive/car/mazda/interface.py

@@ -1,9 +1,11 @@
 #!/usr/bin/env python3
 from cereal import car, custom
+from panda import Panda
 from openpilot.common.conversions import Conversions as CV
-from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS
+from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS, MazdaFlags, GEN1, GEN2
 from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.common.params import Params
 
 ButtonType = car.CarState.ButtonEvent.Type
 FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
@@ -17,12 +19,52 @@ class CarInterface(CarInterfaceBase):
     ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
     ret.radarUnavailable = True
 
-    ret.steerActuatorDelay = 0.1
+    ret.dashcamOnly = False
+    if candidate in GEN1:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_GEN1
+      p = Params()
+      if p.get("TorqueInterceptorEnabled"): # Torque Interceptor Installed
+        print("Torque Interceptor Installed")
+        ret.flags |= MazdaFlags.TORQUE_INTERCEPTOR.value
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_TORQUE_INTERCEPTOR
+      if p.get("RadarInterceptorEnabled"): # Radar Interceptor Installed
+        ret.flags |= MazdaFlags.RADAR_INTERCEPTOR.value
+        ret.experimentalLongitudinalAvailable = 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]
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_RADAR_INTERCEPTOR
+
+      if p.get("NoMRCC"): # No Mazda Radar Cruise Control; Missing CRZ_CTRL signal
+        ret.flags |= MazdaFlags.NO_MRCC.value
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_NO_MRCC
+      if p.get("NoFSC"):  # No Front Sensing Camera
+        ret.flags |= MazdaFlags.NO_FSC.value
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_NO_FSC
+
+      ret.steerActuatorDelay = 0.1
+
+    if candidate in GEN2:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_GEN2
+      ret.experimentalLongitudinalAvailable = True
+      ret.openpilotLongitudinalControl = experimental_long
+      ret.stopAccel = -.5
+      ret.vEgoStarting = .2
+      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.3
+
     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, ) and not ret.flags & MazdaFlags.TORQUE_INTERCEPTOR:
       ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
 
     ret.centerToFront = ret.wheelbase * 0.41
@@ -31,8 +73,7 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c, frogpilot_toggles):
-    ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
-
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_toggles)
      # TODO: add button types for inc and dec
     ret.buttonEvents = [
       *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
@@ -42,10 +83,16 @@ class CarInterface(CarInterfaceBase):
     # events
     events = self.create_common_events(ret)
 
-    if self.CS.lkas_disabled:
-      events.add(EventName.lkasDisabled)
-    elif self.CS.low_speed_alert:
-      events.add(EventName.belowSteerSpeed)
+    if self.CP.flags & MazdaFlags.GEN1:
+      if self.CS.lkas_disabled:
+        events.add(EventName.lkasDisabled)
+      elif self.CS.low_speed_alert:
+        events.add(EventName.belowSteerSpeed)
+
+      if not self.CS.acc_active_last and not self.CS.ti_lkas_allowed:
+        events.add(EventName.steerTempUnavailable)
+      if not self.CS.ti_lkas_allowed and self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR:
+        events.add(EventName.steerTempUnavailable) # torqueInterceptorTemporaryWarning
 
     ret.events = events.to_msg()
 

selfdrive/car/mazda/mazdacan.py

@@ -1,66 +1,107 @@
 from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags
-
+from openpilot.common.params import Params
+from openpilot.common.numpy_fast import clip
 
 def create_steering_control(packer, CP, frame, apply_steer, lkas):
-
-  tmp = apply_steer + 2048
-
-  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"])
-
-  # 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
-
-  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
-
-  if ahi == 1:
-    csum = csum + 15
-
-  if csum < 0:
-    if csum < -256:
-      csum = csum + 512
-    else:
-      csum = csum + 256
-
-  csum = csum % 256
-
-  values = {}
+  msgs = []
   if CP.flags & MazdaFlags.GEN1:
+    tmp = apply_steer + 2048
+
+    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"])
+
+    # 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
+
+    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
+
+    if ahi == 1:
+      csum = csum + 15
+
+    if csum < 0:
+      if csum < -256:
+        csum = csum + 512
+      else:
+        csum = csum + 256
+
+    csum = csum % 256
+    values = {}
+    if CP.flags & MazdaFlags.GEN1:
+      values = {
+        "LKAS_REQUEST": apply_steer,
+        "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))
+
+      if CP.flags & MazdaFlags.TORQUE_INTERCEPTOR:
+        values = {
+            "LKAS_REQUEST"     : apply_steer,
+            "CHKSUM"           : apply_steer,
+            "KEY"              : 3294744160
+        }
+        msgs.append(packer.make_can_msg("CAM_LKAS2", 1, values))
+
+  elif CP.flags & MazdaFlags.GEN2:
+    bus = 1
+    sig_name = "EPS_LKAS"
     values = {
       "LKAS_REQUEST": apply_steer,
-      "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": 12000,
     }
+    msgs.append(packer.make_can_msg(sig_name, bus, values))
 
-  return packer.make_can_msg("CAM_LKAS", 0, values)
+  return msgs
 
+def create_ti_steering_control(packer, CP, apply_steer):
+
+  key = 3294744160
+  chksum = apply_steer
+
+  if CP.flags & MazdaFlags.GEN1:
+    values = {
+        "LKAS_REQUEST"     : apply_steer,
+        "CHKSUM"           : chksum,
+        "KEY"              : key
+     }
+  # TODO
+  # 1. Add new CAR values for MDARS Mazdas so that we can change the rate of the message. This will take some work.
+  # 2. Listen for reply's on both CAN buses if not MDARS version of
+  # Mazda (2021+ or m3 2019+) and warn the user if there is a bad connection
+  # but do not cause disengagment
+
+  # Write to both buses for *future* redundancy, but we only check bus 1 for a response in carstate and safey_mazda.h for now.
+  # if (frame % 2 == 0):
+  #  commands.append(packer.make_can_msg("CAM_LKAS2", 0, values))
+
+  return packer.make_can_msg("CAM_LKAS2", 1, values)
 
 def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):
   values = {s: cam_msg[s] for s in [
@@ -126,3 +167,78 @@ 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]
+
+def create_radar_command(packer, frame, CC, 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 * 1170
+    accel = accel if accel < 1000 else 1000
+  else:
+    accel = int(crz_info["ACCEL_CMD"])
+
+  crz_info["ACC_ACTIVE"] = int(CC.longActive)
+  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"] = int(CC.longActive)
+  crz_ctrl["ACC_ACTIVE_2"] = int(CC.longActive)
+  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
+
+def create_acc_cmd(packer, values, accel, hold, resume):
+  msg_name = "ACC"
+  bus = 2
+
+  if (values["ACC_ENABLED"]):
+    if Params().get_bool("ExperimentalLongitudinalEnabled"):
+      values["ACCEL_CMD"] = (accel * 240) + 2000
+    values["HOLD"] = hold
+    values["RESUME"] = resume
+  else:
+    pass
+
+  return packer.make_can_msg(msg_name, bus, values)
+

selfdrive/car/mazda/radar_interface.py

@@ -1,5 +1,60 @@
 #!/usr/bin/env python3
+import math
+
+from cereal import car
+from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.mazda.values import DBC
+
+def get_radar_can_parser(CP):
+  if DBC[CP.carFingerprint]['radar'] is None:
+    return None
+  messages = [(f"RADAR_TRACK_{addr}", 10) for addr in range(361,367)]
+  return CANParser(DBC[CP.carFingerprint]['radar'], messages, 2)
 
 class RadarInterface(RadarInterfaceBase):
-  pass
+  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 = car.RadarData.new_message()
+    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] = car.RadarData.RadarPoint.new_message()
+        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

selfdrive/car/mazda/values.py

@@ -13,18 +13,38 @@ Ecu = car.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 & MazdaFlags.GEN1:
+      self.STEER_MAX = 600                # 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 & MazdaFlags.GEN2:
+      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
 class MazdaCarDocs(CarDocs):
@@ -41,30 +61,43 @@ class MazdaFlags(IntFlag):
   # Static flags
   # Gen 1 hardware: same CAN messages and same camera
   GEN1 = 1
-
+  GEN2 = 2
+  TORQUE_INTERCEPTOR = 4
+  RADAR_INTERCEPTOR = 8
+  NO_FSC = 16
+  NO_MRCC = 32
 
 @dataclass
 class MazdaPlatformConfig(PlatformConfig):
   dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('mazda_2017', None))
-  flags: int = MazdaFlags.GEN1
+  def init(self):
+    if self.flags & MazdaFlags.GEN2:
+      self.dbc_dict = dbc_dict('mazda_2019', None)
+    elif self.flags & MazdaFlags.GEN1 and self.flags & MazdaFlags.RADAR_INTERCEPTOR:
+      self.dbc_dict = dbc_dict('mazda_2017', 'mazda_radar')
+
 
 
 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=MazdaFlags.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=MazdaFlags.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=MazdaFlags.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=MazdaFlags.GEN1,
   )
   MAZDA_CX9_2021 = MazdaPlatformConfig(
     [MazdaCarDocs("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4")],
@@ -74,13 +107,30 @@ class CAR(Platforms):
     [MazdaCarDocs("Mazda CX-5 2022-24")],
     MAZDA_CX5.specs,
   )
+  MAZDA_3_2019 = MazdaPlatformConfig(
+    [MazdaCarDocs("Mazda 3 2019-24")],
+    MazdaCarSpecs(mass=3000 * CV.LB_TO_KG, wheelbase=2.725, steerRatio=17.0),
+    flags=MazdaFlags.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=MazdaFlags.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=MazdaFlags.GEN2,
+  )
 
 
 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:
   NONE = 0
@@ -88,6 +138,7 @@ class Buttons:
   SET_MINUS = 2
   RESUME = 3
   CANCEL = 4
+  TURN_ON = 5
 
 
 FW_QUERY_CONFIG = FwQueryConfig(
@@ -98,7 +149,20 @@ 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(MazdaFlags.GEN1)
+GEN2 = CAR.with_flags(MazdaFlags.GEN2)

selfdrive/car/torque_data/override.toml

@@ -79,3 +79,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 # Manually checked
 "HONDA_CIVIC_2022" = [2.5, 1.2, 0.15]
 "HONDA_HRV_3G" = [2.5, 1.2, 0.2]
+
+"MAZDA_3_2019" = [1.45, 3.0, 0.33]
+"MAZDA_CX_30" = [1.45, 3.0, 0.33]
+"MAZDA_CX_50" = [1.45, 3.0, 0.33]

selfdrive/ui/qt/offroad/settings.cc

@@ -47,6 +47,30 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       tr("Experimental Mode"),
       "",
       "../assets/img_experimental_white.svg",
+    },
+        {
+      "TorqueInterceptorEnabled",
+      tr("Torque Interceptor Installed"),
+      tr("Enable the torque interceptor to control the steering wheel."),
+      "../assets/offroad/icon_openpilot.png",
+    },
+    {
+      "RadarInterceptorEnabled",
+      tr("Radar Interceptor Installed"),
+      tr("Enable if you have installed the radar Iterceptor."),
+      "../assets/offroad/icon_openpilot.png",
+    },
+    {
+      "NoMRCC",
+      tr("Car Does not have stock MRCC"),
+      tr("Enable if your car does not have stock MRCC."),
+      "../assets/offroad/icon_openpilot.png",
+    },
+    {
+      "NoFSC",
+      tr("Car Does not have stock FSC"),
+      tr("Enable if your car does not have stock FSC."),
+      "../assets/offroad/icon_openpilot.png",
     },
     {
       "DisengageOnAccelerator",
@@ -66,6 +90,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       tr("Upload data from the driver facing camera and help improve the driver monitoring algorithm."),
       "../assets/offroad/icon_monitoring.png",
     },
+    {
+      "RecordBack",
+      tr("Record and Upload Road Cameras"),
+      tr("Upload data from the road cameras."),
+      "../assets/offroad/icon_monitoring.png",
+    },
     {
       "IsMetric",
       tr("Use Metric System"),
@@ -295,8 +325,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
 
   // Delete driving footage
   ButtonControl *deleteDrivingDataBtn = new ButtonControl(tr("Delete Driving Data"), tr("DELETE"), tr("This button provides a swift and secure way to permanently delete all "
-    "stored driving footage and data from your device. Ideal for maintaining privacy or freeing up space.")
-  );
+    "stored driving footage and data from your device. Ideal for maintaining privacy or freeing up space."));
   connect(deleteDrivingDataBtn, &ButtonControl::clicked, [=]() {
     if (ConfirmationDialog::confirm(tr("Are you sure you want to permanently delete all of your driving footage and data?"), tr("Delete"), this)) {
       std::thread([&] {

system/loggerd/loggerd.h

@@ -59,12 +59,14 @@ public:
 const EncoderInfo main_road_encoder_info = {
   .publish_name = "roadEncodeData",
   .filename = "fcamera.hevc",
+  .record = Params().getBool("RecordBack"),
   INIT_ENCODE_FUNCTIONS(RoadEncode),
 };
 
 const EncoderInfo main_wide_road_encoder_info = {
   .publish_name = "wideRoadEncodeData",
   .filename = "ecamera.hevc",
+  .record = Params().getBool("RecordBack"),
   INIT_ENCODE_FUNCTIONS(WideRoadEncode),
 };