can I kick it?

panda

@@ -1 +1 @@
-Subproject commit 571512a392396271e54469c562b2ba08cbb44bfc
+Subproject commit b1205ad3bffbb9f75e7747920e2b91f249620fb2

selfdrive/car/volkswagen/carcontroller.py

@@ -1,10 +1,13 @@
+from common.numpy_fast import clip
 from cereal import car
+from selfdrive.config import Conversions as CV
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.volkswagen import volkswagencan
 from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
@@ -20,12 +23,49 @@ class CarController():
     self.graMsgBusCounterPrev = 0
 
     self.steer_rate_limited = False
+    self.openpilot_stopping = False
 
-  def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
+  def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart,
+             right_lane_depart, lead_visible, set_speed):
     """ Controls thread """
 
     can_sends = []
 
+    # **** Acceleration and Braking Controls ******************************** #
+
+    if CS.CP.openpilotLongitudinalControl:
+      if CS.tsk_status in [2, 3, 4, 5]:
+        acc_status = 3 if enabled else 2
+      else:
+        acc_status = CS.tsk_status
+
+      accel = clip(actuators.accel, P.ACCEL_MIN, P.ACCEL_MAX) if enabled else 0
+      jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
+
+      acc_stopping, acc_starting, acc_hold_request, acc_hold_release = False, False, False, False
+      if actuators.longControlState == LongCtrlState.stopping:
+        accel = -1.0
+        acc_stopping = True
+        acc_hold_request = not CS.esp_hold_confirmation
+      elif enabled:
+        acc_starting = CS.out.vEgo < 0.2
+        acc_hold_release = CS.esp_hold_confirmation
+
+      if acc_hold_request:
+        weird_value = 0x88
+      elif acc_stopping:
+        weird_value = 0x95
+      else:
+        weird_value = 0x7F
+
+      if frame % P.ACC_CONTROL_STEP == 0:
+        idx = (frame / P.ACC_CONTROL_STEP) % 16
+        can_sends.append(volkswagencan.create_mqb_acc_06_control(self.packer_pt, CANBUS.pt, enabled, acc_status,
+                                                                 accel, jerk, acc_stopping, acc_starting, idx))
+        can_sends.append(volkswagencan.create_mqb_acc_07_control(self.packer_pt, CANBUS.pt, enabled,
+                                                                 accel, acc_stopping, acc_starting,
+                                                                 acc_hold_request, acc_hold_release, weird_value, idx))
+
     # **** Steering Controls ************************************************ #
 
     if frame % P.HCA_STEP == 0:
@@ -82,31 +122,39 @@ class CarController():
                                                             right_lane_visible, CS.ldw_stock_values,
                                                             left_lane_depart, right_lane_depart))
 
+    if CS.CP.openpilotLongitudinalControl:
+      if frame % P.ACC_HUD_STEP == 0:
+        idx = (frame / P.ACC_HUD_STEP) % 16
+        can_sends.append(volkswagencan.create_mqb_acc_hud_control(self.packer_pt, CANBUS.pt, CS.tsk_status,
+                                                                  set_speed * CV.MS_TO_KPH, idx))
+
     # **** ACC Button Controls ********************************************** #
 
     # FIXME: this entire section is in desperate need of refactoring
 
-    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.
-        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
+    if not CS.CP.openpilotLongitudinalControl:
+      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 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.
+          # A subset of MQBs like to "creep" too aggressively with this implementation.
+          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
 
     return can_sends

selfdrive/car/volkswagen/carstate.py

@@ -27,7 +27,7 @@ class CarState(CarStateBase):
 
     ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
-    ret.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"])
+    ret.standstill = ret.vEgoRaw < 0.1
 
     # Update steering angle, rate, yaw rate, and driver input torque. VW send
     # the sign/direction in a separate signal so they must be recombined.
@@ -47,6 +47,7 @@ class CarState(CarStateBase):
     ret.gasPressed = ret.gas > 0
     ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0  # FIXME: this is pressure in Bar, not sure what OP expects
     ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
+    self.esp_hold_confirmation = pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]
 
     # Update gear and/or clutch position data.
     if trans_type == TransmissionType.automatic:
@@ -94,13 +95,13 @@ class CarState(CarStateBase):
     ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"])
 
     # Update ACC radar status.
