diff --git a/panda/board/main.c b/panda/board/main.c index 3682d1b769..32533ed79a 100644 --- a/panda/board/main.c +++ b/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; diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index e3d05df728..27040281f4 100644 --- a/panda/board/safety/safety_mazda.h +++ b/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; diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index a0c73c0a73..76560e41c0 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/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)) diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 30f77bfdba..1f78a7c220 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/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 diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 9c736543cb..1e5ce1fb18 100755 --- a/selfdrive/car/mazda/interface.py +++ b/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) diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index cfec716202..39c415e385 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/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):