From edfc0dfc703ed42c5aec68bd085185117325696b Mon Sep 17 00:00:00 2001
From: MoreTore <ryleymccarroll22@gmail.com>
Date: Fri, 17 Oct 2025 14:20:09 -0500
Subject: [PATCH] mazda frogpilot

---
 common/params.cc                        |   1 +
 launch_openpilot.sh                     |   4 +-
 opendbc/mazda_2019.dbc                  |  24 ++-
 selfdrive/car/fingerprints.py           |   3 +
 selfdrive/car/mazda/carcontroller.py    | 167 +++++++++++++----
 selfdrive/car/mazda/carstate.py         | 230 +++++++++++++++++++++---
 selfdrive/car/mazda/fingerprints.py     |  90 ++++++++++
 selfdrive/car/mazda/interface.py        | 124 +++++++++++--
 selfdrive/car/mazda/mazdacan.py         | 223 +++++++++++++++++------
 selfdrive/car/mazda/radar_interface.py  |  60 ++++++-
 selfdrive/car/mazda/values.py           | 103 +++++++++--
 selfdrive/car/torque_data/override.toml |   4 +
 selfdrive/controls/lib/longcontrol.py   |  12 ++
 selfdrive/ui/qt/offroad/settings.cc     |  42 +++++
 system/athena/athenad.py                |  15 +-
 system/athena/registration.py           |   3 +-
 system/loggerd/loggerd.h                |   2 +
 system/manager/manager.py               |  14 +-
 tools/cabana/videowidget.cc             |   6 +-
 tools/lib/auth.py                       |   2 +-
 20 files changed, 959 insertions(+), 170 deletions(-)
 mode change 100644 => 100755 system/athena/registration.py

diff --git a/common/params.cc b/common/params.cc
index 8f8e890b1b..96444aab24 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -585,6 +585,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"NoFSC", PERSISTENT},
     {"BlendedACC", PERSISTENT},
     {"ManualTransmission", PERSISTENT},
+    {"RemoteAccess", PERSISTENT},
 };
 
 } // namespace
diff --git a/launch_openpilot.sh b/launch_openpilot.sh
index 2888814c22..0c9fba2f7b 100755
--- a/launch_openpilot.sh
+++ b/launch_openpilot.sh
@@ -1,3 +1,5 @@
 #!/usr/bin/bash
-
+export API_HOST=https://api.konik.ai
+export ATHENA_HOST=wss://athena.konik.ai
+export MAPS_HOST=https://api.konik.ai/maps
 exec ./launch_chffrplus.sh
diff --git a/opendbc/mazda_2019.dbc b/opendbc/mazda_2019.dbc
index b6fe944b24..7b5746561f 100644
--- a/opendbc/mazda_2019.dbc
+++ b/opendbc/mazda_2019.dbc
@@ -159,10 +159,10 @@ BO_ 129 NEW_MSG_19: 8 XXX
  SG_ NEW_SIGNAL_4 : 47|16@0+ (1,0) [0|65535] "" XXX
  SG_ NEW_SIGNAL_5 : 56|8@1+ (1,0) [0|15] "" XXX
 
-BO_ 130 STEER_ANGLE: 8 XXX
- SG_ STEER_ANGLE : 23|16@0+ (1,0) [0|65535] "" XXX
+BO_ 130 STEER: 8 XXX
+ SG_ STEER_ANGLE : 23|16@0+ (0.05,-1600) [0|65535] "" XXX
  SG_ NEW_SIGNAL_2 : 39|8@0+ (1,0) [0|255] "" XXX
- SG_ NEW_SIGNAL_1 : 55|16@0- (1,0) [0|65535] "" XXX
+ SG_ STEER_RATE : 55|16@0- (0.03,0) [0|65535] "" XXX
 
 BO_ 133 NEW_MSG_35: 8 XXX
  SG_ NEW_SIGNAL_1 : 7|8@0+ (1,0) [0|255] "" XXX
@@ -185,7 +185,7 @@ BO_ 133 NEW_MSG_35: 8 XXX
  SG_ NEW_SIGNAL_7 : 55|8@0+ (1,0) [0|255] "" XXX
  SG_ NEW_SIGNAL_8 : 56|8@1+ (1,0) [0|255] "" XXX
 
-BO_ 134 STEER: 8 XXX
+BO_ 134 STEER_ANGLE2: 8 XXX
  SG_ NEW_SIGNAL_1 : 7|16@0+ (1,0) [0|65535] "" XXX
  SG_ STEER_ANGLE : 25|14@0- (0.1,36) [0|1023] "" XXX
 
@@ -197,6 +197,9 @@ BO_ 136 NEW_MSG_40: 8 XXX
  SG_ NEW_SIGNAL_2 : 59|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_3 : 60|1@0+ (1,0) [0|1] "" XXX
 
