Fixes up seatbelt, door open, gear shift, and adds fingerprint for a 2021 macan turbo

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"]

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

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: {

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]),