From 05bbd08ed362de463cc28a3c9cb634ab803de729 Mon Sep 17 00:00:00 2001 From: Ryley Date: Thu, 21 Apr 2022 21:10:21 -0500 Subject: [PATCH] enable mazda long control --- panda/board/safety/safety_mazda.h | 8 +++ selfdrive/car/mazda/carcontroller.py | 4 ++ selfdrive/car/mazda/carstate.py | 92 +++++++++++++++++++++++++--- selfdrive/car/mazda/interface.py | 8 ++- selfdrive/car/mazda/mazdacan.py | 90 +++++++++++++-------------- 5 files changed, 144 insertions(+), 58 deletions(-) diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index 0756dd36e..439354108 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -201,6 +201,14 @@ static int mazda_fwd_hook(int bus, CANPacket_t *to_fwd) { } else if (bus == MAZDA_CAM) { block |= (addr == MAZDA_LKAS); block |= (addr == MAZDA_LKAS_HUD); + block |= (addr == MAZDA_CRZ_INFO); + block |= (addr == MAZDA_CRZ_CTRL); + block |= (addr == MAZDA_RADAR_361); + block |= (addr == MAZDA_RADAR_362); + block |= (addr == MAZDA_RADAR_363); + block |= (addr == MAZDA_RADAR_364); + block |= (addr == MAZDA_RADAR_365); + block |= (addr == MAZDA_RADAR_366); if (!block) { bus_fwd = MAZDA_MAIN; diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 03c9e9b88..759c3cdec 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -74,4 +74,8 @@ class CarController(): can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint, frame, apply_steer, CS.cam_lkas)) + """ACC RADAR COMMAND""" + if frame % 2 == 0: + can_sends.extend(mazdacan.create_radar_command(self.packer, CS.CP.carFingerprint, frame, c, CS)) + return can_sends diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 2bd631566..de4ca89e7 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -36,8 +36,8 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # Match panda speed reading - speed_kph = cp.vl["ENGINE_DATA"]["SPEED"] - ret.standstill = speed_kph < .1 + self.speed_kph = cp.vl["ENGINE_DATA"]["SPEED"] + ret.standstill = self.speed_kph < .1 can_gear = int(cp.vl["GEAR"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) @@ -86,15 +86,15 @@ class CarState(CarStateBase): # LKAS is enabled at 52kph going up and disabled at 45kph going down # wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes - if speed_kph > LKAS_LIMITS.ENABLE_SPEED: + if self.speed_kph > LKAS_LIMITS.ENABLE_SPEED: self.lkas_allowed_speed = True - elif speed_kph < LKAS_LIMITS.DISABLE_SPEED: + elif self.speed_kph < LKAS_LIMITS.DISABLE_SPEED: self.lkas_allowed_speed = False # 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.available = cp_cam.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1 + ret.cruiseState.enabled = cp.vl["CRZ_EVENTS"]["CRUISE_ACTIVE_CAR_MOVING"] == 1 ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS # On if no driver torque the last 5 seconds @@ -110,6 +110,9 @@ class CarState(CarStateBase): self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 + self.cp_cam = cp_cam + self.cp = cp + return ret @staticmethod @@ -142,8 +145,7 @@ class CarState(CarStateBase): ("LKAS_BLOCK", "STEER_RATE", 0), ("LKAS_TRACK_STATE", "STEER_RATE", 0), ("HANDS_OFF_5_SECONDS", "STEER_RATE", 0), - ("CRZ_ACTIVE", "CRZ_CTRL", 0), - ("CRZ_AVAILABLE", "CRZ_CTRL", 0), + ("CRUISE_ACTIVE_CAR_MOVING", "CRZ_EVENTS", 0), ("CRZ_SPEED", "CRZ_EVENTS", 0), ("STANDSTILL", "PEDALS", 0), ("BRAKE_ON", "PEDALS", 0), @@ -163,7 +165,7 @@ class CarState(CarStateBase): checks += [ ("ENGINE_DATA", 100), - ("CRZ_CTRL", 50), + ("CRZ_EVENTS", 50), ("CRZ_BTNS", 10), ("PEDALS", 50), @@ -219,11 +221,81 @@ class CarState(CarStateBase): ("S1", "CAM_LANEINFO", 0), ("S1_HBEAM", "CAM_LANEINFO", 0), ] - + checks += [ # sig_address, frequency ("CAM_LANEINFO", 2), ("CAM_LKAS", 16), ] + signals += [ + ("CRZ_ACTIVE", "CRZ_CTRL", 0), + ("CRZ_AVAILABLE", "CRZ_CTRL", 0), + ("DISTANCE_SETTING", "CRZ_CTRL", 0), + ("ACC_ACTIVE_2", "CRZ_CTRL", 0), + ("DISABLE_TIMER_1", "CRZ_CTRL", 0), + ("DISABLE_TIMER_2", "CRZ_CTRL", 0), + ("NEW_SIGNAL_1", "CRZ_CTRL", 0), + ("NEW_SIGNAL_2", "CRZ_CTRL", 0), + ("NEW_SIGNAL_3", "CRZ_CTRL", 0), + ("NEW_SIGNAL_4", "CRZ_CTRL", 0), + ("NEW_SIGNAL_5", "CRZ_CTRL", 0), + ("NEW_SIGNAL_6", "CRZ_CTRL", 0), + ] + signals += [ + ("STATUS", "CRZ_INFO", 0), + ("STATIC_1", "CRZ_INFO", 0), + ("ACCEL_CMD", "CRZ_INFO", 0), + ("CRZ_ENDED", "CRZ_INFO", 0), + ("ACC_SET_ALLOWED", "CRZ_INFO", 0), + ("ACC_ACTIVE", "CRZ_INFO", 0), + ("MYSTERY_BIT", "CRZ_INFO", 0), + ("CTR1", "CRZ_INFO", 0), + ("CHECKSUM", "CRZ_INFO", 0), + ] + signals += [ + ("DISTANCE_LEAD", "RADAR_361", 0), + ("RELATIVE_VEL_LEAD", "RADAR_361", 0), + ("STATIC_1", "RADAR_361", 0), + ("DISTANCE_RELATED", "RADAR_361", 0), + ("STATIC_2", "RADAR_361", 0), + ("SPEED_INVERSE", "RADAR_361", 0), + ("IS_MOVING", "RADAR_361", 0), + ("CTR", "RADAR_361", 0), + ] + signals += [ + ("STEER_ANGLE", "RADAR_362", 0), + ("STATIC_1", "RADAR_362", 0), + ("STATIC_2", "RADAR_362", 0), + ("STATIC_3", "RADAR_362", 0), + ("CTR", "RADAR_362", 0), + ] + signals += [ + ("STATIC_1", "RADAR_363", 0), + ("STATIC_2", "RADAR_363", 0), + ] + signals += [ + ("STATIC_1", "RADAR_364", 0), + ("STATIC_2", "RADAR_364", 0), + ] + signals += [ + ("STATIC_1", "RADAR_365", 0), + ("STATIC_2", "RADAR_365", 0), + ] + signals += [ + ("STATIC_1", "RADAR_366", 0), + ("STATIC_2", "RADAR_366", 0), + ] + checks += [ + ("CRZ_CTRL", 50), # Not blocked in panda 0x21BC + ("CRZ_INFO", 50), # Blocked in panda 0x21B + ("RADAR_361", 10), # 0x361 + ("RADAR_362", 10), # 0x362 + ("RADAR_363", 10), # Likely containes radar tracks 0x363 + ("RADAR_364", 10), # Likely containes radar tracks 0x364 + ("RADAR_365", 10), # 0x365 + ("RADAR_366", 10), # 0x366 + #("RADAR_499_STATIC", 10), # 0x499 + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index fadcd2629..252cc52ae 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -28,9 +28,11 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = False # candidate not in [CAR.CX9_2021] - #ret.enableTorqueInterceptor = 0x24A in fingerprint[0] - if ret.enableTorqueInterceptor: - print("Recieving torque interceptor signal.") + ret.openpilotLongitudinalControl = True + ret.longitudinalTuning.kpBP = [0., 10.] + ret.longitudinalTuning.kpV = [.5, 0.3,] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.06] ret.steerActuatorDelay = 0.1 ret.steerRateCost = 1.0 diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index 313ca4e1e..48a369c38 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -132,14 +132,14 @@ def create_button_cmd(packer, car_fingerprint, counter, button): return packer.make_can_msg("CRZ_BTNS", 0, values) -def create_radar_command(packer, car_fingerprint, frame, actuators, enabled, cp_cam, cp, cs): +def create_radar_command(packer, car_fingerprint, frame, c, CS): RI.active = True accel = 0 - radar_accel = int(cp_cam.vl["CRZ_INFO"]["ACCEL_CMD"]) # get stock accel command. dbc offset should be applied already. + radar_accel = int(CS.cp_cam.vl["CRZ_INFO"]["ACCEL_CMD"]) # get stock accel command. dbc offset should be applied already. ret = [] # request low speed mode transition - if cs.speed < 30: #kmh + if CS.speed_kph < 30: if not RI.low_speed_mode: RI.reset = True # request reset of PID loop RI.radar_accel = radar_accel # save radar accel value to have a smooth transition into low speed mode @@ -152,36 +152,36 @@ def create_radar_command(packer, car_fingerprint, frame, actuators, enabled, cp_ # after we have transitioned to low speed mode, we use the vision only accel command if RI.low_speed_mode: # this is set true in longcontrol.py - accel = actuators.accel * 2000 + accel = c.actuators.accel * 2000 clip(accel, -4000, 1000) else: accel = radar_accel if car_fingerprint in GEN1: values_21B = { - "ACC_ACTIVE" : int(enabled), - "ACC_SET_ALLOWED" : int(bool(int(cp.vl["GEAR"]["GEAR"]) & 4)), # we can set ACC_SET_ALLOWED bit when in drive. Allows crz to be set from 1kmh. + "ACC_ACTIVE" : int(c.enabled), + "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_ENDED" : 0, # this should keep acc on down to 5km/h on my 2018 M3 "ACCEL_CMD" : accel, - "STATIC_1" : int(cp_cam.vl["CRZ_INFO"]["STATIC_1"]), #0x7FF, - "STATUS" : int(cp_cam.vl["CRZ_INFO"]["STATUS"]), #1 - "MYSTERY_BIT" : int(cp_cam.vl["CRZ_INFO"]["MYSTERY_BIT"]), - "CTR1" : int(cp_cam.vl["CRZ_INFO"]["CTR1"]) + "STATIC_1" : int(CS.cp_cam.vl["CRZ_INFO"]["STATIC_1"]), #0x7FF, + "STATUS" : int(CS.cp_cam.vl["CRZ_INFO"]["STATUS"]), #1 + "MYSTERY_BIT" : int(CS.cp_cam.vl["CRZ_INFO"]["MYSTERY_BIT"]), + "CTR1" : int(CS.cp_cam.vl["CRZ_INFO"]["CTR1"]) } values_21C = { - "CRZ_ACTIVE" : int(enabled), - "CRZ_AVAILABLE" : int(cp_cam.vl["CRZ_CTRL"]["CRZ_AVAILABLE"]), - "DISTANCE_SETTING" : int(cp_cam.vl["CRZ_CTRL"]["DISTANCE_SETTING"]), - "ACC_ACTIVE_2" : int(enabled), + "CRZ_ACTIVE" : int(c.enabled), + "CRZ_AVAILABLE" : int(CS.cp_cam.vl["CRZ_CTRL"]["CRZ_AVAILABLE"]), + "DISTANCE_SETTING" : int(CS.cp_cam.vl["CRZ_CTRL"]["DISTANCE_SETTING"]), + "ACC_ACTIVE_2" : int(c.enabled), "DISABLE_TIMER_1" : 0, "DISABLE_TIMER_2" : 0, - "NEW_SIGNAL_1" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_1"]), - "NEW_SIGNAL_2" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_2"]), - "NEW_SIGNAL_3" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_3"]), - "NEW_SIGNAL_4" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_4"]), - "NEW_SIGNAL_5" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_5"]), - "NEW_SIGNAL_6" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_6"]), + "NEW_SIGNAL_1" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_1"]), + "NEW_SIGNAL_2" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_2"]), + "NEW_SIGNAL_3" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_3"]), + "NEW_SIGNAL_4" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_4"]), + "NEW_SIGNAL_5" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_5"]), + "NEW_SIGNAL_6" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_6"]), } ret.append(packer.make_can_msg("CRZ_INFO", 0, values_21B)) @@ -189,41 +189,41 @@ def create_radar_command(packer, car_fingerprint, frame, actuators, enabled, cp_ if (frame % 10 == 0): values_361 = { - "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), - "SPEED_INVERSE" : int(cp_cam.vl["RADAR_361"]["SPEED_INVERSE"]), - "IS_MOVING" : int(cp_cam.vl["RADAR_361"]["IS_MOVING"]), - "DISTANCE_LEAD" : int(cp_cam.vl["RADAR_361"]["DISTANCE_LEAD"]), - "DISTANCE_RELATED" : int(cp_cam.vl["RADAR_361"]["DISTANCE_RELATED"]), - "RELATIVE_VEL_LEAD" : int(cp_cam.vl["RADAR_361"]["RELATIVE_VEL_LEAD"]), - "STATIC_1" : int(cp_cam.vl["RADAR_361"]["STATIC_1"]), - "STATIC_2" : int(cp_cam.vl["RADAR_361"]["STATIC_2"]) + "CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]), + "SPEED_INVERSE" : int(CS.cp_cam.vl["RADAR_361"]["SPEED_INVERSE"]), + "IS_MOVING" : int(CS.cp_cam.vl["RADAR_361"]["IS_MOVING"]), + "DISTANCE_LEAD" : int(CS.cp_cam.vl["RADAR_361"]["DISTANCE_LEAD"]), + "DISTANCE_RELATED" : int(CS.cp_cam.vl["RADAR_361"]["DISTANCE_RELATED"]), + "RELATIVE_VEL_LEAD" : int(CS.cp_cam.vl["RADAR_361"]["RELATIVE_VEL_LEAD"]), + "STATIC_1" : int(CS.cp_cam.vl["RADAR_361"]["STATIC_1"]), + "STATIC_2" : int(CS.cp_cam.vl["RADAR_361"]["STATIC_2"]) } values_362 = { - "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), - "STEER_ANGLE" : int(cp_cam.vl["RADAR_362"]["STEER_ANGLE"]), - "STATIC_1" : int(cp_cam.vl["RADAR_362"]["STATIC_1"]), - "STATIC_2" : int(cp_cam.vl["RADAR_362"]["STATIC_2"]), - "STATIC_3" : int(cp_cam.vl["RADAR_362"]["STATIC_3"]) + "CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]), + "STEER_ANGLE" : int(CS.cp_cam.vl["RADAR_362"]["STEER_ANGLE"]), + "STATIC_1" : int(CS.cp_cam.vl["RADAR_362"]["STATIC_1"]), + "STATIC_2" : int(CS.cp_cam.vl["RADAR_362"]["STATIC_2"]), + "STATIC_3" : int(CS.cp_cam.vl["RADAR_362"]["STATIC_3"]) } values_363 = { - "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), - "STATIC_1" : int(cp_cam.vl["RADAR_363"]["STATIC_1"]), - "STATIC_2" : int(cp_cam.vl["RADAR_363"]["STATIC_2"]) + "CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]), + "STATIC_1" : int(CS.cp_cam.vl["RADAR_363"]["STATIC_1"]), + "STATIC_2" : int(CS.cp_cam.vl["RADAR_363"]["STATIC_2"]) } values_364 = { - "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), - "STATIC_1" : int(cp_cam.vl["RADAR_364"]["STATIC_1"]), - "STATIC_2" : int(cp_cam.vl["RADAR_364"]["STATIC_2"]) + "CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]), + "STATIC_1" : int(CS.cp_cam.vl["RADAR_364"]["STATIC_1"]), + "STATIC_2" : int(CS.cp_cam.vl["RADAR_364"]["STATIC_2"]) } values_365 = { - "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), - "STATIC_1" : int(cp_cam.vl["RADAR_365"]["STATIC_1"]), - "STATIC_2" : int(cp_cam.vl["RADAR_365"]["STATIC_2"]) + "CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]), + "STATIC_1" : int(CS.cp_cam.vl["RADAR_365"]["STATIC_1"]), + "STATIC_2" : int(CS.cp_cam.vl["RADAR_365"]["STATIC_2"]) } values_366 = { - "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), - "STATIC_1" : int(cp_cam.vl["RADAR_366"]["STATIC_1"]), - "STATIC_2" : int(cp_cam.vl["RADAR_366"]["STATIC_2"]) + "CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]), + "STATIC_1" : int(CS.cp_cam.vl["RADAR_366"]["STATIC_1"]), + "STATIC_2" : int(CS.cp_cam.vl["RADAR_366"]["STATIC_2"]) } ret.append(packer.make_can_msg("RADAR_361", 0, values_361))