+BO_ 143 BRAKE_PEDAL_PARK: 4 XXX
+ SG_ BRAKE_PRESSED_IN_PARK : 1|1@1+ (1,0) [0|3] "" XXX
+
 BO_ 145 BLINK_INFO: 8 XXX
  SG_ RIGHT_BLINK : 12|1@0+ (1,0) [0|3] "" XXX
  SG_ LEFT_BLINK : 13|1@0+ (1,0) [0|3] "" XXX
@@ -223,13 +226,13 @@ BO_ 157 CRZ_BTNS: 8 XXX
  SG_ CTR : 48|4@1+ (1,0) [0|15] "" XXX
  SG_ CHKSUM : 56|8@1+ (1,0) [0|63] "" XXX
 
-BO_ 159 BRAKE_2: 8 XXX
+BO_ 159 BRAKE_PEDAL: 8 XXX
  SG_ REVERSE : 1|1@0+ (1,0) [0|65535] "" XXX
  SG_ NEW_SIGNAL_1 : 2|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_2 : 4|1@1+ (1,0) [0|3] "" XXX
  SG_ NEW_SIGNAL_3 : 40|6@1+ (1,0) [0|15] "" XXX
  SG_ BRAKE_NOT_PRESSED : 58|1@0+ (1,0) [0|1] "" XXX
- SG_ BRAKE_PEDAL_PRESSED : 59|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_PRESSED : 59|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 253 NEW_MSG_34: 8 XXX
  SG_ NEW_SIGNAL_1 : 36|5@0+ (1,0) [0|31] "" XXX
@@ -545,7 +548,10 @@ BO_ 1067 NEW_MSG_33: 8 XXX
 BO_ 1078 NEW_MSG_44: 8 XXX
  SG_ IGNITION : 10|1@1+ (1,0) [0|255] "" XXX
 
-BO_ 1087 BRAKE_PEDAL: 8 XXX
+BO_ 1086 NEW_MSG_43E: 8 XXX
+ SG_ NEW_SIGNAL_1 : 39|16@0+ (1,0) [0|65535] "" XXX
+
+BO_ 1087 BRAKE_PEDAL_SLOW: 8 XXX
  SG_ NEW_SIGNAL_3 : 7|16@0+ (1,0) [0|65535] "" XXX
  SG_ NEW_SIGNAL_4 : 23|16@0+ (1,0) [0|65535] "" XXX
  SG_ NEW_SIGNAL_2 : 32|1@0+ (1,0) [0|1] "" XXX
@@ -629,8 +635,8 @@ BO_ 1868 EPS_FEEDBACK3: 8 XXX
 
 CM_ SG_ 31 GEAR "13-P, 12-R, 11-N, 1-6-D";
 VAL_ 31 GEAR 13 "P" 12 "R" 11 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D";
-VAL_ 552 GEAR 13 "P" 12 "R" 11 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D";
 VAL_ 537 GEAR 0 "P" 1 "D" 2 "R";
+VAL_ 552 GEAR 13 "P" 12 "R" 11 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D";
 VAL_ 552 GEAR_SHIFT 6 "6th" 5 "5th" 4 "4th" 3 "3rd" 2 "2nd" 1 "1st" 14 "Shift" 13 "Park" 11 "Neutral" 12 "Reverse";
 VAL_ 1098 CRZ_STATE 0 "CRUISE_DISABLED" 1 "CRUISE_READY" 2 "CRUISE_ENABLED" 4 "GAS_OVERRIDE";
-VAL_ 1098 DISTANCE_SETTING 4 "CLOSE" 3 "MEDIUM_CLOSE" 2 "MEDIUM_FAR" 1 "FAR" 0 "ACC_DISABLED";
\ No newline at end of file
+VAL_ 1098 DISTANCE_SETTING 4 "CLOSE" 3 "MEDIUM_CLOSE" 2 "MEDIUM_FAR" 1 "FAR" 0 "ACC_DISABLED";
diff --git a/selfdrive/car/fingerprints.py b/selfdrive/car/fingerprints.py
index 1128a31c29..5b62759f32 100644
--- a/selfdrive/car/fingerprints.py
+++ b/selfdrive/car/fingerprints.py
@@ -255,6 +255,9 @@ MIGRATION = {
   "MAZDA 6": MAZDA.MAZDA_6,
   "MAZDA CX-9 2021": MAZDA.MAZDA_CX9_2021,
   "MAZDA CX-5 2022": MAZDA.MAZDA_CX5_2022,
+  "MAZDA 3 2019": MAZDA.MAZDA_3_2019,
+  "MAZDA CX-30": MAZDA.MAZDA_CX_30,
+  "MAZDA CX-50": MAZDA.MAZDA_CX_50,
   "NISSAN X-TRAIL 2017": NISSAN.NISSAN_XTRAIL,
   "NISSAN LEAF 2018": NISSAN.NISSAN_LEAF,
   "NISSAN LEAF 2018 Instrument Cluster": NISSAN.NISSAN_LEAF_IC,
diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py
index b38ea72e4b..2d36487876 100644
--- a/selfdrive/car/mazda/carcontroller.py
+++ b/selfdrive/car/mazda/carcontroller.py
@@ -1,66 +1,171 @@
 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, DT_CTRL
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
 
 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
+    self.acc_filter = FirstOrderFilter(0.0, .1, DT_CTRL, initialized=False)
+    self.filtered_acc_last = 0
+    self.long_active_last = False
+    self.params = Params()
+    self.params_memory = Params("/dev/shm/params")
+
 
   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)
