GAC: Gap Adjust Cruise implementation (#44)

* Gap Adjust Cruise (GAC): init

* cs: add longitudinal supported cars

* disable for Toyota for now

* send to car cluster

* more

* KrKeegan: More Aggresive Start from Standstill

* fix

* set prev button after

* init bool

* wrong var

* use common update param function

* bruh no wonder why

* lock to default distance when using exp mode

* make vw match other makes

* implement ui button

* gate ui button

* log desired_TF

* move button

* only show when cruise state available

* only allow press when cruise state available

* Revert "KrKeegan: More Aggresive Start from Standstill"

This reverts commit 20bdff34c83d5a0c248d6996155c995c8a99810f.

* unnecessary

* nothing else

* pass it through

* add toyota support

* fixup! add toyota support

* oops, necessary

* don't show ui button when using e2e long

* different signal to use

* Forgot to update

* Clean it up?

* this?

* ugh

* force int!!

* convert then round

* hide when in exp mode

* toyota: log button

* Revert "different signal to use"

This reverts commit edc64d31af527af842aabe2934e5720f73bf75cc.

* toyota flippy flippy

* wrong signal for vw

* oops

selfdrive/car/chrysler/interface.py

@@ -93,7 +93,7 @@ class CarInterface(CarInterfaceBase):
 
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
 

selfdrive/car/gm/carcontroller.py

@@ -114,7 +114,7 @@ class CarController:
         # Send dashboard UI commands (ACC status)
         send_fcw = hud_alert == VisualAlert.fcw
         can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled and CS.out.cruiseState.enabled,
-                                                            hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw))
+                                                            hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw, CS.gac_tr))
 
       # Radar needs to know current speed and yaw rate (50hz),
       # and that ADAS is alive (10hz)

selfdrive/car/gm/gmcan.py

@@ -92,14 +92,14 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_s
   return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values)
 
 
-def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw):
+def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw, gac_tr):
   target_speed = min(target_speed_kph, 255)
 
   values = {
     "ACCAlwaysOne": 1,
     "ACCResumeButton": 0,
     "ACCSpeedSetpoint": target_speed,
-    "ACCGapLevel": 3 * enabled,  # 3 "far", 0 "inactive"
+    "ACCGapLevel": gac_tr * enabled,  # 3 "far", 0 "inactive"
     "ACCCmdActive": enabled,
     "ACCAlwaysOne2": 1,
     "ACCLeadCar": lead_car_in_sight,

selfdrive/car/gm/interface.py

@@ -251,7 +251,7 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
 
@@ -277,6 +277,7 @@ class CarInterface(CarInterfaceBase):
         if self.CS.prev_lkas_enabled != 1 and self.CS.lkas_enabled == 1:
           self.CS.madsEnabled = not self.CS.madsEnabled
         self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+      self.toggle_gac(ret, self.CS, bool(self.CS.gap_dist_button), 1, 3, 3, "-")
     else:
       self.CS.madsEnabled = False
 

selfdrive/car/honda/carcontroller.py

@@ -248,7 +248,7 @@ class CarController:
     if self.frame % 10 == 0:
       hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
                     hud_control.lanesVisible, fcw_display, acc_alert, steer_required, CS.madsEnabled and not CC.latActive)
-      can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive))
+      can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive, CS.gac_tr))
 
       if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
         self.speed = pcm_speed

selfdrive/car/honda/hondacan.py

@@ -102,7 +102,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint):
   return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values)
 
 
-def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active):
+def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active, gac_tr):
   commands = []
   bus_pt = get_pt_bus(CP.carFingerprint)
   radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl
@@ -112,7 +112,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud,
     acc_hud_values = {
       'CRUISE_SPEED': hud.v_cruise,
       'ENABLE_MINI_CAR': 1 if enabled else 0,
-      'HUD_DISTANCE': 0,  # max distance setting on display
+      'HUD_DISTANCE': gac_tr,  # max distance setting on display
       'IMPERIAL_UNIT': int(not is_metric),
       'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0,
       'SET_ME_X01_2': 1,

selfdrive/car/honda/interface.py

@@ -311,7 +311,7 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
 
@@ -333,6 +333,7 @@ class CarInterface(CarInterfaceBase):
         if self.CS.prev_cruise_setting != 1 and self.CS.cruise_setting == 1:
           self.CS.madsEnabled = not self.CS.madsEnabled
         self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+      self.toggle_gac(ret, self.CS, (self.CS.cruise_setting == 3), 1, 3, 0, "-")
     else:
       self.CS.madsEnabled = False
 

selfdrive/car/hyundai/hyundaican.py

@@ -104,7 +104,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
 
   scc11_values = {
     "MainMode_ACC": 1 if main_enabled else 0,
-    "TauGapSet": 4,
+    "TauGapSet": CS.gac_tr,
     "VSetDis": set_speed if enabled else 0,
     "AliveCounterACC": idx % 0x10,
     "ObjValid": 1, # close lead makes controls tighter

selfdrive/car/hyundai/interface.py

@@ -323,7 +323,7 @@ class CarInterface(CarInterfaceBase):
 
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
 
@@ -346,6 +346,7 @@ class CarInterface(CarInterfaceBase):
         if self.CS.prev_lfa_enabled != 1 and self.CS.lfa_enabled == 1:
           self.CS.madsEnabled = not self.CS.madsEnabled
         self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+      self.toggle_gac(ret, self.CS, (self.CS.cruise_buttons[-1] == 3), 1, 3, 4, "-")
     else:
       self.CS.madsEnabled = False
 

selfdrive/car/interfaces.py

@@ -1,4 +1,5 @@
 import yaml
+import operator
 import os
 import time
 from abc import abstractmethod, ABC
@@ -31,6 +32,8 @@ TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yam
 TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml')
 TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.yaml')
 
+GAC_DICT = {1: 1, 2: 2, 3: 3}
+
 
 def get_torque_params(candidate):
   with open(TORQUE_SUBSTITUTE_PATH) as f:
@@ -100,6 +103,13 @@ class CarInterfaceBase(ABC):
     self.experimental_mode_hold = False
     self.experimental_mode = self.param_s.get_bool("ExperimentalMode")
     self._frame = 0
+    self.op_lookup = {"+": operator.add, "-": operator.sub}
+    self.gac = self.param_s.get_bool("GapAdjustCruise")
+    self.gac_mode = round(float(self.param_s.get("GapAdjustCruiseMode", encoding="utf8")))
+    self.prev_gac_button = False
+    self.gac_button_counter = 0
+    self.gac_min = -1
+    self.gac_max = -1
 
   @staticmethod
   def get_pid_accel_limits(CP, current_speed, cruise_speed):
@@ -425,12 +435,51 @@ class CarInterfaceBase(ABC):
         if self.gap_button_counter > 50:
           self.gap_button_counter = 0
           self.experimental_mode_hold = True
-          self.experimental_mode = self.param_s.get_bool("ExperimentalMode")
           self.param_s.put_bool("ExperimentalMode", not self.experimental_mode)
     else:
       self.gap_button_counter = 0
       self.experimental_mode_hold = False
 
+  def get_sp_gac_state(self, gac_tr, gac_min, gac_max, inc_dec):
+    op = self.op_lookup.get(inc_dec)
+    gac_tr = op(gac_tr, 1)
+    if inc_dec == "+":
+      gac_tr = gac_min if gac_tr > gac_max else gac_tr
+    else:
+      gac_tr = gac_max if gac_tr < gac_min else gac_tr
+    return int(gac_tr)
+
+  def get_sp_distance(self, gac_tr, gac_max, gac_dict=None):
+    if gac_dict is None:
+      gac_dict = GAC_DICT
+    for key, value in gac_dict.items():
+      if gac_tr == value:
+        return key
+    return gac_max
+
+  def toggle_gac(self, cs_out, CS, gac_button, gac_min, gac_max, gac_default, inc_dec):
+    if (not (self.CP.openpilotLongitudinalControl or self.gac)) or (self.experimental_mode and self.CP.openpilotLongitudinalControl):
+      cs_out.gapAdjustCruiseTr = 4
+      CS.gac_tr = gac_default
+      return
+    if self.gac_min != gac_min:
+      self.gac_min = gac_min
+      self.param_s.put("GapAdjustCruiseMin", str(self.gac_min))
+    if self.gac_max != gac_max:
+      self.gac_max = gac_max
+      self.param_s.put("GapAdjustCruiseMax", str(self.gac_max))
+    if self.gac_mode in (0, 2):
+      if gac_button:
+        self.gac_button_counter += 1
+      elif self.prev_gac_button and not gac_button and self.gac_button_counter < 50:
+        self.gac_button_counter = 0
+        CS.gac_tr = self.get_sp_gac_state(CS.gac_tr, gac_min, gac_max, inc_dec)
+        self.param_s.put("GapAdjustCruiseTr", str(CS.gac_tr))
+      else:
+        self.gac_button_counter = 0
+    self.prev_gac_button = gac_button
+    cs_out.gapAdjustCruiseTr = self.get_sp_distance(CS.gac_tr, gac_max)
+
   def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
                        enable_from_brake=False, enable_pressed_long=False,
                        enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
@@ -493,11 +542,14 @@ class CarInterfaceBase(ABC):
 
     return events, cs_out
 
-  def sp_update_params(self):
+  def sp_update_params(self, CS):
+    self.experimental_mode = self.param_s.get_bool("ExperimentalMode")
+    CS.gac_tr = round(float(self.param_s.get("GapAdjustCruiseTr", encoding="utf8")))
     self._frame += 1
     if self._frame % 300 == 0:
       self._frame = 0
-      self.experimental_mode = self.param_s.get_bool("ExperimentalMode")
+      self.gac = self.param_s.get_bool("GapAdjustCruise")
+      self.gac_mode = round(float(self.param_s.get("GapAdjustCruiseMode", encoding="utf8")))
 
 class RadarInterfaceBase(ABC):
   def __init__(self, CP):
@@ -529,6 +581,7 @@ class CarStateBase(ABC):
     self.cluster_speed_hyst_gap = 0.0
     self.cluster_min_speed = 0.0  # min speed before dropping to 0
 
+    self.param_s = Params()
     self.accEnabled = False
     self.madsEnabled = False
     self.disengageByBrake = False
@@ -536,6 +589,7 @@ class CarStateBase(ABC):
     self.prev_mads_enabled = False
     self.control_initialized = False
     self.gap_dist_button = 0
+    self.gac_tr = round(float(self.param_s.get("GapAdjustCruiseTr", encoding="utf8")))
 
     # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]])
     # R = 0.3

selfdrive/car/mazda/interface.py

@@ -57,7 +57,7 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
 

selfdrive/car/nissan/interface.py

@@ -44,7 +44,7 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
     #be = car.CarState.ButtonEvent.new_message()

selfdrive/car/subaru/interface.py

@@ -110,7 +110,7 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c):
 
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
 

selfdrive/car/toyota/carcontroller.py

@@ -121,10 +121,10 @@ class CarController:
       if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, reverse_acc))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, reverse_acc, CS.gac_send))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, reverse_acc))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, reverse_acc, CS.gac_send))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.

