def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) if CP.safetyModel == car.CarParams.SafetyModel.volkswagenPq: # Configure for PQ35/PQ46/NMS network messaging self.get_can_parser = self.get_pq_can_parser self.get_cam_can_parser = self.get_pq_cam_can_parser self.update = self.update_pq if CP.transmissionType == TRANS.automatic: self.shifter_values = can_define.dv["Getriebe_1"][ 'Waehlhebelposition__Getriebe_1_'] else: # Configure for MQB network messaging (default) self.get_can_parser = self.get_mqb_can_parser self.get_cam_can_parser = self.get_mqb_cam_can_parser self.update = self.update_mqb if CP.transmissionType == TRANS.automatic: self.shifter_values = can_define.dv["Getriebe_11"][ 'GE_Fahrstufe'] elif CP.transmissionType == TRANS.direct: self.shifter_values = can_define.dv["EV_Gearshift"][ 'GearPosition'] self.buttonStates = BUTTON_STATES.copy()
def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe'] elif CP.transmissionType == TransmissionType.direct: self.shifter_values = can_define.dv["EV_Gearshift"]['GearPosition'] self.buttonStates = BUTTON_STATES.copy()
def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC_FILES.mqb) if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"] elif CP.transmissionType == TransmissionType.direct: self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"] self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] self.buttonStates = BUTTON_STATES.copy()
def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) self.buttonStatesPrev = BUTTON_STATES.copy() if CP.networkLocation == NetworkLocation.fwdCamera: self.ext_bus = CANBUS.pt self.cp_ext = self.cp else: self.ext_bus = CANBUS.cam self.cp_ext = self.cp_cam
def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) self.displayMetricUnitsPrev = None self.gas_pressed_prev = False self.brake_pressed_prev = False self.cruise_enabled_prev = False self.low_speed_alert = False self.buttonStatesPrev = BUTTON_STATES.copy() # Set up an alias to PT/CAM parser for ACC depending on its detected network location self.cp_acc = self.cp if CP.networkLocation == NWL.fwdCamera else self.cp_cam
def __init__(self, CP, canbus): # initialize can parser self.CP = CP self.car_fingerprint = CP.carFingerprint self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["Getriebe_11"]['GE_Fahrstufe'] self.buttonStates = BUTTON_STATES.copy() # vEgo Kalman filter dt = 0.01 self.v_ego_kf = KF1D(x0=[[0.], [0.]], A=[[1., dt], [0., 1.]], C=[1., 0.], K=[[0.12287673], [0.29666309]])
def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) self.displayMetricUnitsPrev = None self.gas_pressed_prev = False self.brake_pressed_prev = False self.cruise_enabled_prev = False self.low_speed_alert = False self.buttonStatesPrev = BUTTON_STATES.copy() # Set up an alias to PT/CAM parser for ACC depending on its detected network location self.cp_acc = self.cp_cam # timebomb_counter mod self.timebomb_counter = 0 self.wheel_grabbed = False self.timebomb_bypass_counter = 0
def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) self.displayMetricUnitsPrev = None self.buttonStatesPrev = BUTTON_STATES.copy() if CP.networkLocation == NetworkLocation.fwdCamera: self.ext_bus = CANBUS.pt self.cp_ext = self.cp else: self.ext_bus = CANBUS.cam self.cp_ext = self.cp_cam # timebomb_counter mod self.cruise_enabled_prev = False self.timebomb_counter = 0 self.wheel_grabbed = False self.timebomb_bypass_counter = 0
def __init__(self, CP, CarController): self.CP = CP self.CC = None self.frame = 0 self.gasPressedPrev = False self.brakePressedPrev = False self.cruiseStateEnabledPrev = False self.displayMetricUnitsPrev = None self.buttonStatesPrev = BUTTON_STATES.copy() # *** init the major players *** self.CS = CarState(CP, CANBUS) self.VM = VehicleModel(CP) self.pt_cp = get_mqb_pt_can_parser(CP, CANBUS) self.cam_cp = get_mqb_cam_can_parser(CP, CANBUS) # sending if read only is False if CarController is not None: self.CC = CarController(CANBUS, CP.carFingerprint)
def update(self, CC, CS, ext_bus): actuators = CC.actuators hud_control = CC.hudControl can_sends = [] # **** Steering Controls ************************************************ # if self.frame % P.HCA_STEP == 0: # Logic to avoid HCA state 4 "refused": # * Don't steer unless HCA is in state 3 "ready" or 5 "active" # * Don't steer at standstill # * Don't send > 3.00 Newton-meters torque # * Don't send the same torque for > 6 seconds # * Don't send uninterrupted steering for > 360 seconds # One frame of HCA disabled is enough to reset the timer, without zeroing the # torque value. Do that anytime we happen to have 0 torque, or failing that, # when exceeding ~1/3 the 360 second timer. if CC.latActive: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer if apply_steer == 0: hcaEnabled = False self.hcaEnabledFrameCount = 0 else: self.hcaEnabledFrameCount += 1 if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s hcaEnabled = False self.hcaEnabledFrameCount = 0 else: hcaEnabled = True if self.apply_steer_last == apply_steer: self.hcaSameTorqueCount += 1 if self.hcaSameTorqueCount > 1.9 * ( 100 / P.HCA_STEP): # 1.9s apply_steer -= (1, -1)[apply_steer < 0] self.hcaSameTorqueCount = 0 else: self.hcaSameTorqueCount = 0 else: hcaEnabled = False apply_steer = 0 self.apply_steer_last = apply_steer idx = (self.frame / P.HCA_STEP) % 16 can_sends.append( volkswagencan.create_mqb_steering_control( self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) # **** HUD Controls ***************************************************** # if self.frame % P.LDW_STEP == 0: if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw): hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] can_sends.append( volkswagencan.create_mqb_hud_control( self.packer_pt, CANBUS.pt, CC.enabled, CS.out.steeringPressed, hud_alert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, CS.ldw_stock_values, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) # **** ACC Button Controls ********************************************** # # FIXME: this entire section is in desperate need of refactoring if self.CP.pcmCruise: if self.frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: if CC.cruiseControl.cancel: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True elif CC.cruiseControl.resume: # Send Resume button when planner wants car to move self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["resumeCruise"] = True if CS.graMsgBusCounter != self.graMsgBusCounterPrev: self.graMsgBusCounterPrev = CS.graMsgBusCounter if self.graButtonStatesToSend is not None: if self.graMsgSentCount == 0: self.graMsgStartFramePrev = self.frame idx = (CS.graMsgBusCounter + 1) % 16 can_sends.append( volkswagencan.create_mqb_acc_buttons_control( self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None self.graMsgSentCount = 0 new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / P.STEER_MAX self.frame += 1 return new_actuators, can_sends
def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) self.displayMetricUnitsPrev = None self.buttonStatesPrev = BUTTON_STATES.copy()
def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe'] self.buttonStates = BUTTON_STATES.copy()
def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ can_sends = [] # **** Steering Controls ************************************************ # if frame % P.HCA_STEP == 0: # Logic to avoid HCA state 4 "refused": # * Don't steer unless HCA is in state 3 "ready" or 5 "active" # * Don't steer at standstill # * Don't send > 3.00 Newton-meters torque # * Don't send the same torque for > 6 seconds # * Don't send uninterrupted steering for > 360 seconds # One frame of HCA disabled is enough to reset the timer, without zeroing the # torque value. Do that anytime we happen to have 0 torque, or failing that, # when exceeding ~1/3 the 360 second timer. if enabled and CS.out.vEgo > CS.CP.minSteerSpeed and not ( CS.out.standstill or CS.out.steerError or CS.out.steerWarning): new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer if apply_steer == 0: hcaEnabled = False self.hcaEnabledFrameCount = 0 else: self.hcaEnabledFrameCount += 1 if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s hcaEnabled = False self.hcaEnabledFrameCount = 0 else: hcaEnabled = True if self.apply_steer_last == apply_steer: self.hcaSameTorqueCount += 1 if self.hcaSameTorqueCount > 1.9 * ( 100 / P.HCA_STEP): # 1.9s apply_steer -= (1, -1)[apply_steer < 0] self.hcaSameTorqueCount = 0 else: self.hcaSameTorqueCount = 0 else: hcaEnabled = False apply_steer = 0 self.apply_steer_last = apply_steer idx = (frame / P.HCA_STEP) % 16 can_sends.append( volkswagencan.create_mqb_steering_control( self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) # **** HUD Controls ***************************************************** # if frame % P.LDW_STEP == 0: if visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]: hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] can_sends.append( volkswagencan.create_mqb_hud_control( self.packer_pt, CANBUS.pt, enabled, CS.out.steeringPressed, hud_alert, left_lane_visible, right_lane_visible, CS.ldw_stock_values, left_lane_depart, right_lane_depart)) # **** ACC Button Controls ********************************************** # # FIXME: this entire section is in desperate need of refactoring if CS.CP.pcmCruise: if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: if not enabled and CS.out.cruiseState.enabled: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True elif enabled and CS.esp_hold_confirmation: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["resumeCruise"] = True if CS.graMsgBusCounter != self.graMsgBusCounterPrev: self.graMsgBusCounterPrev = CS.graMsgBusCounter if self.graButtonStatesToSend is not None: if self.graMsgSentCount == 0: self.graMsgStartFramePrev = frame idx = (CS.graMsgBusCounter + 1) % 16 can_sends.append( volkswagencan.create_mqb_acc_buttons_control( self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None self.graMsgSentCount = 0 new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / P.STEER_MAX return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ P = CarControllerParams # Send CAN commands. can_sends = [] #-------------------------------------------------------------------------- # # # Prepare HCA_01 Heading Control Assist messages with steering torque. # # # #-------------------------------------------------------------------------- # The factory camera sends at 50Hz while steering and 1Hz when not. When # OP is active, Panda filters HCA_01 from the factory camera and OP emits # HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and # doesn't seem to add value at this time. The rack will accept HCA_01 at # 100Hz if we want to control at finer resolution in the future. if frame % P.HCA_STEP == 0: # FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop # commanding HCA if there's a fault, so the steering rack recovers. if enabled and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning): # FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This # is inherently handled by scaling to STEER_MAX. The rack doesn't seem # to care about up/down rate, but we have some evidence it may do its # own rate limiting, and matching OP helps for accurate tuning. new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer # FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending # a single frame with HCA disabled is an effective workaround. if apply_steer == 0: # We can usually reset the timer for free, just by disabling HCA # when apply_steer is exactly zero, which happens by chance during # many steer torque direction changes. This could be expanded with # a small dead-zone to capture all zero crossings, but not seeing a # major need at this time. hcaEnabled = False self.hcaEnabledFrameCount = 0 else: self.hcaEnabledFrameCount += 1 if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s # The Kansas I-70 Crosswind Problem: if we truly do need to steer # in one direction for > 360 seconds, we have to disable HCA for a # frame while actively steering. Testing shows we can just set the # disabled flag, and keep sending non-zero torque, which keeps the # Panda torque rate limiting safety happy. Do so 3x within the 360 # second window for safety and redundancy. hcaEnabled = False self.hcaEnabledFrameCount = 0 else: hcaEnabled = True # FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds. # This is to detect the sending camera being stuck or frozen. OP # can trip this on a curve if steering is saturated. Avoid this by # reducing torque 0.01 Nm for one frame. Do so 3x within the 6 # second period for safety and redundancy. if self.apply_steer_last == apply_steer: self.hcaSameTorqueCount += 1 if self.hcaSameTorqueCount > 1.9 * ( 100 / P.HCA_STEP): # 1.9s apply_steer -= (1, -1)[apply_steer < 0] self.hcaSameTorqueCount = 0 else: self.hcaSameTorqueCount = 0 else: # Continue sending HCA_01 messages, with the enable flags turned off. hcaEnabled = False apply_steer = 0 self.apply_steer_last = apply_steer idx = (frame / P.HCA_STEP) % 16 can_sends.append( volkswagencan.create_mqb_steering_control( self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) #-------------------------------------------------------------------------- # # # Prepare LDW_02 HUD messages with lane borders, confidence levels, and # # the LKAS status LED. # # # #-------------------------------------------------------------------------- # The factory camera emits this message at 10Hz. When OP is active, Panda # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz. if frame % P.LDW_STEP == 0: if visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired: hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] can_sends.append( volkswagencan.create_mqb_hud_control( self.packer_pt, CANBUS.pt, enabled, CS.out.steeringPressed, hud_alert, left_lane_visible, right_lane_visible, CS.ldw_lane_warning_left, CS.ldw_lane_warning_right, CS.ldw_side_dlc_tlc, CS.ldw_dlc, CS.ldw_tlc, CS.out.standstill, left_lane_depart, right_lane_depart)) #-------------------------------------------------------------------------- # # # Prepare GRA_ACC_01 ACC control messages with button press events. # # # #-------------------------------------------------------------------------- # The car sends this message at 33hz. OP sends it on-demand only for # virtual button presses. # # First create any virtual button press event needed by openpilot, to sync # stock ACC with OP disengagement, or to auto-resume from stop. if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: if not enabled and CS.out.cruiseState.enabled: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True elif enabled and CS.out.standstill: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. # A subset of MQBs like to "creep" too aggressively with this implementation. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["resumeCruise"] = True # OP/Panda can see this message but can't filter it when integrated at the # R242 LKAS camera. It could do so if integrated at the J533 gateway, but # we need a generalized solution that works for either. The message is # counter-protected, so we need to time our transmissions very precisely # to achieve fast and fault-free switching between message flows accepted # at the J428 ACC radar. # # Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP): # # CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6 # EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^ # # If OP needs to send a button press, it waits to see a GRA_ACC_01 message # counter change, and then immediately follows up with the next increment. # The OP message will be sent within about 1ms of the car's message, which # is about 2ms before the car's next message is expected. OP sends for an # arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new # message from the car. # # Because OP's counter is synced to the car, J428 immediately accepts the # OP messages as valid. Further messages from the car get discarded as # duplicates without a fault. When OP stops sending, the extra time gap # (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428 # tolerates the gap just fine and control returns to the car immediately. if CS.graMsgBusCounter != self.graMsgBusCounterPrev: self.graMsgBusCounterPrev = CS.graMsgBusCounter if self.graButtonStatesToSend is not None: if self.graMsgSentCount == 0: self.graMsgStartFramePrev = frame idx = (CS.graMsgBusCounter + 1) % 16 can_sends.append( volkswagencan.create_mqb_acc_buttons_control( self.packer_pt, CANBUS.pt, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None self.graMsgSentCount = 0 return can_sends