diff --git a/opendbc b/opendbc index 45eb0465f34dc3..5468ed8e94db41 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 45eb0465f34dc3d837bbe77733002650464678cf +Subproject commit 5468ed8e94db41160233c0313bb3028c65b4e962 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index cde7705f1ed800..732aaa4dbd59cd 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -29,14 +29,18 @@ def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 self.gra_acc_counter_last = None self.frame = 0 + self.eps_timer_workaround = True # For testing, replace with CP.carFingerprint in (PQ_CARS, MLB_CARS) self.eps_timer_soft_disable_alert = False self.hca_frame_timer_running = 0 + self.hca_frame_timer_resetting = 0 + self.hca_frame_low_torque = 0 self.hca_frame_same_torque = 0 def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl can_sends = [] + output_steer = 0 # **** Steering Controls ************************************************ # @@ -49,6 +53,10 @@ def update(self, CC, CS, now_nanos): # * Don't send uninterrupted steering for > 360 seconds # MQB racks reset the uninterrupted steering timer after a single frame # of HCA disabled; this is done whenever output happens to be zero. + # PQ35, PQ46, NMS and MLB racks need > 1 second to reset; try to reset + # if engaged for a long time and torque output is currently low. Resets + # are aborted early if torque demand rises. Long resets, completed or + # not, need apply_steer reset to 0 on exit due to rate limit safety. if CC.latActive: new_steer = int(round(actuators.steer * self.CCP.STEER_MAX)) @@ -62,16 +70,33 @@ def update(self, CC, CS, now_nanos): else: self.hca_frame_same_torque = 0 hca_enabled = abs(apply_steer) > 0 + if self.eps_timer_workaround and self.hca_frame_timer_running >= self.CCP.STEER_TIME_BM / DT_CTRL: + if abs(apply_steer) <= self.CCP.STEER_LOW_TORQUE: + self.hca_frame_low_torque += self.CCP.STEER_STEP + if self.hca_frame_low_torque >= self.CCP.STEER_TIME_LOW_TORQUE / DT_CTRL: + hca_enabled = False + else: + self.hca_frame_low_torque = 0 + if self.hca_frame_timer_resetting > 0: + apply_steer = 0 else: + self.hca_frame_low_torque = 0 hca_enabled = False apply_steer = 0 - if not hca_enabled: - self.hca_frame_timer_running = 0 + if hca_enabled: + output_steer = apply_steer + self.hca_frame_timer_resetting = 0 + else: + output_steer = 0 + self.hca_frame_timer_resetting += self.CCP.STEER_STEP + if self.hca_frame_timer_resetting >= 1.1 / DT_CTRL or not self.eps_timer_workaround: + self.hca_frame_timer_running = 0 + apply_steer = 0 self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL self.apply_steer_last = apply_steer - can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hca_enabled)) + can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, output_steer, hca_enabled)) if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT: # Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque @@ -120,7 +145,7 @@ def update(self, CC, CS, now_nanos): cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume)) new_actuators = actuators.copy() - new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX + new_actuators.steer = output_steer / self.CCP.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index a8f68b3474b8fe..cf6a40ccc4de83 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -52,14 +52,15 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type): brake_pedal_pressed = bool(pt_cp.vl["Motor_03"]["MO_Fahrer_bremst"]) ret.espDisabled = pt_cp.vl["ESP_01"]["ESP_Tastung_passiv"] != 0 - # TODO: find gearshift signal - ret.gearShifter = GearShifter.drive + # Gearshift signal + # TODO: alternatives to string parsing? edit parse_gear_shifter or edit DBC string values + ret.gearShifter = self.parse_gear_shifter(str(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_03"]["GE_Waehlhebel"], "P")).replace('POSITION_', '')) # TODO: this is only present on powertrain - #ret.doorOpen = any([pt_cp.vl["Gateway_05"]["FT_Tuer_geoeffnet"], - # pt_cp.vl["Gateway_05"]["BT_Tuer_geoeffnet"], - # pt_cp.vl["Gateway_05"]["HL_Tuer_geoeffnet"], - # pt_cp.vl["Gateway_05"]["HR_Tuer_geoeffnet"]]) + ret.doorOpen = any([pt_cp.vl["Gateway_05"]["FT_Tuer_geoeffnet"], + pt_cp.vl["Gateway_05"]["BT_Tuer_geoeffnet"], + pt_cp.vl["Gateway_05"]["HL_Tuer_geoeffnet"], + pt_cp.vl["Gateway_05"]["HR_Tuer_geoeffnet"]]) # TODO: is this the instantaneous or the comfort blink signal? ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_01"]["BM_links"]) @@ -158,8 +159,7 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type): ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # Update seatbelt fastened status. - # FIXME: disabled for Macan testing - #ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3 + ret.seatbeltUnlatched = pt_cp.vl["Gateway_06"]["AB_Gurtschloss_FA"] != 3 # Consume blind-spot monitoring info/warning LED states, if available. # Infostufe: BSM LED on, Warnung: BSM LED flashing @@ -174,11 +174,12 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type): ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation # Update ACC setpoint. When the setpoint is zero or there's an error, the - # radar sends a set-speed of ~90.69 m/s / 203mph. - # TODO: ugly hack while testing CC-only S4 + # radar sends a set-speed of ~90.69 m/s / 203.19 mph / 327.04 kph + # TODO: ugly hack while testing CC-only S4, also macan. + # EDIT: bumping the upper limit to 100 to match the stock functionality (100 max CC set point on USA Macan) if self.CP.pcmCruise and not self.CP.flags & VolkswagenFlags.MLB: ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw_02"] * CV.KPH_TO_MS - if ret.cruiseState.speed > 90: + if ret.cruiseState.speed > 100: ret.cruiseState.speed = 0 # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough @@ -374,12 +375,12 @@ def get_can_parser_mlb(CP): ("LS_01", 5), # From J533 CAN gateway (via LIN from steering wheel controls) # FIXME: Testing using radar state instead of TSK state for Macan #("TSK_02", 33), # From J623 Engine control module - # FIXME: Macan gateway and airbag state on powertrain - #("Airbag_02", 5), # From J234 Airbag control module - #("Gateway_05", 10), # From J533 CAN gateway (aggregated data) + ("Gateway_05", 10), # From J533 CAN gateway (aggregated data) + ("Gateway_06", 10), # TODO: what is source of this signal? ("Kombi_01", 2), # From J285 Instrument cluster ("Blinkmodi_01", 0), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active) ("Kombi_03", 0), # From J285 instrument cluster (not present on older cars, 1Hz when present) + ("Getriebe_03", 50), # TODO: what is the source of this signal? transmission ecu? ] # TODO: gear shift parsing diff --git a/selfdrive/car/volkswagen/fingerprints.py b/selfdrive/car/volkswagen/fingerprints.py index c595f9be7464b2..dcfaabddb96cc4 100644 --- a/selfdrive/car/volkswagen/fingerprints.py +++ b/selfdrive/car/volkswagen/fingerprints.py @@ -955,19 +955,38 @@ }, CAR.PORSCHE_MACAN_MK1: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8795B906259 \xf1\x890005', b'\xf1\x8795B90652013\xf1\x893485', + b'\xf1\x8795B907551D \xf1\x890006', + b'\xf1\x8795B907559E \xf1\x890006', + b'\xf1\x8795B906259BE\xf1\x890001', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8795B927156AH\xf1\x890014', b'\xf1\x8795B927156CE\xf1\x890022', + b'\xf1\x8795B927156FG\xf1\x890005', + b'\xf1\x8795B927156JH\xf1\x890001', + b'\xf1\x8795B927156KB\xf1\x890001', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x8795B959655D \xf1\x890110\xf1\x82\x05064702D312', b'\xf1\x8795B959655F \xf1\x890130\xf1\x82\x05065Q033513', + b'\xf1\x8795B959655F \xf1\x890130\xf1\x82\x050690033514', + b'\xf1\x8795B959655G \xf1\x890150\xf1\x82\x0506B1033514', + b'\xf1\x8795B959655G \xf1\x890150\xf1\x82\x0506CJ02D417', ], (Ecu.eps, 0x712, None): [ + b'\xf1\x8795B909144B \xf1\x890902\xf1\x82\x01m', b'\xf1\x8795B909144K \xf1\x891902\xf1\x82\x02\x04#\x04#', + b'\xf1\x8795B909144K \xf1\x891902\xf1\x82\x02\x04/\x04/', + #TODO: garon missing + b'\xf1\x8795B909144K \xf1\x891902\xf1\x82\x02\x046\x046', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x8795B907567B\x00\xf1\x890800\xf1\x82108', + b'\xf1\x8795B907567C\x00\xf1\x890400\xf1\x82104', + b'\xf1\x8795B907567G\x00\xf1\x890410\xf1\x82104', + b'\xf1\x8795B907567H\x00\xf1\x890430\xf1\x82104', ], }, CAR.SKODA_FABIA_MK4: { diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index eb6dcb0df7061d..ed3abba28ccf3f 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -31,8 +31,12 @@ class CarControllerParams: STEER_DRIVER_FACTOR = 1 # from dbc STEER_TIME_MAX = 360 # Max time that EPS allows uninterrupted HCA steering control + STEER_TIME_BM = STEER_TIME_MAX - 120 # Attempts to mitigate the EPS max steer timer begin STEER_TIME_ALERT = STEER_TIME_MAX - 10 # If mitigation fails, time to soft disengage before EPS timer expires + STEER_LOW_TORQUE = STEER_MAX * 0.2 # Steer timer mitigation performed when torque output under 20% + STEER_TIME_LOW_TORQUE = 0.5 # Wait for this duration of STEER_LOW_TORQUE to begin mitigation STEER_TIME_STUCK_TORQUE = 1.9 # EPS limits same torque to 6 seconds, reset timer 3x within that period + STEER_TIME_RESET = 1.1 # Duration of HCA disable needed for effective EPS timer reset DEFAULT_MIN_STEER_SPEED = 0.4 # m/s, newer EPS racks fault below this speed, don't show a low speed alert @@ -72,7 +76,7 @@ def __init__(self, CP): } else: - self.LDW_STEP = 10 # LDW_02 message frequency 10Hz + self.LDW_STEP = 4 # LDW_02 message frequency 10Hz self.ACC_HUD_STEP = 6 # ACC_02 message frequency 16Hz self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] @@ -83,7 +87,7 @@ def __init__(self, CP): self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) # TODO: populate shifter enums - self.shifter_values = None + self.shifter_values = can_define.dv["Getriebe_03"]["GE_Waehlhebel"] self.BUTTONS = [ Button(car.CarState.ButtonEvent.Type.setCruise, "LS_01", "LS_Tip_Setzen", [1]), Button(car.CarState.ButtonEvent.Type.resumeCruise, "LS_01", "LS_Tip_Wiederaufnahme", [1]),