-
-    if CC.cruiseControl.cancel:
-      # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
-      # a race condition with the stock system, where the second cancel from openpilot
-      # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
-      # read 3 messages and most likely sync state before we attempt cancel.
-      self.brake_counter = self.brake_counter + 1
-      if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
-        # Cancel Stock ACC if it's enabled while OP is disengaged
-        # Send at a rate of 10hz until we sync with stock ACC state
-        can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL))
-    else:
-      self.brake_counter = 0
-      if CC.cruiseControl.resume and self.frame % 5 == 0:
-        # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
-        # Send Resume button when planner wants car to move
-        can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
-
+                                                     CS.out.steeringTorque, self.ccp)
+      if self.CP.flags & 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)
     self.apply_steer_last = apply_steer
+    self.ti_apply_steer_last = ti_apply_steer
 
-    # 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.GEN1:
+      if CC.cruiseControl.cancel:
+        # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
+        # a race condition with the stock system, where the second cancel from openpilot
+        # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
+        # read 3 messages and most likely sync state before we attempt cancel.
+        self.brake_counter = self.brake_counter + 1
+        if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
+          # Cancel Stock ACC if it's enabled while OP is disengaged
+          # Send at a rate of 10hz until we sync with stock ACC state
+          can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL))
+      else:
+        self.brake_counter = 0
+        if CC.cruiseControl.resume and self.frame % 5 == 0:
+          # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
+          # Send Resume button when planner wants car to move
+          can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
+
+      # send HUD alerts
+      if self.frame % 50 == 0:
+        ldw = CC.hudControl.visualAlert == VisualAlert.ldw
+        steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
+        # TODO: find a way to silence audible warnings so we can add more hud alerts
+        steer_required = steer_required and CS.lkas_allowed_speed
+        if not self.CP.flags & MazdaFlags.NO_FSC:
+          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 CC.longActive:
+          raw_acc_output = CC.actuators.accel * 1150
+          raw_acc_output = max(-1000, min(raw_acc_output, 1000))
+
+          if self.params.get_bool("BlendedACC"):
+            if self.params_memory.get_int("CEStatus"):
+              self.acc_filter.update_alpha(abs(raw_acc_output-self.filtered_acc_last)/1000)
+              filtered_acc_output = int(self.acc_filter.update(raw_acc_output))
+            else:
+              # we want to use the stock value in this case but we need a smooth transition.
+              self.acc_filter.update_alpha(abs(CS.crz_info["ACCEL_CMD"]-self.filtered_acc_last)/1000)
+              filtered_acc_output = int(self.acc_filter.update(CS.crz_info["ACCEL_CMD"]))
+
+            CS.crz_info["ACCEL_CMD"] = int(filtered_acc_output)
+            self.filtered_acc_last = filtered_acc_output
+          else:
+            acc_output = raw_acc_output
+
+        if self.frame % 2 == 0:
+          can_sends.extend(mazdacan.create_radar_command(self.packer, self.frame, CC.longActive, CS, hold))
+
+    else:
+      raw_acc_output = (CC.actuators.accel * 200) + 2000
+      if CC.longActive:
+        if self.params.get_bool("BlendedACC"):
+          if not self.long_active_last:
+            # reset the filter when we start ACC
+            self.acc_filter.initialized = False
+
+          if self.params_memory.get_int("CEStatus"):
+            self.acc_filter.update_alpha(abs(raw_acc_output-self.filtered_acc_last)/1000)
+            filtered_acc_output = int(self.acc_filter.update(raw_acc_output))
+          else:
+            # we want to use the stock value in this case but we need a smooth transition.
+            self.acc_filter.update_alpha(abs(CS.acc["ACCEL_CMD"]-self.filtered_acc_last)/1000)
+            filtered_acc_output = int(self.acc_filter.update(CS.acc["ACCEL_CMD"]))
+
+          acc_output = filtered_acc_output
+          self.filtered_acc_last = filtered_acc_output
+
+        else:
+          acc_output = raw_acc_output
+
+        if self.params.get_bool("ExperimentalLongitudinalEnabled"):
+          CS.acc["ACCEL_CMD"] = acc_output
+
+      self.long_active_last = CC.longActive
+      resume = False
+      hold = False
+      if Timer.interval(2): # send ACC command at 50hz
+        """
+        Without this hold/resum logic, the car will only stop momentarily.
+        It will then start creeping forward again. This logic allows the car to
+        apply the electric brake to hold the car. The hold delay also fixes a
+        bug with the stock ACC where it sometimes will apply the brakes too early
+        when coming to a stop.
+        """
+        if CS.out.standstill: # if we're stopped
+          if not self.hold_delay.active(): # and we have been stopped for more than hold_delay duration. This prevents a hard brake if we aren't fully stopped.
+            if ((CC.cruiseControl.resume and CC.actuators.longControlState != LongCtrlState.stopping) or
+                CC.cruiseControl.override or CS.out.gasPressed or
+                (CC.actuators.longControlState == LongCtrlState.starting) or CS.acc["RESUME"]): # if we are resuming or overriding, we want to release the brake
+              self.resume_timer.reset() # reset the resume timer so its active
+            else: # otherwise we're holding
+              hold = self.hold_timer.active() # hold for 6s. This allows the electric brake to hold the car.
+
+        else: # if we're moving
+          self.hold_timer.reset() # reset the hold timer so its active when we stop
+          self.hold_delay.reset() # reset the hold delay
+
+        resume = self.resume_timer.active() # stay on for 0.5s to release the brake. This allows the car to move.
+        can_sends.append(mazdacan.create_acc_cmd(self, self.packer, CS.acc, 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
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index 13a642f496..86c91b3582 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -1,9 +1,11 @@
+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, CarControllerParams
+from openpilot.common.realtime import DT_CTRL
 
 class CarState(CarStateBase):
   def __init__(self, CP, FPCP):
@@ -11,17 +13,38 @@ class CarState(CarStateBase):
 
     can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
     self.shifter_values = can_define.dv["GEAR"]["GEAR"]
+    if CP.flags & MazdaFlags.MANUAL_TRANSMISSION:
+      self.shifter_values = can_define.dv["MANUAL_GEAR"]["GEAR"]
 
     self.crz_btns_counter = 0
     self.acc_active_last = False
     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.shifting = False
+    self.torque_converter_lock = True
+    self._prev_steering_angle = 0.0
+
+    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 +74,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,31 +124,99 @@ class CarState(CarStateBase):
 
     # TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on
     #       it should be used for carState.cruiseState.nonAdaptive instead
-    ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1
-    ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1
-    ret.cruiseState.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1
+    ret.cruiseState.standstill = ret.standstill
     ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
-
-    if 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.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
 
     # Check if LKAS is disabled due to lack of driver torque when all other states indicate
     # it should be enabled (steer lockout). Don't warn until we actually get lkas active
     # and lose it again, i.e, after initial lkas activation
-    ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked
+    ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked and not self.ti_lkas_allowed
 
     self.acc_active_last = ret.cruiseState.enabled
 
     self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
 
     # camera signals
-    self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
-    self.cam_lkas = cp_cam.vl["CAM_LKAS"]
-    self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
-    ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
+    if not self.CP.flags & MazdaFlags.NO_FSC:
+      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 if not self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR else False
+    self.cp_cam = cp_cam
+    self.cp = cp
+
+    # FrogPilot CarState functions
+    self.lkas_previously_enabled = self.lkas_enabled
+    self.lkas_enabled = not self.lkas_disabled
+
+    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.engineRpm = cp_cam.vl["ENGINE_DATA"]["RPM"]
+    #self.shifting = cp_cam.vl["GEAR"]["SHIFT"]
+    #self.torque_converter_lock = cp_cam.vl["GEAR"]["TORQUE_CONVERTER_LOCK"]
+
+    ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] # updated at 100hz and its high resolution
+    ret.steeringRateDeg = (ret.steeringAngleDeg - self._prev_steering_angle) / DT_CTRL
+    self._prev_steering_angle = ret.steeringAngleDeg
+    #ret.steeringRateDeg = cp.vl["STEER"]["STEER_RATE"] # This signal doesn't seem accurate
+
+    ret.steeringTorque = cp_body.vl["EPS_FEEDBACK"]["STEER_TORQUE_SENSOR"]
+    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
+    if self.CP.flags & MazdaFlags.MANUAL_TRANSMISSION:
+      can_gear = int(cp_cam.vl["MANUAL_GEAR"]["GEAR"])
+    else:
+      can_gear = int(cp_cam.vl["GEAR"]["GEAR"])
+    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_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.1
+    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 if not self.CP.openpilotLongitudinalControl else False
+
+    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,23 +224,40 @@ 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, FPCP):
-    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
+        ("CRZ_BTNS", 10),
+        ("BLINK_INFO", 10),
+        ("STEER", 67),
+        ("STEER_RATE", 83),
+        ("STEER_TORQUE", 83),
+        ("WHEEL_SPEEDS", 100),
+      ]
 
     if CP.flags & MazdaFlags.GEN1:
       messages += [
         ("ENGINE_DATA", 100),
-        ("CRZ_CTRL", 50),
         ("CRZ_EVENTS", 50),
-        ("CRZ_BTNS", 10),
         ("PEDALS", 50),
         ("BRAKE", 50),
         ("SEATBELT", 10),
@@ -142,6 +266,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", 5),
+        ("CRUZE_STATE", 10),
+        ("BLINK_INFO", 10),
+        ("ACC", 50),
+        ("SYSTEM_SETTINGS", 10),
+        ("STEER", 100)
+      ]
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
@@ -149,10 +288,43 @@ class CarState(CarStateBase):
     messages = []
 
     if CP.flags & MazdaFlags.GEN1:
