Merge branch 'master-new' of https://github.com/sunnypilot/sunnypilot into master-new

selfdrive/controls/controlsd.py

@@ -49,7 +49,7 @@ class Controls(ControlsExt):
 
     self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
                                    'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
-                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext,
+                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'liveDelay'] + self.sm_services_ext,
                                   poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
 
@@ -116,6 +116,7 @@ class Controls(ControlsExt):
                                            torque_params.frictionCoefficientFiltered)
 
       self.LaC.extension.update_model_v2(self.sm['modelV2'])
+      self.LaC.extension.update_lateral_lag(self.sm['liveDelay'].lateralDelay)
 
     long_plan = self.sm['longitudinalPlan']
     model_v2 = self.sm['modelV2']

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py

@@ -11,7 +11,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
 LAT_PLAN_MIN_IDX = 5
-
+LATERAL_LAG_MOD = 0.0 # seconds, modifies how far in the future we look ahead for the lateral plan
 
 def get_predicted_lateral_jerk(lat_accels, t_diffs):
   # compute finite difference between subsequent model_v2.acceleration.y values
@@ -85,12 +85,15 @@ class LatControlTorqueExtBase:
 
     # precompute time differences between ModelConstants.T_IDXS
     self.t_diffs = np.diff(ModelConstants.T_IDXS)
-    self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3
+    self.desired_lat_jerk_time = CP.steerActuatorDelay + LATERAL_LAG_MOD
 
   def update_model_v2(self, model_v2):
     self.model_v2 = model_v2
     self.model_valid = self.model_v2 is not None and len(self.model_v2.orientation.x) >= CONTROL_N
 
+  def update_lateral_lag(self, lag):
+    self.desired_lat_jerk_time = max(0.01, lag) + LATERAL_LAG_MOD
+
   def update_friction_input(self, val_1, val_2):
     _error = val_1 - val_2
     _value = self.lat_accel_friction_factor * _error + self.lat_jerk_friction_factor * self.lookahead_lateral_jerk

sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py

@@ -45,9 +45,8 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
     self.pitch_last = 0.0
 
     # setup future time offsets
-    self.nn_time_offset = CP.steerActuatorDelay + 0.2
-    future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future
-    self.nn_future_times = [i + self.nn_time_offset for i in future_times]
+    self.future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future
+    self.nn_future_times = [i + self.desired_lat_jerk_time for i in self.future_times]
 
     # setup past time offsets
     self.past_times = [-0.3, -0.2, -0.1]
@@ -58,6 +57,10 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
     self.error_deque = deque(maxlen=history_check_frames[0])
     self.past_future_len = len(self.past_times) + len(self.nn_future_times)
 
+  def update_lateral_lag(self, lag):
+    super().update_lateral_lag(lag)
+    self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times]
+
   def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
     if not self.enabled or not self.model_valid or not self.has_nn_model:
       return

sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py

@@ -73,17 +73,21 @@ class TestNeuralNetworkLateralControl:
     controller.extension.model_v2 = model_v2
 
     # Saturate for curvature limited and controller limited
+    test_lag = 0.3
     for _ in range(1000):
       controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
       _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True)
     assert lac_log.saturated
 
     for _ in range(1000):
       controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
       _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False)
     assert not lac_log.saturated
 
     for _ in range(1000):
       controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
       _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False)
     assert lac_log.saturated