diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 13a642f49..2656b09be 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -1,9 +1,10 @@ +import copy from cereal import car, custom from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags +from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, TI_STATE, CarControllerParams class CarState(CarStateBase): def __init__(self, CP, FPCP): @@ -11,17 +12,37 @@ class CarState(CarStateBase): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["GEAR"]["GEAR"] + if CP.flags & MazdaFlags.MANUAL_TRANSMISSION: + self.shifter_values = can_define.dv["MANUAL_GEAR"]["GEAR"] self.crz_btns_counter = 0 self.acc_active_last = False self.low_speed_alert = False self.lkas_allowed_speed = False self.lkas_disabled = False + self.cam_lkas = 0 + self.params = CarControllerParams(CP) self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_cam, frogpilot_toggles): + self.ti_ramp_down = False + self.ti_version = 1 + self.ti_state = TI_STATE.RUN + self.ti_violation = 0 + self.ti_error = 0 + self.ti_lkas_allowed = False + + self.shifting = False + self.torque_converter_lock = True + + self.update = self.update_gen1 + if CP.flags & MazdaFlags.GEN1: + self.update = self.update_gen1 + if CP.flags & MazdaFlags.GEN2: + self.update = self.update_gen2 + + def update_gen1(self, cp, cp_cam, cp_body, frogpilot_variables): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() @@ -51,6 +72,22 @@ class CarState(CarStateBase): ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) + if self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR: + ret.steeringTorque = cp_body.vl["TI_FEEDBACK"]["TI_TORQUE_SENSOR"] + + 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_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 + else: + ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] + ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD + ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD @@ -85,31 +122,96 @@ class CarState(CarStateBase): # 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.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1 ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS - - if ret.cruiseState.enabled: - if not self.lkas_allowed_speed and self.acc_active_last: - self.low_speed_alert = True - else: - self.low_speed_alert = False + if self.CP.flags & MazdaFlags.RADAR_INTERCEPTOR: + self.crz_info = copy.copy(cp_cam.vl["CRZ_INFO"]) + self.crz_cntr = copy.copy(cp_cam.vl["CRZ_CTRL"]) + self.cp_cam = cp_cam + ret.cruiseState.enabled = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1 + ret.cruiseState.available = cp.vl["PEDALS"]["CRZ_AVAILABLE"] == 1 + elif self.CP.flags & MazdaFlags.NO_MRCC: + ret.cruiseState.enabled = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1 + ret.cruiseState.available = cp.vl["PEDALS"]["CRZ_AVAILABLE"] == 1 + else: + ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1 + ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1 # Check if LKAS is disabled due to lack of driver torque when all other states indicate # it should be enabled (steer lockout). Don't warn until we actually get lkas active # and lose it again, i.e, after initial lkas activation - ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked + ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked and not self.ti_lkas_allowed self.acc_active_last = ret.cruiseState.enabled self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] # camera signals - self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 - self.cam_lkas = cp_cam.vl["CAM_LKAS"] - self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] - ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 + if not self.CP.flags & MazdaFlags.NO_FSC: + self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 if not self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR else False + self.cam_lkas = cp_cam.vl["CAM_LKAS"] + self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] + ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 if not self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR else False + self.cp_cam = cp_cam + self.cp = cp + + # FrogPilot CarState functions + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = not self.lkas_disabled + + return ret, fp_ret + + def update_gen2(self, cp, cp_cam, cp_body, frogpilot_variables): + ret = car.CarState.new_message() + fp_ret = custom.FrogPilotCarState.new_message() + + ret.wheelSpeeds = self.get_wheel_speeds( + cp_cam.vl["WHEEL_SPEEDS"]["FL"], + cp_cam.vl["WHEEL_SPEEDS"]["FR"], + cp_cam.vl["WHEEL_SPEEDS"]["RL"], + cp_cam.vl["WHEEL_SPEEDS"]["RR"], + ) + + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # Doesn't match cluster speed exactly + + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(100, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, + cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) + + ret.engineRpm = cp_cam.vl["ENGINE_DATA"]["RPM"] + #self.shifting = cp_cam.vl["GEAR"]["SHIFT"] + #self.torque_converter_lock = cp_cam.vl["GEAR"]["TORQUE_CONVERTER_LOCK"] + + ret.steeringAngleDeg = cp_cam.vl["STEER"]["STEER_ANGLE"] + + ret.steeringTorque = cp_body.vl["EPS_FEEDBACK"]["STEER_TORQUE_SENSOR"] + ret.gas = cp_cam.vl["ENGINE_DATA"]["PEDAL_GAS"] + + unit_conversion = CV.MPH_TO_MS if cp.vl["SYSTEM_SETTINGS"]["IMPERIAL_UNIT"] else CV.KPH_TO_MS + + ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_DRIVER_ALLOWANCE + if self.CP.flags & MazdaFlags.MANUAL_TRANSMISSION: + can_gear = int(cp_cam.vl["MANUAL_GEAR"]["GEAR"]) + else: + can_gear = int(cp_cam.vl["GEAR"]["GEAR"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + ret.gasPressed = ret.gas > 0 + ret.seatbeltUnlatched = False # Cruise will not engage if seatbelt is unlatched (handled by car) + ret.doorOpen = False # Cruise will not engage if door is open (handled by car) + ret.brakePressed = cp.vl["BRAKE_PEDAL"]["BRAKE_PEDAL_PRESSED"] == 1 + ret.brake = .1 + ret.steerFaultPermanent = False # TODO locate signal. Car shows light on dash if there is a fault + ret.steerFaultTemporary = False # TODO locate signal. Car shows light on dash if there is a fault + + ret.standstill = cp_cam.vl["SPEED"]["SPEED"] * unit_conversion < 0.1 + ret.cruiseState.speed = cp.vl["CRUZE_STATE"]["CRZ_SPEED"] * unit_conversion + ret.cruiseState.enabled = (cp.vl["CRUZE_STATE"]["CRZ_STATE"] >= 2) + ret.cruiseState.available = (cp.vl["CRUZE_STATE"]["CRZ_STATE"] != 0) + ret.cruiseState.standstill = ret.standstill + + self.cp = cp + self.cp_cam = cp_cam + self.acc = copy.copy(cp.vl["ACC"]) # FrogPilot CarState functions self.lkas_previously_enabled = self.lkas_enabled @@ -117,23 +219,40 @@ class CarState(CarStateBase): return ret, fp_ret + @staticmethod + def get_ti_messages(CP): + messages = [] + if CP.flags & (MazdaFlags.TORQUE_INTERCEPTOR | MazdaFlags.GEN1): + messages += [ + ("TI_FEEDBACK", 50), + ] + elif CP.flags & MazdaFlags.GEN2: + messages += [ + ("EPS_FEEDBACK", 50), + ("EPS_FEEDBACK2", 50), + ("EPS_FEEDBACK3", 50), + ] + return messages + @staticmethod def get_can_parser(CP, FPCP): messages = [ - # sig_address, frequency - ("BLINK_INFO", 10), - ("STEER", 67), - ("STEER_RATE", 83), - ("STEER_TORQUE", 83), - ("WHEEL_SPEEDS", 100), + ("CRZ_BTNS", 10), ] + if not (CP.flags & MazdaFlags.GEN2): + messages += [ + # sig_address, frequency + ("BLINK_INFO", 10), + ("STEER", 67), + ("STEER_RATE", 83), + ("STEER_TORQUE", 83), + ("WHEEL_SPEEDS", 100), + ] if CP.flags & MazdaFlags.GEN1: messages += [ ("ENGINE_DATA", 100), - ("CRZ_CTRL", 50), ("CRZ_EVENTS", 50), - ("CRZ_BTNS", 10), ("PEDALS", 50), ("BRAKE", 50), ("SEATBELT", 10), @@ -142,6 +261,20 @@ class CarState(CarStateBase): ("BSM", 10), ] + if not (CP.flags & MazdaFlags.RADAR_INTERCEPTOR) and not (CP.flags & MazdaFlags.NO_MRCC): + messages += [ + ("CRZ_CTRL", 50), + ] + + if CP.flags & MazdaFlags.GEN2: + messages += [ + ("BRAKE_PEDAL", 20), + ("CRUZE_STATE", 10), + ("BLINK_INFO", 10), + ("ACC", 50), + ("SYSTEM_SETTINGS", 10), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod @@ -149,10 +282,44 @@ class CarState(CarStateBase): messages = [] if CP.flags & MazdaFlags.GEN1: + if not CP.flags & MazdaFlags.NO_FSC: + messages += [ + # address, frequency + ("CAM_LANEINFO", 2), + ("CAM_LKAS", 16), + ] + + if CP.flags & MazdaFlags.RADAR_INTERCEPTOR: + messages += [ + ("CRZ_INFO", 50), + ("CRZ_CTRL", 50), + ] + for addr in range(361,367): + msg = f"RADAR_{addr}" + messages += [ + (msg,10), + ] + + if CP.flags & MazdaFlags.GEN2: messages += [ - # sig_address, frequency - ("CAM_LANEINFO", 2), - ("CAM_LKAS", 16), + ("ENGINE_DATA", 100), + ("STEER_TORQUE", 100), + ("WHEEL_SPEEDS", 100), + ("STEER", 100), + ("SPEED", 50), ] + if CP.flags & MazdaFlags.MANUAL_TRANSMISSION: + messages += [ + ("MANUAL_GEAR", 50), + ] + else: + messages += [ + ("GEAR", 40), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) + + @staticmethod + def get_body_can_parser(CP): + return CANParser(DBC[CP.carFingerprint]["pt"], CarState.get_ti_messages(CP), 1)