+      if not CP.flags & MazdaFlags.NO_FSC:
+        messages += [
+          #  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 += [
-        # sig_address, frequency
-        ("CAM_LANEINFO", 2),
-        ("CAM_LKAS", 16),
+        ("ENGINE_DATA", 100),
+        ("STEER_TORQUE", 100),
+        ("WHEEL_SPEEDS", 100),
+        ("SPEED", 50),
       ]
 
+      if CP.flags & MazdaFlags.MANUAL_TRANSMISSION:
+        messages += [
+          ("MANUAL_GEAR", 50),
+        ]
+      else:
+        messages += [
+          ("GEAR", 40),
+        ]
+
     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)
diff --git a/selfdrive/car/mazda/fingerprints.py b/selfdrive/car/mazda/fingerprints.py
index f460fe9950..e8006a5d9a 100644
--- a/selfdrive/car/mazda/fingerprints.py
+++ b/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',
+    ],
+  },
 }
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index 915e0dd2db..baacff26d9 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -1,42 +1,134 @@
 #!/usr/bin/env python3
+from math import exp, fabs
+import numpy as np
+
 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.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType
+from openpilot.common.params import Params
 
 ButtonType = car.CarState.ButtonEvent.Type
 FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 
+NON_LINEAR_TORQUE_PARAMS = {
+  CAR.MAZDA_3_2019: (3.8818, 0.6873, 0.0999, 0.3605),
+  CAR.MAZDA_CX_30: (4.68689, 0.79999, 0.18244, 0.38763),
+  CAR.MAZDA_CX_50: (4.68689, 0.79999, 0.18244, 0.38763)
+}
+
 class CarInterface(CarInterfaceBase):
 
