mazda-9.4

common/params.cc

@@ -180,10 +180,12 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
     {"OpenpilotEnabledToggle", PERSISTENT},
     {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
+    {"OpenpilotLongitudinalControl", PERSISTENT},
     {"PandaSignatures", CLEAR_ON_MANAGER_START},
     {"Passive", PERSISTENT},
     {"PrimeType", PERSISTENT},
     {"RecordFront", PERSISTENT},
+    {"RecordRoad", PERSISTENT},
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
     {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"ShouldDoUpdate", CLEAR_ON_MANAGER_START},

common/realtime.py

@@ -93,3 +93,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.reset() if (self.timer == (self.max or self.min)) else None
+
+  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 = 0 # type: int
+  objects = [] # type: List[DurationTimer]
+  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/boardd/boardd.cc

@@ -380,6 +380,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
     ps.setSpiChecksumErrorCount(health.spi_checksum_error_count);
     ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f);
     ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f);
+    ps.setTorqueInterceptorDetected(health.torque_interceptor_detected_pkt);
 
     std::array<cereal::PandaState::PandaCanState::Builder, PANDA_CAN_CNT> cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
 

selfdrive/car/__init__.py

@@ -76,6 +76,25 @@ def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor
 def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, str]:
   return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc}
 
+#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_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS):
 

selfdrive/car/car_helpers.py

@@ -15,6 +15,7 @@ import cereal.messaging as messaging
 from openpilot.selfdrive.car import gen_empty_fingerprint
 
 FRAME_FINGERPRINT = 100  # 1s
+from openpilot.selfdrive.global_ti import TI
 
 EventName = car.CarEvent.EventName
 
@@ -23,7 +24,7 @@ def get_startup_event(car_recognized, controller_available, fw_seen):
   if is_comma_remote() and is_tested_branch():
     event = EventName.startup
   else:
-    event = EventName.startupMaster
+    event = EventName.startup
 
   if not car_recognized:
     if fw_seen:
@@ -189,6 +190,8 @@ def fingerprint(logcan, sendcan, num_pandas):
   cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached,
                  fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, fingerprints=finger,
                  fw_query_time=fw_query_time, error=True)
+  TI.saved_candidate = car_fingerprint
+  TI.saved_finger = finger
   return car_fingerprint, finger, vin, car_fw, source, exact_match
 
 
@@ -205,5 +208,13 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1):
   CP.carFw = car_fw
   CP.fingerprintSource = source
   CP.fuzzyFingerprint = not exact_match
+  
+  TI.saved_CarInterface = CarInterface
 
   return CarInterface(CP, CarController, CarState), CP
+
+def get_ti():
+  print("get_ti, entering get_params")
+  car_params = TI.saved_CarInterface.get_params(TI.saved_candidate, TI.saved_finger, list(), False, False)
+
+  return car_params

selfdrive/car/ford/values.py

@@ -100,26 +100,7 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
 }
 
 FW_QUERY_CONFIG = FwQueryConfig(
-  requests=[
-    # CAN and CAN FD queries are combined.
-    # FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus
-    # TODO: properly handle auxiliary requests to separate queries and add back whitelists
-    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],
-      # whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
-      bus=0,
-      auxiliary=True,
-    ),
-  ],
-  extra_ecus=[
-    (Ecu.shiftByWire, 0x732, None),
-  ],
+  requests=[],
 )
 
 FW_VERSIONS = {

selfdrive/car/interfaces.py

@@ -19,7 +19,7 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 ButtonType = car.CarState.ButtonEvent.Type
 GearShifter = car.CarState.GearShifter
 EventName = car.CarEvent.EventName
-TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, bool], float]
+TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, float, float, bool], float]
 
 MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
 ACCEL_MAX = 2.0
@@ -131,7 +131,8 @@ class CarInterfaceBase(ABC):
     return self.get_steer_feedforward_default
 
   def torque_from_lateral_accel_linear(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
-                                       lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float:
+                                       lateral_accel_error: float, lateral_accel_deadzone: float,
+                                       steering_angle: float, vego: float, friction_compensation: bool) -> float:
     # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
     friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
     return (lateral_accel_value / float(torque_params.latAccelFactor)) + friction
@@ -175,6 +176,10 @@ class CarInterfaceBase(ABC):
     ret.longitudinalActuatorDelayLowerBound = 0.15
     ret.longitudinalActuatorDelayUpperBound = 0.15
     ret.steerLimitTimer = 1.0
