refactor all the things

panda

@@ -1 +1 @@
-Subproject commit 0e8854b48ff81cc61e30e9135091846ab5d9b887
+Subproject commit b0472024d324984d0453cc5d0ae506d58980bf91

selfdrive/car/__init__.py

@@ -29,7 +29,7 @@ def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilde
   for b in buttonEvents:
     # do enable on both accel and decel buttons
     if not pcm_cruise:
-      if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed:
+      if b.type in (ButtonType.setCruise, ButtonType.resumeCruise, ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed:
         events.append(EventName.buttonEnable)
     # do disable on button down
     if b.type == ButtonType.cancel and b.pressed:

selfdrive/car/volkswagen/carcontroller.py

@@ -15,6 +15,7 @@ class CarController:
     self.CP = CP
     self.CCP = CarControllerParams(CP)
     self.CCS = pqcan if CP.carFingerprint in PQ_CARS else mqbcan
+    self.pass_through_enabled = CP.carFingerprint not in PQ_CARS
     self.packer_pt = CANPacker(dbc_name)
 
     self.apply_steer_last = 0
@@ -27,61 +28,8 @@ class CarController:
   def update(self, CC, CS, ext_bus):
     actuators = CC.actuators
     hud_control = CC.hudControl
-
     can_sends = []
 
-    # **** Acceleration and Braking Controls ******************************** #
-
-    if CS.CP.openpilotLongitudinalControl:
-      if self.frame % self.CCP.ACC_CONTROL_STEP == 0:
-        if CS.tsk_status in [2, 3, 4, 5]:
-          acc_status = 3 if CC.longActive else 2
-        else:
-          acc_status = CS.tsk_status
-
-        accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0
-
-        # FIXME: this needs to become a proper state machine
-        acc_hold_request, acc_hold_release, acc_hold_type, stopping_distance = False, False, 0, 20.46
-        if actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < 0.2:
-          self.acc_stopping = True
-          acc_hold_request = True
-          if CS.out.cruiseState.standstill:
-            acc_hold_type = 1  # hold
-            stopping_distance = 3.5
-          else:
-            acc_hold_type = 3  # hold_standby
-            stopping_distance = 0.5
-        elif CC.longActive:
-          if self.acc_stopping:
-            self.acc_starting = True
-            self.acc_stopping = False
-          self.acc_starting &= CS.out.vEgo < 1.5
-          acc_hold_release = self.acc_starting
-          acc_hold_type = 4 if self.acc_starting and CS.out.vEgo < 0.1 else 0  # startup
-        else:
-          self.acc_stopping, self.acc_starting = False, False
-
-        cb_pos = 0.0 if hud_control.leadVisible or CS.out.vEgo < 2.0 else 0.1  # react faster to lead cars, also don't get hung up at DSG clutch release/kiss points when creeping to stop
-        cb_neg = 0.0 if accel < 0 else 0.2  # IDK why, but stock likes to zero this out when accel is negative
-
-        can_sends.append(self.CCS.create_acc_06_control(self.packer_pt, CANBUS.pt, CC.longActive, acc_status,
-                                                                 accel, self.acc_stopping, self.acc_starting,
-                                                                 cb_pos, cb_neg, CS.acc_type))
-        can_sends.append(self.CCS.create_acc_07_control(self.packer_pt, CANBUS.pt, CC.longActive,
-                                                                 accel, acc_hold_request, acc_hold_release,
-                                                                 acc_hold_type, stopping_distance))
-
-      if self.frame % self.CCP.ACC_HUD_STEP == 0:
-        if hud_control.leadVisible:
-          lead_distance = 512 if CS.digital_cluster_installed else 8  # TODO: look up actual distance to lead
-        else:
-          lead_distance = 0
-
-        can_sends.append(self.CCS.create_acc_02_control(self.packer_pt, CANBUS.pt, CS.tsk_status,
-                                                                 hud_control.setSpeed * CV.MS_TO_KPH, lead_distance))
-        can_sends.append(self.CCS.create_acc_04_control(self.packer_pt, CANBUS.pt, CS.acc_04_stock_values))
-
     # **** Steering Controls ************************************************ #
 
     if self.frame % self.CCP.HCA_STEP == 0:
@@ -125,9 +73,9 @@ class CarController:
     # **** Acceleration Controls ******************************************** #
 
     if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl:
-      tsk_status = self.CCS.tsk_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
+      acc_status = self.CCS.acc_control_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
       accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0
-      can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, tsk_status, accel))
+      can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CC.longActive, acc_status, accel))
 
     # **** HUD Controls ***************************************************** #
 