+  def get_lataccel_torque_siglin(self) -> float:
+
+    def torque_from_lateral_accel_siglin_func(lateral_acceleration: float) -> float:
+      # The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
+      # An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
+      # This has big effect on the stability about 0 (noise when going straight)
+      non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
+      assert non_linear_torque_params, "The params are not defined"
+      a, b, c, _ = non_linear_torque_params
+      sig_input = a * lateral_acceleration
+      sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5)
+      steer_torque = (sig * b) + (lateral_acceleration * c)
+      return float(steer_torque)
+
+    lataccel_values = np.arange(-5.0, 5.0, 0.01)
+    torque_values = [torque_from_lateral_accel_siglin_func(x) for x in lataccel_values]
+    assert min(torque_values) < -1 and max(torque_values) > 1, "The torque values should cover the range [-1, 1]"
+    return torque_values, lataccel_values
+
+  def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
+    if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
+      torque_values, lataccel_values = self.get_lataccel_torque_siglin()
+
+      def torque_from_lateral_accel_siglin(lateral_acceleration: float, torque_params: car.CarParams.LateralTorqueTuning):
+        return np.interp(lateral_acceleration, lataccel_values, torque_values)
+      return torque_from_lateral_accel_siglin
+    else:
+      return self.torque_from_lateral_accel_linear
+
+  def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType:
+    if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
+      torque_values, lataccel_values = self.get_lataccel_torque_siglin()
+
+      def lateral_accel_from_torque_siglin(torque: float, torque_params: car.CarParams.LateralTorqueTuning):
+        return np.interp(torque, torque_values, lataccel_values)
+      return lateral_accel_from_torque_siglin
+    else:
+      return self.lateral_accel_from_torque_linear
+
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, frogpilot_toggles):
     ret.carName = "mazda"
     ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
     ret.radarUnavailable = True
+    ret.dashcamOnly = False
+    ret.openpilotLongitudinalControl = True
+    p = Params()
+    if p.get_bool("ManualTransmission"):
+      ret.flags |= MazdaFlags.MANUAL_TRANSMISSION.value
+      ret.transmissionType = car.CarParams.TransmissionType.manual
+    else:
+      ret.transmissionType = car.CarParams.TransmissionType.automatic
 