+
+    # No Torque Interceptor by default
+    ret.enableTorqueInterceptor = False
+
     return ret
 
   @staticmethod

selfdrive/car/mazda/carcontroller.py

@@ -1,65 +1,114 @@
 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.mazda import mazdacan
-from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
+from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons, GEN1
+from openpilot.common.realtime import ControlsTimer as Timer
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
 class CarController:
   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.params = CarControllerParams(CP)
+    self.hold_timer = Timer(6.0)
+    self.hold_delay = Timer(1.0) # delay before we start holding as to not hit the brakes too hard
+    self.resume_timer = Timer(0.5)
+    self.cancel_delay = Timer(0.07) # 70ms delay to try to avoid a race condition with stock system
+      
   def update(self, CC, CS, now_nanos):
     can_sends = []
 
     apply_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))
+      if CS.CP.enableTorqueInterceptor:
+        if CS.ti_lkas_allowed:
+          ti_new_steer = int(round(CC.actuators.steer * self.params.TI_STEER_MAX))
+          ti_apply_steer = apply_ti_steer_torque_limits(ti_new_steer, self.ti_apply_steer_last,
+                                                    CS.out.steeringTorque, self.params)
+
+      new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX))
       apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_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.carFingerprint, 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.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
-
+                                                     CS.out.steeringTorque, self.params)
     self.apply_steer_last = apply_steer
+    self.ti_apply_steer_last = ti_apply_steer
+    
+    if self.CP.carFingerprint in 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.carFingerprint, 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.carFingerprint, 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))
+      # 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
+        steer_required = CS.out.steerFaultTemporary
+        can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
+      
+      # send steering command if GEN1
+      #The ti cannot be detected unless OP sends a can message to it because the ti only transmits when it 
+      #sees the signature key in the designated address range.
+      can_sends.append(mazdacan.create_ti_steering_control(self.packer, self.CP.carFingerprint,
+                                                        self.frame, ti_apply_steer))
+      can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
+                                                      self.frame, apply_steer, CS.cam_lkas))
+    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 (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, self.packer, CS, CC, hold, resume))
 
-    # send steering command
-    can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
+      can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
                                                       self.frame, apply_steer, CS.cam_lkas))
 
     new_actuators = CC.actuators.copy()
-    new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
+    new_actuators.steer = apply_steer / self.params.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,11 @@
+import copy
+
 from cereal import car
 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, GEN1
+from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1, GEN2, TI_STATE, CAR, CarControllerParams
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -17,8 +19,69 @@ 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.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.carFingerprint in GEN1:
+      self.update = self.update_gen1
+    if CP.carFingerprint in GEN2:
+      self.update = self.update_gen2
 
-  def update(self, cp, cp_cam):
+  def update_gen2(self, cp, cp_cam, cp_body):
+    ret = car.CarState.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.cruiseState.available = True # TODO locate signal.
+    ret.cruiseState.speed = cp.vl["CRUZE_STATE"]["CRZ_SPEED"] * unit_conversion 
+    ret.cruiseState.enabled = ( (cp.vl["CRUZE_STATE"]["CRZ_ENABLED"] == 1) or (cp.vl["CRUZE_STATE"]["PRE_ENABLE"] == 1) )
+
+    speed_kph = cp_cam.vl["SPEED"]["SPEED"] * unit_conversion
+    ret.standstill = speed_kph < .1
+    ret.cruiseState.standstill = False
+    self.cp = cp
+    self.cp_cam = cp_cam
+    self.acc = copy.copy(cp.vl["ACC"])
+    
+    return ret
+    # end GEN2
+  
+  
+  def update_gen1(self, cp, cp_cam, cp_body):
 
     ret = car.CarState.new_message()
     ret.wheelSpeeds = self.get_wheel_speeds(
@@ -43,9 +106,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)
 
-    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
+    if self.CP.enableTorqueInterceptor:
+      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.steeringTorqueEps = cp.vl["STEER_TORQUE"]["STEER_TORQUE_MOTOR"]
     ret.steeringRateDeg = cp.vl["STEER_RATE"]["STEER_ANGLE_RATE"]
@@ -82,16 +159,11 @@ class CarState(CarStateBase):
     ret.cruiseState.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1
     ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
 
-    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
-
-    # 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
+    # On if no driver torque the last 5 seconds
+    if self.CP.carFingerprint not in (CAR.CX5_2022, CAR.CX9_2021): 
+      ret.steerFaultTemporary = cp.vl["STEER_RATE"]["HANDS_OFF_5_SECONDS"] == 1
+    else:
+      ret.steerFaultTemporary = False
 
     self.acc_active_last = ret.cruiseState.enabled
 