-    accStatus = pt_cp.vl["TSK_06"]["TSK_Status"]
-    if accStatus == 2:
+    self.tsk_status = pt_cp.vl["TSK_06"]["TSK_Status"]
+    if self.tsk_status == 2:
       # ACC okay and enabled, but not currently engaged
       ret.cruiseState.available = True
       ret.cruiseState.enabled = False
-    elif accStatus in [3, 4, 5]:
-      # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5)
+    elif self.tsk_status in [3, 4, 5]:
+      # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5)
       ret.cruiseState.available = True
       ret.cruiseState.enabled = True
     else:
@@ -108,11 +109,12 @@ class CarState(CarStateBase):
       ret.cruiseState.available = False
       ret.cruiseState.enabled = False
 
-    # 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.
-    ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS
-    if ret.cruiseState.speed > 90:
-      ret.cruiseState.speed = 0
+    if not self.CP.openpilotLongitudinalControl:
+      # 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.
+      ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS
+      if ret.cruiseState.speed > 90:
+        ret.cruiseState.speed = 0
 
     # Update control button states for turn signals and ACC controls.
     self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Hoch"])

selfdrive/car/volkswagen/interface.py

@@ -1,8 +1,11 @@
 from cereal import car
+from common.params import Params
+from panda import Panda
 from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
+ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 
 
@@ -31,6 +34,14 @@ class CarInterface(CarInterfaceBase):
       ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)]
       ret.enableBsm = 0x30F in fingerprint[0]  # SWA_01
 
+      # Disable the radar and let openpilot control longitudinal
+      # WARNING: THIS DISABLES FACTORY FCW/AEB!
+      if Params().get_bool("DisableRadar"):
+        ret.pcmCruise = False
+        ret.openpilotLongitudinalControl = True
+        ret.directAccelControl = True
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONGITUDINAL
+
       if 0xAD in fingerprint[0]:  # Getriebe_11
         ret.transmissionType = TransmissionType.automatic
       elif 0x187 in fingerprint[0]:  # EV_Gearshift
@@ -43,7 +54,7 @@ class CarInterface(CarInterfaceBase):
       else:
         ret.networkLocation = NetworkLocation.fwdCamera
 
-    # Global tuning defaults, can be overridden per-vehicle
+    # Global lateral tuning defaults, can be overridden per-vehicle
 
     ret.steerActuatorDelay = 0.05
     ret.steerRateCost = 1.0
@@ -56,6 +67,17 @@ class CarInterface(CarInterfaceBase):
     ret.lateralTuning.pid.kpV = [0.6]
     ret.lateralTuning.pid.kiV = [0.2]
 
+    # Global longitudinal tuning defaults, can be overridden per-vehicle
+
+    ret.longitudinalActuatorDelayUpperBound = 1.0  # s
+    ret.minSpeedCan = 0.0
+    ret.stoppingControl = True
+    ret.vEgoStopping = 1.0
+    ret.stopAccel = 0.0
+    ret.startAccel = 0.0
+    ret.longitudinalTuning.kpV = [0.1]
+    ret.longitudinalTuning.kiV = [0.0]
+
     # Per-chassis tuning values, override tuning defaults here if desired
 
     if candidate == CAR.ARTEON_MK1:
@@ -181,11 +203,14 @@ class CarInterface(CarInterfaceBase):
         be.pressed = self.CS.buttonStates[button]
         buttonEvents.append(be)
 
-    events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic])
+    events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
+                                       pcm_enable=not self.CS.CP.openpilotLongitudinalControl)
 
     # Vehicle health and operation safety checks
     if self.CS.parkingBrakeSet:
       events.add(EventName.parkBrake)
+    if self.CS.tsk_status in [6, 7]:
+      events.add(EventName.accFaulted)
 
     # Low speed steer alert hysteresis logic
     if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):
@@ -195,6 +220,15 @@ class CarInterface(CarInterfaceBase):
     if self.low_speed_alert:
       events.add(EventName.belowSteerSpeed)
 
+    if self.CS.CP.openpilotLongitudinalControl:
+      for b in buttonEvents:
+        # do enable on falling edge of both accel and decel buttons
+        if b.type in [ButtonType.setCruise, ButtonType.resumeCruise] and not b.pressed:
+          events.add(EventName.buttonEnable)
+        # do disable on rising edge of cancel
+        if b.type == "cancel" and b.pressed:
+          events.add(EventName.buttonCancel)
+
     ret.events = events.to_msg()
     ret.buttonEvents = buttonEvents
 
@@ -211,6 +245,8 @@ class CarInterface(CarInterfaceBase):
                    c.hudControl.leftLaneVisible,
                    c.hudControl.rightLaneVisible,
                    c.hudControl.leftLaneDepart,
-                   c.hudControl.rightLaneDepart)
+                   c.hudControl.rightLaneDepart,
+                   c.hudControl.leadVisible,
+                   c.hudControl.setSpeed)
     self.frame += 1
     return can_sends

selfdrive/car/volkswagen/values.py

@@ -15,6 +15,8 @@ class CarControllerParams:
   HCA_STEP = 2                   # HCA_01 message frequency 50Hz
   LDW_STEP = 10                  # LDW_02 message frequency 10Hz
   GRA_ACC_STEP = 3               # GRA_ACC_01 message frequency 33Hz
+  ACC_CONTROL_STEP = 2           # ACC_06 message frequency 50Hz
+  ACC_HUD_STEP = 6               # ACC_02 message frequency 16Hz
 
   GRA_VBP_STEP = 100             # Send ACC virtual button presses once a second
   GRA_VBP_COUNT = 16             # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16)
@@ -29,6 +31,9 @@ class CarControllerParams:
   STEER_DRIVER_MULTIPLIER = 3    # weight driver torque heavily
   STEER_DRIVER_FACTOR = 1        # from dbc
 
+  ACCEL_MAX = 2.0                # 2.0 m/s max acceleration
+  ACCEL_MIN = -3.5               # 3.5 m/s max deceleration
+
 class CANBUS:
   pt = 0
   cam = 2

selfdrive/car/volkswagen/volkswagencan.py

@@ -47,3 +47,48 @@ def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx):
     "GRA_ButtonTypeInfo": CS.graButtonTypeInfo
   }
   return packer.make_can_msg("GRA_ACC_01", bus, values, idx)
+
+def create_mqb_acc_06_control(packer, bus, enabled, acc_status, accel, jerk, acc_stopping, acc_starting,
+                              idx):
+  values = {
+    "ACC_Typ": 2,  # FIXME: locked to stop and go, need to tweak for cars that only support follow-to-stop
+    "ACC_Status_ACC": acc_status,
+    "ACC_StartStopp_Info": enabled,
+    "ACC_Sollbeschleunigung_02": accel if enabled else 3.01,
+    "ACC_zul_Regelabw_unten": 0.2 if enabled else 0,  # FIXME: need comfort regulation logic here
+    "ACC_zul_Regelabw_oben": 0.1 if enabled else 0,  # FIXME: need comfort regulation logic here
+    "ACC_neg_Sollbeschl_Grad_02": jerk if enabled else 0,
+    "ACC_pos_Sollbeschl_Grad_02": jerk if enabled else 0,
+    "ACC_Anfahren": acc_starting,
+    "ACC_Anhalten": acc_stopping,
+  }
+
+  return packer.make_can_msg("ACC_06", bus, values, idx)
+
+def create_mqb_acc_07_control(packer, bus, enabled, accel, acc_stopping, acc_starting, acc_hold_request,
+                              acc_hold_release, weird_value, idx):
+  values = {
+    "XXX_Maybe_Engine_Start_Request": 2,  # TODO
+    "XXX_Always_1": 1,
+    "XXX_Maybe_Engine_Stop_Release": not acc_hold_request,  # TODO this isn't S/S, AHR but also slight delay past AHR
+    "XXX_Unknown": weird_value,  # FIXME: Should try to figure out what's really going on here
+    "ACC_Engaged": enabled,
+    "ACC_Anhalten": acc_stopping,
+    "ACC_Anhaltevorgang": acc_hold_request,
+    "ACC_Anfahrvorgang": acc_hold_release,
+    "ACC_Anfahren": acc_starting,
+    "XXX_Lead_Car_Relative_Speed": 0x65,
+    "ACC_Sollbeschleunigung_01": accel if enabled else 3.01,
+  }
+
+  return packer.make_can_msg("ACC_07", bus, values, idx)
+
+def create_mqb_acc_hud_control(packer, bus, acc_status, set_speed, idx):
+  values = {
+    "ACC_Status_Anzeige": acc_status,
+    "ACC_Wunschgeschw": 327.36 if set_speed == 0 else set_speed,
+    "ACC_Gesetzte_Zeitluecke": 3,
+    "ACC_Display_Prio": 3
+  }
+
+  return packer.make_can_msg("ACC_02", bus, values, idx)