-    ret.dashcamOnly = candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)
+    if candidate in GEN1:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_GEN1
+      if p.get_bool("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_bool("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_bool("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_bool("NoFSC"):  # No Front Sensing Camera
+        ret.flags |= MazdaFlags.NO_FSC.value
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_NO_FSC
+
+      ret.steerActuatorDelay = 0.1
+      ret.enableBsm = True
+
+    if candidate in GEN2:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_GEN2
+      ret.experimentalLongitudinalAvailable = True
+      ret.stopAccel = -.5
+      ret.vEgoStarting = .2
+      ret.longitudinalActuatorDelay = 0.35 # gas is 0.25s and brake looks like 0.5
+      ret.longitudinalTuning.kpBP = [0., 5., 35.]
+      ret.longitudinalTuning.kpV = [0.0, 0.0, 0.0]
+      ret.longitudinalTuning.kiBP = [0., 35.]
+      ret.longitudinalTuning.kiV = [0.1, 0.1]
+      ret.startingState = True
+      ret.steerActuatorDelay = 0.335
 
-    ret.steerActuatorDelay = 0.1
     ret.steerLimitTimer = 0.8
 
     CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
-    if candidate not in (CAR.MAZDA_CX5_2022, ):
+    if candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_3_2019, CAR.MAZDA_CX_30, CAR.MAZDA_CX_50) and not ret.flags & MazdaFlags.TORQUE_INTERCEPTOR:
       ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
 
     ret.centerToFront = ret.wheelbase * 0.41
 
-    ret.enableBsm = True
-
     return ret
 
   # 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}),
@@ -46,10 +138,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()
 
diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py
index 74f6af04c5..919e19677f 100644
--- a/selfdrive/car/mazda/mazdacan.py
+++ b/selfdrive/car/mazda/mazdacan.py
@@ -1,66 +1,105 @@
 from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags
-
+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:
+    if not CP.flags & MazdaFlags.NO_FSC:
+      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 = {
+        "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 +165,79 @@ def create_button_cmd(packer, CP, counter, button):
     }
 
     return packer.make_can_msg("CRZ_BTNS", 0, values)
+
+STATIC_DATA_21B = [0x01FFE000, 0x00000000]
+STATIC_DATA_361 = [0xFFF7FEFE, 0x1FC]
+STATIC_DATA_362 = [0xFFF7FEFE, 0x1FC]
+STATIC_DATA_363 = [0xFFF7FEFE, 0x1FC0000]
+STATIC_DATA_364 = [0xFFF7FEFE, 0x1FC0000]
+STATIC_DATA_365 = [0xFFF7FE7F, 0xFBFF3FC]
+STATIC_DATA_366 = [0xFFF7FE7F, 0xFBFF3FC]
+static_data_list = [STATIC_DATA_361, STATIC_DATA_362, STATIC_DATA_363, STATIC_DATA_364, STATIC_DATA_365, STATIC_DATA_366]
+
+# GEN1 radar interceptor
+def create_radar_command(packer, frame, active, CS, hold):
+  #accel = 0
+  ret = []
+  crz_ctrl = CS.crz_cntr
+  crz_info = CS.crz_info
+
+  # if CC.longActive: # this is set true in longcontrol.py
+  #   accel = CC.actuators.accel * 1150
+  #   accel = accel if accel < 1000 else 1000
+  # else:
+  #   accel = int(crz_info["ACCEL_CMD"])
+
+  crz_info["ACC_ACTIVE"] = active
+  crz_info["ACC_SET_ALLOWED"] = int(bool(int(CS.cp.vl["GEAR"]["GEAR"]) & 4)) # we can set ACC_SET_ALLOWED bit when in drive. Allows crz to be set from 1kmh.
+  crz_info["CRZ_ENDED"] = 0 # this should keep acc on down to 5km/h on my 2018 M3
+  #crz_info["ACCEL_CMD"] = accel
+  crz_info["STOPPING_MAYBE"] = hold
+  crz_info["STOPPING_MAYBE2"] = hold
+
+  crz_ctrl["CRZ_ACTIVE"] = active
+  crz_ctrl["ACC_ACTIVE_2"] = active
+  crz_ctrl["DISABLE_TIMER_1"] = 0
+  crz_ctrl["DISABLE_TIMER_2"] = 0
+
+  ret.append(packer.make_can_msg("CRZ_INFO", 0, crz_info))
+  ret.append(packer.make_can_msg("CRZ_CTRL", 0, crz_ctrl))
+  # convert steering angle to radar units and clip to range
+  steer_angle = (CS.out.steeringAngleDeg *-17.4) + 2048
+
+  if (frame % 10 == 0):
+    for i, addr in enumerate(range(361,367)):
+      addr_name = f"RADAR_{addr}"
+      msg = CS.cp_cam.vl[addr_name]
+      values = {
+        "MSGS_1" : static_data_list[i][0],
+        "MSGS_2" : static_data_list[i][1],
+        "CTR"    : int(msg["CTR"]) #frame % 16
+      }
+      if addr == 361:
+        values.update({
+          "INVERSE_SPEED" : int(CS.out.vEgo * -4.4),
+          "BIT" : 1,
+        })
+      if addr == 362:
+        values.update({
+          "CLIPPED_STEER_ANGLE" : int(clip(steer_angle, 0, 4092)),
+        })
+      ret.append(packer.make_can_msg(addr_name, 0, values))
+
+  return ret
+
+# GEN2 new mazdas
+def create_acc_cmd(self, packer, values, hold, resume):
+  msg_name = "ACC"
+  bus = 2
+
+  if (values["ACC_ENABLED"]):
+    values["HOLD"] = hold
+    values["RESUME"] = resume
+  else:
+    pass
+
+  return packer.make_can_msg(msg_name, bus, values)
+
+
diff --git a/selfdrive/car/mazda/radar_interface.py b/selfdrive/car/mazda/radar_interface.py
index b461fcd5f8..a34733488b 100755
--- a/selfdrive/car/mazda/radar_interface.py
+++ b/selfdrive/car/mazda/radar_interface.py
@@ -1,5 +1,63 @@
 #!/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, MazdaFlags
+
+def get_radar_can_parser(CP):
+  if DBC[CP.carFingerprint]['radar'] is None:
+    return None
+  if not CP.flags & MazdaFlags.RADAR_INTERCEPTOR:
+    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
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index a8c808d582..c8f3e243c1 100644
--- a/selfdrive/car/mazda/values.py
+++ b/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,38 +61,69 @@ 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
+  MANUAL_TRANSMISSION = 64
 
 @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")],
-    MAZDA_CX9.specs
+    MAZDA_CX9.specs,
+    flags=MazdaFlags.GEN1,
   )
   MAZDA_CX5_2022 = MazdaPlatformConfig(
     [MazdaCarDocs("Mazda CX-5 2022-24")],
     MAZDA_CX5.specs,
+    flags=MazdaFlags.GEN1,
+  )
+  MAZDA_3_2019 = MazdaPlatformConfig(
+    [MazdaCarDocs("Mazda 3 2019-24")],
+    MazdaCarSpecs(mass=3000 * CV.LB_TO_KG, wheelbase=2.725, steerRatio=18.8),
+    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,
   )
 
 
