diff --git a/panda b/panda index 0d4e98f605..da4efd21c4 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 0d4e98f6056a69da8933c8a690a803bc96e0e528 +Subproject commit da4efd21c49fcdfaddca7641bfbaff7dd88b763f diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index bda5378dd6..f5347b9d64 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,8 +1,11 @@ +from cereal import car +from opendbc.can.packer import CANPacker from selfdrive.car.mazda import mazdacan from selfdrive.car.mazda.values import CarControllerParams, Buttons -from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits +VisualAlert = car.CarControl.HUDControl.VisualAlert + class CarController(): def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 @@ -10,16 +13,13 @@ class CarController(): self.steer_rate_limited = False self.brake_counter = 0 - def update(self, enabled, CS, frame, actuators): - """ Controls thread """ - + def update(self, c, CS, frame): can_sends = [] + apply_steer = 0 - ### STEER ### - - if enabled: + if c.enabled: # calculate steer and also set limits due to driver torque - new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) + new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer @@ -30,7 +30,6 @@ class CarController(): # TODO: improve the resume trigger logic by looking at actual radar data can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.RESUME)) else: - apply_steer = 0 self.steer_rate_limited = False if CS.out.cruiseState.enabled: # if brake is pressed, let us wait >20ms before trying to disable crz to avoid @@ -46,6 +45,13 @@ class CarController(): self.apply_steer_last = apply_steer + # send HUD alerts + if frame % 50 == 0: + ldw = c.hudControl.visualAlert == VisualAlert.ldw + steer_required = c.hudControl.visualAlert == VisualAlert.steerRequired + can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) + + # send steering command can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint, frame, apply_steer, CS.cam_lkas)) return can_sends diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 6f0721e84e..7b7a1d50bd 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -33,6 +33,7 @@ class CarState(CarStateBase): can_gear = int(cp.vl["GEAR"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"]) ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS1"] == 1 ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS1"] == 1 ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, @@ -82,6 +83,7 @@ class CarState(CarStateBase): self.acc_active_last = ret.cruiseState.enabled self.cam_lkas = cp_cam.vl["CAM_LKAS"] + self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 return ret @@ -93,6 +95,7 @@ class CarState(CarStateBase): # sig_name, sig_address, default ("LEFT_BLINK", "BLINK_INFO", 0), ("RIGHT_BLINK", "BLINK_INFO", 0), + ("HIGH_BEAMS", "BLINK_INFO", 0), ("STEER_ANGLE", "STEER", 0), ("STEER_ANGLE_RATE", "STEER_RATE", 0), ("STEER_TORQUE_SENSOR", "STEER_TORQUE", 0), @@ -162,21 +165,31 @@ class CarState(CarStateBase): if CP.carFingerprint in GEN1: signals += [ # sig_name, sig_address, default - ("LKAS_REQUEST", "CAM_LKAS", 0), - ("CTR", "CAM_LKAS", 0), - ("ERR_BIT_1", "CAM_LKAS", 0), + ("LKAS_REQUEST", "CAM_LKAS", 0), + ("CTR", "CAM_LKAS", 0), + ("ERR_BIT_1", "CAM_LKAS", 0), ("LINE_NOT_VISIBLE", "CAM_LKAS", 0), - ("LDW", "CAM_LKAS", 0), - ("BIT_1", "CAM_LKAS", 1), - ("ERR_BIT_2", "CAM_LKAS", 0), - ("STEERING_ANGLE", "CAM_LKAS", 0), - ("ANGLE_ENABLED", "CAM_LKAS", 0), - ("CHKSUM", "CAM_LKAS", 0), + ("BIT_1", "CAM_LKAS", 1), + ("ERR_BIT_2", "CAM_LKAS", 0), + ("STEERING_ANGLE", "CAM_LKAS", 0), + ("ANGLE_ENABLED", "CAM_LKAS", 0), + ("CHKSUM", "CAM_LKAS", 0), + + ("LINE_VISIBLE", "CAM_LANEINFO", 0), + ("LINE_NOT_VISIBLE", "CAM_LANEINFO", 1), + ("LANE_LINES", "CAM_LANEINFO", 0), + ("BIT1", "CAM_LANEINFO", 0), + ("BIT2", "CAM_LANEINFO", 0), + ("BIT3", "CAM_LANEINFO", 0), + ("NO_ERR_BIT", "CAM_LANEINFO", 1), + ("S1", "CAM_LANEINFO", 0), + ("S1_HBEAM", "CAM_LANEINFO", 0), ] checks += [ # sig_address, frequency - ("CAM_LKAS", 16), + ("CAM_LANEINFO", 2), + ("CAM_LKAS", 16), ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index db9872eeae..3776613fb5 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -95,6 +95,6 @@ class CarInterface(CarInterfaceBase): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators) + can_sends = self.CC.update(c, self.CS, self.frame) self.frame += 1 return can_sends diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index 456f7f672d..a5400e8864 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,5 +1,8 @@ +import copy + from selfdrive.car.mazda.values import GEN1, Buttons + def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): tmp = apply_steer + 2048 @@ -7,10 +10,11 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): lo = tmp & 0xFF hi = tmp >> 8 + # copy values from camera b1 = int(lkas["BIT_1"]) - ldw = int(lkas["LDW"]) 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. @@ -28,7 +32,7 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): # bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ] csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5) - #bytes [ 5 ] [ 6 ] [ 7 ] + # bytes [ 5 ] [ 6 ] [ 7 ] csum = csum - ahi - amd - alo - b2 if ahi == 1: @@ -44,63 +48,72 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): if car_fingerprint in GEN1: values = { - "LKAS_REQUEST" : apply_steer, - "CTR" : ctr, - "ERR_BIT_1" : er1, + "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 + "LDW": ldw, + "BIT_1": b1, + "ERR_BIT_2": er2, + "STEERING_ANGLE": steering_angle, + "ANGLE_ENABLED": b2, + "CHKSUM": csum } return packer.make_can_msg("CAM_LKAS", 0, values) +def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool): + values = copy.copy(cam_msg) + values.update({ + # TODO: what's the difference between all these? do we need to send all? + "HANDS_WARN_3_BITS": 0b111 if steer_required else 0, + "HANDS_ON_STEER_WARN": steer_required, + "HANDS_ON_STEER_WARN_2": steer_required, + + # TODO: right lane works, left doesn't + # TODO: need to do something about L/R + "LDW_WARN_LL": 0, + "LDW_WARN_RL": 0, + }) + return packer.make_can_msg("CAM_LANEINFO", 0, values) + + def create_button_cmd(packer, car_fingerprint, button): - if button == Buttons.CANCEL: - can = 1 - res = 0 - elif button == Buttons.RESUME: - can = 0 - res = 1 - else: - can = 0 - res = 0 + can = int(button == Buttons.CANCEL) + res = int(button == Buttons.RESUME) if car_fingerprint in GEN1: values = { - "CAN_OFF" : can, - "CAN_OFF_INV" : (can + 1) % 2, + "CAN_OFF": can, + "CAN_OFF_INV": (can + 1) % 2, - "SET_P" : 0, - "SET_P_INV" : 1, + "SET_P": 0, + "SET_P_INV": 1, - "RES" : res, - "RES_INV" : (res + 1) % 2, + "RES": res, + "RES_INV": (res + 1) % 2, - "SET_M" : 0, - "SET_M_INV" : 1, + "SET_M": 0, + "SET_M_INV": 1, - "DISTANCE_LESS" : 0, - "DISTANCE_LESS_INV" : 1, + "DISTANCE_LESS": 0, + "DISTANCE_LESS_INV": 1, - "DISTANCE_MORE" : 0, - "DISTANCE_MORE_INV" : 1, + "DISTANCE_MORE": 0, + "DISTANCE_MORE_INV": 1, - "MODE_X" : 0, - "MODE_X_INV" : 1, + "MODE_X": 0, + "MODE_X_INV": 1, - "MODE_Y" : 0, - "MODE_Y_INV" : 1, + "MODE_Y": 0, + "MODE_Y_INV": 1, - "BIT1" : 1, - "BIT2" : 1, - "BIT3" : 1, - "CTR" : 0 + "BIT1": 1, + "BIT2": 1, + "BIT3": 1, + "CTR": 0 } return packer.make_can_msg("CRZ_BTNS", 0, values)