selfdrive/car/toyota/carstate.py

@@ -43,6 +43,10 @@ class CarState(CarStateBase):
     self.lta_status = False
     self.prev_lta_status = False
     self.lta_status_active = False
+    self.gac_send = False
+    self.gac_send_counter = 0
+    self.follow_distance = 0
+    self.follow_distance_converted = 0
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -50,6 +54,7 @@ class CarState(CarStateBase):
     self.prev_mads_enabled = self.mads_enabled
     self.prev_lkas_enabled = self.lkas_enabled
     self.prev_lta_status = self.lta_status
+    self.prev_gap_dist_button = self.gap_dist_button
 
     ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
                         cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
@@ -155,6 +160,8 @@ class CarState(CarStateBase):
     if self.CP.flags & ToyotaFlags.SMART_DSU:
       self.gap_dist_button = cp.vl["SDSU"]["FD_BUTTON"]
 
+    self.follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
+
     # some TSS2 cars have low speed lockout permanently set, so ignore on those cars
     # these cars are identified by an ACC_TYPE value of 2.
     # TODO: it is possible to avoid the lockout and gain stop and go if you
@@ -336,6 +343,7 @@ class CarState(CarStateBase):
       signals.append(("SET_SPEED", "PCM_CRUISE_2"))
       signals.append(("ACC_FAULTED", "PCM_CRUISE_2"))
       signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2"))
+      signals.append(("PCM_FOLLOW_DISTANCE", "PCM_CRUISE_2"))
       checks.append(("PCM_CRUISE_2", 33))
 
     # add gas interceptor reading if we are using it

selfdrive/car/toyota/interface.py

@@ -4,13 +4,15 @@ from common.conversions import Conversions as CV
 from common.params import Params
 from panda import Panda
 from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR
-from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config, create_mads_event
+from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config, create_mads_event
 from selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 GearShifter = car.CarState.GearShifter
 
+GAC_DICT = {3: 1, 2: 2, 1: 3}
+
 
 class CarInterface(CarInterfaceBase):
   @staticmethod
@@ -244,10 +246,13 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
 
+    if self.CS.gap_dist_button != self.CS.prev_gap_dist_button:
+      buttonEvents.append(create_button_event(self.CS.gap_dist_button, self.CS.prev_gap_dist_button, {1: ButtonType.gapAdjustCruise}))
+
     self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
 
     if ret.cruiseState.available:
@@ -263,6 +268,33 @@ class CarInterface(CarInterfaceBase):
             (self.CS.prev_lkas_enabled == 1 and not self.CS.lkas_enabled):
             self.CS.madsEnabled = not self.CS.madsEnabled
         self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+      if (not (self.CP.openpilotLongitudinalControl or self.gac)) or (self.experimental_mode and self.CP.openpilotLongitudinalControl):
+        ret.gapAdjustCruiseTr = 3
+      else:
+        if self.gac_min != 1:
+          self.gac_min = 1
+          self.param_s.put("GapAdjustCruiseMin", str(self.gac_min))
+        if self.gac_max != 3:
+          self.gac_max = 3
+          self.param_s.put("GapAdjustCruiseMax", str(self.gac_max))
+        if self.gac_mode in (0, 2):
+          if bool(self.CS.gap_dist_button):
+            self.gac_button_counter += 1
+          elif self.prev_gac_button and not bool(self.CS.gap_dist_button) and self.gac_button_counter < 50:
+            self.gac_button_counter = 0
+            self.CS.follow_distance_converted = self.get_sp_gac_state(self.CS.follow_distance, self.gac_min, self.gac_max, "+")
+            self.CS.gac_tr = self.get_sp_distance(self.CS.follow_distance_converted, self.gac_max, gac_dict=GAC_DICT)
+            self.param_s.put("GapAdjustCruiseTr", str(self.CS.gac_tr))
+          else:
+            self.gac_button_counter = 0
+        self.prev_gac_button = bool(self.CS.gap_dist_button)
+        ret.gapAdjustCruiseTr = self.CS.gac_tr
+      if self.CS.gac_send_counter < 10 and (self.get_sp_distance(ret.gapAdjustCruiseTr, self.gac_max, gac_dict=GAC_DICT) != self.CS.follow_distance):
+        self.CS.gac_send_counter += 1
+        self.CS.gac_send = 1
+      else:
+        self.CS.gac_send_counter = 0
+        self.CS.gac_send = 0
     else:
       self.CS.madsEnabled = False
 

selfdrive/car/toyota/toyotacan.py

@@ -27,12 +27,12 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, reverse_acc):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, reverse_acc, gac_send):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
     "ACC_TYPE": acc_type,
-    "DISTANCE": 0,
+    "DISTANCE": gac_send,
     "MINI_CAR": lead,
     "PERMIT_BRAKING": 1,
     "RELEASE_STANDSTILL": not standstill_req,

selfdrive/car/volkswagen/carcontroller.py

@@ -95,7 +95,7 @@ class CarController:
       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,
-                                                       lead_distance))
+                                                       lead_distance, CS.gac_tr))
 
     # **** Stock ACC Button Controls **************************************** #
 

selfdrive/car/volkswagen/interface.py

@@ -223,7 +223,7 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
-    self.sp_update_params()
+    self.sp_update_params(self.CS)
 
     buttonEvents = []
 
@@ -247,6 +247,7 @@ class CarInterface(CarInterfaceBase):
         if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
           self.CS.madsEnabled = True
         self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+      self.toggle_gac(ret, self.CS, bool(self.CS.gap_dist_button), 1, 3, 3, "-")
     else:
       self.CS.madsEnabled = False
 

selfdrive/car/volkswagen/mqbcan.py

@@ -96,11 +96,11 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control,
   return commands
 
 
-def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance):
+def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, gac_tr):
   values = {
     "ACC_Status_Anzeige": acc_hud_status,
     "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36,
-    "ACC_Gesetzte_Zeitluecke": 3,
+    "ACC_Gesetzte_Zeitluecke": gac_tr,
     "ACC_Display_Prio": 3,
     "ACC_Abstandsindex": lead_distance,
   }

