diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 352dbb1bc..e9e150859 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -292,7 +292,15 @@ struct StarPilotLateralManeuverPlanDEPRECATED @0xcb9fd56c7057593a { desiredCurvature @0 :Float32; # 1/m } -struct CustomReserved11 @0xc2243c65e0340384 { +struct StarPilotLateralState @0xc2243c65e0340384 { + active @0 :Bool; + frictionThreshold @1 :Float32; + frictionScale @2 :Float32; + feedforward @3 :Float32; + frictionJerk @4 :Float32; + frictionJerkDeadzone @5 :Float32; + lowSpeedFactor @6 :Float32; + unwindDetected @7 :Bool; } struct CustomReserved12 @0x9ccdc8676701b412 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 7f3876b33..b34322caa 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2742,7 +2742,7 @@ struct Event { starpilotSelfdriveState @115 :Custom.StarPilotSelfdriveState; customReserved9 @116 :Custom.CustomReserved9; starpilotLateralManeuverPlanDEPRECATED @136 :Custom.StarPilotLateralManeuverPlanDEPRECATED; - customReserved11 @137 :Custom.CustomReserved11; + starpilotLateralState @137 :Custom.StarPilotLateralState; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; customReserved14 @140 :Custom.CustomReserved14; diff --git a/cereal/services.py b/cereal/services.py index 2e326c695..797b6cf27 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -101,6 +101,7 @@ _services: dict[str, tuple] = { "livestreamRoadEncodeData": (False, 20., None, QueueSize.MEDIUM), "livestreamDriverEncodeData": (False, 20., None, QueueSize.MEDIUM), "customReserved9": (True, 0., 1), + "starpilotLateralState": (True, 100., 10), "customReservedRawData0": (True, 0.), "customReservedRawData1": (True, 0.), "customReservedRawData2": (True, 0.), diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index dae8dd508..ec5b7a0e6 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -307,7 +307,7 @@ class Controls: self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'radarState'], poll='selfdriveState') - self.pm = messaging.PubMaster(['carControl', 'controlsState']) + self.pm = messaging.PubMaster(['carControl', 'controlsState', 'starpilotLateralState']) self.steer_limited_by_safety = False self.curvature = 0.0 @@ -772,6 +772,11 @@ class Controls: self.pm.send('controlsState', dat) + if hasattr(self.LaC, 'starpilot_lateral_state'): + debug_dat = messaging.new_message('starpilotLateralState') + debug_dat.starpilotLateralState = self.LaC.starpilot_lateral_state + self.pm.send('starpilotLateralState', debug_dat) + # carControl cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f5a2fc4df..7bc6b972b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -2,7 +2,7 @@ import math import numpy as np from collections import deque -from cereal import log +from cereal import custom, log from opendbc.car.honda.values import CAR as HONDA_CAR, HondaFlags from opendbc.car.hyundai.values import HyundaiFlags from opendbc.car.lateral import get_friction @@ -46,6 +46,23 @@ UNWIND_D_DES_THRESHOLD = -1.0 UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 MIN_LATERAL_CONTROL_SPEED = 0.3 +# Small planner jerk changes around the lane center can repeatedly re-trigger the +# friction compensation term. Keep this correction out of the center band while +# leaving actual turn-in and unwind commands unchanged. +CENTER_CHATTER_JERK_DEADZONE_SPEED_BP = [0.0, 5.0, 12.0, 25.0] # m/s +CENTER_CHATTER_JERK_DEADZONE_SPEED_V = [0.08, 0.12, 0.18, 0.18] # m/s^3 +CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_BP = [0.0, 0.18, 0.35] # m/s^2 +CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_V = [1.0, 1.0, 0.0] + + +def get_center_chatter_friction_jerk_deadzone(v_ego, setpoint, vehicle_deadzone=0.0): + """Return the small-signal jerk deadzone without changing turn commands.""" + speed_deadzone = np.interp(max(v_ego, 0.0), CENTER_CHATTER_JERK_DEADZONE_SPEED_BP, + CENTER_CHATTER_JERK_DEADZONE_SPEED_V) + center_weight = np.interp(abs(setpoint), CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_BP, + CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_V) + return max(float(vehicle_deadzone), float(speed_deadzone * center_weight)) + # Roll compensation and latAccelOffset are lateral-accel-domain corrections; below # walking pace the desired lateral accel is ~0 so an unfaded road-crown term dominates # the whole feedforward and actively unwinds a held wheel at pull-away (newturn rlog @@ -78,6 +95,17 @@ class LatControlTorque(LatControl): self.prev_steering_pressed = False self.debug_counter = 0 self.prev_desired_lateral_accel = 0.0 + self.starpilot_lateral_state = custom.StarPilotLateralState.new_message() + + def _clear_starpilot_lateral_state(self): + self.starpilot_lateral_state.active = False + self.starpilot_lateral_state.frictionThreshold = 0.0 + self.starpilot_lateral_state.frictionScale = 0.0 + self.starpilot_lateral_state.feedforward = 0.0 + self.starpilot_lateral_state.frictionJerk = 0.0 + self.starpilot_lateral_state.frictionJerkDeadzone = 0.0 + self.starpilot_lateral_state.lowSpeedFactor = 0.0 + self.starpilot_lateral_state.unwindDetected = False self.is_bolt = CP.carFingerprint in BOLT_CARS self.is_bolt_2022_2023 = CP.carFingerprint in BOLT_2022_2023_CARS @@ -194,6 +222,7 @@ class LatControlTorque(LatControl): if not active: output_torque = 0.0 pid_log.active = False + self._clear_starpilot_lateral_state() self.pid.reset() # Keep the request buffer and rate state primed with the live command (which tracks # the measured curvature while inactive) instead of zeroing them. Re-engaging with a @@ -408,10 +437,12 @@ class LatControlTorque(LatControl): if trailer_load_kg > 0.0: ff *= get_trailer_lateral_ff_scale(trailer_load_kg, CS.vEgo, setpoint) friction_scale *= get_trailer_lateral_friction_scale(trailer_load_kg, CS.vEgo, setpoint) - friction_jerk = desired_lateral_jerk - if ioniq_6_active: - # planner jerk noise on straights (< ~0.3 m/s^3) chatters the friction compensation - friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - IONIQ_6_FRICTION_JERK_DEADZONE, 0.0), desired_lateral_jerk) + vehicle_friction_jerk_deadzone = IONIQ_6_FRICTION_JERK_DEADZONE if ioniq_6_active else 0.0 + friction_jerk_deadzone = get_center_chatter_friction_jerk_deadzone( + CS.vEgo, setpoint, vehicle_friction_jerk_deadzone + ) + friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), + desired_lateral_jerk) ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) deadzone_boost_active = False if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL: @@ -485,6 +516,14 @@ class LatControlTorque(LatControl): pid_log.actualLateralAccel = float(measurement) pid_log.desiredLateralAccel = float(setpoint) pid_log.desiredLateralJerk = float(desired_lateral_jerk) + self.starpilot_lateral_state.active = True + self.starpilot_lateral_state.frictionThreshold = float(friction_threshold) + self.starpilot_lateral_state.frictionScale = float(friction_scale) + self.starpilot_lateral_state.feedforward = float(ff) + self.starpilot_lateral_state.frictionJerk = float(friction_jerk) + self.starpilot_lateral_state.frictionJerkDeadzone = float(friction_jerk_deadzone) + self.starpilot_lateral_state.lowSpeedFactor = float(low_speed_factor) + self.starpilot_lateral_state.unwindDetected = bool(unwind_detected) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) if DEBUG_TORQUE_TUNE and self.is_bolt: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index c67361b82..104c87e74 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -38,6 +38,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( ) from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_civic_bosch_modified_a_center_taper_scale, + get_center_chatter_friction_jerk_deadzone, LatControlTorque, get_civic_bosch_modified_b_ff_scale, get_civic_bosch_modified_b_friction_scale, @@ -123,6 +124,30 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( class TestLatControl: + def test_center_chatter_friction_jerk_deadzone_is_center_and_speed_gated(self): + low_speed_center = get_center_chatter_friction_jerk_deadzone(2.0, 0.0) + highway_center = get_center_chatter_friction_jerk_deadzone(25.0, 0.0) + highway_curve = get_center_chatter_friction_jerk_deadzone(25.0, 0.6) + + assert low_speed_center == pytest.approx(0.08) + assert highway_center == pytest.approx(0.18) + assert highway_curve == pytest.approx(0.0) + + def test_center_chatter_friction_jerk_deadzone_preserves_vehicle_override(self): + assert get_center_chatter_friction_jerk_deadzone(25.0, 0.6, 0.30) == pytest.approx(0.30) + + def test_torque_log_exposes_friction_controller_state(self): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023) + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles) + + debug_state = controller.starpilot_lateral_state + assert debug_state.active + assert debug_state.frictionThreshold > 0.0 + assert debug_state.frictionScale > 0.0 + assert debug_state.frictionJerkDeadzone > 0.0 + assert debug_state.lowSpeedFactor > 0.0 + @staticmethod def _build_torque_controller(car_name, force_torque=False): CarInterface = interfaces[car_name]