@@ -104,19 +176,39 @@ class CarState(CarStateBase):
     ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
     return ret
-
+  
+  @staticmethod
+  def get_ti_messages(CP):
+    messages = []
+    if CP.enableTorqueInterceptor and CP.carFingerprint in GEN1:
+      messages += [
+        ("TI_FEEDBACK", 50),
+      ]
+    elif CP.carFingerprint in 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 CP.carFingerprint not in GEN2:
+      messages += [
+        # sig_address, frequency
+        ("BLINK_INFO", 10),
+        ("STEER", 67),
+        ("STEER_RATE", 83),
+        ("STEER_TORQUE", 83),
+        ("WHEEL_SPEEDS", 100),
+      ]
 
     if CP.carFingerprint in GEN1:
+      # get real driver torque if we are using a torque interceptor
+      messages += CarState.get_ti_messages(CP)
       messages += [
         ("ENGINE_DATA", 100),
         ("CRZ_CTRL", 50),
@@ -129,6 +221,16 @@ class CarState(CarStateBase):
         ("GEAR", 20),
         ("BSM", 10),
       ]
+      
+    if CP.carFingerprint in 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)
 
@@ -143,4 +245,18 @@ class CarState(CarStateBase):
         ("CAM_LKAS", 16),
       ]
 
+    if CP.carFingerprint in 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/interface.py

@@ -1,24 +1,59 @@
 #!/usr/bin/env python3
 from cereal import car
 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, GEN2, GEN1
 from openpilot.selfdrive.car import get_safety_config
-from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD
+from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
+from openpilot.selfdrive.global_ti import TI
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 
 class CarInterface(CarInterfaceBase):
+  
+  @staticmethod
+  def torque_from_lateral_accel_mazda(lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
+                                           lateral_accel_error: float, lateral_accel_deadzone: float,
+                                           steering_angle: float, vego: float, friction_compensation: bool) -> float:
+    steering_angle = abs(steering_angle)
+    lat_factor = torque_params.latAccelFactor * ((steering_angle * torque_params.latAngleFactor) + 1)
+    
+    friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
+    return float(lateral_accel_value / lat_factor) + friction
+
+  def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
+    if self.CP.carFingerprint in GEN2:
+      return self.torque_from_lateral_accel_mazda
+    else:
+      return self.torque_from_lateral_accel_linear
 
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     ret.carName = "mazda"
-    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
+
+    if candidate in GEN1:
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
+      ret.steerActuatorDelay = 0.1
+      
+    if candidate in GEN2:
+      ret.experimentalLongitudinalAvailable = True
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda2019)]
+      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.radarUnavailable = True
 
-    ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021)
+    ret.dashcamOnly = False
 
-    ret.steerActuatorDelay = 0.1
+    
     ret.steerLimitTimer = 0.8
     ret.tireStiffnessFactor = 0.70   # not optimized yet
 
@@ -40,8 +75,23 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 3443 * CV.LB_TO_KG
       ret.wheelbase = 2.83
       ret.steerRatio = 15.5
+    elif candidate in CAR.MAZDA3_2019:
+      ret.mass = 3000 * CV.LB_TO_KG
+      ret.wheelbase = 2.725
+      ret.steerRatio = 17.0
+      ret.lateralTuning.torque.latAngleFactor = .14
+    elif candidate in (CAR.CX_30, CAR.CX_50):
+      ret.mass = 3375 * CV.LB_TO_KG
+      ret.wheelbase = 2.814 # Measured
+      ret.steerRatio = 15.5
+      ret.lateralTuning.torque.latAngleFactor = .14
+    elif candidate in (CAR.CX_60, CAR.CX_80, CAR.CX_70, CAR.CX_90):
+      ret.mass = 4217 * CV.LB_TO_KG
+      ret.wheelbase = 3.1
+      ret.steerRatio = 17.6
+      ret.lateralTuning.torque.latAngleFactor = .14
 
-    if candidate not in (CAR.CX5_2022, ):
+    if candidate not in (CAR.CX5_2022, CAR.MAZDA3_2019, CAR.CX_30, CAR.CX_50, CAR.CX_60, CAR.CX_70, CAR.CX_80, CAR.CX_90):
       ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
 
     ret.centerToFront = ret.wheelbase * 0.41
@@ -50,15 +100,24 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    if self.CP.carFingerprint in GEN1:
+      if self.CP.enableTorqueInterceptor and not TI.enabled:
+        TI.enabled = True
+        self.cp_body = self.CS.get_body_can_parser(self.CP)
+        self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback]
+
+    ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
 
     # events
     events = self.create_common_events(ret)
+    if self.CP.carFingerprint in GEN1:
+      if self.CS.lkas_disabled:
+        events.add(EventName.lkasDisabled)
+      elif self.CS.low_speed_alert:
+        events.add(EventName.belowSteerSpeed)
 
-    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)
 
     ret.events = events.to_msg()
 

selfdrive/car/mazda/mazdacan.py

@@ -1,51 +1,56 @@
 from openpilot.selfdrive.car.mazda.values import GEN1, Buttons
 
+from selfdrive.car.mazda.values import GEN1, GEN2, Buttons
+from common.params import Params
+
 
 def create_steering_control(packer, car_fingerprint, 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 = {}
   if car_fingerprint in 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
+    
+    bus = 0
+    sig_name = "CAM_LKAS"
+
     values = {
       "LKAS_REQUEST": apply_steer,
       "CTR": ctr,
@@ -58,8 +63,40 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
       "ANGLE_ENABLED": b2,
       "CHKSUM": csum
     }
+      
+  elif car_fingerprint in GEN2:
+    bus = 1
+    sig_name = "EPS_LKAS"
+    values = {
+      "LKAS_REQUEST": apply_steer,
+      "STEER_FEEL": 12000,
+    }
+
+  return packer.make_can_msg(sig_name, bus, values)
+
+def create_ti_steering_control(packer, car_fingerprint, frame, apply_steer):
+
+  key = 3294744160
+  chksum = apply_steer
+
+  if car_fingerprint in 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)
 
-  return packer.make_can_msg("CAM_LKAS", 0, values)
 
 
 def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):
@@ -126,3 +163,20 @@ def create_button_cmd(packer, car_fingerprint, counter, button):
     }
 
     return packer.make_can_msg("CRZ_BTNS", 0, values)
+  
+def create_acc_cmd(self, packer, CS, CC, hold, resume):
+  if self.CP.carFingerprint in GEN2:
+    values = CS.acc
+    msg_name = "ACC"
+    bus = 2
+
+    if (values["ACC_ENABLED"]):
+      if Params().get_bool("ExperimentalLongitudinalEnabled"):
+        values["ACCEL_CMD"] = (CC.actuators.accel * 240) + 2000
+      values["HOLD"] = hold
+      values["RESUME"] = resume
+    else:
+      pass
+
+  return packer.make_can_msg(msg_name, bus, values)
+

selfdrive/car/mazda/values.py

@@ -9,23 +9,43 @@ from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request,
 
 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.carFingerprint in 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.carFingerprint in 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
+  
+
 class CAR(StrEnum):
   CX5 = "MAZDA CX-5"
   CX9 = "MAZDA CX-9"
@@ -33,6 +53,13 @@ class CAR(StrEnum):
   MAZDA6 = "MAZDA 6"
   CX9_2021 = "MAZDA CX-9 2021"
   CX5_2022 = "MAZDA CX-5 2022"
+  MAZDA3_2019 = "MAZDA 3 2019"
+  CX_30 = "MAZDA CX-30"
+  CX_50 = "MAZDA CX-50"
+  CX_60 = "MAZDA CX-60"
+  CX_70 = "MAZDA CX-70"
+  CX_80 = "MAZDA CX-80"
+  CX_90 = "MAZDA CX-90"
 
 
 @dataclass
@@ -48,13 +75,24 @@ CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = {
   CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"),
   CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"),
   CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-24"),
+  CAR.MAZDA3_2019: MazdaCarInfo("Mazda 3 2019-24"),
+  CAR.CX_30: MazdaCarInfo("Mazda CX-30 2019-24"),
+  CAR.CX_50: MazdaCarInfo("Mazda CX-50 2022-24"),
+  CAR.CX_60: MazdaCarInfo("Mazda CX-60 unreleased"),
+  CAR.CX_70: MazdaCarInfo("Mazda CX-70 unreleased"),
+  CAR.CX_80: MazdaCarInfo("Mazda CX-80 unreleased"),
+  CAR.CX_90: MazdaCarInfo("Mazda CX-90 2023"),
 }
 
 
 class LKAS_LIMITS:
-  STEER_THRESHOLD = 15
-  DISABLE_SPEED = 45    # kph
-  ENABLE_SPEED = 52     # kph
+  STEER_THRESHOLD = 6
+  DISABLE_SPEED = 0    # kph
+  ENABLE_SPEED = 0     # kph
+  TI_STEER_THRESHOLD = 6
+  TI_DISABLE_SPEED = 0    # kph
+  TI_ENABLE_SPEED = 0     # kph
+
 
 
 class Buttons:
@@ -63,7 +101,7 @@ class Buttons:
   SET_MINUS = 2
   RESUME = 3
   CANCEL = 4
-
+  TURN_ON = 5
 
 FW_QUERY_CONFIG = FwQueryConfig(
   requests=[
@@ -71,13 +109,17 @@ FW_QUERY_CONFIG = FwQueryConfig(
       [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
       [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
     ),
-    # Log responses on powertrain bus
     Request(
-      [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
-      [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
-      bus=0,
-      logging=True,
+      [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],
+    )
   ],
 )
 
@@ -277,31 +319,37 @@ FW_VERSIONS = {
       b'GBEF-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'GBEF-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'GFBC-3210X-A-00\000\000\000\000\000\000\000\000\000',
+      b'GFBC-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'PA34-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PX4F-188K2-D\000\000\000\000\000\000\000\000\000\000\000\000',
       b'PYH7-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PYH7-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PANX-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x764, None): [
       b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'K131-67XK2-E\000\000\000\000\000\000\000\000\000\000\000\000',
+      b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x760, None): [
       b'GBVH-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'GBVH-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'GDDM-437K2-A\000\000\000\000\000\000\000\000\000\000\000\000',
+      b'GDDM-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x706, None): [
       b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'GSH7-67XK2-P\000\000\000\000\000\000\000\000\000\000\000\000',
+      b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'PA28-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PYH3-21PS1-D\000\000\000\000\000\000\000\000\000\000\000\000',
       b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PA28-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
   },
 
@@ -334,7 +382,98 @@ FW_VERSIONS = {
       b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PXM6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
-  }
+  },
+  
+  CAR.MAZDA3_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.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.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',
+    ],
+  },
 }
 
 
@@ -345,7 +484,15 @@ DBC = {
   CAR.MAZDA6: dbc_dict('mazda_2017', None),
   CAR.CX9_2021: dbc_dict('mazda_2017', None),
   CAR.CX5_2022: dbc_dict('mazda_2017', None),
+  CAR.MAZDA3_2019: dbc_dict('mazda_2019', None),
+  CAR.CX_30: dbc_dict('mazda_2019', None),
+  CAR.CX_50: dbc_dict('mazda_2019', None),
+  CAR.CX_60: dbc_dict('mazda_2019', None),
+  CAR.CX_70: dbc_dict('mazda_2019', None),
+  CAR.CX_80: dbc_dict('mazda_2019', None),
+  CAR.CX_90: dbc_dict('mazda_2019', None),
 }
 
 # Gen 1 hardware: same CAN messages and same camera
 GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022}
+GEN2 = {CAR.MAZDA3_2019, CAR.CX_30, CAR.CX_50, CAR.CX_60, CAR.CX_70, CAR.CX_80, CAR.CX_90}

selfdrive/car/tests/test_car_interfaces.py

@@ -40,6 +40,8 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict:
   params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0) for fw in params['car_fw']]
   return params
 
+from openpilot.selfdrive.global_ti import TI
+from openpilot.selfdrive.car.mazda.values import GEN1
 
 class TestCarInterfaces(unittest.TestCase):
 
@@ -60,6 +62,13 @@ class TestCarInterfaces(unittest.TestCase):
 
     car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'],
                                          experimental_long=args['experimental_long'], docs=False)
+    if car_name in GEN1:
+      TI.saved_candidate = car_name
+      TI.saved_CarInterface = CarInterface
+      TI.saved_finger = args['fingerprints']
+      car_params = TI.saved_CarInterface.get_params(TI.saved_candidate, TI.saved_finger, list(), experimental_long=False, docs=False)
+      car_params.enableTorqueInterceptor = True
+      
     car_interface = CarInterface(car_params, CarController, CarState)
     assert car_params
     assert car_interface

selfdrive/car/torque_data/override.yaml

@@ -64,6 +64,14 @@ HYUNDAI AZERA HYBRID 6TH GEN: [1.8, 1.8, 0.1]
 KIA K8 HYBRID 1ST GEN: [2.5, 2.5, 0.1]
 HYUNDAI CUSTIN 1ST GEN: [2.5, 2.5, 0.1]
 
