switch to mazda_aux

panda/board/main.c

@@ -89,14 +89,14 @@ void set_safety_mode(uint16_t mode, int16_t param) {
     case SAFETY_SILENT:
       set_intercept_relay(false);
       if (current_board->has_obd) {
-        current_board->set_can_mode(CAN_MODE_NORMAL);
+        current_board->set_can_mode(CAN_MODE_OBD_CAN2);
       }
       can_silent = ALL_CAN_SILENT;
       break;
     case SAFETY_NOOUTPUT:
       set_intercept_relay(false);
       if (current_board->has_obd) {
-        current_board->set_can_mode(CAN_MODE_NORMAL);
+        current_board->set_can_mode(CAN_MODE_OBD_CAN2);
       }
       can_silent = ALL_CAN_LIVE;
       break;
@@ -108,7 +108,7 @@ void set_safety_mode(uint16_t mode, int16_t param) {
         if (param == 0) {
           current_board->set_can_mode(CAN_MODE_OBD_CAN2);
         } else {
-          current_board->set_can_mode(CAN_MODE_NORMAL);
+          current_board->set_can_mode(CAN_MODE_OBD_CAN2);
         }
       }
       can_silent = ALL_CAN_LIVE;
@@ -118,7 +118,7 @@ void set_safety_mode(uint16_t mode, int16_t param) {
       heartbeat_counter = 0U;
       heartbeat_lost = false;
       if (current_board->has_obd) {
-        current_board->set_can_mode(CAN_MODE_NORMAL);
+        current_board->set_can_mode(CAN_MODE_OBD_CAN2);
       }
       can_silent = ALL_CAN_LIVE;
       break;

panda/board/safety/safety_mazda.h

@@ -26,7 +26,7 @@
 #define MAZDA_DRIVER_TORQUE_FACTOR 1
 #define MAZDA_MAX_TORQUE_ERROR 350
 
-const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS2, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}};
+const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS2, 1, 8}, {MAZDA_LKAS_HUD, 0, 8}};
 
 AddrCheckStruct mazda_addr_checks[] = {
   {.msg = {{MAZDA_CRZ_CTRL,     0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}},
@@ -39,7 +39,7 @@ AddrCheckStruct mazda_addr_checks[] = {
 addr_checks mazda_rx_checks = {mazda_addr_checks, MAZDA_ADDR_CHECKS_LEN};
 
 AddrCheckStruct mazda_ti_addr_checks[] = {
-  {.msg = {{TI_STEER_TORQUE,    0, 8, .expected_timestep = 12000U}}},
+  {.msg = {{TI_STEER_TORQUE,    1, 8, .expected_timestep = 22000U}}},
   // TI_STEER_TORQUE expected_timestep should be the same as the tx rate of MAZDA_LKAS2
 };
 #define MAZDA_TI_ADDR_CHECKS_LEN (sizeof(mazda_ti_addr_checks) / sizeof(mazda_ti_addr_checks[0]))
@@ -64,12 +64,7 @@ static int mazda_rx_hook(CANPacket_t *to_push) {
       vehicle_moving = speed > 10; // moving when speed > 0.1 kph
     }
 
-    if (torque_interceptor_detected) {
-      if (addr == TI_STEER_TORQUE) {
-        int torque_driver_new = GET_BYTE(to_push, 0) - 126;
-        update_sample(&torque_driver, torque_driver_new);
-      }
-    }else{
+    if (!torque_interceptor_detected) {
       if (addr == MAZDA_STEER_TORQUE) {
         int torque_driver_new = GET_BYTE(to_push, 0) - 127;
         update_sample(&torque_driver, torque_driver_new);
@@ -99,6 +94,15 @@ static int mazda_rx_hook(CANPacket_t *to_push) {
 
     generic_rx_checks((addr == MAZDA_LKAS));
   }
+  
+  if (valid && (GET_BUS(to_push) == MAZDA_AUX)) {
+    int addr = GET_ADDR(to_push);
+    if (addr == TI_STEER_TORQUE) {
+      int torque_driver_new = GET_BYTE(to_push, 0) - 126;
+      update_sample(&torque_driver, torque_driver_new);
+    }
+  }
+
   return valid;
 }
 
@@ -113,7 +117,6 @@ static int mazda_tx_hook(CANPacket_t *to_send) {
 
   // Check if msg is sent on the main BUS
   if (bus == MAZDA_MAIN) {
-
     // steer cmd checks
     if (addr == MAZDA_LKAS) {
       int desired_torque = (((GET_BYTE(to_send, 0) & 0x0FU) << 8) | GET_BYTE(to_send, 1)) - MAZDA_MAX_STEER;

selfdrive/car/mazda/carcontroller.py

@@ -33,7 +33,7 @@ class CarController():
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
                                                     CS.out.steeringTorque, CarControllerParams)
 
-      self.steer_rate_limited = (ti_new_steer != ti_apply_steer) or (new_steer != apply_steer)
+      self.steer_rate_limited = (new_steer != apply_steer) and (ti_new_steer != ti_apply_steer)
 
       if CS.out.standstill and frame % 5 == 0:
         # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
@@ -69,7 +69,7 @@ class CarController():
 
     #The ti cannot be detected unless OP sends a can message to it becasue 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, CS.CP.carFingerprint,ti_apply_steer))
+    can_sends.extend(mazdacan.create_ti_steering_control(self.packer, CS.CP.carFingerprint, frame, ti_apply_steer))
     # always send to the stock system
     can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
                                                       frame, apply_steer, CS.cam_lkas))

selfdrive/car/mazda/carstate.py

@@ -24,7 +24,7 @@ class CarState(CarStateBase):
     self.ti_error = 0
     self.ti_lkas_allowed = False
 
-  def update(self, cp, cp_cam):
+  def update(self, cp, cp_cam, cp_body):
 
     ret = car.CarState.new_message()
     ret.wheelSpeeds = self.get_wheel_speeds(
@@ -50,14 +50,14 @@ class CarState(CarStateBase):
                                                                       cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1)
 
     if self.CP.enableTorqueInterceptor:
-      ret.steeringTorque = cp.vl["TI_FEEDBACK"]["TI_TORQUE_SENSOR"]
+      ret.steeringTorque = cp_body.vl["TI_FEEDBACK"]["TI_TORQUE_SENSOR"]
 
-      self.ti_version = cp.vl["TI_FEEDBACK"]["VERSION_NUMBER"]
-      self.ti_state = cp.vl["TI_FEEDBACK"]["STATE"] # DISCOVER = 0, OFF = 1, DRIVER_OVER = 2, RUN=3
-      self.ti_violation = cp.vl["TI_FEEDBACK"]["VIOL"] # 0 = no violation
-      self.ti_error = cp.vl["TI_FEEDBACK"]["ERROR"] # 0 = no error
+      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.vl["TI_FEEDBACK"]["RAMP_DOWN"] == 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
@@ -231,3 +231,27 @@ class CarState(CarStateBase):
       ]
 
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+
+
+  @staticmethod
+  def get_body_can_parser(CP):
+    # this function generates lists for signal, messages and initial values
+    signals = []
+    checks = []
+    # get real driver torque if we are using a torque interceptor
+    if CP.enableTorqueInterceptor:
+      signals += [
+        ("TI_TORQUE_SENSOR", "TI_FEEDBACK", 0),
+        ("CHKSUM", "TI_FEEDBACK", 0),
+        ("VERSION_NUMBER", "TI_FEEDBACK", 0),
+        ("STATE", "TI_FEEDBACK", 0),
+        ("VIOL", "TI_FEEDBACK", 0),
+        ("ERROR", "TI_FEEDBACK", 0),
+        ("RAMP_DOWN", "TI_FEEDBACK", 0),
+      ]
+
+      checks += [
+        ("TI_FEEDBACK", 50),
+      ]
+      
+    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) # changed to back to 0 because my OBD2 port works

selfdrive/car/mazda/interface.py

@@ -147,11 +147,12 @@ class CarInterface(CarInterfaceBase):
 
     self.cp.update_strings(can_strings)
     self.cp_cam.update_strings(can_strings)
+    self.cp_body.update_strings(can_strings)
     if self.CP.enableTorqueInterceptor and not TI.enabled:
       TI.enabled = True
-      self.cp = self.CS.get_can_parser(self.CP)
-    ret = self.CS.update(self.cp, self.cp_cam)
-    ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
+      self.cp_body = self.CS.get_body_can_parser(self.CP)
+    ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
+    ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
 
     # events
     events = self.create_common_events(ret)

selfdrive/car/mazda/mazdacan.py

@@ -1,6 +1,6 @@
 import copy
 
-from selfdrive.car.mazda.values import GEN1, Buttons
+from selfdrive.car.mazda.values import GEN1, Buttons, CAR
 
 
 def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
@@ -60,7 +60,9 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
 
   return packer.make_can_msg("CAM_LKAS", 0, values)
 
-def create_ti_steering_control(packer, car_fingerprint, apply_steer):
+def create_ti_steering_control(packer, car_fingerprint, frame, apply_steer):
+
+  commands = []
 
   key = 3294744160
   chksum = apply_steer
@@ -71,8 +73,18 @@ def create_ti_steering_control(packer, car_fingerprint, 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
 
-  return packer.make_can_msg("CAM_LKAS2", 0, values)
+  # Write to both busses 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))
+
+  commands.append(packer.make_can_msg("CAM_LKAS2", 1, values))
+  return commands
 
 
 def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):