From 9e68801db094ed81300274d3bd2779a9d86b0a74 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 19 Jul 2026 14:16:16 -0700 Subject: [PATCH] Deadband lateral jerk before friction compensation Planner replan jitter leaks into desired_lateral_jerk on straights and clears the friction deadzone, injecting torque chatter with no steering need behind it. Off by default via FrictionJerkDeadzoneEnabled. --- common/params_keys.h | 1 + selfdrive/controls/lib/latcontrol_torque.py | 3 +- .../lib/latcontrol_torque_ext_override.py | 18 ++++++++ .../test_latcontrol_torque_ext_override.py | 42 +++++++++++++++++++ sunnypilot/sunnylink/params_metadata.json | 24 ++++++----- sunnypilot/sunnylink/settings_ui.json | 31 ++++++++++++++ .../settings_ui_src/pages/steering.yaml | 19 +++++++++ 7 files changed, 127 insertions(+), 11 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_ext_override.py diff --git a/common/params_keys.h b/common/params_keys.h index c8cd1cf3ee..618d00b22b 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -283,6 +283,7 @@ inline static std::unordered_map keys = { // Torque lateral control custom params {"CustomTorqueParams", {PERSISTENT | BACKUP , BOOL}}, {"EnforceTorqueControl", {PERSISTENT | BACKUP, BOOL}}, + {"FrictionJerkDeadzoneEnabled", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LiveTorqueParamsToggle", {PERSISTENT | BACKUP , BOOL}}, {"LiveTorqueParamsRelaxedToggle", {PERSISTENT | BACKUP , BOOL}}, {"TorqueControlTune", {PERSISTENT | BACKUP, FLOAT, "0.0"}}, diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d0eb40d096..bc0f0a55e9 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -84,11 +84,12 @@ class LatControlTorque(LatControl): lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2)) raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt) desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk) + friction_jerk = self.extension.get_friction_jerk(desired_lateral_jerk) gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation ff = gravity_adjusted_future_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += get_friction(error + JERK_GAIN * friction_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) if not active: output_torque = 0.0 diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py index f07a7292c1..d8b741faf6 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py @@ -5,8 +5,19 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ +import math + from openpilot.common.params import Params +# Planner-replan jitter shows up as small nonzero desired_lateral_jerk even on a straight +# road with a near-zero lateral accel error. Fed into get_friction() that noise still +# clears the (much smaller, sensor-deadzone-sized) lateral_accel_deadzone and comes out +# as a real friction-compensation torque command, i.e. chatter with no steering need +# behind it. Deadbanding the jerk itself (not the combined error) kills that noise floor +# while leaving real jerk (an actual curve entry or lane change, an order of magnitude +# larger) essentially unaffected. +FRICTION_JERK_DEADZONE = 0.3 # m/s^3 + class LatControlTorqueExtOverride: def __init__(self, CP): @@ -14,8 +25,15 @@ class LatControlTorqueExtOverride: self.params = Params() self.enforce_torque_control_toggle = self.params.get_bool("EnforceTorqueControl") # only during init self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled") + self.friction_jerk_deadzone_enabled = self.params.get_bool("FrictionJerkDeadzoneEnabled") self.frame = -1 + def get_friction_jerk(self, desired_lateral_jerk: float) -> float: + if not self.friction_jerk_deadzone_enabled: + return desired_lateral_jerk + + return math.copysign(max(abs(desired_lateral_jerk) - FRICTION_JERK_DEADZONE, 0.0), desired_lateral_jerk) + def update_override_torque_params(self, torque_params) -> bool: if not self.enforce_torque_control_toggle: return False diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_ext_override.py b/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_ext_override.py new file mode 100644 index 0000000000..5aeb87f1ee --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_ext_override.py @@ -0,0 +1,42 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_override import ( + FRICTION_JERK_DEADZONE, LatControlTorqueExtOverride, +) + + +class TestLatControlTorqueExtOverrideFrictionJerkDeadzone: + + def setup_method(self): + self.override = LatControlTorqueExtOverride(None) + + def test_disabled_by_default(self): + assert self.override.friction_jerk_deadzone_enabled is False + + def test_disabled_is_passthrough(self): + self.override.friction_jerk_deadzone_enabled = False + + for jerk in (0.0, 0.05, -0.05, FRICTION_JERK_DEADZONE, -FRICTION_JERK_DEADZONE, 2.0, -2.0): + assert self.override.get_friction_jerk(jerk) == jerk + + def test_enabled_kills_jerk_below_deadzone(self): + self.override.friction_jerk_deadzone_enabled = True + + for jerk in (0.0, 0.05, -0.05, 0.29, -0.29): + assert self.override.get_friction_jerk(jerk) == 0.0 + + def test_enabled_boundary_at_deadzone(self): + self.override.friction_jerk_deadzone_enabled = True + + assert self.override.get_friction_jerk(FRICTION_JERK_DEADZONE) == 0.0 + assert self.override.get_friction_jerk(-FRICTION_JERK_DEADZONE) == 0.0 + + def test_enabled_shifts_jerk_above_deadzone_preserving_sign(self): + self.override.friction_jerk_deadzone_enabled = True + + assert self.override.get_friction_jerk(2.0) == 2.0 - FRICTION_JERK_DEADZONE + assert self.override.get_friction_jerk(-2.0) == -(2.0 - FRICTION_JERK_DEADZONE) diff --git a/sunnypilot/sunnylink/params_metadata.json b/sunnypilot/sunnylink/params_metadata.json index fed5217300..2188368732 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/sunnypilot/sunnylink/params_metadata.json @@ -332,6 +332,10 @@ "title": "Force Power Down", "description": "" }, + "FrictionJerkDeadzoneEnabled": { + "title": "Reduce Friction Torque Chatter", + "description": "Ignores small, short-lived steering jerk from planner noise when computing friction compensation, reducing steering chatter on straight roads. Does not change torque during real turns or lane changes." + }, "GitBranch": { "title": "Git Branch", "description": "" @@ -1350,19 +1354,11 @@ "step": 0.1, "unit": "m/s\u00b2" }, - "ToyotaAutoHold": { + "ToyotaAutoHold": { "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS", "description": "" }, - "ToyotaEnhancedBsm": { - "title": "Toyota: Prius TSS2 BSM and some tssp", - "description": "" - }, - "ToyotaTSS2Long": { - "title": "Toyota: custom longitudinal for TSS2", - "description": "" - }, - "ToyotaDriveMode": { + "ToyotaDriveMode": { "title": "Enable drive mode btn link", "description": "" }, @@ -1370,10 +1366,18 @@ "title": "Toyota: Enforce Factory Longitudinal Control", "description": "When enabled, sunnypilot will not take over control of gas and brakes. Factory Toyota longitudinal control will be used." }, + "ToyotaEnhancedBsm": { + "title": "Toyota: Prius TSS2 BSM and some tssp", + "description": "" + }, "ToyotaStopAndGoHack": { "title": "Toyota: Stop and Go Hack (Alpha)", "description": "sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk." }, + "ToyotaTSS2Long": { + "title": "Toyota: custom longitudinal for TSS2", + "description": "" + }, "TrainingVersion": { "title": "Training Version", "description": "" diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 4205697617..ba8a35bc34 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -310,6 +310,37 @@ "equals": false } ] + }, + { + "key": "FrictionJerkDeadzoneEnabled", + "widget": "toggle", + "title": "Reduce Friction Torque Chatter", + "description": "Ignores small, short-lived steering jerk from planner noise when computing friction compensation, reducing steering chatter on straight roads. Does not change torque during real turns or lane changes.", + "visibility": [ + { + "type": "not", + "condition": { + "type": "capability", + "field": "steer_control_type", + "equals": "angle" + } + } + ], + "enablement": [ + { + "type": "offroad_only" + }, + { + "type": "capability", + "field": "torque_allowed", + "equals": true + }, + { + "type": "param", + "key": "NeuralNetworkLateralControl", + "equals": false + } + ] } ], "sub_panels": [ diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml index 697c5f4f21..130be06b2e 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml @@ -118,6 +118,25 @@ sections: - type: param key: NeuralNetworkLateralControl equals: false + - key: FrictionJerkDeadzoneEnabled + widget: toggle + title: Reduce Friction Torque Chatter + description: Ignores small, short-lived steering jerk from planner noise when computing friction compensation, reducing + steering chatter on straight roads. Does not change torque during real turns or lane changes. + visibility: + - type: not + condition: + type: capability + field: steer_control_type + equals: angle + enablement: + - $ref: '#/macros/offroad' + - type: capability + field: torque_allowed + equals: true + - type: param + key: NeuralNetworkLateralControl + equals: false sub_panels: - id: torque_settings label: Torque Settings