mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-07 17:35:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

Better Lat 4 U

Browse Source
This commit is contained in:
firestar5683
2026-08-06 20:08:43 -05:00
parent a149d2624a
commit dd870ef29c
6 changed files with 86 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+9 -1
View File
@@ -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 {
cereal/log.capnp
+1 -1
View File
@@ -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;
cereal/services.py
+1
View File
@@ -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.),
selfdrive/controls/controlsd.py
+6 -1
View File
@@ -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
selfdrive/controls/lib/latcontrol_torque.py
+44 -5
View File
@@ -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:
selfdrive/controls/tests/test_latcontrol.py
+25
View File
@@ -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]