@@ -141,8 +89,9 @@ class CarController:
     if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
       acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
       set_speed = hud_control.setSpeed * CV.MS_TO_KPH  # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem?
-      can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed,
+      pass_through_data = CS.acc_04_stock_values if self.pass_through_enabled else None
-                                                       hud_control.leadVisible))
+      can_sends.extend(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed,
+                                                       hud_control.leadVisible, pass_through_data))
 
     # **** Stock ACC Button Controls **************************************** #
 

selfdrive/car/volkswagen/interface.py

@@ -75,6 +75,7 @@ class CarInterface(CarInterfaceBase):
       if disable_radar and ret.networkLocation == NetworkLocation.gateway:
         ret.openpilotLongitudinalControl = True
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
+        ret.minEnableSpeed = 5 * CV.MPH_TO_MS  # FIXME: temp hack during refactor
         if ret.transmissionType == TransmissionType.manual:
           ret.minEnableSpeed = 4.5  # FIXME: estimated, fine-tune
 

selfdrive/car/volkswagen/mqbcan.py

@@ -38,54 +38,68 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False,
   return packer.make_can_msg("GRA_ACC_01", bus, values)
 
 
-def create_acc_02_control(packer, bus, acc_status, set_speed, lead_distance):
+def acc_control_status_value(main_switch_on, acc_faulted, long_active):
-  values = {
+  if acc_faulted:
-    "ACC_Status_Anzeige": 3 if acc_status == 5 else acc_status,
+    tsk_status = 6
-    "ACC_Wunschgeschw": set_speed if set_speed < 250 else 327.36,
+  elif long_active:
-    "ACC_Gesetzte_Zeitluecke": 3,
+    tsk_status = 3
-    "ACC_Display_Prio": 3,
+  elif main_switch_on:
-    "ACC_Abstandsindex": lead_distance,
+    tsk_status = 2
-  }
+  else:
-
+    tsk_status = 0
-  return packer.make_can_msg("ACC_02", bus, values)
 
+  return tsk_status
 
-def create_acc_04_control(packer, bus, acc_04_stock_values):
-  values = acc_04_stock_values.copy()
 
-  # Suppress disengagement alert from stock radar when OP long is in use, but passthru FCW/AEB alerts
+def create_acc_accel_control(packer, bus, enabled, acc_status, accel):
-  if values["ACC_Texte_braking_guard"] == 4:
+  commands = []
-    values["ACC_Texte_braking_guard"] = 0
-
-  return packer.make_can_msg("ACC_04", bus, values)
 