@@ -80,7 +131,9 @@ class LKAS_LIMITS:
   STEER_THRESHOLD = 15
   DISABLE_SPEED = 45    # kph
   ENABLE_SPEED = 52     # kph
-
+  TI_STEER_THRESHOLD = 6
+  TI_DISABLE_SPEED = 0    # kph
+  TI_ENABLE_SPEED = 0     # kph
 
 class Buttons:
   NONE = 0
@@ -88,6 +141,7 @@ class Buttons:
   SET_MINUS = 2
   RESUME = 3
   CANCEL = 4
+  TURN_ON = 5
 
 
 FW_QUERY_CONFIG = FwQueryConfig(
@@ -98,7 +152,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)
diff --git a/selfdrive/car/torque_data/override.toml b/selfdrive/car/torque_data/override.toml
index b37817b4cc..53f5ace97f 100644
--- a/selfdrive/car/torque_data/override.toml
+++ b/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]
\ No newline at end of file
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 63950d4c3a..d4905ca516 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
 from openpilot.selfdrive.controls.lib.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.common.params import Params
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
@@ -95,6 +96,9 @@ class LongControl:
                              k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
     self.v_pid = 0.0
     self.last_output_accel = 0.0
+    self.params = Params()
+    self.params_memory = Params("/dev/shm/params")
+    self.experimental_mode_last = False
 
   def reset(self):
     self.pid.reset()
@@ -107,6 +111,14 @@ class LongControl:
     self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill, frogpilot_toggles)
+
+
+    if self.params.get_bool("BlendedACC"):
+      experimental_mode = self.params_memory.get_int("CEStatus") # 0 means expereimental mode is off
+      if experimental_mode and not self.experimental_mode_last:
+        self.reset()
+      self.experimental_mode_last = experimental_mode
+
     if self.long_control_state == LongCtrlState.off:
       self.reset()
       output_accel = 0.
diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
index d6466bab48..a65e8f0847 100644
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@@ -41,6 +41,42 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       "",
       "../assets/img_experimental_white.svg",
     },
+    {
+      "BlendedACC",
+      tr("Blended Acc (Experimental)"),
+      tr("Blend stock MRCC and Experimental Mode longitudinal control."),
+      "../assets/offroad/icon_openpilot.png",
+    },
+    {
+      "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",
+    },
+    {
+      "ManualTransmission",
+      tr("Manual Transmission"),
+      tr("Enable if your is a manual."),
+      "../assets/offroad/icon_openpilot.png",
+    },
     {
       "DisengageOnAccelerator",
       tr("Disengage on Accelerator Pedal"),
@@ -59,6 +95,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",
     },
+    {
+      "RecordRoad",
+      tr("Record and Upload Road Cameras"),
+      tr("Upload data from the road cameras."),
+      "../assets/offroad/icon_monitoring.png",
+    },
     {
       "IsMetric",
       tr("Use Metric System"),
diff --git a/system/athena/athenad.py b/system/athena/athenad.py
index d606ec0a08..356650443c 100755
--- a/system/athena/athenad.py
+++ b/system/athena/athenad.py
@@ -145,6 +145,9 @@ def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None:
     threading.Thread(target=ws_recv, args=(ws, end_event), name='ws_recv'),
     threading.Thread(target=ws_send, args=(ws, end_event), name='ws_send'),
     threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
+    threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler2'),
+    threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler3'),
+    threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler4'),
     threading.Thread(target=log_handler, args=(end_event,), name='log_handler'),
     threading.Thread(target=stat_handler, args=(end_event,), name='stat_handler'),
   ] + [
@@ -259,13 +262,13 @@ def upload_handler(end_event: threading.Event) -> None:
           sz = -1
 
         cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count)