selfdrive/car/volkswagen/pqcan.py

@@ -77,7 +77,7 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control,
   return commands
 
 
-def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance):
+def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, gac_tr):
   values = {
     "ACA_StaACC": acc_hud_status,
     "ACA_Zeitluecke": 2,

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -3,7 +3,7 @@ import os
 import numpy as np
 
 from common.realtime import sec_since_boot
-from common.numpy_fast import clip
+from common.numpy_fast import clip, interp
 from system.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
 from selfdrive.modeld.constants import index_function
@@ -64,8 +64,8 @@ def get_stopped_equivalence_factor(v_lead):
 def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
 
-def desired_follow_distance(v_ego, v_lead):
-  return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
+def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW):
+  return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
 
 
 def gen_long_model():
@@ -201,6 +201,7 @@ class LongitudinalMpc:
   def __init__(self, mode='acc'):
     self.mode = mode
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.desired_TF = T_FOLLOW
     self.reset()
     self.source = SOURCES[2]
 
@@ -251,11 +252,21 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
+  def get_cost_multipliers(self):
+    TFs = [1.0, 1.25, T_FOLLOW]
+    # KRKeegan adjustments to costs for different TFs
+    # these were calculated using the test_longitudinal.py deceleration tests
+    a_change_tf = interp(self.desired_TF, TFs, [.1, .8, 1.])
+    j_ego_tf = interp(self.desired_TF, TFs, [.6, .8, 1.])
+    d_zone_tf = interp(self.desired_TF, TFs, [1.6, 1.3, 1.])
+    return a_change_tf, j_ego_tf, d_zone_tf
+
   def set_weights(self, prev_accel_constraint=True):
     if self.mode == 'acc':
+      cost_mulitpliers = self.get_cost_multipliers()
       a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
-      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]
-      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost * cost_mulitpliers[0], J_EGO_COST * cost_mulitpliers[1]]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * cost_mulitpliers[2]]
     elif self.mode == 'blended':
       a_change_cost = 40.0 if prev_accel_constraint else 0
       cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
@@ -309,13 +320,25 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j):
+  def update_TF(self, carstate):
+    gac_tr = carstate.gapAdjustCruiseTr
+    if gac_tr == 1:
+      self.desired_TF = 1.0
+    elif gac_tr == 2:
+      self.desired_TF = 1.25
+    else:
+      self.desired_TF = T_FOLLOW
+
+  def update(self, carstate, radarstate, v_cruise, x, v, a, j, prev_accel_constraint):
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
+    self.update_TF(carstate)
+    self.set_weights(prev_accel_constraint)
+
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
@@ -343,7 +366,7 @@ class LongitudinalMpc:
       v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
                                  v_lower,
                                  v_upper)
-      cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped)
+      cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, self.desired_TF)
       x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
       self.source = SOURCES[np.argmin(x_obstacles[0])]
 
@@ -380,6 +403,8 @@ class LongitudinalMpc:
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.prev_a)
     self.params[:,4] = T_FOLLOW
+    if self.mode == 'acc':
+      self.params[:,4] = self.desired_TF
 
     self.run()
     if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and

selfdrive/controls/lib/longitudinal_planner.py

@@ -132,11 +132,10 @@ class LongitudinalPlanner:
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
-    self.mpc.set_weights(prev_accel_constraint)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j)
+    self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol, x, v, a, j, prev_accel_constraint)
 
     self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
     self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N]
@@ -173,6 +172,7 @@ class LongitudinalPlanner:
     longitudinalPlan.solverExecutionTime = self.mpc.solve_time
 
     longitudinalPlan.e2eX = self.mpc.e2e_x.tolist()
+    longitudinalPlan.desiredTF = self.mpc.desired_TF
 
     longitudinalPlan.visionTurnControllerState = self.vision_turn_controller.state
     longitudinalPlan.visionTurnSpeed = float(self.vision_turn_controller.v_turn)

