diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a0c46f73a..23908ab3b 100755 --- a/selfdrive/controls/controlsd.py +++ b/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'] diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 800bbac67..8ccd81d8f 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py index 218bbb9f6..2f89594c8 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py index 699327cd8..01ddec68a 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py +++ b/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