From 9a44e7ac200faebb720a860d244dce6810ecb856 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Sat, 26 Apr 2025 12:41:14 -0500 Subject: [PATCH] MazdaCan --- selfdrive/car/mazda/mazdacan.py | 223 ++++++++++++++++++++++++-------- 1 file changed, 169 insertions(+), 54 deletions(-) diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index 74f6af04c..919e19677 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) + +