selfdrive/test/longitudinal_maneuvers/test_accel.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+import os
+import unittest
+
+from common.params import Params
+from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+
+maneuvers = [
+  # In Stock with jerk cost of 5.0 this results in a maximum desired_dist_diff of 51m
+  # Setting jerk cost to 0 drops it down to 25m
+  # Setting jerk cost to .5 drops it down to 38m
+  Maneuver(
+    'Start from standstill behind car acc at 1.2m/s',
+    duration=20.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=4,  # In real world the stopping distance is less than desired
+    speed_lead_values=[0., 12., 12.],
+    breakpoints=[0., 10., 20.],
+    cruise_values=[35., 35., 35.],
+  ),
+]
+
+
+class LongitudinalControl(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    os.environ['SIMULATION'] = "1"
+    os.environ['SKIP_FW_QUERY'] = "1"
+    os.environ['NO_CAN_TIMEOUT'] = "1"
+
+    params = Params()
+    params.clear_all()
+    params.put_bool("Passive", bool(os.getenv("PASSIVE")))
+    params.put_bool("OpenpilotEnabledToggle", True)
+
+  # hack
+  def test_longitudinal_setup(self):
+    pass
+
+
+def run_maneuver_worker(k):
+  def run(self):
+    man = maneuvers[k]
+    print(man.title)
+    valid, _ = man.evaluate()
+    self.assertTrue(valid, msg=man.title)
+  return run
+
+
+for k in range(len(maneuvers)):
+  setattr(LongitudinalControl, f"test_longitudinal_maneuvers_{k+1}",
+          run_maneuver_worker(k))
+
+if __name__ == "__main__":
+  unittest.main(failfast=True)

selfdrive/ui/qt/onroad.cc

@@ -152,8 +152,11 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
   UIScene &scene = s->scene;
   SubMaster &sm = *(uiState()->sm);
   auto longitudinal_plan = sm["longitudinalPlan"].getLongitudinalPlan();
+  auto car_state = sm["carState"].getCarState();
+  auto controls_state = sm["controlsState"].getControlsState();
 
   QRect dlp_btn_rect = QRect(bdr_s * 2 + 220, (rect().bottom() - footer_h / 2 - 75), 150, 150);
+  QRect gac_btn_rect = QRect(bdr_s * 2 + 220 + 180, (rect().bottom() - footer_h / 2 - 75), 150, 150);
   QRect debug_tap_rect = QRect(rect().center().x() - 200, rect().center().y() - 200, 400, 400);
   QRect speed_limit_touch_rect = speed_sgn_rc.adjusted(-50, -50, 50, 50);
 
@@ -162,6 +165,12 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
     scene.dynamic_lane_profile = scene.dynamic_lane_profile > 2 ? 0 : scene.dynamic_lane_profile;
     params.put("DynamicLaneProfile", std::to_string(scene.dynamic_lane_profile));
     propagate_event = false;
+  } else if (scene.gac && scene.gac_mode != 0 && scene.longitudinal_control && !controls_state.getExperimentalMode() &&
+             car_state.getCruiseState().getAvailable() && gac_btn_rect.contains(e->x(), e->y())) {
+    scene.gac_tr--;
+    scene.gac_tr = scene.gac_tr < scene.gac_min ? scene.gac_max : scene.gac_tr;
+    params.put("GapAdjustCruiseTr", std::to_string(scene.gac_tr));
+    propagate_event = false;
   } else if (longitudinal_plan.getSpeedLimit() > 0.0 && speed_limit_touch_rect.contains(e->x(), e->y())) {
     // If touching the speed limit sign area when visible
     scene.last_speed_limit_sign_tap = seconds_since_boot();
@@ -423,6 +432,10 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
 
   setProperty("hideVEgoUi", s.scene.hide_vego_ui);
 
+  setProperty("gac", s.scene.gac && s.scene.gac_mode != 0 && s.scene.longitudinal_control && !cs.getExperimentalMode() &&
+              car_state.getCruiseState().getAvailable());
+  setProperty("gacTr", s.scene.gac_tr);
+
   // update engageability/experimental mode button
   experimental_btn->updateState(s);
 
@@ -682,6 +695,10 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     drawDlpButton(p, bdr_s * 2 + 220, (rect().bottom() - footer_h / 2 - 75), 150, 150);
   }
 
