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

pri pri

Browse Source
This commit is contained in:
firestar5683
2026-06-27 23:45:56 -05:00
parent e82a4f034b
commit a7ad72489b
2 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/toyota/carcontroller.py
+7 -2
View File
@@ -26,6 +26,7 @@ ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame
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 = 0.85
MAX_PITCH_COMPENSATION = 1.5 # m/s^2
TOYOTA_COAST_BRAKE_MIN_SPEED = 15.0 # m/s
@@ -66,6 +67,11 @@ def get_long_tune(CP, params):
rate=1 / (DT_CTRL * 3))
def get_prius_positive_feedforward_scale(v_ego: float) -> float:
return float(np.interp(v_ego, [0.0, 8.0, 20.0],
[PRIUS_POSITIVE_FEEDFORWARD_SCALE, PRIUS_POSITIVE_FEEDFORWARD_SCALE, PRIUS_CRUISE_FEEDFORWARD_SCALE]))
def update_permit_braking(current: bool, net_acceleration_request_min: float, stopping: bool,
long_active: bool, v_ego: float, lead_visible: bool) -> bool:
if stopping or not long_active:
@@ -435,9 +441,8 @@ class CarController(CarControllerBase):
feedforward = pcm_accel_cmd
if self.CP.carFingerprint == CAR.TOYOTA_PRIUS:
# Keep Prius positive handoffs softer than the stock tune, while restoring some launch authority.
if feedforward > 0.0:
feedforward *= PRIUS_POSITIVE_FEEDFORWARD_SCALE
feedforward *= get_prius_positive_feedforward_scale(CS.out.vEgo)
pcm_accel_cmd = self.long_pid.update(error_future,
speed=CS.out.vEgo,
opendbc_repo/opendbc/car/toyota/tests/test_toyota.py
+10 -2
View File
@@ -7,8 +7,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, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, \
limit_prius_stopping_accel, update_permit_braking
from opendbc.car.toyota.carcontroller import CarController, get_prius_positive_feedforward_scale, limit_interceptor_pcm_accel, \
limit_interceptor_stopping_accel, limit_prius_stopping_accel, update_permit_braking
from opendbc.car.toyota.carstate import calculate_interceptor_gas_pressed
from opendbc.car.toyota.fingerprints import FW_VERSIONS
from opendbc.car.toyota.interface import CarInterface
@@ -300,6 +300,14 @@ class TestToyotaCarController:
limited = limit_prius_stopping_accel(-3.28, -2.0, True, 0.0, True)
assert limited == -3.28
def test_prius_positive_feedforward_scale_stays_soft_at_launch_speed(self):
assert abs(get_prius_positive_feedforward_scale(0.0) - 0.7) < 1e-6
assert abs(get_prius_positive_feedforward_scale(8.0) - 0.7) < 1e-6
def test_prius_positive_feedforward_scale_restores_cruise_authority(self):
assert get_prius_positive_feedforward_scale(20.0) > get_prius_positive_feedforward_scale(8.0)
assert abs(get_prius_positive_feedforward_scale(20.0) - 0.85) < 1e-6
def test_sng_hack_clears_existing_standstill_latch(self):
controller = self._make_controller(standstill_req=True, last_standstill=True)