-
+  acc_06_values = {
-def create_acc_06_control(packer, bus, enabled, acc_status, accel, acc_stopping, acc_starting, cb_pos, cb_neg, acc_type):
+    "ACC_Typ": 2,  # FIXME: require SnG during refactoring, re-enable FtS later
-  values = {
-    "ACC_Typ": acc_type,
     "ACC_Status_ACC": acc_status,
     "ACC_StartStopp_Info": enabled,
     "ACC_Sollbeschleunigung_02": accel if enabled else 3.01,
-    "ACC_zul_Regelabw_unten": cb_neg,
+    "ACC_zul_Regelabw_unten": 0.1,  # FIXME: reintroduce comfort band support
-    "ACC_zul_Regelabw_oben": cb_pos,
+    "ACC_zul_Regelabw_oben": 0.1,  # FIXME: reintroduce comfort band support
     "ACC_neg_Sollbeschl_Grad_02": 4.0 if enabled else 0,
     "ACC_pos_Sollbeschl_Grad_02": 4.0 if enabled else 0,
-    "ACC_Anfahren": acc_starting,
+    "ACC_Anfahren": 0,  # FIXME: minspeed > 0 during refactor
-    "ACC_Anhalten": acc_stopping,
+    "ACC_Anhalten": 0,  # FIXME: minspeed > 0 during refactor
   }
+  commands.append(packer.make_can_msg("ACC_06", bus, acc_06_values))
 
-  return packer.make_can_msg("ACC_06", bus, values)
+  acc_07_values = {
-
+    "ACC_Distance_to_Stop": 20.46,  # FIXME: default value, minspeed > 0 during refactor
-
+    "ACC_Hold_Request": 0,  # FIXME: default value, minspeed > 0 during refactor
-def create_acc_07_control(packer, bus, enabled, accel, acc_hold_request, acc_hold_release, acc_hold_type, stopping_distance):
-  values = {
-    "ACC_Distance_to_Stop": stopping_distance,
-    "ACC_Hold_Request": acc_hold_request,
     "ACC_Freewheel_Type": 2 if enabled else 0,
-    "ACC_Hold_Type": acc_hold_type,
+    "ACC_Hold_Type": 0,  # FIXME: default value, minspeed > 0 during refactor
-    "ACC_Hold_Release": acc_hold_release,
+    "ACC_Hold_Release": 0,  # FIXME: default value, minspeed > 0 during refactor
     "ACC_Accel_Secondary": 3.02,  # not using this unless and until we understand its impact
     "ACC_Accel_TSK": accel if enabled else 3.01,
   }
+  commands.append(packer.make_can_msg("ACC_07", bus, acc_07_values))
+
+  return commands
+
+
+def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible, pass_through_data):
+  commands = []
+
+  acc_02_values = {
+    "ACC_Status_Anzeige": 3 if acc_status == 5 else acc_status,
+    "ACC_Wunschgeschw": set_speed if set_speed < 250 else 327.36,
+    "ACC_Gesetzte_Zeitluecke": 3,
+    "ACC_Display_Prio": 3,
+    "ACC_Abstandsindex": 512 if lead_visible else 0,  # FIXME: will break analog clusters during refactor
+  }
+  commands.append(packer.make_can_msg("ACC_02", bus, acc_02_values))
+
+  # Suppress disengagement alert from stock radar when OP long is in use, but passthru FCW/AEB alerts
+  acc_04_values = pass_through_data.copy()
+  if acc_04_values["ACC_Texte_braking_guard"] == 4:
+    acc_04_values["ACC_Texte_braking_guard"] = 0
+
+  commands.append(packer.make_can_msg("ACC_04", bus, acc_04_values))
+
+  return commands
 
-  return packer.make_can_msg("ACC_07", bus, values)

selfdrive/car/volkswagen/pqcan.py

@@ -35,7 +35,7 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False,
   return packer.make_can_msg("GRA_Neu", bus, values)
 
 
-def tsk_status_value(main_switch_on, acc_faulted, long_active):
+def acc_control_status_value(main_switch_on, acc_faulted, long_active):
   if long_active:
     tsk_status = 1
   elif main_switch_on:
@@ -59,20 +59,20 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active):
   return hud_status
 
 
-def create_acc_accel_control(packer, bus, adr_status, accel):
+def create_acc_accel_control(packer, bus, enabled, acc_status, accel):
   values = {
-    "ACS_Sta_ADR": adr_status,
+    "ACS_Sta_ADR": acc_status,
-    "ACS_StSt_Info": adr_status != 1,
+    "ACS_StSt_Info": acc_status != 1,
     "ACS_Typ_ACC": 0,  # TODO: this is ACC "basic", find a way to detect FtS support (1)
-    "ACS_Sollbeschl": accel if adr_status == 1 else 3.01,
+    "ACS_Sollbeschl": accel if acc_status == 1 else 3.01,
-    "ACS_zul_Regelabw": 0.2 if adr_status == 1 else 1.27,
+    "ACS_zul_Regelabw": 0.2 if acc_status == 1 else 1.27,
-    "ACS_max_AendGrad": 3.0 if adr_status == 1 else 5.08,
+    "ACS_max_AendGrad": 3.0 if acc_status == 1 else 5.08,
   }
 
-  return packer.make_can_msg("ACC_System", bus, values)
+  packer.make_can_msg("ACC_System", bus, values)
 
 
-def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible):
+def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible, pass_through_data=None):
   values = {
     "ACA_StaACC": acc_status,
     "ACA_Zeitluecke": 2,