+MAZDA 3 2019: [1.45, 3.0, .33]
+MAZDA CX-30: [1.45, 3.0, .33]
+MAZDA CX-50: [1.45, 3.0, .33]
+MAZDA CX-60: [1.45, 3.0, .33]
+MAZDA CX-70: [1.45, 3.0, .33]
+MAZDA CX-80: [1.45, 3.0, .33]
+MAZDA CX-90: [1.45, 3.0, .33]
+
 # Dashcam or fallback configured as ideal car
 mock: [10.0, 10, 0.0]
 

selfdrive/controls/controlsd.py

@@ -16,7 +16,7 @@ from panda import ALTERNATIVE_EXPERIENCE
 from openpilot.system.swaglog import cloudlog
 from openpilot.system.version import is_release_branch, get_short_branch
 from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
-from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
+from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can, get_ti
 from openpilot.selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET
 from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
@@ -103,6 +103,8 @@ class Controls:
     if not self.disengage_on_accelerator:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
 
+    self.ti_ready = False
+
     # read params
     self.is_metric = self.params.get_bool("IsMetric")
     self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
@@ -322,6 +324,15 @@ class Controls:
       if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
         self.events.add(EventName.relayMalfunction)
 
+      if pandaState.torqueInterceptorDetected and not self.ti_ready:
+        self.ti_ready = True
+        self.CP.enableTorqueInterceptor = True
+        #Update CP based on torque_interceptor_ready
+        self.CP = get_ti()
+        # set alternative experiences since get_ti() reset it to default.
+        if not self.disengage_on_accelerator:
+          self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
+
     # Handle HW and system malfunctions
     # Order is very intentional here. Be careful when modifying this.
     # All events here should at least have NO_ENTRY and SOFT_DISABLE.

selfdrive/controls/lib/latcontrol_torque.py

@@ -43,11 +43,12 @@ class LatControlTorque(LatControl):
       output_torque = 0.0
       pid_log.active = False
     else:
+      steering_angle = CS.steeringAngleDeg - params.angleOffsetDeg
       if self.use_steering_angle:
-        actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+        actual_curvature = -VM.calc_curvature(math.radians(steering_angle), CS.vEgo, params.roll)
         curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
       else:
-        actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+        actual_curvature_vm = -VM.calc_curvature(math.radians(steering_angle), CS.vEgo, params.roll)
         actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
         actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
         curvature_deadzone = 0.0
@@ -63,13 +64,13 @@ class LatControlTorque(LatControl):
       measurement = actual_lateral_accel + low_speed_factor * actual_curvature
       gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
       torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.torque_params, setpoint,
-                                                     lateral_accel_deadzone, friction_compensation=False)
+                                                     lateral_accel_deadzone, steering_angle, CS.vEgo, friction_compensation=False)
       torque_from_measurement = self.torque_from_lateral_accel(measurement, self.torque_params, measurement,
-                                                     lateral_accel_deadzone, friction_compensation=False)
+                                                     lateral_accel_deadzone, steering_angle, CS.vEgo, friction_compensation=False)
       pid_log.error = torque_from_setpoint - torque_from_measurement
       ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params,
                                           desired_lateral_accel - actual_lateral_accel,
-                                          lateral_accel_deadzone, friction_compensation=True)
+                                          lateral_accel_deadzone,steering_angle, CS.vEgo, friction_compensation=True)
 
       freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5
       output_torque = self.pid.update(pid_log.error,

selfdrive/global_ti.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python3
+from selfdrive.car import gen_empty_fingerprint
+
+class TI:
+  saved_candidate: str
+  saved_finger = gen_empty_fingerprint()
+  saved_CarInterface: object
+  enabled = False

selfdrive/ui/qt/offroad/settings.cc

@@ -65,6 +65,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"),

selfdrive/updated.py

@@ -32,8 +32,8 @@ FINALIZED = os.path.join(STAGING_ROOT, "finalized")
 
 OVERLAY_INIT = Path(os.path.join(BASEDIR, ".overlay_init"))
 
-DAYS_NO_CONNECTIVITY_MAX = 14     # do not allow to engage after this many days
-DAYS_NO_CONNECTIVITY_PROMPT = 10  # send an offroad prompt after this many days
+DAYS_NO_CONNECTIVITY_MAX = 9999     # do not allow to engage after this many days
+DAYS_NO_CONNECTIVITY_PROMPT = 9998  # send an offroad prompt after this many days
 
 class WaitTimeHelper:
   def __init__(self):

system/loggerd/loggerd.h

@@ -58,12 +58,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),
 };