-        response = _do_upload(item, partial(cb, sm, item, tid, end_event))
 
-        if response.status_code not in (200, 201, 401, 403, 412):
-          cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
-          retry_upload(tid, end_event)
-        else:
-          cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered)
+        with _do_upload(item, partial(cb, sm, item, tid, end_event)) as response:
+          if response.status_code not in (200, 201, 401, 403, 412):
+            cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
+            retry_upload(tid, end_event)
+          else:
+            cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered)
 
         UploadQueueCache.cache(upload_queue)
       except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError):
diff --git a/system/athena/registration.py b/system/athena/registration.py
old mode 100644
new mode 100755
index c32ad6ec17..066b8ded19
--- a/system/athena/registration.py
+++ b/system/athena/registration.py
@@ -10,8 +10,7 @@ from datetime import datetime, timedelta
 from openpilot.common.api import api_get
 from openpilot.common.params import Params
 from openpilot.common.spinner import Spinner
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
-from openpilot.system.hardware import HARDWARE, PC
+from openpilot.system.hardware import HARDWARE
 from openpilot.system.hardware.hw import Paths
 from openpilot.common.swaglog import cloudlog
 
diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h
index 30536f0507..2cc2007b3a 100644
--- a/system/loggerd/loggerd.h
+++ b/system/loggerd/loggerd.h
@@ -59,12 +59,14 @@ public:
 const EncoderInfo main_road_encoder_info = {
   .publish_name = "roadEncodeData",
   .filename = "fcamera.hevc",
+  .record = Params().getBool("RecordRoad"),
   INIT_ENCODE_FUNCTIONS(RoadEncode),
 };
 
 const EncoderInfo main_wide_road_encoder_info = {
   .publish_name = "wideRoadEncodeData",
   .filename = "ecamera.hevc",
+  .record = Params().getBool("RecordRoad"),
   INIT_ENCODE_FUNCTIONS(WideRoadEncode),
 };
 
diff --git a/system/manager/manager.py b/system/manager/manager.py
index a2a26b4447..8597f356e7 100755
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@@ -46,6 +46,9 @@ def manager_init() -> None:
     ("LanguageSetting", "main_en"),
     ("OpenpilotEnabledToggle", "1"),
     ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)),
+    ("BlendedACC", "0"),
+    ("RecordRoad", "1"),
+    ("RemoteAccess", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))
@@ -152,7 +155,10 @@ def manager_thread() -> None:
   pm = messaging.PubMaster(['managerState'])
 
   write_onroad_params(False, params)
-  ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore, classic_model=False, tinygrad_model=False, frogpilot_toggles=get_frogpilot_toggles())
+  ensure_running(
+    managed_processes.values(), False, params=params,CP=sm['carParams'], not_run=ignore,
+    classic_model=False, tinygrad_model=False, frogpilot_toggles=get_frogpilot_toggles()
+  )
 
   started_prev = False
 
@@ -187,7 +193,11 @@ def manager_thread() -> None:
 
     started_prev = started
 
-    ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore, classic_model=classic_model, tinygrad_model=tinygrad_model, frogpilot_toggles=frogpilot_toggles)
+    ensure_running(
+      managed_processes.values(), started, params=params, CP=sm['carParams'],
+      not_run=ignore, classic_model=classic_model, tinygrad_model=tinygrad_model,
+      frogpilot_toggles=frogpilot_toggles
+    )
 
     running = ' '.join("{}{}\u001b[0m".format("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
                        for p in managed_processes.values() if p.proc)
diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc
index 7fca45c393..532c85f3ec 100644
--- a/tools/cabana/videowidget.cc
+++ b/tools/cabana/videowidget.cc
@@ -300,9 +300,9 @@ void Slider::paintEvent(QPaintEvent *ev) {
   };
 
   const auto replay = qobject_cast<ReplayStream *>(can)->getReplay();
-  for (auto [begin, end, type] : replay->getTimeline()) {
-    fillRange(begin, end, timeline_colors[(int)type]);
-  }
+  // for (auto [begin, end, type] : replay->getTimeline()) {
+  //   fillRange(begin, end, timeline_colors[(int)type]);
+  // }
 
   QColor empty_color = palette().color(QPalette::Window);
   empty_color.setAlpha(160);
diff --git a/tools/lib/auth.py b/tools/lib/auth.py
index 5988397d0a..6987ae7bd0 100755
--- a/tools/lib/auth.py
+++ b/tools/lib/auth.py
@@ -66,7 +66,7 @@ def auth_redirect_link(method):
   }[method]
 
   params = {
-    'redirect_uri': f"https://api.comma.ai/v2/auth/{provider_id}/redirect/",
+    'redirect_uri': f"https://api.konik.ai/v2/auth/{provider_id}/redirect/",
     'state': f'service,localhost:{PORT}',
   }