From 224ea93a7491aca8df941c118ab50bc875aab361 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 9 Sep 2025 15:10:32 -0700 Subject: [PATCH 01/43] use current desired curvature for error instead of plan indexed with live delay frames --- cereal/log.capnp | 2 ++ selfdrive/controls/controlsd.py | 6 +++++- selfdrive/controls/lib/latcontrol_torque.py | 9 +++++---- selfdrive/modeld/fill_model_msg.py | 4 +++- selfdrive/modeld/modeld.py | 4 +++- 5 files changed, 18 insertions(+), 7 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index b98c1f52423ac7..5e38e8de52fffa 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1018,6 +1018,7 @@ struct DrivingModelData { modelExecutionTime @7 :Float32; action @2 :ModelDataV2.Action; + rawAction @8: :ModelDataV2.Action; laneLineMeta @3 :LaneLineMeta; meta @4 :MetaData; @@ -1089,6 +1090,7 @@ struct ModelDataV2 { # e2e lateral planner action @26: Action; + rawAction @27: Action; gpuExecutionTimeDEPRECATED @17 :Float32; navEnabledDEPRECATED @22 :Bool; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 029d16e59e7f49..67f31a916f995f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -43,6 +43,7 @@ def __init__(self) -> None: self.steer_limited_by_safety = False self.curvature = 0.0 self.desired_curvature = 0.0 + self.raw_desired_curvature = 0.0 self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -116,11 +117,14 @@ def state_control(self): # Steering PID loop and lateral MPC # Reset desired curvature to current to avoid violating the limits on engage new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature + new_raw_desired_curvature = model_v2.rawAction.desiredCurvature if CC.latActive else self.curvature self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) + self.raw_desired_curvature, raw_curvature_limited = clip_curvature(CS.vEgo, self.raw_desired_curvature, new_raw_desired_curvature, lp.roll) + curvature_limited = curvature_limited or raw_curvature_limited actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, + self.steer_limited_by_safety, self.desired_curvature, new_raw_desired_curvature, curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 5a2814e0894879..f221d0714683c3 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -26,7 +26,7 @@ class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) self.torque_params = CP.lateralTuning.torque.as_builder() - self.torque_from_lateral_accel = CI.torque_from_lateral_accel() + self.torque_from_lateral_accel = CI.cH() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_f=self.torque_params.kf) @@ -43,7 +43,7 @@ def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -54,15 +54,16 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + raw_desired_lateral_accel = raw_desired_curvature * CS.vEgo ** 2 # current time desired lateral acceleration actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 - setpoint = desired_lateral_accel + low_speed_factor * desired_curvature + setpoint = raw_desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation - # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly + # do error correction in current time lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(setpoint - measurement) ff = gravity_adjusted_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 82c4c92b1d53c7..b9500cc240e675 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -56,7 +56,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs): builder.rightProb = lane_line_probs[2] def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, - net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, + net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, raw_action: log.ModelDataV2.Action, publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float, valid: bool) -> None: @@ -73,6 +73,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.modelExecutionTime = model_execution_time driving_model_data.action = action + driving_model_data.rawAction = raw_action modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -94,6 +95,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # action modelV2.action = action + modelV2.rawAction = raw_action # times at X_IDXS of edges and lines aren't used LINE_T_IDXS: list[float] = [] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 006eeef6f5ed56..4caab4d7915438 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -378,8 +378,10 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) + raw_action = get_action_from_model(model_output, prev_action, DT_MDL, long_delay + DT_MDL, v_ego) + prev_action = action - fill_model_msg(drivingdata_send, modelv2_send, model_output, action, + fill_model_msg(drivingdata_send, modelv2_send, model_output, action, raw_action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) From 4fea1665da8bba580478569f20496cdb9840a953 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 9 Sep 2025 16:09:46 -0700 Subject: [PATCH 02/43] fix typo --- cereal/log.capnp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 5e38e8de52fffa..f8af19ad6a311b 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1018,7 +1018,7 @@ struct DrivingModelData { modelExecutionTime @7 :Float32; action @2 :ModelDataV2.Action; - rawAction @8: :ModelDataV2.Action; + rawAction @8 :ModelDataV2.Action; laneLineMeta @3 :LaneLineMeta; meta @4 :MetaData; From cf0aa85d619a4292d3d588807cc3cf912237d89d Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 9 Sep 2025 16:20:00 -0700 Subject: [PATCH 03/43] correct function call --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f221d0714683c3..fcff12a215a87b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -26,7 +26,7 @@ class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) self.torque_params = CP.lateralTuning.torque.as_builder() - self.torque_from_lateral_accel = CI.cH() + self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_f=self.torque_params.kf) From f55fb02dfb719db8446468856e618c57ab555b68 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 9 Sep 2025 16:25:03 -0700 Subject: [PATCH 04/43] test should accomodate change --- selfdrive/controls/tests/test_latcontrol.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 0ce06dc9962765..db00120532668b 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -32,13 +32,13 @@ def test_saturation(self, car_name, controller): # Saturate for curvature limited and controller limited for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, 0, True) assert lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, 0, False) assert not lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, 1, False) assert lac_log.saturated From 8582b92ace37a67bde231f7f358c00f5dfa00eb8 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 9 Sep 2025 16:37:16 -0700 Subject: [PATCH 05/43] only give raw desired curvature to torque controlled platforms --- selfdrive/controls/controlsd.py | 12 +++++++++--- selfdrive/controls/lib/latcontrol.py | 2 +- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 67f31a916f995f..2b787b33bf5ab6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -123,9 +123,15 @@ def state_control(self): curvature_limited = curvature_limited or raw_curvature_limited actuators.curvature = self.desired_curvature - steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, new_raw_desired_curvature, - curvature_limited) # TODO what if not available + + if self.CP.lateralTuning.which() == 'torque': + steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, + self.steer_limited_by_safety, self.desired_curvature, new_raw_desired_curvature, + curvature_limited) # TODO what if not available + else: + steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, + self.steer_limited_by_safety, self.desired_curvature, + curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 2a8b873e2e592a..c0d077d795b843 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -15,7 +15,7 @@ def __init__(self, CP, CI): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): pass def reset(self): From 33f9aadbfe9a38d9870bc930381c8c573b587c10 Mon Sep 17 00:00:00 2001 From: felsager Date: Wed, 10 Sep 2025 10:20:43 -0700 Subject: [PATCH 06/43] raw desired curvature as input to all controllers --- selfdrive/controls/controlsd.py | 10 ++-------- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- 4 files changed, 5 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2b787b33bf5ab6..56f69f42e5181a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -123,14 +123,8 @@ def state_control(self): curvature_limited = curvature_limited or raw_curvature_limited actuators.curvature = self.desired_curvature - - if self.CP.lateralTuning.which() == 'torque': - steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, new_raw_desired_curvature, - curvature_limited) # TODO what if not available - else: - steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, + steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, + self.steer_limited_by_safety, self.desired_curvature, self.raw_desired_curvature, curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index c0d077d795b843..45ca859cd4e843 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -15,7 +15,7 @@ def __init__(self, CP, CI): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index ac35151487c1c5..5cdeb979fdcbad 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 00a083509fb756..31c6fda037b804 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) From e841e9c516a5b7d3d4a5c0936c33115ad62099a1 Mon Sep 17 00:00:00 2001 From: felsager Date: Wed, 10 Sep 2025 12:15:30 -0700 Subject: [PATCH 07/43] update ref --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a833fadb94813f..7c40f39fd4903e 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -afcab1abb62b9d5678342956cced4712f44e909e \ No newline at end of file +d1129a9af46cca9f6509bde84873397de2656f06 \ No newline at end of file From 57687aa2bcab45373b8634873be1320ba8294ef0 Mon Sep 17 00:00:00 2001 From: felsager Date: Wed, 10 Sep 2025 12:54:28 -0700 Subject: [PATCH 08/43] . From faa3d02055f28f02c40235d0139351106d313726 Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 11 Sep 2025 11:45:04 -0700 Subject: [PATCH 09/43] keep it clean --- cereal/log.capnp | 2 -- selfdrive/controls/controlsd.py | 6 +----- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 5 ++--- 6 files changed, 6 insertions(+), 13 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index f8af19ad6a311b..b98c1f52423ac7 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1018,7 +1018,6 @@ struct DrivingModelData { modelExecutionTime @7 :Float32; action @2 :ModelDataV2.Action; - rawAction @8 :ModelDataV2.Action; laneLineMeta @3 :LaneLineMeta; meta @4 :MetaData; @@ -1090,7 +1089,6 @@ struct ModelDataV2 { # e2e lateral planner action @26: Action; - rawAction @27: Action; gpuExecutionTimeDEPRECATED @17 :Float32; navEnabledDEPRECATED @22 :Bool; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 56f69f42e5181a..2fc41eb9300d02 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -43,7 +43,6 @@ def __init__(self) -> None: self.steer_limited_by_safety = False self.curvature = 0.0 self.desired_curvature = 0.0 - self.raw_desired_curvature = 0.0 self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -117,14 +116,11 @@ def state_control(self): # Steering PID loop and lateral MPC # Reset desired curvature to current to avoid violating the limits on engage new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature - new_raw_desired_curvature = model_v2.rawAction.desiredCurvature if CC.latActive else self.curvature self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) - self.raw_desired_curvature, raw_curvature_limited = clip_curvature(CS.vEgo, self.raw_desired_curvature, new_raw_desired_curvature, lp.roll) - curvature_limited = curvature_limited or raw_curvature_limited actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, self.raw_desired_curvature, + self.steer_limited_by_safety, self.desired_curvature, curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 45ca859cd4e843..c0d077d795b843 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -15,7 +15,7 @@ def __init__(self, CP, CI): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 5cdeb979fdcbad..ac35151487c1c5 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 31c6fda037b804..00a083509fb756 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index fcff12a215a87b..bcd08190dbd3bc 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -43,7 +43,7 @@ def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -54,12 +54,11 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - raw_desired_lateral_accel = raw_desired_curvature * CS.vEgo ** 2 # current time desired lateral acceleration actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 - setpoint = raw_desired_lateral_accel + low_speed_factor * desired_curvature + setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation From 592fb97479eb5771256e3cf431925036d40e7cca Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 11 Sep 2025 11:47:04 -0700 Subject: [PATCH 10/43] keep it clean --- selfdrive/controls/controlsd.py | 4 ++-- selfdrive/controls/tests/test_latcontrol.py | 6 +++--- selfdrive/modeld/fill_model_msg.py | 4 +--- selfdrive/modeld/modeld.py | 3 +-- 4 files changed, 7 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2fc41eb9300d02..029d16e59e7f49 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -120,8 +120,8 @@ def state_control(self): actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, - curvature_limited) # TODO what if not available + self.steer_limited_by_safety, self.desired_curvature, + curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index db00120532668b..0ce06dc9962765 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -32,13 +32,13 @@ def test_saturation(self, car_name, controller): # Saturate for curvature limited and controller limited for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, 0, True) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True) assert lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, 0, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False) assert not lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, 1, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False) assert lac_log.saturated diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index b9500cc240e675..82c4c92b1d53c7 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -56,7 +56,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs): builder.rightProb = lane_line_probs[2] def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, - net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, raw_action: log.ModelDataV2.Action, + net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float, valid: bool) -> None: @@ -73,7 +73,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.modelExecutionTime = model_execution_time driving_model_data.action = action - driving_model_data.rawAction = raw_action modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -95,7 +94,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # action modelV2.action = action - modelV2.rawAction = raw_action # times at X_IDXS of edges and lines aren't used LINE_T_IDXS: list[float] = [] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 4caab4d7915438..c8109ce1ed49fe 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -378,10 +378,9 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) - raw_action = get_action_from_model(model_output, prev_action, DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action - fill_model_msg(drivingdata_send, modelv2_send, model_output, action, raw_action, + fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) From cc665622e498744d95106840ddfcac3c680fe20a Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 11 Sep 2025 11:47:30 -0700 Subject: [PATCH 11/43] keep it clean --- selfdrive/modeld/modeld.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index c8109ce1ed49fe..006eeef6f5ed56 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -378,7 +378,6 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) - prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, From 762f012b4c661827ae6b22c78b0c9878bf13ebe8 Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 11 Sep 2025 14:14:18 -0700 Subject: [PATCH 12/43] desired lat accel buffer --- selfdrive/controls/controlsd.py | 6 ++++-- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 20 ++++++++++++-------- selfdrive/controls/tests/test_latcontrol.py | 6 +++--- 6 files changed, 22 insertions(+), 16 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 029d16e59e7f49..592a78909fe352 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -36,7 +36,7 @@ def __init__(self) -> None: self.CI = interfaces[self.CP.carFingerprint](self.CP) self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', - 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', + 'liveCalibration', 'liveDelay', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) @@ -118,10 +118,12 @@ def state_control(self): new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) + lat_delay = self.sm["liveDelay"].lateralDelay + actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_safety, self.desired_curvature, - curvature_limited) # TODO what if not available + curvature_limited, lat_delay) actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index c0d077d795b843..9bfff7e3f7dc36 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -15,7 +15,7 @@ def __init__(self, CP, CI): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index ac35151487c1c5..ff9eab2f6f5199 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 00a083509fb756..38faeb17fda655 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index bcd08190dbd3bc..1d12eec8791d6f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,7 +4,7 @@ from cereal import log from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY -from openpilot.selfdrive.controls.lib.latcontrol import LatControl +from openpilot.selfdrive.controls.lib.latcontrol import DT_CTRL, LatControl from openpilot.common.pid import PIDController # At higher speeds (25+mph) we can assume: @@ -32,6 +32,7 @@ def __init__(self, CP, CI): k_f=self.torque_params.kf) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg + self.desired_lateral_accel_buffer = [0.0] * int(1/DT_CTRL) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -43,7 +44,7 @@ def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -53,21 +54,24 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + self.desired_lateral_accel_buffer[1:] = self.desired_lateral_accel_buffer[:-1] + self.desired_lateral_accel_buffer[0] = lag_compensated_desired_lateral_accel + current_desired_lateral_accel = np.interp(lat_delay, DT_CTRL * np.arange(1 / DT_CTRL), self.desired_lateral_accel_buffer) actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 - setpoint = desired_lateral_accel + low_speed_factor * desired_curvature + setpoint = current_desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature - gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation + gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation - # do error correction in current time lateral acceleration space, convert at end to handle non-linear torque responses correctly + # do error correction at current time in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(setpoint - measurement) ff = gravity_adjusted_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += get_friction(current_desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, @@ -83,7 +87,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat pid_log.f = float(self.pid.f) pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(actual_lateral_accel) - pid_log.desiredLateralAccel = float(desired_lateral_accel) + pid_log.desiredLateralAccel = float(current_desired_lateral_accel) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 0ce06dc9962765..8fff214602fc26 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -32,13 +32,13 @@ def test_saturation(self, car_name, controller): # Saturate for curvature limited and controller limited for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True, 0) assert lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False, 0) assert not lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False, 0) assert lac_log.saturated From 44319b723ab3c9e6b4b5473e27f1825db7169113 Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 11 Sep 2025 14:16:49 -0700 Subject: [PATCH 13/43] update ref --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7c40f39fd4903e..b7df7feadf0a48 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d1129a9af46cca9f6509bde84873397de2656f06 \ No newline at end of file +bf449ac2ead17d43c064385ca1ac826975a7caea \ No newline at end of file From cafa6b9a839d4c434c1ff99f9c9585c2224be9d7 Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 12 Sep 2025 11:42:40 -0700 Subject: [PATCH 14/43] use deque and add type annotation for confusing variables --- selfdrive/controls/lib/drive_helpers.py | 2 +- selfdrive/controls/lib/latcontrol.py | 4 ++-- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 22 ++++++++++++--------- 5 files changed, 18 insertions(+), 14 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index e28fa3021c49bd..bf6dd04f603a2f 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -22,7 +22,7 @@ def smooth_value(val, prev_val, tau, dt=DT_MDL): alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1 return alpha * val + (1 - alpha) * prev_val -def clip_curvature(v_ego, prev_curvature, new_curvature, roll): +def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, bool]: # This function respects ISO lateral jerk and acceleration limits + a max curvature v_ego = max(v_ego, MIN_SPEED) max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 9bfff7e3f7dc36..6bda370fee3e00 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -15,13 +15,13 @@ def __init__(self, CP, CI): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay): pass def reset(self): self.sat_count = 0. - def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited): + def _check_saturation(self, saturated: bool, CS, steer_limited_by_safety: bool, curvature_limited: bool): # Saturated only if control output is not being limited by car torque/angle rate limits if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed: self.sat_count += self.sat_count_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index ff9eab2f6f5199..c9669f71b1103c 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 38faeb17fda655..a71eda323386d7 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 1d12eec8791d6f..e0373272d28423 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,3 +1,4 @@ +from collections import deque import math import numpy as np @@ -21,6 +22,8 @@ LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [15, 13, 10, 5] +LATACCEL_REQUEST_BUFFER_SIZE = int(1/DT_CTRL) + class LatControlTorque(LatControl): def __init__(self, CP, CI): @@ -32,7 +35,7 @@ def __init__(self, CP, CI): k_f=self.torque_params.kf) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.desired_lateral_accel_buffer = [0.0] * int(1/DT_CTRL) + self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -44,7 +47,7 @@ def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -54,24 +57,25 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) + delay_frames = int(lat_delay / DT_CTRL) lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - self.desired_lateral_accel_buffer[1:] = self.desired_lateral_accel_buffer[:-1] - self.desired_lateral_accel_buffer[0] = lag_compensated_desired_lateral_accel - current_desired_lateral_accel = np.interp(lat_delay, DT_CTRL * np.arange(1 / DT_CTRL), self.desired_lateral_accel_buffer) + self.requested_lateral_accel_buffer.appendleft(lag_compensated_desired_lateral_accel) + current_expected_lateral_accel = self.requested_lateral_accel_buffer[delay_frames] actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 - setpoint = current_desired_lateral_accel + low_speed_factor * desired_curvature + # pid error calculated as difference between expected and measured lateral acceleration + setpoint = current_expected_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation - # do error correction at current time in lateral acceleration space, convert at end to handle non-linear torque responses correctly + # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(setpoint - measurement) ff = gravity_adjusted_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(current_desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += get_friction(current_expected_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, @@ -87,7 +91,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat pid_log.f = float(self.pid.f) pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(actual_lateral_accel) - pid_log.desiredLateralAccel = float(current_desired_lateral_accel) + pid_log.desiredLateralAccel = float(current_expected_lateral_accel) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention From 9d9f94c667fc52a62effa6c97fe53c80c27842c5 Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 12 Sep 2025 11:49:13 -0700 Subject: [PATCH 15/43] use deque and add type annotation for confusing variables --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index b7df7feadf0a48..6971cb6afa4428 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -bf449ac2ead17d43c064385ca1ac826975a7caea \ No newline at end of file +759449bb5e84917e9f18a11e8e4afdb944be4472 \ No newline at end of file From d5a522b020b183ddf50c903d30bdc62082fb6286 Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 12 Sep 2025 11:50:03 -0700 Subject: [PATCH 16/43] don't use expected lat accel for friction --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index e0373272d28423..01fdd14a056cfc 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -75,7 +75,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_ ff = gravity_adjusted_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(current_expected_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += get_friction(lag_compensated_desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, From 639afc83ea33846c9b817f270922bdc3d633f8b5 Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 12 Sep 2025 11:50:57 -0700 Subject: [PATCH 17/43] update ref --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 6971cb6afa4428..4a57fd3ada98ba 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -759449bb5e84917e9f18a11e8e4afdb944be4472 \ No newline at end of file +4e2926cc457a057980633163eabe062e421e9ff6 \ No newline at end of file From acf3863c56fc0eba7ddc7de89c55e6dd83ffb9f7 Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 12 Sep 2025 12:44:47 -0700 Subject: [PATCH 18/43] use current expected curvature for low speed factor --- selfdrive/controls/lib/latcontrol_torque.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 01fdd14a056cfc..f90de0504cf20a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -61,12 +61,13 @@ def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_ lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.requested_lateral_accel_buffer.appendleft(lag_compensated_desired_lateral_accel) current_expected_lateral_accel = self.requested_lateral_accel_buffer[delay_frames] + current_expected_curvature = current_expected_lateral_accel / (CS.vEgo ** 2) actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 # pid error calculated as difference between expected and measured lateral acceleration - setpoint = current_expected_lateral_accel + low_speed_factor * desired_curvature + setpoint = current_expected_lateral_accel + low_speed_factor * current_expected_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation From 106b3b601798fd0aa72aba672db4f56b5a0ba035 Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 12 Sep 2025 13:29:41 -0700 Subject: [PATCH 19/43] type lat_delay --- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 6bda370fee3e00..ccef1f62e1310e 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -15,7 +15,7 @@ def __init__(self, CP, CI): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay): + def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay: float): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index c9669f71b1103c..af824ee1319e55 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay): + def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay: float): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index a71eda323386d7..ac772159da28bb 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay): + def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay: float): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f90de0504cf20a..936ae6b95003b5 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -47,7 +47,7 @@ def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay): + def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay: float): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 From f8c2a7dfffdcfef1f29ef1bfd0cd4aecd1ba4448 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 16 Sep 2025 14:25:15 -0700 Subject: [PATCH 20/43] types in different pr and index from end of array --- selfdrive/controls/lib/latcontrol.py | 4 ++-- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 6 +++--- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index ccef1f62e1310e..a816b6a018eede 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -15,13 +15,13 @@ def __init__(self, CP, CI): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay: float): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float): pass def reset(self): self.sat_count = 0. - def _check_saturation(self, saturated: bool, CS, steer_limited_by_safety: bool, curvature_limited: bool): + def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited): # Saturated only if control output is not being limited by car torque/angle rate limits if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed: self.sat_count += self.sat_count_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index af824ee1319e55..a8c71ad412ebbd 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay: float): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index ac772159da28bb..6b318c78e53375 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay: float): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 936ae6b95003b5..236f750d4c6f3f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -47,7 +47,7 @@ def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_curvature, curvature_limited: bool, lat_delay: float): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -59,8 +59,8 @@ def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_ delay_frames = int(lat_delay / DT_CTRL) lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - self.requested_lateral_accel_buffer.appendleft(lag_compensated_desired_lateral_accel) - current_expected_lateral_accel = self.requested_lateral_accel_buffer[delay_frames] + self.requested_lateral_accel_buffer.append(lag_compensated_desired_lateral_accel) + current_expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] current_expected_curvature = current_expected_lateral_accel / (CS.vEgo ** 2) actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 From 69b3ea13db7c6088a432d57c41d6b2c7f130a102 Mon Sep 17 00:00:00 2001 From: felsager Date: Wed, 17 Sep 2025 14:39:53 -0700 Subject: [PATCH 21/43] try half delay --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 236f750d4c6f3f..85645b3e84d307 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -57,7 +57,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - delay_frames = int(lat_delay / DT_CTRL) + delay_frames = int(0.5 * lat_delay / DT_CTRL) lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.requested_lateral_accel_buffer.append(lag_compensated_desired_lateral_accel) current_expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] From 997cfeddcc0b878ae72d053e6a9810e70d4c11a7 Mon Sep 17 00:00:00 2001 From: felsager Date: Wed, 17 Sep 2025 14:43:49 -0700 Subject: [PATCH 22/43] try zero delay --- selfdrive/controls/lib/latcontrol_torque.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 85645b3e84d307..7b60b223e9b697 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -57,7 +57,9 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - delay_frames = int(0.5 * lat_delay / DT_CTRL) + delay_frames = int(0.0 * lat_delay / DT_CTRL) + if delay_frames == 0: + delay_frames = 1 lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.requested_lateral_accel_buffer.append(lag_compensated_desired_lateral_accel) current_expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] From 26886636de55655261e3a1e006e1517eb5b62215 Mon Sep 17 00:00:00 2001 From: felsager Date: Wed, 17 Sep 2025 15:06:25 -0700 Subject: [PATCH 23/43] full lag for testing --- selfdrive/controls/lib/latcontrol_torque.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 7b60b223e9b697..236f750d4c6f3f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -57,9 +57,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - delay_frames = int(0.0 * lat_delay / DT_CTRL) - if delay_frames == 0: - delay_frames = 1 + delay_frames = int(lat_delay / DT_CTRL) lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.requested_lateral_accel_buffer.append(lag_compensated_desired_lateral_accel) current_expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] From 843295d1a567b3dde1dacc9c3cc34c5798bc230b Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 18 Sep 2025 11:34:54 -0700 Subject: [PATCH 24/43] tweak torqued --- selfdrive/locationd/torqued.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 3f9b846e822147..43851efd64a0fc 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -13,10 +13,10 @@ from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose HISTORY = 5 # secs -POINTS_PER_BUCKET = 1500 -MIN_POINTS_TOTAL = 4000 +POINTS_PER_BUCKET = 1000 +MIN_POINTS_TOTAL = 6000 MIN_POINTS_TOTAL_QLOG = 600 -FIT_POINTS_TOTAL = 2000 +FIT_POINTS_TOTAL = 12000 FIT_POINTS_TOTAL_QLOG = 600 MIN_VEL = 15 # m/s FRICTION_FACTOR = 1.5 # ~85% of data coverage @@ -28,8 +28,9 @@ MIN_FILTER_DECAY = 50 MAX_FILTER_DECAY = 250 LAT_ACC_THRESHOLD = 1 -STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)] -MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100]) +STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.15), (-0.15, -0.1), (-0.1, -0.05), (-0.05, -STEER_MIN_THRESHOLD), + (STEER_MIN_THRESHOLD, 0.05), (0.05, 0.1), (0.1, 0.15), (0.15, 0.2), (0.2, 0.3), (0.3, 0.5)] +MIN_BUCKET_POINTS = np.array([100, 300, 300, 400, 500, 600, 600, 500, 400, 300, 300, 100]) MIN_ENGAGE_BUFFER = 2 # secs VERSION = 1 # bump this to invalidate old parameter caches From fb93974e0699620c60878328bce68d04d02f4eed Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 18 Sep 2025 12:57:58 -0700 Subject: [PATCH 25/43] only use expected error for pid --- common/pid.py | 4 +++- selfdrive/controls/lib/latcontrol_torque.py | 11 +++++++---- selfdrive/locationd/torqued.py | 11 +++++------ 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/common/pid.py b/common/pid.py index 99142280ca4839..bd88ff2529b9ee 100644 --- a/common/pid.py +++ b/common/pid.py @@ -44,13 +44,15 @@ def set_limits(self, pos_limit, neg_limit): self.pos_limit = pos_limit self.neg_limit = neg_limit - def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): + def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False, error_expected = None): self.speed = speed self.p = float(error) * self.k_p self.f = feedforward * self.k_f self.d = error_rate * self.k_d if not freeze_integrator: + if error_expected_error is not None: + error = error_expected_error i = self.i + error * self.k_i * self.i_rate # Don't allow windup if already clipping diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 236f750d4c6f3f..d0b1704292dac2 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -67,9 +67,11 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 # pid error calculated as difference between expected and measured lateral acceleration - setpoint = current_expected_lateral_accel + low_speed_factor * current_expected_curvature + setpoint = lag_compensated_desired_lateral_accel + low_speed_factor * lag_compensated_desired_lateral_accel + setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_lateral_accel measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation + error_expected = float(setpoint_expected - measurement) # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(setpoint - measurement) @@ -80,9 +82,10 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator, + error_expected=error_expected) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) pid_log.active = True diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 43851efd64a0fc..3f9b846e822147 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -13,10 +13,10 @@ from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose HISTORY = 5 # secs -POINTS_PER_BUCKET = 1000 -MIN_POINTS_TOTAL = 6000 +POINTS_PER_BUCKET = 1500 +MIN_POINTS_TOTAL = 4000 MIN_POINTS_TOTAL_QLOG = 600 -FIT_POINTS_TOTAL = 12000 +FIT_POINTS_TOTAL = 2000 FIT_POINTS_TOTAL_QLOG = 600 MIN_VEL = 15 # m/s FRICTION_FACTOR = 1.5 # ~85% of data coverage @@ -28,9 +28,8 @@ MIN_FILTER_DECAY = 50 MAX_FILTER_DECAY = 250 LAT_ACC_THRESHOLD = 1 -STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.15), (-0.15, -0.1), (-0.1, -0.05), (-0.05, -STEER_MIN_THRESHOLD), - (STEER_MIN_THRESHOLD, 0.05), (0.05, 0.1), (0.1, 0.15), (0.15, 0.2), (0.2, 0.3), (0.3, 0.5)] -MIN_BUCKET_POINTS = np.array([100, 300, 300, 400, 500, 600, 600, 500, 400, 300, 300, 100]) +STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)] +MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100]) MIN_ENGAGE_BUFFER = 2 # secs VERSION = 1 # bump this to invalidate old parameter caches From ffadc7074ba63a9e62f0ef1200779df4b63c0ad3 Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 18 Sep 2025 13:03:20 -0700 Subject: [PATCH 26/43] only use expected error for pid --- common/pid.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/pid.py b/common/pid.py index bd88ff2529b9ee..12a9ce192bf2af 100644 --- a/common/pid.py +++ b/common/pid.py @@ -51,8 +51,8 @@ def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integr self.d = error_rate * self.k_d if not freeze_integrator: - if error_expected_error is not None: - error = error_expected_error + if error_expected is not None: + error = error_expected i = self.i + error * self.k_i * self.i_rate # Don't allow windup if already clipping From 2fa7e51b09c3deab1c48c7f3730029dea8a90d35 Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 18 Sep 2025 13:06:12 -0700 Subject: [PATCH 27/43] only use expected error for pid --- common/pid.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/pid.py b/common/pid.py index 12a9ce192bf2af..5fc710e8768b63 100644 --- a/common/pid.py +++ b/common/pid.py @@ -51,8 +51,8 @@ def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integr self.d = error_rate * self.k_d if not freeze_integrator: - if error_expected is not None: - error = error_expected + # if error_expected is not None: + # error = error_expected i = self.i + error * self.k_i * self.i_rate # Don't allow windup if already clipping From e517c4d031d9bb511bae1addefb25cc5f188e635 Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 18 Sep 2025 13:16:31 -0700 Subject: [PATCH 28/43] correct low speed compensation --- selfdrive/controls/lib/latcontrol_torque.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d0b1704292dac2..ae3548d5e9394a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -67,8 +67,8 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 # pid error calculated as difference between expected and measured lateral acceleration - setpoint = lag_compensated_desired_lateral_accel + low_speed_factor * lag_compensated_desired_lateral_accel - setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_lateral_accel + setpoint = lag_compensated_desired_lateral_accel + low_speed_factor * desired_curvature + setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation error_expected = float(setpoint_expected - measurement) @@ -95,7 +95,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat pid_log.f = float(self.pid.f) pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(actual_lateral_accel) - pid_log.desiredLateralAccel = float(current_expected_lateral_accel) + pid_log.desiredLateralAccel = float(lag_compensated_desired_lateral_accel) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention From d18423b77706285f04bc05d78b7a7bce0bbc2c28 Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 18 Sep 2025 13:17:47 -0700 Subject: [PATCH 29/43] use expected error for the integrator --- common/pid.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/pid.py b/common/pid.py index 5fc710e8768b63..12a9ce192bf2af 100644 --- a/common/pid.py +++ b/common/pid.py @@ -51,8 +51,8 @@ def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integr self.d = error_rate * self.k_d if not freeze_integrator: - # if error_expected is not None: - # error = error_expected + if error_expected is not None: + error = error_expected i = self.i + error * self.k_i * self.i_rate # Don't allow windup if already clipping From 176d5faa8033df39956cea45a3fc1d879fc5e855 Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 26 Sep 2025 14:43:38 -0700 Subject: [PATCH 30/43] activate derivative action --- common/pid.py | 4 ++-- selfdrive/controls/lib/latcontrol_torque.py | 15 ++++++++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/common/pid.py b/common/pid.py index 12a9ce192bf2af..1244e5d68bdb06 100644 --- a/common/pid.py +++ b/common/pid.py @@ -51,8 +51,8 @@ def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integr self.d = error_rate * self.k_d if not freeze_integrator: - if error_expected is not None: - error = error_expected + #if error_expected is not None: + # error = error_expected i = self.i + error * self.k_i * self.i_rate # Don't allow windup if already clipping diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index ae3548d5e9394a..4dc9d1bf7c8d80 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -6,6 +6,7 @@ from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.controls.lib.latcontrol import DT_CTRL, LatControl +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.pid import PIDController # At higher speeds (25+mph) we can assume: @@ -36,6 +37,7 @@ def __init__(self, CP, CI): self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE) + self.error_pre = 0.0 def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -68,11 +70,13 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 # pid error calculated as difference between expected and measured lateral acceleration setpoint = lag_compensated_desired_lateral_accel + low_speed_factor * desired_curvature - setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature + #setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation - error_expected = float(setpoint_expected - measurement) - + #error_expected = float(setpoint_expected - measurement) + error = float(setpoint - measurement) + error_rate = (error - self.error_pre) / DT_CTRL + self.error_pre = error # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(setpoint - measurement) ff = gravity_adjusted_lateral_accel @@ -82,10 +86,11 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, + error_rate, feedforward=ff, speed=CS.vEgo, - freeze_integrator=freeze_integrator, - error_expected=error_expected) + freeze_integrator=freeze_integrator,) + #error_expected=error_expected) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) pid_log.active = True From c1cf773d48fd172023086acaf1b6ff46532eb774 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 30 Sep 2025 15:11:11 -0700 Subject: [PATCH 31/43] derivative to compensate large changes --- selfdrive/controls/lib/latcontrol_torque.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 4dc9d1bf7c8d80..8a1c8cea56d85b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -32,8 +32,7 @@ def __init__(self, CP, CI): self.torque_params = CP.lateralTuning.torque.as_builder() self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, - k_f=self.torque_params.kf) + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_d=self.torque_params.kd, k_f=self.torque_params.kf) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE) From 603fb8a4ed4ad3792c1dc6b6a6ad0899d7f87156 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 30 Sep 2025 15:40:37 -0700 Subject: [PATCH 32/43] no kd for now --- opendbc_repo | 2 +- panda | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/opendbc_repo b/opendbc_repo index aa3d32a63b7d05..2eec1af104972b 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit aa3d32a63b7d05e69777fa90147192220abde8a3 +Subproject commit 2eec1af104972b7784644bf38c4c5afb52fc070a diff --git a/panda b/panda index 615009cf0f8fb8..1289337ceb6205 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 615009cf0f8fb8f3feadac160fbb0a07e4de171b +Subproject commit 1289337ceb6205ad985a5469baa950b319329327 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 8a1c8cea56d85b..523ab7527c216e 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -32,7 +32,7 @@ def __init__(self, CP, CI): self.torque_params = CP.lateralTuning.torque.as_builder() self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_d=self.torque_params.kd, k_f=self.torque_params.kf) + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_f=self.torque_params.kf) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE) From 354193121ca99b6c303980aa3ad2a8002b723c1b Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 30 Sep 2025 15:48:57 -0700 Subject: [PATCH 33/43] updated opendbc with branch --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 2eec1af104972b..5e9db0dba11f09 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 2eec1af104972b7784644bf38c4c5afb52fc070a +Subproject commit 5e9db0dba11f09055fa284ba14f4844b4b5d7dc4 From 32ea793159aeb2fa6ea681b3a694f4fae57be537 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 30 Sep 2025 15:51:04 -0700 Subject: [PATCH 34/43] initialize pid with derivative coefficient --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 523ab7527c216e..f0557cbc6c3c26 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -32,7 +32,7 @@ def __init__(self, CP, CI): self.torque_params = CP.lateralTuning.torque.as_builder() self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_f=self.torque_params.kf) + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, self.torque_params.kd, k_f=self.torque_params.kf) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE) From fd14a2dd37760d7f9daad3a0e11ea0d0c80ef97a Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 30 Sep 2025 15:58:07 -0700 Subject: [PATCH 35/43] set k_d properly --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f0557cbc6c3c26..8a1c8cea56d85b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -32,7 +32,7 @@ def __init__(self, CP, CI): self.torque_params = CP.lateralTuning.torque.as_builder() self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, self.torque_params.kd, k_f=self.torque_params.kf) + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_d=self.torque_params.kd, k_f=self.torque_params.kf) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE) From 8cbb481c8e9c4cf181e46b425c6db35cfd2d181e Mon Sep 17 00:00:00 2001 From: felsager Date: Wed, 1 Oct 2025 17:03:26 -0700 Subject: [PATCH 36/43] use expected error --- selfdrive/controls/lib/latcontrol_torque.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 8a1c8cea56d85b..aa966062d65707 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -69,15 +69,15 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 # pid error calculated as difference between expected and measured lateral acceleration setpoint = lag_compensated_desired_lateral_accel + low_speed_factor * desired_curvature - #setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature + setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation - #error_expected = float(setpoint_expected - measurement) - error = float(setpoint - measurement) - error_rate = (error - self.error_pre) / DT_CTRL - self.error_pre = error + error_expected = float(setpoint_expected - measurement) + #error = float(setpoint - measurement) + error_rate = (error_expected - self.error_pre) / DT_CTRL + self.error_pre = error_expected # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(setpoint - measurement) + pid_log.error = float(error_expected) ff = gravity_adjusted_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset From e00931a7e32ee2c85ab66d6de7ba17f9924e30db Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 2 Oct 2025 14:53:18 -0700 Subject: [PATCH 37/43] reference derivative feedforward --- selfdrive/controls/lib/latcontrol_torque.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index aa966062d65707..95458575f556ed 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -37,6 +37,7 @@ def __init__(self, CP, CI): self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE) self.error_pre = 0.0 + self.gravity_adjusted_lateral_accel_pre = 0.0 def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -59,8 +60,8 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) delay_frames = int(lat_delay / DT_CTRL) - lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - self.requested_lateral_accel_buffer.append(lag_compensated_desired_lateral_accel) + plan_future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + self.requested_lateral_accel_buffer.append(plan_future_desired_lateral_accel) current_expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] current_expected_curvature = current_expected_lateral_accel / (CS.vEgo ** 2) actual_lateral_accel = actual_curvature * CS.vEgo ** 2 @@ -68,20 +69,23 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 # pid error calculated as difference between expected and measured lateral acceleration - setpoint = lag_compensated_desired_lateral_accel + low_speed_factor * desired_curvature + #setpoint = plan_future_desired_lateral_accel + low_speed_factor * desired_curvature setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature - gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation + gravity_adjusted_lateral_accel = plan_future_desired_lateral_accel - roll_compensation error_expected = float(setpoint_expected - measurement) #error = float(setpoint - measurement) error_rate = (error_expected - self.error_pre) / DT_CTRL + jerk_ff = (gravity_adjusted_lateral_accel - self.gravity_adjusted_lateral_accel_pre) / DT_CTRL self.error_pre = error_expected + self.gravity_adjusted_lateral_accel_pre = gravity_adjusted_lateral_accel # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(error_expected) ff = gravity_adjusted_lateral_accel + ff += 0.5*jerk_ff # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(lag_compensated_desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += get_friction(plan_future_desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, @@ -99,7 +103,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat pid_log.f = float(self.pid.f) pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(actual_lateral_accel) - pid_log.desiredLateralAccel = float(lag_compensated_desired_lateral_accel) + pid_log.desiredLateralAccel = float(plan_future_desired_lateral_accel) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention From 6f81ce7a1e5b0256519190487cf0d52cecd6b348 Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 3 Oct 2025 13:33:42 -0700 Subject: [PATCH 38/43] improve pid naming convention and use dt for controls --- common/pid.py | 14 ++++---- selfdrive/controls/lib/latcontrol.py | 17 +++++----- selfdrive/controls/lib/latcontrol_torque.py | 37 +++++++++++---------- 3 files changed, 34 insertions(+), 34 deletions(-) diff --git a/common/pid.py b/common/pid.py index 1244e5d68bdb06..ff5084781f7e4a 100644 --- a/common/pid.py +++ b/common/pid.py @@ -16,7 +16,7 @@ def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, self.set_limits(pos_limit, neg_limit) - self.i_rate = 1.0 / rate + self.i_dt = 1.0 / rate self.speed = 0.0 self.reset() @@ -44,16 +44,14 @@ def set_limits(self, pos_limit, neg_limit): self.pos_limit = pos_limit self.neg_limit = neg_limit - def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False, error_expected = None): + def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): self.speed = speed - self.p = float(error) * self.k_p - self.f = feedforward * self.k_f - self.d = error_rate * self.k_d + self.p = self.k_p * float(error) + self.d = self.k_d * error_rate + self.f = self.k_f * feedforward if not freeze_integrator: - #if error_expected is not None: - # error = error_expected - i = self.i + error * self.k_i * self.i_rate + i = self.i + self.k_i * self.i_dt * error # Don't allow windup if already clipping test_control = self.p + i + self.d + self.f diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index a816b6a018eede..80d33d9751d693 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -5,10 +5,11 @@ class LatControl(ABC): - def __init__(self, CP, CI): - self.sat_count_rate = 1.0 * DT_CTRL + def __init__(self, CP, CI, dt=DT_CTRL): + self.dt = dt + self.sat_time_dt = 1.0 * dt self.sat_limit = CP.steerLimitTimer - self.sat_count = 0. + self.sat_time = 0. self.sat_check_min_speed = 10. # we define the steer torque scale as [-1.0...1.0] @@ -19,13 +20,13 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat pass def reset(self): - self.sat_count = 0. + self.sat_time = 0. def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited): # Saturated only if control output is not being limited by car torque/angle rate limits if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed: - self.sat_count += self.sat_count_rate + self.sat_time += self.sat_time_dt else: - self.sat_count -= self.sat_count_rate - self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit) - return self.sat_count > (self.sat_limit - 1e-3) + self.sat_time -= self.sat_time_dt + self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit) + return self.sat_time > (self.sat_limit - 1e-3) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 95458575f556ed..71ee0357123ca4 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,8 +4,9 @@ from cereal import log from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction +from opendbc.car.tests.test_lateral_limits import MAX_LAT_JERK_UP from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY -from openpilot.selfdrive.controls.lib.latcontrol import DT_CTRL, LatControl +from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.pid import PIDController @@ -23,21 +24,21 @@ LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [15, 13, 10, 5] -LATACCEL_REQUEST_BUFFER_SIZE = int(1/DT_CTRL) - - class LatControlTorque(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + super().__init__(CP, CI, dt) self.torque_params = CP.lateralTuning.torque.as_builder() self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_d=self.torque_params.kd, k_f=self.torque_params.kf) + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_d=self.torque_params.kd, k_f=self.torque_params.kf, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE) + lataccel_request_buffer_size = int(1 / self.dt) + self.requested_lateral_accel_buffer = deque([0.] * lataccel_request_buffer_size , maxlen=lataccel_request_buffer_size) self.error_pre = 0.0 self.gravity_adjusted_lateral_accel_pre = 0.0 + self.error_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt) + self.jerk_ff_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -59,7 +60,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - delay_frames = int(lat_delay / DT_CTRL) + delay_frames = int(lat_delay / self.dt) plan_future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.requested_lateral_accel_buffer.append(plan_future_desired_lateral_accel) current_expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] @@ -69,27 +70,27 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 # pid error calculated as difference between expected and measured lateral acceleration - #setpoint = plan_future_desired_lateral_accel + low_speed_factor * desired_curvature setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = plan_future_desired_lateral_accel - roll_compensation - error_expected = float(setpoint_expected - measurement) - #error = float(setpoint - measurement) - error_rate = (error_expected - self.error_pre) / DT_CTRL - jerk_ff = (gravity_adjusted_lateral_accel - self.gravity_adjusted_lateral_accel_pre) / DT_CTRL - self.error_pre = error_expected + error = float(setpoint_expected - measurement) + error_rate = (error - self.error_pre) / self.dt + error_rate_filtered = self.error_rate_filter.update(error_rate) + jerk_ff = (gravity_adjusted_lateral_accel - self.gravity_adjusted_lateral_accel_pre) / self.dt + jerk_ff_filtered = self.jerk_ff_filter.update(jerk_ff) + self.error_pre = error self.gravity_adjusted_lateral_accel_pre = gravity_adjusted_lateral_accel # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(error_expected) + pid_log.error = float(error) ff = gravity_adjusted_lateral_accel - ff += 0.5*jerk_ff + ff += 0.0 * jerk_ff_filtered # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset ff += get_friction(plan_future_desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, - error_rate, + error_rate_filtered, feedforward=ff, speed=CS.vEgo, freeze_integrator=freeze_integrator,) From 1ca982c411c7b8dda70bc0e963d62eb05944e06f Mon Sep 17 00:00:00 2001 From: felsager Date: Fri, 3 Oct 2025 16:57:28 -0700 Subject: [PATCH 39/43] only use measurement derivative for derivative compensation --- selfdrive/controls/lib/latcontrol_torque.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 71ee0357123ca4..c7c5af0992bcad 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -36,8 +36,10 @@ def __init__(self, CP, CI, dt): lataccel_request_buffer_size = int(1 / self.dt) self.requested_lateral_accel_buffer = deque([0.] * lataccel_request_buffer_size , maxlen=lataccel_request_buffer_size) self.error_pre = 0.0 + self.measurement_pre = 0.0 self.gravity_adjusted_lateral_accel_pre = 0.0 self.error_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt) + self.ff_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt) self.jerk_ff_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): @@ -74,11 +76,12 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = plan_future_desired_lateral_accel - roll_compensation error = float(setpoint_expected - measurement) - error_rate = (error - self.error_pre) / self.dt - error_rate_filtered = self.error_rate_filter.update(error_rate) + meas_rate = (measurement - self.measurement_pre) / self.dt + meas_rate_filtered = self.error_rate_filter.update(meas_rate) jerk_ff = (gravity_adjusted_lateral_accel - self.gravity_adjusted_lateral_accel_pre) / self.dt jerk_ff_filtered = self.jerk_ff_filter.update(jerk_ff) self.error_pre = error + self.measurement_pre = measurement self.gravity_adjusted_lateral_accel_pre = gravity_adjusted_lateral_accel # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(error) @@ -90,7 +93,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, - error_rate_filtered, + meas_rate_filtered, feedforward=ff, speed=CS.vEgo, freeze_integrator=freeze_integrator,) From 2b1e5abae33a0fe2b654423312c3ba41d6672446 Mon Sep 17 00:00:00 2001 From: felsager Date: Sun, 5 Oct 2025 16:01:02 -0700 Subject: [PATCH 40/43] negative meas rate instead of err rate --- panda | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/panda b/panda index 1289337ceb6205..615009cf0f8fb8 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 1289337ceb6205ad985a5469baa950b319329327 +Subproject commit 615009cf0f8fb8f3feadac160fbb0a07e4de171b diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c7c5af0992bcad..5f605e397d76b5 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -93,7 +93,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, - meas_rate_filtered, + -meas_rate_filtered, feedforward=ff, speed=CS.vEgo, freeze_integrator=freeze_integrator,) From 0375886f3afbe3dea99c4aa9a1046f278283f9f2 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 7 Oct 2025 16:58:56 -0700 Subject: [PATCH 41/43] friction based on expected jerk --- opendbc_repo | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 13 ++++++------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/opendbc_repo b/opendbc_repo index 5e9db0dba11f09..d192e1e501d92d 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 5e9db0dba11f09055fa284ba14f4844b4b5d7dc4 +Subproject commit d192e1e501d92d9d269e608507b847adbb27ba7b diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 5f605e397d76b5..18d3a4f9a6cf02 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -33,8 +33,8 @@ def __init__(self, CP, CI, dt): self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_d=self.torque_params.kd, k_f=self.torque_params.kf, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - lataccel_request_buffer_size = int(1 / self.dt) - self.requested_lateral_accel_buffer = deque([0.] * lataccel_request_buffer_size , maxlen=lataccel_request_buffer_size) + self.lataccel_request_buffer_size = int(1 / self.dt) + self.requested_lateral_accel_buffer = deque([0.] * self.lataccel_request_buffer_size , maxlen=self.lataccel_request_buffer_size) self.error_pre = 0.0 self.measurement_pre = 0.0 self.gravity_adjusted_lateral_accel_pre = 0.0 @@ -62,10 +62,10 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - delay_frames = int(lat_delay / self.dt) + delay_frames = min(max(int(lat_delay / self.dt), 1), self.lataccel_request_buffer_size - 1) plan_future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.requested_lateral_accel_buffer.append(plan_future_desired_lateral_accel) - current_expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] + current_expected_lateral_accel = self.requested_lateral_accel_buffer[-(delay_frames + 1)] current_expected_curvature = current_expected_lateral_accel / (CS.vEgo ** 2) actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 @@ -78,7 +78,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat error = float(setpoint_expected - measurement) meas_rate = (measurement - self.measurement_pre) / self.dt meas_rate_filtered = self.error_rate_filter.update(meas_rate) - jerk_ff = (gravity_adjusted_lateral_accel - self.gravity_adjusted_lateral_accel_pre) / self.dt + jerk_ff = (gravity_adjusted_lateral_accel - setpoint_expected) / (delay_frames * self.dt) jerk_ff_filtered = self.jerk_ff_filter.update(jerk_ff) self.error_pre = error self.measurement_pre = measurement @@ -86,10 +86,9 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(error) ff = gravity_adjusted_lateral_accel - ff += 0.0 * jerk_ff_filtered # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(plan_future_desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += get_friction(jerk_ff_filtered, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, From d97fd3b03c9c299f8577ea73840541adb7e30aa2 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 7 Oct 2025 17:08:09 -0700 Subject: [PATCH 42/43] friction based on expected jerk use tanh --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 18d3a4f9a6cf02..a3a35f01e2b941 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -88,7 +88,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat ff = gravity_adjusted_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(jerk_ff_filtered, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += self.torque_params.friction * np.tanh(3.0 * jerk_ff_filtered) # get_friction(jerk_ff_filtered, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, From 0729235551575aa7395538487045a9e6cd392292 Mon Sep 17 00:00:00 2001 From: felsager Date: Wed, 8 Oct 2025 09:16:26 -0700 Subject: [PATCH 43/43] ff friction --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index a3a35f01e2b941..013e990702c870 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -88,7 +88,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat ff = gravity_adjusted_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += self.torque_params.friction * np.tanh(3.0 * jerk_ff_filtered) # get_friction(jerk_ff_filtered, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += self.torque_params.friction * np.tanh(2.5 * jerk_ff_filtered + 0.5*error) # get_friction(jerk_ff_filtered, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error,