+  if (gac) {
+    drawGacButton(p, bdr_s * 2 + 220 + 180, (rect().bottom() - footer_h / 2 - 75), 150, 150);
+  }
+
   // Stand Still Timer
   if (standStillTimer && standStill) {
     drawStandstillTimer(p, rect().right() - 650, 30 + 160 + 250);
@@ -790,6 +807,34 @@ void AnnotatedCameraWidget::drawDlpButton(QPainter &p, int x, int y, int w, int
   p.drawText(dlpBtn, Qt::AlignCenter, dlp_text);
 }
 
+void AnnotatedCameraWidget::drawGacButton(QPainter &p, int x, int y, int w, int h) {
+  int prev_gac_tr = -1;
+  QString gac_text = "";
+  QColor gac_border = QColor(255, 255, 255, 255);
+
+  if (prev_gac_tr != gacTr) {
+    prev_gac_tr = gacTr;
+    if (gacTr == 1) {
+      gac_text = "Aggro\nGap";
+      gac_border = QColor("#ff4b4b");
+    } else if (gacTr == 2) {
+      gac_text = "Mild\nGap";
+      gac_border = QColor("#fcff4b");
+    } else {
+      gac_text = "Stock\nGap";
+      gac_border = QColor("#4bff66");
+    }
+  }
+
+  QRect gacBtn(x, y, w, h);
+  p.setPen(QPen(gac_border, 6));
+  p.setBrush(QColor(75, 75, 75, 75));
+  p.drawEllipse(gacBtn);
+  p.setPen(QColor(Qt::white));
+  configFont(p, "Inter", 36, "SemiBold");
+  p.drawText(gacBtn, Qt::AlignCenter, gac_text);
+}
+
 void AnnotatedCameraWidget::drawStandstillTimer(QPainter &p, int x, int y) {
   char lab_str[16];
   char val_str[16];

selfdrive/ui/qt/onroad.h

@@ -106,6 +106,9 @@ class AnnotatedCameraWidget : public CameraWidget {
 
   Q_PROPERTY(bool hideVEgoUi MEMBER hideVEgoUi);
 
+  Q_PROPERTY(bool gac MEMBER gac);
+  Q_PROPERTY(int gacTr MEMBER gacTr);
+
 public:
   explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0);
   void updateState(const UIState &s);
@@ -123,6 +126,7 @@ private:
                          bool is_active);
 
   void drawDlpButton(QPainter &p, int x, int y, int w, int h);
+  void drawGacButton(QPainter &p, int x, int y, int w, int h);
   void drawColoredText(QPainter &p, int x, int y, const QString &text, QColor color);
   void drawStandstillTimer(QPainter &p, int x, int y);
 
@@ -192,6 +196,9 @@ private:
 
   bool hideVEgoUi;
 
+  bool gac;
+  int gacTr;
+
 protected:
   void paintGL() override;
   void initializeGL() override;

selfdrive/ui/ui.cc

@@ -216,6 +216,9 @@ static void update_state(UIState *s) {
     scene.dynamic_lane_profile = sm["lateralPlan"].getLateralPlan().getDynamicLaneProfile();
     scene.dynamic_lane_profile_status = sm["lateralPlan"].getLateralPlan().getDynamicLaneProfileStatus();
   }
+  if (sm.updated("carState")) {
+    scene.gac_tr = sm["carState"].getCarState().getGapAdjustCruiseTr();
+  }
 }
 
 void ui_update_params(UIState *s) {
@@ -236,6 +239,10 @@ void ui_update_params(UIState *s) {
   s->scene.hide_vego_ui = params.getBool("HideVEgoUi");
   s->scene.true_vego_ui = params.getBool("TrueVEgoUi");
   s->scene.chevron_data = std::atoi(params.get("ChevronInfo").c_str());
+  s->scene.gac = params.getBool("GapAdjustCruise");
+  s->scene.gac_mode = std::atoi(params.get("GapAdjustCruiseMode").c_str());
+  s->scene.gac_min = std::atoi(params.get("GapAdjustCruiseMin").c_str());
+  s->scene.gac_max = std::atoi(params.get("GapAdjustCruiseMax").c_str());
 
   if (s->scene.onroadScreenOff > 0) {
     s->scene.osoTimer = s->scene.onroadScreenOff * 60 * UI_FREQ;

selfdrive/ui/ui.h

@@ -166,6 +166,9 @@ typedef struct UIScene {
   bool hide_vego_ui, true_vego_ui;
 
   int chevron_data;
+
+  bool gac;
+  int gac_mode, gac_tr, gac_min, gac_max;
 } UIScene;
 
 class UIState : public QObject {