import copy from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car import structs from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.helpers import mean from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD TransmissionType = structs.CarParams.TransmissionType NetworkLocation = structs.CarParams.NetworkLocation STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. self.cluster_min_speed = CV.KPH_TO_MS / 2. self.loopback_lka_steering_cmd_updated = False self.loopback_lka_steering_cmd_ts_nanos = 0 self.pt_lka_steering_cmd_counter = 0 self.cam_lka_steering_cmd_counter = 0 self.buttons_counter = 0 self.prev_distance_button = 0 self.distance_button = 0 def update(self, pt_cp, cam_cp, loopback_cp) -> structs.CarState: ret = structs.CarState() self.prev_cruise_buttons = self.cruise_buttons self.prev_distance_button = self.distance_button self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"] self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"] self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"]) # This is to avoid a fault where you engage while still moving backwards after shifting to D. # An Equinox has been seen with an unsupported status (3), so only check if either wheel is in reverse (2) self.moving_backward = (pt_cp.vl["EBCMWheelSpdRear"]["RLWheelDir"] == 2) or (pt_cp.vl["EBCMWheelSpdRear"]["RRWheelDir"] == 2) # Variables used for avoiding LKAS faults self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 if self.loopback_lka_steering_cmd_updated: self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"] if self.CP.networkLocation == NetworkLocation.fwdCamera: self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] ret.wheelSpeeds = self.get_wheel_speeds( pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"], pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"], pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"], ) ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # sample rear wheel speeds, standstill=True if ECM allows engagement with brake ret.standstill = ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: ret.gearShifter = self.parse_gear_shifter("T") else: ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"] if self.CP.networkLocation == NetworkLocation.fwdCamera: ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0 else: # Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe # that the brake is being intermittently pressed without user interaction. # To avoid a cruise fault we need to use a conservative brake position threshold # https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf ret.brakePressed = ret.brake >= 8 # Regen braking is braking if self.CP.transmissionType == TransmissionType.direct: ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. ret.gasPressed = ret.gas > 1e-5 ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"] ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"] ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"] ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD # 0 inactive, 1 active, 2 temporarily limited, 3 failed self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"] ret.steerFaultTemporary = self.lkas_status == 2 ret.steerFaultPermanent = self.lkas_status == 3 # 1 - open, 0 - closed ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1) # 1 - latched ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0 ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1 ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1) ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL if self.CP.networkLocation == NetworkLocation.fwdCamera: ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0 # openpilot controls nonAdaptive when not pcmCruise if self.CP.pcmCruise: ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3) if self.CP.enableBsm: ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 return ret @staticmethod def get_cam_can_parser(CP): messages = [] if CP.networkLocation == NetworkLocation.fwdCamera: messages += [ ("AEBCmd", 10), ("ASCMLKASteeringCmd", 10), ("ASCMActiveCruiseControlStatus", 25), ] return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.CAMERA) @staticmethod def get_can_parser(CP): messages = [ ("BCMTurnSignals", 1), ("ECMPRDNL2", 10), ("PSCMStatus", 10), ("ESPStatus", 10), ("BCMDoorBeltStatus", 10), ("BCMGeneralPlatformStatus", 10), ("EBCMWheelSpdFront", 20), ("EBCMWheelSpdRear", 20), ("EBCMFrictionBrakeStatus", 20), ("AcceleratorPedal2", 33), ("ASCMSteeringButton", 33), ("ECMEngineStatus", 100), ("PSCMSteeringAngle", 100), ("ECMAcceleratorPos", 80), ] if CP.enableBsm: messages.append(("BCMBlindSpotMonitor", 10)) # Used to read back last counter sent to PT by camera if CP.networkLocation == NetworkLocation.fwdCamera: messages += [ ("ASCMLKASteeringCmd", 0), ] if CP.transmissionType == TransmissionType.direct: messages.append(("EBCMRegenPaddle", 50)) return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.POWERTRAIN) @staticmethod def get_loopback_can_parser(CP): messages = [ ("ASCMLKASteeringCmd", 0), ] return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.LOOPBACK)