mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-10-05 05:44:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

gv70

Browse Source
This commit is contained in:
firestar5683
2026-10-03 16:44:36 -05:00
parent f973d151c6
commit 385428f6ea
3 changed files with 92 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+17 -16
View File
@@ -95,6 +95,7 @@ class LatControlTorque(LatControl):
self.steer_release_i_decay = 0.8
self.prev_steering_pressed = False
self.prev_output_torque = 0.0
self.gv70_previous_feedforward = None
self.debug_counter = 0
self.prev_desired_lateral_accel = 0.0
self.starpilot_lateral_state = custom.StarPilotLateralState.new_message()
@@ -107,6 +108,8 @@ class LatControlTorque(LatControl):
self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS
self.is_genesis_g70 = CP.carFingerprint in GENESIS_G70_CARS
self.is_genesis_gv70 = CP.carFingerprint in GENESIS_GV70_CARS
if self.is_genesis_gv70:
self.pid._k_d = [GENESIS_GV70_MEASUREMENT_DAMPING_SPEED_BP, GENESIS_GV70_MEASUREMENT_DAMPING_V]
self.is_palisade = CP.carFingerprint in PALISADE_CARS
self.is_prius = CP.carFingerprint in PRIUS_CARS
self.is_standard_prius = CP.carFingerprint == TOYOTA_CAR.TOYOTA_PRIUS
@@ -236,6 +239,7 @@ class LatControlTorque(LatControl):
if not active:
output_torque = 0.0
self.prev_output_torque = 0.0
self.gv70_previous_feedforward = None
pid_log.active = False
self._clear_starpilot_lateral_state()
self.pid.reset()
@@ -547,9 +551,21 @@ class LatControlTorque(LatControl):
if CS.vEgo < self.low_speed_reset_threshold:
self.pid.reset()
if self.is_genesis_gv70:
ff *= get_genesis_gv70_center_output_scale(setpoint, CS.vEgo)
ff *= get_genesis_gv70_low_speed_center_overshoot_scale(setpoint, measurement, CS.vEgo)
ff *= get_genesis_gv70_high_speed_error_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
ff *= get_genesis_gv70_reversal_output_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
if not CS.steeringPressed and not self.prev_steering_pressed and self.gv70_previous_feedforward is not None:
ff = get_genesis_gv70_stabilized_output(
ff, self.gv70_previous_feedforward, setpoint, desired_lateral_jerk, CS.vEgo, self.dt,
)
self.gv70_previous_feedforward = ff
freeze_integrator = (steer_limited_by_safety or CS.steeringPressed or
CS.vEgo < self.low_speed_reset_threshold or unwind_detected)
output_lataccel = self.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
error_rate = 0.0 if self.is_genesis_gv70 and CS.steeringPressed else -measurement_rate
output_lataccel = self.pid.update(pid_log.error, error_rate=error_rate, speed=CS.vEgo, feedforward=ff,
freeze_integrator=freeze_integrator)
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
if bolt_2022_2023_tuned_path_active:
output_torque *= get_bolt_2022_2023_center_output_scale(setpoint, CS.vEgo)
@@ -660,21 +676,6 @@ class LatControlTorque(LatControl):
output_torque = get_genesis_g70_stabilized_output(
output_torque, self.prev_output_torque, setpoint, measurement, desired_lateral_jerk, CS.vEgo, self.dt,
)
elif self.is_genesis_gv70:
output_torque *= get_genesis_gv70_center_output_scale(setpoint, CS.vEgo)
output_torque *= get_genesis_gv70_low_speed_center_overshoot_scale(
setpoint, measurement, CS.vEgo,
)
output_torque *= get_genesis_gv70_high_speed_error_scale(
setpoint, measurement, desired_lateral_jerk, CS.vEgo,
)
output_torque *= get_genesis_gv70_reversal_output_scale(
setpoint, measurement, desired_lateral_jerk, CS.vEgo,
)
if not CS.steeringPressed:
output_torque = get_genesis_gv70_stabilized_output(
output_torque, self.prev_output_torque, setpoint, desired_lateral_jerk, CS.vEgo, self.dt,
)
elif sonata_hybrid_active:
output_torque *= sonata_hybrid_center_taper
output_torque *= sonata_hybrid_center_output_taper
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+2
View File
@@ -275,6 +275,8 @@ GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08
GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55
GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.065
GENESIS_GV70_MEASUREMENT_DAMPING_SPEED_BP = [10.0 * CV.MPH_TO_MS, 35.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS]
GENESIS_GV70_MEASUREMENT_DAMPING_V = [0.0, 0.08, 0.12]
GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP = [40.0 * CV.MPH_TO_MS, 50.0 * CV.MPH_TO_MS]
GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [0.75, 1.0]
GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC = 0.85
selfdrive/controls/tests/test_latcontrol.py
+73 -2
View File
@@ -1,4 +1,6 @@
import pytest
import math
from collections import deque
from parameterized import parameterized
from types import SimpleNamespace
@@ -1806,7 +1808,7 @@ class TestLatControl:
assert high_speed_unwind > high_speed_wind > 0.1
assert abs(high_speed_direction_change - 0.3) > abs(high_speed_center - 0.2)
def test_genesis_gv70_output_stabilizer_update_path(self, monkeypatch):
def test_genesis_gv70_stabilizer_filters_feedforward_not_feedback(self, monkeypatch):
calls = []
def stabilized_output(output_torque, prev_output_torque, desired_lateral_accel,
@@ -1818,13 +1820,27 @@ class TestLatControl:
monkeypatch.setattr(latcontrol_torque, "get_genesis_gv70_stabilized_output", stabilized_output)
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
CS.vEgo = 25.0
controller.update(
True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles,
)
output, _, lac_log = controller.update(
True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles,
)
assert calls
assert lac_log.active
assert output == pytest.approx(-0.123)
assert lac_log.f == pytest.approx(0.123)
expected_accel = lac_log.p + lac_log.i + lac_log.d + lac_log.f
expected_torque = controller.torque_from_lateral_accel(expected_accel, controller.torque_params)
assert output == pytest.approx(-expected_torque)
previous_p = lac_log.p
CS.steeringAngleDeg += 0.5
_, _, next_log = controller.update(
True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles,
)
assert next_log.f == pytest.approx(0.123)
assert next_log.p != pytest.approx(previous_p)
call_count = len(calls)
CS.steeringPressed = True
@@ -1832,6 +1848,61 @@ class TestLatControl:
True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles,
)
assert len(calls) == call_count
assert controller.pid.d == 0.0
CS.steeringPressed = False
controller.update(
True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles,
)
assert len(calls) == call_count
controller.update(False, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles)
assert controller.gv70_previous_feedforward is None
controller.update(
True, CS, VM, params, False, -0.0002, False, 0.2, None, None, starpilot_toggles,
)
assert len(calls) == call_count
@pytest.mark.parametrize("car_name", [HYUNDAI.GENESIS_G70_2020, HYUNDAI.GENESIS_GV70_1ST_GEN, HYUNDAI.KIA_EV6,
HYUNDAI.HYUNDAI_IONIQ_6])
def test_genesis_gv70_measurement_damping_does_not_change_other_cars(self, car_name):
controller, _, _, _, _ = self._build_torque_controller(car_name)
assert not controller.is_genesis_gv70
assert max(controller.pid._k_d[1]) == 0.0
def test_genesis_gv70_measurement_damping_is_bounded_and_has_no_steady_bias(self):
controller, VM, CS, params, toggles = self._build_torque_controller(HYUNDAI.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
CS.vEgo = 30.0
CS.steeringAngleDeg = 0.0
controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
CS.steeringAngleDeg = -5.0
output, _, moving_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
assert -0.300001 <= moving_log.d < 0.0
assert abs(output) <= controller.steer_max
for _ in range(150):
_, _, steady_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
assert steady_log.d == pytest.approx(0.0, abs=1e-6)
CS.vEgo = 3.0
CS.steeringAngleDeg = 5.0
_, _, low_speed_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
assert low_speed_log.d == 0.0
@pytest.mark.parametrize("direction", [-1.0, 1.0])
def test_genesis_gv70_overshoot_correction_bypasses_old_turn_output(self, monkeypatch, direction):
controller, VM, CS, params, toggles = self._build_torque_controller(HYUNDAI.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
CS.vEgo = 22.0
CS.steeringAngleDeg = math.degrees(VM.get_steer_from_curvature(-direction * 2.4 / CS.vEgo ** 2, CS.vEgo, 0.0))
controller.curvature_request_buffer = deque([direction * 0.8 / CS.vEgo ** 2] * controller.request_buffer_len,
maxlen=controller.request_buffer_len)
controller.prev_output_torque = direction * 0.8
controller.gv70_previous_feedforward = direction * 0.8
monkeypatch.setattr(latcontrol_torque, "get_genesis_gv70_stabilized_output", lambda *_args: direction * 0.2)
output, _, lac_log = controller.update(
True, CS, VM, params, False, direction * 0.8 / CS.vEgo ** 2, False, 0.2, None, None, toggles,
)
assert lac_log.p * direction < 0.0
assert output * direction > 0.0
assert abs(output) <= controller.steer_max
def test_genesis_g70_low_speed_output_guard_update_path(self):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020)