stopping behind lead is actually consistent now!

selfdrive/car/hyundai/carcontroller.py

@@ -55,6 +55,8 @@ class CarController(CarControllerBase):
     self.apply_steer_last = 0
     self.car_fingerprint = CP.carFingerprint
     self.last_button_frame = 0
+    self.accel_raw = 0
+    self.accel_val = 0
 
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
@@ -144,10 +146,11 @@ class CarController(CarControllerBase):
 
       if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
         # TODO: unclear if this is needed
-        jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
+        jerk = 2.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
-        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
-                                                        hud_control, set_speed_in_units, stopping,
+        self.create_accel_value(CC, accel)
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, self.accel_raw, self.accel_val, jerk,
+                                                        int(self.frame / 2), hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca))
 
       # 20 Hz LFA MFA message
@@ -205,3 +208,15 @@ class CarController(CarControllerBase):
             self.last_button_frame = self.frame
 
     return can_sends
+
+  # We currently use static jerk limits for all accel commands, the PCM is very sensitive to how accels are being
+  # sent from the SCC source of truth. Dynamic jerk upper/lower limits are required to make PCM accept controls
+  # more smoothly
+  def create_accel_value(self, CC, accel):
+    rate = 0.1  # TODO: Dynamic jerk upper/lower limits should change this for more accurate and smoother controls
+    if not CC.enabled:
+      self.accel_raw, self.accel_val = 0, 0
+    else:
+      self.accel_raw = accel
+      self.accel_val = clip(self.accel_raw, self.accel_last - rate, self.accel_last + rate)
+    self.accel_last = self.accel_val

selfdrive/car/hyundai/hyundaican.py

@@ -126,7 +126,8 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca):
+def create_acc_commands(packer, enabled, accel_raw, accel_val, upper_jerk, idx, hud_control, set_speed, stopping,
+                        long_override, use_fca):
   commands = []
 
   scc11_values = {
@@ -145,8 +146,8 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
   scc12_values = {
     "ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
     "StopReq": 1 if stopping else 0,
-    "aReqRaw": accel,
-    "aReqValue": accel,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
+    "aReqRaw": accel_raw,
+    "aReqValue": accel_val,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
     "CR_VSM_Alive": idx % 0xF,
   }
 
@@ -165,7 +166,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
     "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
     "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
     "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
-    "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
+    "JerkLowerLimit": 3.0, # stock usually is 0.5 but sometimes uses higher values
     "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
     "ObjGap": 2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
   }

selfdrive/car/hyundai/interface.py

@@ -89,7 +89,9 @@ class CarInterface(CarInterfaceBase):
     ret.stoppingControl = True
     ret.startingState = True
     ret.vEgoStarting = 0.1
-    ret.startAccel = 1.0
+    ret.startAccel = 1.8
+    ret.stopAccel = 0.0
+    ret.stoppingDecelRate = 10
     ret.longitudinalActuatorDelay = 0.5
 
     # *** feature detection ***

selfdrive/controls/lib/longcontrol.py

@@ -73,6 +73,9 @@ class LongControl:
       if output_accel > self.CP.stopAccel:
         output_accel = min(output_accel, 0.0)
         output_accel -= self.CP.stoppingDecelRate * DT_CTRL
+      elif output_accel < self.CP.stopAccel:
+        output_accel = min(output_accel, 0.0)
+        output_accel += self.CP.stoppingDecelRate * DT_CTRL
       self.reset()
 
     elif self.long_control_state == LongCtrlState.starting:

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -55,7 +55,7 @@ T_IDXS = np.array(T_IDXS_LST)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
-STOP_DISTANCE = 6.0
+STOP_DISTANCE = 8.0
 
 def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
   if personality==log.LongitudinalPersonality.relaxed: