This commit is contained in:
firestar5683
2026-07-21 11:34:56 -05:00
parent 665f538533
commit 85dd1cbe7e
12 changed files with 244 additions and 8 deletions
@@ -27,6 +27,7 @@ ACCEL_PID_UNWIND = 0.03 * DT_CTRL * 3 # m/s^2 / frame
PRIUS_INTEGRAL_MISMATCH_UNWIND = 8.0
PRIUS_POSITIVE_FEEDFORWARD_SCALE = 0.7
PRIUS_CRUISE_FEEDFORWARD_SCALE = 1.0
PRIUS_NEGATIVE_FEEDFORWARD_SCALE = 1.125
CAMRY_HYBRID_POSITIVE_FEEDFORWARD_SCALE = 0.8
MAX_PITCH_COMPENSATION = 1.5 # m/s^2
@@ -82,6 +83,12 @@ def get_prius_positive_feedforward_scale(v_ego: float) -> float:
[PRIUS_POSITIVE_FEEDFORWARD_SCALE, PRIUS_POSITIVE_FEEDFORWARD_SCALE, PRIUS_CRUISE_FEEDFORWARD_SCALE]))
def get_prius_feedforward(accel: float, v_ego: float) -> float:
if accel > 0.0:
return accel * get_prius_positive_feedforward_scale(v_ego)
return accel * PRIUS_NEGATIVE_FEEDFORWARD_SCALE
def get_camry_hybrid_feedforward(accel: float) -> float:
return accel * CAMRY_HYBRID_POSITIVE_FEEDFORWARD_SCALE if accel > 0.0 else accel
@@ -468,8 +475,7 @@ class CarController(CarControllerBase):
feedforward = pcm_accel_cmd
if self.CP.carFingerprint == CAR.TOYOTA_PRIUS:
if feedforward > 0.0:
feedforward *= get_prius_positive_feedforward_scale(CS.out.vEgo)
feedforward = get_prius_feedforward(feedforward, CS.out.vEgo)
elif is_camry_hybrid(self.CP) and feedforward > 0.0:
# Preserve the established Camry Hybrid acceleration response while
# allowing negative requests to track the planner at full scale.
@@ -8,7 +8,8 @@ from opendbc.can import CANPacker, CANParser
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict
from opendbc.car.toyota import toyotacan
from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_positive_feedforward_scale, \
from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
get_prius_positive_feedforward_scale, \
limit_interceptor_pcm_accel, \
limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \
limit_prius_stopping_accel, update_permit_braking
@@ -524,6 +525,10 @@ class TestToyotaCarController:
assert get_prius_positive_feedforward_scale(20.0) > get_prius_positive_feedforward_scale(8.0)
assert abs(get_prius_positive_feedforward_scale(20.0) - 1.0) < 1e-6
def test_prius_feedforward_adds_braking_authority_without_changing_acceleration(self):
assert get_prius_feedforward(-2.0, 8.0) == pytest.approx(-2.25)
assert get_prius_feedforward(1.0, 8.0) == pytest.approx(0.7)
def test_camry_hybrid_feedforward_only_softens_acceleration(self):
assert get_camry_hybrid_feedforward(1.0) == pytest.approx(0.8)
assert get_camry_hybrid_feedforward(-2.0) == pytest.approx(-2.0)
@@ -87,6 +87,7 @@ class LatControlTorque(LatControl):
self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS
self.is_palisade = CP.carFingerprint in PALISADE_CARS
self.is_prius = CP.carFingerprint in PRIUS_CARS
self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS
self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS
self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS
@@ -241,6 +242,7 @@ class LatControlTorque(LatControl):
genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active()
palisade_active = self.is_palisade
prius_active = self.is_prius
rav4_prime_active = self.is_rav4_prime
ioniq_5_active = self.is_ioniq_5
ioniq_ev_old_active = self.is_ioniq_ev_old
ioniq_6_active = self.is_ioniq_6
@@ -305,6 +307,10 @@ class LatControlTorque(LatControl):
friction_threshold = get_prius_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = get_prius_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = 1.0 + ((friction_scale - 1.0) * prius_center_taper)
elif rav4_prime_active:
ff *= get_rav4_prime_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
friction_threshold = get_rav4_prime_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = get_rav4_prime_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
elif ioniq_5_active:
ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper
friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
@@ -132,6 +132,10 @@ PRIUS_CARS = (
TOYOTA_CAR.TOYOTA_PRIUS,
)
RAV4_PRIME_CARS = (
TOYOTA_CAR.TOYOTA_RAV4_PRIME,
)
BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3
BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045
BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS
@@ -712,6 +716,19 @@ PRIUS_CENTER_TAPER_LAT_WIDTH = 0.035
PRIUS_CENTER_TAPER_SPEED = 18.0
PRIUS_CENTER_TAPER_SPEED_WIDTH = 2.2
RAV4_PRIME_PHASE_SCALE = 0.12
RAV4_PRIME_UNWIND_FF_REDUCTION_LEFT = 0.15
RAV4_PRIME_UNWIND_FF_REDUCTION_RIGHT = 0.13
RAV4_PRIME_UNWIND_FRICTION_REDUCTION_LEFT = 0.16
RAV4_PRIME_UNWIND_FRICTION_REDUCTION_RIGHT = 0.14
RAV4_PRIME_FRICTION_THRESHOLD_GAIN = 0.24
RAV4_PRIME_FRICTION_CENTER_LAT = 0.30
RAV4_PRIME_FRICTION_CENTER_LAT_WIDTH = 0.07
RAV4_PRIME_SPEED_ONSET = 5.0
RAV4_PRIME_SPEED_ONSET_WIDTH = 1.5
RAV4_PRIME_SPEED_MAX = 20.0
RAV4_PRIME_SPEED_MAX_WIDTH = 2.5
TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG
TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS
TRAILER_LATERAL_FULL_SPEED = 35.0 * CV.MPH_TO_MS
@@ -961,6 +978,47 @@ def get_prius_center_taper_scale(desired_lateral_accel: float, v_ego: float) ->
return 1.0 - reduction
def _rav4_prime_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _rav4_prime_speed_weight(v_ego: float) -> float:
onset = _sigmoid((v_ego - RAV4_PRIME_SPEED_ONSET) / RAV4_PRIME_SPEED_ONSET_WIDTH)
cutoff = _sigmoid((RAV4_PRIME_SPEED_MAX - v_ego) / RAV4_PRIME_SPEED_MAX_WIDTH)
return onset * cutoff
def _rav4_prime_unwind_weight(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / RAV4_PRIME_PHASE_SCALE)
lat_weight = _sigmoid((abs(desired_lateral_accel) - 0.20) / 0.07)
return max(-phase, 0.0) * lat_weight
def get_rav4_prime_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
reduction = _rav4_prime_side_value(desired_lateral_accel,
RAV4_PRIME_UNWIND_FF_REDUCTION_LEFT,
RAV4_PRIME_UNWIND_FF_REDUCTION_RIGHT)
return 1.0 - (reduction * _rav4_prime_unwind_weight(desired_lateral_accel, desired_lateral_jerk) *
_rav4_prime_speed_weight(v_ego))
def get_rav4_prime_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
center_weight = _sigmoid((RAV4_PRIME_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /
RAV4_PRIME_FRICTION_CENTER_LAT_WIDTH)
scale = 1.0 + (RAV4_PRIME_FRICTION_THRESHOLD_GAIN * center_weight * _rav4_prime_speed_weight(v_ego))
return get_standard_friction_threshold(v_ego) * scale
def get_rav4_prime_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
reduction = _rav4_prime_side_value(desired_lateral_accel,
RAV4_PRIME_UNWIND_FRICTION_REDUCTION_LEFT,
RAV4_PRIME_UNWIND_FRICTION_REDUCTION_RIGHT)
return 1.0 - (reduction * _rav4_prime_unwind_weight(desired_lateral_accel, desired_lateral_jerk) *
_rav4_prime_speed_weight(v_ego))
def civic_bosch_modified_lateral_testing_ground_active() -> bool:
return testing_ground.use("8", "B")
@@ -2790,7 +2848,7 @@ def get_flm_capabilities(car_fingerprint, brand: str = "", hyundai_canfd: bool =
dedicated_friction = car_fingerprint in (
set(BOLT_2022_2023_CARS) | set(BOLT_2018_2021_CARS) | set(VOLT_STANDARD_CARS) | set(PALISADE_CARS) |
set(PRIUS_CARS) | set(IONIQ_5_CARS) | set(IONIQ_6_CARS) | set(KIA_EV6_CARS) | set(KIA_FORTE_CARS) |
set(PRIUS_CARS) | set(RAV4_PRIME_CARS) | set(IONIQ_5_CARS) | set(IONIQ_6_CARS) | set(KIA_EV6_CARS) | set(KIA_FORTE_CARS) |
set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_CARNIVAL_CARS) | set(GENESIS_G90_CARS)
)
dedicated_center_taper = car_fingerprint in (
+1
View File
@@ -266,6 +266,7 @@ class LongControl:
self.reset()
else: # LongCtrlState.pid
a_target = self.vehicle_tuning.shape_gm_truck_accel_target(a_target, CS.vEgo, should_stop)
error = a_target - CS.aEgo
self.update_mpc_mode(self.experimental_mode)
self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo)
@@ -1,6 +1,7 @@
import numpy as np
from opendbc.car.gm.values import CAR, GMFlags
from openpilot.common.realtime import DT_CTRL
from openpilot.starpilot.common.testing_grounds import testing_ground
@@ -11,6 +12,11 @@ BOLT_ACC_PEDAL_REGEN_LIMIT_BP = [0.0, 1.5, 4.0, 8.0, 15.0, 30.0]
BOLT_ACC_PEDAL_REGEN_LIMIT_V = [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95]
NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35
NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40
GM_TRUCK_TARGET_FILTER_MIN_SPEED = 12.0
GM_TRUCK_TARGET_FILTER_UP_TAU = 0.10
GM_TRUCK_TARGET_FILTER_DOWN_TAU = 0.06
GM_TRUCK_TARGET_FILTER_BRAKE_BYPASS = -0.65
GM_TRUCK_TARGET_FILTER_DROP_BYPASS = 0.45
def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
@@ -86,6 +92,29 @@ class LongControlVehicleTuning:
def reset(self):
self.last_a_target = 0.0
self.integrator_hold_frames = 0
self.gm_truck_filtered_a_target = 0.0
self.gm_truck_target_filter_initialized = False
def shape_gm_truck_accel_target(self, a_target, v_ego, should_stop):
if not self.is_gm_stock_truck:
return a_target
bypass_filter = (
v_ego < GM_TRUCK_TARGET_FILTER_MIN_SPEED or
should_stop or
a_target <= GM_TRUCK_TARGET_FILTER_BRAKE_BYPASS or
(self.gm_truck_target_filter_initialized and
a_target < self.gm_truck_filtered_a_target - GM_TRUCK_TARGET_FILTER_DROP_BYPASS)
)
if not self.gm_truck_target_filter_initialized or bypass_filter:
self.gm_truck_filtered_a_target = float(a_target)
self.gm_truck_target_filter_initialized = True
return float(a_target)
tau = GM_TRUCK_TARGET_FILTER_DOWN_TAU if a_target < self.gm_truck_filtered_a_target else GM_TRUCK_TARGET_FILTER_UP_TAU
alpha = DT_CTRL / (tau + DT_CTRL)
self.gm_truck_filtered_a_target += alpha * (float(a_target) - self.gm_truck_filtered_a_target)
return self.gm_truck_filtered_a_target
def get_integrator_freeze(self, last_output_accel, a_target, error, v_ego, accel_limits):
volt_test_tune_handoff = self.is_volt and testing_ground.use_2
+37 -1
View File
@@ -6,6 +6,7 @@ from cereal import car, custom, log
import openpilot.selfdrive.controls.lib.latcontrol_torque as latcontrol_torque
import openpilot.selfdrive.controls.lib.latcontrol_pid as latcontrol_pid
from opendbc.car.car_helpers import interfaces
from opendbc.car.interfaces import CarInterfaceBase
from opendbc.car.honda.values import CAR as HONDA, HondaFlags
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.nissan.values import CAR as NISSAN
@@ -56,6 +57,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_prius_ff_scale,
get_prius_friction_scale,
get_prius_friction_threshold,
get_rav4_prime_ff_scale,
get_rav4_prime_friction_scale,
get_rav4_prime_friction_threshold,
get_ioniq_5_ff_scale,
get_ioniq_5_friction_scale,
get_ioniq_5_friction_threshold,
@@ -98,9 +102,11 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
class TestLatControl:
@staticmethod
def _build_torque_controller(car_name):
def _build_torque_controller(car_name, force_torque=False):
CarInterface = interfaces[car_name]
CP = CarInterface.get_non_essential_params(car_name)
if force_torque:
CarInterfaceBase.configure_torque_tune(car_name, CP.lateralTuning)
CI = CarInterface(CP, custom.StarPilotCarParams.new_message())
controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL)
VM = VehicleModel(CP)
@@ -566,6 +572,36 @@ class TestLatControl:
assert unwind_right_scale <= unwind_left_scale
assert get_ioniq_5_friction_threshold(25.0, 0.0, 0.0) >= get_hkg_canfd_base_friction_threshold(25.0)
def test_rav4_prime_unwind_relief_preserves_turn_in(self):
left_turn_in = get_rav4_prime_ff_scale(1.0, 0.8, 13.0)
right_turn_in = get_rav4_prime_ff_scale(-1.0, -0.8, 13.0)
left_unwind = get_rav4_prime_ff_scale(1.0, -0.8, 13.0)
right_unwind = get_rav4_prime_ff_scale(-1.0, 0.8, 13.0)
assert left_turn_in == pytest.approx(1.0)
assert right_turn_in == pytest.approx(1.0)
assert left_unwind < right_unwind < 1.0
assert get_rav4_prime_ff_scale(1.0, -0.8, 25.0) > left_unwind
def test_rav4_prime_friction_targets_center_and_unwind(self):
base = get_standard_friction_threshold(13.0)
center = get_rav4_prime_friction_threshold(13.0, 0.0)
turn = get_rav4_prime_friction_threshold(13.0, 1.0)
assert center > base
assert turn == pytest.approx(base, rel=0.01)
assert get_rav4_prime_friction_scale(13.0, 1.0, 0.8) == pytest.approx(1.0)
assert get_rav4_prime_friction_scale(13.0, 1.0, -0.8) < 1.0
def test_rav4_prime_forced_torque_update_path(self):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(TOYOTA.TOYOTA_RAV4_PRIME, force_torque=True)
CS.vEgo = 13.0
_, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
assert controller.is_rav4_prime
assert lac_log.active
def test_ioniq_5_center_taper_curve(self):
assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0)
assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.20, 25.0) <= 1.0
@@ -791,6 +791,63 @@ def test_gm_stock_truck_positive_i_bleeds_on_coast_request():
assert lc.pid.i < 0.25
def test_gm_stock_truck_target_filter_smooths_mild_follow_reversals():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
tuning = LongControl(CP).vehicle_tuning
assert tuning.shape_gm_truck_accel_target(0.25, 20.0, False) == pytest.approx(0.25)
filtered_brake = tuning.shape_gm_truck_accel_target(-0.10, 20.0, False)
filtered_accel = tuning.shape_gm_truck_accel_target(0.25, 20.0, False)
assert -0.10 < filtered_brake < 0.25
assert filtered_brake < filtered_accel < 0.25
def test_gm_stock_truck_target_filter_bypasses_urgent_braking():
CP = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
tuning = LongControl(CP).vehicle_tuning
tuning.shape_gm_truck_accel_target(0.40, 20.0, False)
assert tuning.shape_gm_truck_accel_target(-0.70, 20.0, False) == pytest.approx(-0.70)
tuning.reset()
tuning.shape_gm_truck_accel_target(0.40, 20.0, False)
assert tuning.shape_gm_truck_accel_target(-0.10, 20.0, False) == pytest.approx(-0.10)
tuning.reset()
tuning.shape_gm_truck_accel_target(0.25, 20.0, False)
assert tuning.shape_gm_truck_accel_target(0.10, 20.0, True) == pytest.approx(0.10)
def test_gm_stock_truck_target_filter_bypasses_low_speed_and_other_cars():
truck_cp = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_SILVERADO,
enableGasInterceptorDEPRECATED=False,
)
truck_tuning = LongControl(truck_cp).vehicle_tuning
truck_tuning.shape_gm_truck_accel_target(0.40, 20.0, False)
bolt_cp = make_longcontrol_cp(
brand="gm",
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023,
enableGasInterceptorDEPRECATED=False,
)
bolt_tuning = LongControl(bolt_cp).vehicle_tuning
assert truck_tuning.shape_gm_truck_accel_target(-0.10, 10.0, False) == pytest.approx(-0.10)
assert bolt_tuning.shape_gm_truck_accel_target(-0.10, 20.0, False) == pytest.approx(-0.10)
def test_gm_stock_truck_positive_i_bleeds_during_light_highway_accel_request():
CP = car.CarParams.new_message()
CP.brand = "gm"
+3 -1
View File
@@ -502,7 +502,9 @@ void pandad_run(std::vector<Panda *> &pandas) {
sm["selfdriveState"].getSelfdriveState().getEnabled() || preap_aol_engaged
);
is_onroad = params.getBool("IsOnroad");
const bool ignore_ignition_line = params.getBool("IgnoreIgnitionLine");
const std::string car_make = params.get("CarMake");
const bool is_gm = car_make == "gm" || car_make == "GM" || car_make == "Gm";
const bool ignore_ignition_line = is_gm && params.getBool("IgnoreIgnitionLine");
process_panda_state(pandas, &pm, engaged, is_onroad, spoofing_started, ignore_ignition_line);
panda_safety.configureSafetyMode(is_onroad);
}
+1 -1
View File
@@ -64,7 +64,7 @@ def get_hkg_remote_start_boots_comma(params: Params) -> bool:
def get_ignore_ignition_line(params: Params) -> bool:
try:
return params.get_bool("IgnoreIgnitionLine")
return (params.get("CarMake", encoding="utf-8") or "").lower() == "gm" and params.get_bool("IgnoreIgnitionLine")
except UnknownKeyName:
return False
@@ -0,0 +1,36 @@
import importlib.util
from pathlib import Path
import pytest
PANDAD_PATH = Path(__file__).resolve().parents[1] / "pandad.py"
SPEC = importlib.util.spec_from_file_location("pandad_under_test", PANDAD_PATH)
assert SPEC is not None and SPEC.loader is not None
PANDAD = importlib.util.module_from_spec(SPEC)
SPEC.loader.exec_module(PANDAD)
class FakeParams:
def __init__(self, car_make, ignore_ignition_line):
self.car_make = car_make
self.ignore_ignition_line = ignore_ignition_line
def get(self, key, encoding=None):
assert key == "CarMake"
return self.car_make
def get_bool(self, key):
assert key == "IgnoreIgnitionLine"
return self.ignore_ignition_line
@pytest.mark.parametrize(("car_make", "enabled", "expected"), [
("gm", True, True),
("GM", True, True),
("tesla", True, False),
("tesla", False, False),
(None, True, False),
])
def test_ignore_ignition_line_is_gm_only(car_make, enabled, expected):
assert PANDAD.get_ignore_ignition_line(FakeParams(car_make, enabled)) == expected
+1 -1
View File
@@ -199,7 +199,7 @@ def flash_panda(params_memory):
except Exception:
hkg_remote_start = False
try:
ignore_ignition_line = params.get_bool("IgnoreIgnitionLine")
ignore_ignition_line = (params.get("CarMake", encoding="utf-8") or "").lower() == "gm" and params.get_bool("IgnoreIgnitionLine")
except Exception:
ignore_ignition_line = False