From bbdaa61b289eeaa1baea5eec4f7107909d551da0 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 5 Oct 2026 23:49:31 -0500 Subject: [PATCH] Prius EPS Retrofit --- .../opendbc/car/toyota/carcontroller.py | 44 ++++++-- opendbc_repo/opendbc/car/toyota/carstate.py | 15 ++- opendbc_repo/opendbc/car/toyota/interface.py | 25 ++++- .../opendbc/car/toyota/prius_longitudinal.py | 103 ++++++++++++++++++ .../opendbc/car/toyota/tests/test_retrofit.py | 37 +++++++ opendbc_repo/opendbc/car/toyota/values.py | 4 +- .../dbc/generator/toyota/toyota_nodsu_pt.dbc | 1 + opendbc_repo/opendbc/safety/modes/toyota.h | 21 +++- .../opendbc/safety/tests/test_toyota.py | 38 +++++++ .../starpilot/car/toyota/prius_preferences.py | 33 ++++++ openpilot/starpilot/controller_extensions.py | 9 ++ .../starpilot/longitudinal/vehicle_policy.py | 4 + .../tests/test_vehicle_preferences.py | 57 +++++++++- openpilot/starpilot/vehicle_preferences.py | 10 +- tools/ci/run_local_tests.py | 1 + 15 files changed, 377 insertions(+), 25 deletions(-) create mode 100644 opendbc_repo/opendbc/car/toyota/prius_longitudinal.py create mode 100644 openpilot/starpilot/car/toyota/prius_preferences.py diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 38981f1f23..2e87c57a4f 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -8,6 +8,7 @@ from opendbc.car.common.pid import PIDController from opendbc.car.secoc import add_mac, build_sync_mac from opendbc.car.interfaces import CarControllerBase from opendbc.car.toyota import toyotacan +from opendbc.car.toyota import prius_longitudinal from opendbc.car.toyota.values import CAR, CarControllerParams, ToyotaFlags, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS from opendbc.can import CANPacker from opendbc.safety import ALTERNATIVE_EXPERIENCE @@ -47,14 +48,15 @@ def supports_toyota_auto_hold(CP): def get_long_tune(CP, params): - if CP.flags & ToyotaFlags.TSS2: + prius = prius_longitudinal.enabled(CP) + if prius or CP.flags & ToyotaFlags.TSS2: kiBP = [2., 5.] kiV = [0.5, 0.25] else: kiBP = [0., 5., 35.] kiV = [3.6, 2.4, 1.5] - return PIDController(0.0, (kiBP, kiV), k_f=1.0, + return PIDController(0.0, (kiBP, kiV), k_f=.8 if prius else 1.0, pos_limit=params.ACCEL_MAX, neg_limit=params.ACCEL_MIN, rate=1 / (DT_CTRL * 3)) @@ -63,11 +65,14 @@ class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) self.reverse_cruise_input = None + self.prius_filter_input = None self.params = CarControllerParams(self.CP) self.last_torque = 0 self.last_angle = 0 self.alert_active = False self.standstill_req = False + self.last_standstill = False + self.prius_longitudinal = prius_longitudinal.enabled(CP) self.permit_braking = True self.steer_rate_counter = 0 self.distance_button = 0 @@ -132,6 +137,9 @@ class CarController(CarControllerBase): self._auto_hold_rearm_blocked = False def update(self, CC, CS, now_nanos): + filter_allowed = (not self.prius_longitudinal or self.prius_filter_input is not None and + self.prius_filter_input.update(now_nanos)) + long_active = CC.longActive and filter_allowed actuators = CC.actuators stopping = actuators.longControlState == LongCtrlState.stopping hud_control = CC.hudControl @@ -229,7 +237,7 @@ class CarController(CarControllerBase): # *** gas and brake *** if supports_toyota_auto_hold(self.CP): self.update_auto_hold_state(CS, pcm_cancel_cmd if self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS else False, - long_active=CC.longActive, stopping=stopping) + long_active=long_active, stopping=stopping) if (self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS and self.frame % 2 == 0 and CS.out.standstill and CS.out.cruiseState.available and not CS.out.gasPressed): can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active)) @@ -240,7 +248,13 @@ class CarController(CarControllerBase): # brakes can take a while to ramp up causing a lurch forward. prevent resume press until planner wants to move. # don't use CC.cruiseControl.resume since it is gated on CS.cruiseState.standstill which goes false for 3s after resume press # whitelist hybrids as they do not have this issue and can stay stopped after resume press - if not self.CP.flags & ToyotaFlags.HYBRID.value: + if self.prius_longitudinal: + if CS.out.standstill and not self.last_standstill: + self.standstill_req = True + if CS.pcm_acc_status != 8 or CC.cruiseControl.resume: + self.standstill_req = False + self.last_standstill = CS.out.standstill + elif not self.CP.flags & ToyotaFlags.HYBRID.value: should_resume = actuators.accel > 0 if should_resume: self.standstill_req = False @@ -261,8 +275,8 @@ class CarController(CarControllerBase): self.distance_button = 0 # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit - pcm_accel_cmd = actuators.accel - if CC.longActive: + pcm_accel_cmd = actuators.accel if filter_allowed else 0. + if long_active: pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT) self.prev_accel = pcm_accel_cmd @@ -286,9 +300,10 @@ class CarController(CarControllerBase): future_t = float(np.interp(CS.out.vEgo, [2., 5.], [0.25, 0.5])) a_ego_future = a_ego_blended + j_ego * future_t - if CC.longActive: + if long_active: # constantly slowly unwind integral to recover from large temporary errors - self.long_pid.i -= ACCEL_PID_UNWIND * float(np.sign(self.long_pid.i)) + unwind = ACCEL_PID_UNWIND * (8. if self.prius_longitudinal and pcm_accel_cmd * self.long_pid.i < 0. else 1.) + self.long_pid.i -= unwind * float(np.sign(self.long_pid.i)) error_future = pcm_accel_cmd - a_ego_future @@ -301,7 +316,8 @@ class CarController(CarControllerBase): pcm_accel_cmd = self.long_pid.update(error_future, speed=CS.out.vEgo, - feedforward=pcm_accel_cmd, + feedforward=(prius_longitudinal.get_prius_feedforward(pcm_accel_cmd, CS.out.vEgo) + if self.prius_longitudinal else pcm_accel_cmd), freeze_integrator=actuators.longControlState != LongCtrlState.pid) else: self.long_pid.reset() @@ -309,11 +325,19 @@ class CarController(CarControllerBase): # Along with rate limiting positive jerk above, this greatly improves gas response time # Consider the net acceleration request that the PCM should be applying (pitch included) net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request) - if net_acceleration_request_min < 0.2 or stopping or not CC.longActive: + if self.prius_longitudinal: + self.permit_braking = prius_longitudinal.update_permit_braking( + self.permit_braking, net_acceleration_request_min, stopping, long_active, CS.out.vEgo, lead) + elif net_acceleration_request_min < 0.2 or stopping or not long_active: self.permit_braking = True elif net_acceleration_request_min > 0.3: self.permit_braking = False + if self.prius_longitudinal: + pcm_accel_cmd = prius_longitudinal.limit_no_lead_cruise_sign_flip( + pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, CS.out.cruiseState.speed, lead) + pcm_accel_cmd = prius_longitudinal.limit_prius_stopping_accel( + pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, lead) pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)) if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS: diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 64b8ebaa3c..e18948b103 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -8,6 +8,7 @@ from opendbc.car.common.filter_simple import FirstOrderFilter from opendbc.car.interfaces import CarStateBase from opendbc.car.dashboard_speed_limit import Tracker as LimitTracker, parser_expiry, toyota_sign from opendbc.can.dbc import DBC as CANDBC +from opendbc.car.toyota.prius_longitudinal import configured as prius_filter_configured from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, EPS_SCALE, TOYOTA_AUTO_HOLD_AEB_CARS ButtonType = structs.CarState.ButtonEvent.Type @@ -29,7 +30,7 @@ class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) - self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. + self.eps_torque_scale = (.73 if prius_filter_configured(CP) else EPS_SCALE[CP.carFingerprint] / 100.) self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. self.cluster_min_speed = CV.KPH_TO_MS / 2. @@ -51,6 +52,7 @@ class CarState(CarStateBase): self.pcm_follow_distance = 0 self.acc_type = 1 + self.pcm_acc_status = 0 self.lkas_hud = {} self.gvc = 0.0 self.secoc_synchronization = None @@ -171,6 +173,7 @@ class CarState(CarStateBase): ret.accFaulted = ret.accFaulted or cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2 pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] + self.pcm_acc_status = pcm_acc_status ret.cruiseState.standstill = pcm_acc_status == 7 ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) ret.cruiseState.nonAdaptive = pcm_acc_status in (1, 2, 3, 4, 5, 6) @@ -189,7 +192,7 @@ class CarState(CarStateBase): self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"] buttonEvents = [] - if self.CP.flags & ToyotaFlags.TSS2 or self.CP.carFingerprint == CAR.TOYOTA_PRIUS_RETROFIT: + if self.CP.flags & (ToyotaFlags.TSS2 | ToyotaFlags.LONG_FILTER) or self.CP.carFingerprint == CAR.TOYOTA_PRIUS_RETROFIT: # lkas button is wired to the camera prev_lkas_button = self.lkas_button self.lkas_button = cp_cam.vl["LKAS_HUD"]["LDA_ON_MESSAGE"] @@ -206,9 +209,10 @@ class CarState(CarStateBase): buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) - if self.CP.carFingerprint == CAR.TOYOTA_PRIUS_RETROFIT: + if self.CP.carFingerprint == CAR.TOYOTA_PRIUS_RETROFIT or self.CP.flags & ToyotaFlags.LONG_FILTER: prev_distance_button = self.distance_button - self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] + self.distance_button = (cp.vl["SDSU"]["FD_BUTTON"] if self.CP.flags & ToyotaFlags.LONG_FILTER + else cp_acc.vl["ACC_CONTROL"]["DISTANCE"]) buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) ret.buttonEvents = buttonEvents @@ -230,6 +234,9 @@ class CarState(CarStateBase): ("BLINKERS_STATE", float('nan')), ] + if CP.flags & ToyotaFlags.LONG_FILTER: + pt_messages.append(("SDSU", 50)) + return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0), Bus.cam: CANParser(dbc_name, cam_messages, 2), diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 1567965ce9..02529d2132 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -72,6 +72,9 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 + smart_dsu = (candidate in (CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT) and 0x2FF in fingerprint[0] and + any(fw.ecu == Ecu.eps and bytes(fw.fwVersion) == b"8965B47070\x00\x00\x00\x00\x00\x00" + for fw in car_fw)) stop_and_go = bool(ret.flags & ToyotaFlags.TSS2) # In TSS2 cars, the camera does long control @@ -87,11 +90,11 @@ class CarInterface(CarInterfaceBase): # Only give steer angle deadzone to for bad angle sensor prius for fw in car_fw: if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': - retrofit = candidate == CAR.TOYOTA_PRIUS_RETROFIT + retrofit = candidate == CAR.TOYOTA_PRIUS_RETROFIT or smart_dsu ret.steerActuatorDelay = 0.14 if retrofit else 0.25 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.3 if retrofit else 0.2) # 2021+ TSS2 steering rack swapped into a TSS-P car, not supported - if candidate == CAR.TOYOTA_PRIUS and fw.ecu == "eps" and fw.fwVersion == b'8965B47070\x00\x00\x00\x00\x00\x00': + if candidate == CAR.TOYOTA_PRIUS and not smart_dsu and fw.ecu == "eps" and fw.fwVersion == b'8965B47070\x00\x00\x00\x00\x00\x00': ret.dashcamOnly = True elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2): @@ -141,13 +144,23 @@ class CarInterface(CarInterfaceBase): if late_camera: bypass = ((0x343 in camera_fp and not any(0x343 in fingerprint.get(bus, {}) for bus in (0, 1))) or (0x4CB in camera_fp and 0x4CB not in fingerprint[0])) + smart_dsu = (candidate == CAR.TOYOTA_PRIUS_RETROFIT and 0x2FF in fingerprint[0] and + any(fw.ecu == Ecu.eps and bytes(fw.fwVersion) == b"8965B47070\x00\x00\x00\x00\x00\x00" + for fw in car_fw)) takeover = 0x2FF in fingerprint[0] or bypass - if takeover: + if smart_dsu: + ret.flags |= int(ToyotaFlags.LONG_FILTER) + ret.safetyConfigs[0].safetyParam |= int(ToyotaSafetyFlags.LONG_FILTER) + elif takeover: ret.dashcamOnly = True - ret.openpilotLongitudinalControl = False + ret.openpilotLongitudinalControl = smart_dsu ret.alphaLongitudinalAvailable = False - ret.autoResumeSng = ret.openpilotLongitudinalControl + if smart_dsu: + ret.flags |= int(ToyotaFlags.HYBRID | ToyotaFlags.LONG_FILTER) + ret.safetyConfigs[0].safetyParam = 4169 + ret.openpilotLongitudinalControl = True + ret.autoResumeSng = ret.openpilotLongitudinalControl and not ret.flags & ToyotaFlags.LONG_FILTER if not ret.openpilotLongitudinalControl: ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value @@ -156,7 +169,7 @@ class CarInterface(CarInterfaceBase): # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED - if ret.flags & ToyotaFlags.TSS2: + if ret.flags & (ToyotaFlags.TSS2 | ToyotaFlags.LONG_FILTER): ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value # Hybrids have much quicker longitudinal actuator response diff --git a/opendbc_repo/opendbc/car/toyota/prius_longitudinal.py b/opendbc_repo/opendbc/car/toyota/prius_longitudinal.py new file mode 100644 index 0000000000..cdf0a58701 --- /dev/null +++ b/opendbc_repo/opendbc/car/toyota/prius_longitudinal.py @@ -0,0 +1,103 @@ +import numpy as np +from opendbc.car import structs +from opendbc.car.toyota.values import CAR, ToyotaFlags + +PRIUS_POSITIVE_FEEDFORWARD_SCALE = .7 +PRIUS_CRUISE_FEEDFORWARD_SCALE = 1. +PRIUS_NEGATIVE_FEEDFORWARD_SCALE = 1.125 +TOYOTA_COAST_BRAKE_MIN_SPEED = 15. +TOYOTA_COAST_BRAKE_ENABLE_ACCEL = -.10 +TOYOTA_COAST_BRAKE_DISABLE_ACCEL = -.06 +TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL = -.30 +TOYOTA_NO_LEAD_CRUISE_SIGN_FLIP_MIN_SET_SPEED_ERROR = .35 + + +def configured(cp): + return (cp.brand == 'toyota' and cp.carFingerprint in (CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT) and + cp.pcmCruise and not cp.dashcamOnly and not cp.notCar and + not cp.passive and cp.steerControlType == structs.CarParams.SteerControlType.torque and + cp.flags == int(ToyotaFlags.HYBRID | ToyotaFlags.LONG_FILTER | ToyotaFlags.RAISED_ACCEL_LIMIT) and + len(cp.safetyConfigs) == 1 and cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.toyota and + cp.safetyConfigs[0].safetyParam in (4169, 4681) and + cp.openpilotLongitudinalControl == (cp.safetyConfigs[0].safetyParam == 4169)) + + +def enabled(cp): + return configured(cp) and cp.openpilotLongitudinalControl + + +def stopping_decel_rate(cp): + return .3 if enabled(cp) else None + + +def prepare_stock(cp): + if not configured(cp): + return False + cp.openpilotLongitudinalControl = False + cp.safetyConfigs[0].safetyParam = 4681 + cp.autoResumeSng = False + return True + + +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 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 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: + return True + + if not lead_visible and v_ego >= TOYOTA_COAST_BRAKE_MIN_SPEED: + return net_acceleration_request_min <= TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL + + # At cruising speeds, some Toyota platforms turn tiny negative accel corrections + # into noticeable brake taps. Keep a small coast-only band so mild follow/cruise + # trims stay off the brakes while still allowing real negative requests through. + if v_ego >= TOYOTA_COAST_BRAKE_MIN_SPEED: + if net_acceleration_request_min <= TOYOTA_COAST_BRAKE_ENABLE_ACCEL: + return True + if net_acceleration_request_min >= TOYOTA_COAST_BRAKE_DISABLE_ACCEL: + return False + return current + + if net_acceleration_request_min < 0.2: + return True + if net_acceleration_request_min > 0.3: + return False + return current + +def limit_prius_stopping_accel(pcm_accel_cmd: float, target_accel: float, stopping: bool, v_ego: float, lead_visible: bool) -> float: + if not stopping or pcm_accel_cmd >= 0.0 or v_ego >= 1.5: + return pcm_accel_cmd + + # Prius can hold onto a stale full negative stop command at standstill even after the + # planner has already softened. Keep enough brake to hold the stop, but let the command + # unwind toward the live planner target so launches are not delayed and stop transitions + # are less abrupt. + if target_accel <= -1.8: + return pcm_accel_cmd + + stop_floor = float(np.interp(v_ego, + [0.0, 0.2, 0.5, 0.9, 1.5], + [-0.96, -1.00, -1.08, -1.18, -1.35] if lead_visible else [-0.84, -0.88, -0.96, -1.08, -1.24])) + target_buffer = float(np.interp(v_ego, [0.0, 0.5, 1.5], [0.06, 0.10, 0.16])) + planner_floor = float(target_accel) - target_buffer + return max(pcm_accel_cmd, max(stop_floor, planner_floor)) + +def limit_no_lead_cruise_sign_flip(pcm_accel_cmd: float, target_accel: float, stopping: bool, v_ego: float, + set_speed: float, lead_visible: bool) -> float: + if stopping or lead_visible or pcm_accel_cmd >= 0.0 or v_ego < TOYOTA_COAST_BRAKE_MIN_SPEED: + return pcm_accel_cmd + if target_accel < -0.02 or set_speed <= 0.0: + return pcm_accel_cmd + + if float(set_speed) - float(v_ego) >= TOYOTA_NO_LEAD_CRUISE_SIGN_FLIP_MIN_SET_SPEED_ERROR: + return max(pcm_accel_cmd, 0.0) + + return pcm_accel_cmd diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py b/opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py index 13e18a454c..2fbf470a19 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py @@ -10,6 +10,43 @@ def params(car, fp=None, alpha=False): class TestRetrofit(unittest.TestCase): + def test_prius_smart_dsu_requires_exact_rack_and_source(self): + from opendbc.car.structs import CarParams + rack = CarParams.CarFw.new_message(ecu=CarParams.Ecu.eps, fwVersion=b'8965B47070\x00\x00\x00\x00\x00\x00') + from opendbc.car.toyota.prius_longitudinal import enabled + from opendbc.car.toyota.prius_longitudinal import stopping_decel_rate + for alpha in (False, True): + cp = CarInterface.get_params(CAR.TOYOTA_PRIUS_RETROFIT, {0: {0x2FF: 4}, 1: {}, 2: {}}, [rack], alpha, False, False) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 4169) + self.assertTrue(enabled(cp)) + self.assertFalse(cp.dashcamOnly) + self.assertTrue(cp.openpilotLongitudinalControl) + self.assertTrue(cp.pcmCruise) + self.assertFalse(cp.flags & ToyotaFlags.TSS2) + self.assertAlmostEqual(cp.steerActuatorDelay, .14) + self.assertAlmostEqual(cp.longitudinalActuatorDelay, .05) + self.assertEqual(stopping_decel_rate(cp), .3) + automatic = CarInterface.get_params(CAR.TOYOTA_PRIUS, {0: {0x2FF: 4}, 1: {}, 2: {}}, [rack], False, False, False) + self.assertTrue(enabled(automatic)) + self.assertEqual(automatic.carFingerprint, CAR.TOYOTA_PRIUS) + self.assertEqual(automatic.safetyConfigs[0].safetyParam, 4169) + self.assertFalse(automatic.flags & ToyotaFlags.TSS2) + self.assertAlmostEqual(automatic.steerActuatorDelay, .14) + from opendbc.car.toyota.prius_longitudinal import configured, prepare_stock + self.assertTrue(prepare_stock(automatic)) + self.assertTrue(configured(automatic)) + self.assertFalse(enabled(automatic)) + self.assertEqual(automatic.safetyConfigs[0].safetyParam, 4681) + self.assertFalse(automatic.openpilotLongitudinalControl) + self.assertTrue(automatic.pcmCruise) + self.assertTrue(prepare_stock(automatic)) + for identity, fw, length in ((CAR.TOYOTA_PRIUS, [], 8), (CAR.TOYOTA_PRIUS_RETROFIT, [], 8), + (CAR.TOYOTA_PRIUS_RETROFIT, [], 7)): + cp = CarInterface.get_params(identity, {0: {0x2FF: length}, 1: {}, 2: {}}, fw, False, False, False) + self.assertFalse(enabled(cp)) + if identity == CAR.TOYOTA_PRIUS_RETROFIT: + self.assertTrue(cp.dashcamOnly) + def test_original_stock_contracts(self): for car, word, factor, friction in ((CAR.TOYOTA_MATRIX_RETROFIT, 856, 4.05, .10), (CAR.TOYOTA_PRIUS_RETROFIT, 585, 1.60, .151515)): diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index e7547784c6..45a227c70d 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -56,12 +56,14 @@ class ToyotaSafetyFlags(IntFlag): STOCK_LONGITUDINAL = (2 << 8) LTA = (4 << 8) SECOC = (8 << 8) + LONG_FILTER = (16 << 8) class ToyotaFlags(IntFlag): # Detected flags HYBRID = 1 HAS_BSM = 2 # blind spot monitoring + LONG_FILTER = 8192 DISABLE_RADAR = 4 # Static flags @@ -248,7 +250,7 @@ class CAR(Platforms): dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), ) TOYOTA_PRIUS_RETROFIT = PlatformConfig( - [ToyotaCommunityCarDocs("Toyota Prius 2016-20", package="TSS2 EPS retrofit; stock longitudinal")], + [ToyotaCommunityCarDocs("Toyota Prius 2016-20", package="TSS2 EPS retrofit; smartDSU required for openpilot longitudinal")], TOYOTA_PRIUS.specs, dbc_dict("toyota_nodsu_pt_generated", "toyota_adas"), ) diff --git a/opendbc_repo/opendbc/dbc/generator/toyota/toyota_nodsu_pt.dbc b/opendbc_repo/opendbc/dbc/generator/toyota/toyota_nodsu_pt.dbc index a7ffbf1564..109e85498f 100644 --- a/opendbc_repo/opendbc/dbc/generator/toyota/toyota_nodsu_pt.dbc +++ b/opendbc_repo/opendbc/dbc/generator/toyota/toyota_nodsu_pt.dbc @@ -1,3 +1,4 @@ +CM_ "IMPORT _community.dbc"; CM_ "IMPORT _toyota_2017.dbc"; CM_ "IMPORT _toyota_adas_standard.dbc"; diff --git a/opendbc_repo/opendbc/safety/modes/toyota.h b/opendbc_repo/opendbc/safety/modes/toyota.h index 711382a43b..63eaca6a58 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota.h +++ b/opendbc_repo/opendbc/safety/modes/toyota.h @@ -32,6 +32,17 @@ /* ACC */ \ {0x343, 0, 8, .check_relay = true}, \ +#define TOYOTA_FILTER_LONG_TX_MSGS \ + TOYOTA_COMMON_TX_MSGS \ + /* DSU bus 0 */ \ + {0x283, 0, 7, .check_relay = false}, {0x2E6, 0, 8, .check_relay = false}, {0x2E7, 0, 8, .check_relay = false}, {0x33E, 0, 7, .check_relay = false}, \ + {0x344, 0, 8, .check_relay = false}, {0x365, 0, 7, .check_relay = false}, {0x366, 0, 7, .check_relay = false}, {0x4CB, 0, 8, .check_relay = false}, \ + /* DSU bus 1 */ \ + {0x128, 1, 6, .check_relay = false}, {0x141, 1, 4, .check_relay = false}, {0x160, 1, 8, .check_relay = false}, {0x161, 1, 7, .check_relay = false}, \ + {0x470, 1, 4, .check_relay = false}, \ + /* PCS_HUD */ \ + {0x411, 0, 8, .check_relay = false}, + #define TOYOTA_COMMON_SECOC_LONG_TX_MSGS \ TOYOTA_COMMON_SECOC_TX_MSGS \ {0x343, 0, 8, .check_relay = true}, \ @@ -468,6 +479,10 @@ static safety_config toyota_init(uint16_t param) { TOYOTA_COMMON_LONG_TX_MSGS }; + static const CanMsg TOYOTA_FILTER_LONG_TX_MSGS_SET[] = { + TOYOTA_FILTER_LONG_TX_MSGS + }; + static const CanMsg TOYOTA_SECOC_LONG_TX_MSGS[] = { TOYOTA_COMMON_SECOC_LONG_TX_MSGS }; @@ -501,7 +516,11 @@ static safety_config toyota_init(uint16_t param) { if (toyota_stock_longitudinal) { SET_TX_MSGS(TOYOTA_TX_MSGS, ret); } else { - SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret); + if (param == 0x1049U) { + SET_TX_MSGS(TOYOTA_FILTER_LONG_TX_MSGS_SET, ret); + } else { + SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret); + } } } diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota.py b/opendbc_repo/opendbc/safety/tests/test_toyota.py index 8ba1c5ab01..53f2415a87 100755 --- a/opendbc_repo/opendbc/safety/tests/test_toyota.py +++ b/opendbc_repo/opendbc/safety/tests/test_toyota.py @@ -413,6 +413,44 @@ class TestToyotaSecOcSafety(TestToyotaSecOcSafetyBase): self.assertEqual(should_tx, self._tx(self._accel_msg_343(accel, cancel_req=1))) + + +class TestToyotaPriusLongFilter(TestToyotaSafetyTorque): + SAFETY_PARAM = 0x1049 + TX_MSGS = TOYOTA_COMMON_TX_MSGS + [msg for msg in TOYOTA_COMMON_LONG_TX_MSGS if msg != [0x750, 0]] + RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x191, 0x412)} + FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191]} + + def test_diagnostics(self): + super().test_diagnostics(ecu_disabled=False) + + def test_filter_relay_exception_is_exact(self): + for word, relay in ((73, True), (585, False), (0x1049, False), (0x1249, False), + (0x1048, True), (0x104A, True), (0x1149, True)): + with self.subTest(word=word): + self.safety.set_safety_hooks(self.SAFETY_MODEL, word) + self.safety.init_tests() + self.assertTrue(self._rx(self._accel_msg_343(0))) + self.assertEqual(self.safety.get_relay_malfunction(), relay) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x343), -1 if relay else 0) + + def test_filter_does_not_admit_camera_bus_commands(self): + self.safety.set_controls_allowed(True) + for address, length in ((0x283, 7), (0x343, 8), (0x411, 8)): + with self.subTest(address=address): + self.assertFalse(self._tx(libsafety_py.make_CANPacket(address, 2, bytes(length)))) + for length in (7, 6): + self.assertFalse(self._tx(libsafety_py.make_CANPacket(0x343, 0, bytes(length)))) + + +class TestToyotaPriusLongFilterStock(TestToyotaStockLongitudinalTorque): + SAFETY_PARAM = 0x1249 + + def test_filter_stock_does_not_admit_dsu_commands(self): + self.safety.set_controls_allowed(True) + for address, length in ((0x283, 7), (0x411, 8), (0x750, 8)): + self.assertFalse(self._tx(libsafety_py.make_CANPacket(address, 0, bytes(length)))) + if __name__ == "__main__": unittest.main() diff --git a/openpilot/starpilot/car/toyota/prius_preferences.py b/openpilot/starpilot/car/toyota/prius_preferences.py new file mode 100644 index 0000000000..685b119dea --- /dev/null +++ b/openpilot/starpilot/car/toyota/prius_preferences.py @@ -0,0 +1,33 @@ +from opendbc.car.toyota.prius_longitudinal import enabled as filter_enabled +from openpilot.starpilot.saved_source import read_saved + + +def filter_preference_enabled(params): + try: + master = read_saved(params, 'OpenpilotEnabledToggle', 8) + safe, safe_valid = read_saved(params, 'SafeMode', 8) + disabled, disable_valid = read_saved(params, 'DisableOpenpilotLongitudinal', 8) + return bool(master == (b'1', True) and safe_valid and safe in (None, b'0') and + disable_valid and disabled in (None, b'0')) + except (OSError, RuntimeError, TypeError, ValueError): + return False + + +class PriusFilterPreference: + def __init__(self, cp, params): + self.cp, self.params = cp, params + self.read_ns = 0 + self.enabled = False + + def update(self, now_ns): + if type(now_ns) is not int or now_ns <= 0 or not filter_enabled(self.cp): + self.enabled = False + return False + if now_ns < self.read_ns: + self.read_ns = 0 + self.enabled = False + return False + if self.read_ns == 0 or now_ns - self.read_ns >= 250_000_000: + self.read_ns = now_ns + self.enabled = filter_preference_enabled(self.params) + return self.enabled diff --git a/openpilot/starpilot/controller_extensions.py b/openpilot/starpilot/controller_extensions.py index 7a9309a83b..b9315526ce 100644 --- a/openpilot/starpilot/controller_extensions.py +++ b/openpilot/starpilot/controller_extensions.py @@ -110,6 +110,15 @@ class ResumePlanInputs: def configure_controller(CI, params): cp = CI.CP controller = CI.CC + from opendbc.car.toyota.prius_longitudinal import enabled as prius_filter_enabled, prepare_stock as prepare_prius_stock + if prius_filter_enabled(cp): + from openpilot.starpilot.car.toyota.prius_preferences import PriusFilterPreference, filter_preference_enabled + if not filter_preference_enabled(params): + prepare_prius_stock(cp) + if controller is not None: + controller.prius_longitudinal = False + elif controller is not None: + controller.prius_filter_input = PriusFilterPreference(cp, params) from opendbc.car.gm.values import is_gm_auto_hold, is_volt_one_pedal, is_bolt_pedal_profile, camera_acc_pedal_profile, volt_cc_pedal_profile cc_pedal = volt_cc_pedal_profile(cp) camera_pedal = camera_acc_pedal_profile(cp) or cc_pedal diff --git a/openpilot/starpilot/longitudinal/vehicle_policy.py b/openpilot/starpilot/longitudinal/vehicle_policy.py index 011c9d01b6..787c6022cf 100644 --- a/openpilot/starpilot/longitudinal/vehicle_policy.py +++ b/openpilot/starpilot/longitudinal/vehicle_policy.py @@ -20,6 +20,10 @@ def policy_for(cp): def stopping_decel_rate(cp, policy): + from opendbc.car.toyota.prius_longitudinal import stopping_decel_rate as prius_filter_stop_rate + rate = prius_filter_stop_rate(cp) + if rate is not None: + return rate from opendbc.car.honda.parameter_profiles import stopping_decel_rate as honda_stopping_decel_rate rate = honda_stopping_decel_rate(cp) if rate is not None: diff --git a/openpilot/starpilot/tests/test_vehicle_preferences.py b/openpilot/starpilot/tests/test_vehicle_preferences.py index e8103755e7..05df2f7d7d 100644 --- a/openpilot/starpilot/tests/test_vehicle_preferences.py +++ b/openpilot/starpilot/tests/test_vehicle_preferences.py @@ -25,6 +25,56 @@ class TestVehicleStartupPreferences(unittest.TestCase): else: path.write_bytes(value) + def test_prius_filter_card_rechecks_final_preferences_without_stock_promotion(self): + observed = gen_empty_fingerprint() + observed[0][0x2FF] = 4 + eps = structs.CarParams.CarFw.new_message( + ecu=structs.CarParams.Ecu.eps, fwVersion=b'8965B47070\x00\x00\x00\x00\x00\x00') + for key in ('OpenpilotEnabledToggle', 'SafeMode', 'DisableOpenpilotLongitudinal'): + for initially_stock in (False, True): + with self.subTest(key=key, initially_stock=initially_stock): + self.raw('SafeMode', b'0') + self.raw('DisableOpenpilotLongitudinal', b'1' if initially_stock else b'0') + + def change(ci, key=key, initially_stock=initially_stock): + self.raw(key, b'0' if key == 'OpenpilotEnabledToggle' else b'1') + if initially_stock: + self.raw('OpenpilotEnabledToggle', b'1') + self.raw('SafeMode', b'0') + self.raw('DisableOpenpilotLongitudinal', b'0') + + host, constructed, published = self.start( + CAR.TOYOTA_PRIUS_RETROFIT, observed=observed, car_fw=[eps], + key='LongPitch', requested=True, after_construct=change, + capture=lambda ci: int(ci.CP.safetyConfigs[0].safetyParam)) + self.assertEqual(constructed, [4681 if initially_stock else 4169]) + self.assertEqual(published.safetyConfigs[0].safetyParam, 4681) + self.assertTrue(published.pcmCruise) + self.assertFalse(published.openpilotLongitudinalControl) + self.assertFalse(host.CI.CC.prius_longitudinal) + self.assertIsNone(host.CI.CC.prius_filter_input) + + def test_prius_filter_live_preference_withdraws_at_read_deadline(self): + from opendbc.car.toyota.interface import CarInterface + from openpilot.starpilot.car.toyota.prius_preferences import PriusFilterPreference + observed = gen_empty_fingerprint() + observed[0][0x2FF] = 4 + eps = structs.CarParams.CarFw.new_message( + ecu=structs.CarParams.Ecu.eps, fwVersion=b'8965B47070\x00\x00\x00\x00\x00\x00') + cp = CarInterface.get_params(CAR.TOYOTA_PRIUS_RETROFIT, observed, [eps], False, False, False) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 4169) + for key in ('OpenpilotEnabledToggle', 'SafeMode', 'DisableOpenpilotLongitudinal'): + with self.subTest(key=key): + self.raw('OpenpilotEnabledToggle', b'1') + self.raw('SafeMode', b'0') + self.raw('DisableOpenpilotLongitudinal', b'0') + preference = PriusFilterPreference(cp, self.params) + self.assertTrue(preference.update(1_000_000_000)) + self.raw(key, b'0' if key == 'OpenpilotEnabledToggle' else b'1') + self.assertTrue(preference.update(1_249_999_999)) + self.assertFalse(preference.update(1_250_000_000)) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 4169) + def test_gm_stop_preferences_require_strict_saved_opt_in(self): from openpilot.common.params import ParamKeyType for key, name in (("VoltSNG", "volt_sng"), ("GMAutoHold", "gm_auto_hold"), ("VoltOnePedalMode", "volt_one_pedal")): @@ -268,17 +318,20 @@ class TestVehicleStartupPreferences(unittest.TestCase): path.symlink_to("SafeMode") self.assertFalse(VehicleStartupPreferences.read(self.params, enabled=True).toyota_auto_hold) - def start(self, candidate, *, enabled=True, requested=True, change_saved=False, key="ToyotaAutoHold", observed=None, capture=None): + def start(self, candidate, *, enabled=True, requested=True, change_saved=False, key="ToyotaAutoHold", observed=None, + capture=None, car_fw=None, after_construct=None): self.raw(key, b"1" if requested else b"0") self.params.put_bool("OpenpilotEnabledToggle", enabled, block=True) snapshots = [] def fingerprint(*args, **kwargs): - return candidate, observed if observed is not None else gen_empty_fingerprint(), "0" * 17, [], structs.CarParams.FingerprintSource.can, True + return candidate, observed if observed is not None else gen_empty_fingerprint(), "0" * 17, car_fw or [], structs.CarParams.FingerprintSource.can, True def create(*args, **kwargs): ci = car_helpers.get_car(*args, **kwargs) snapshots.append(capture(ci) if capture is not None else (bool(ci.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD), int(ci.CP.alternativeExperience))) + if after_construct is not None: + after_construct(ci) if change_saved: self.raw(key, b"0") return ci diff --git a/openpilot/starpilot/vehicle_preferences.py b/openpilot/starpilot/vehicle_preferences.py index d08895e6ef..cb9a286481 100644 --- a/openpilot/starpilot/vehicle_preferences.py +++ b/openpilot/starpilot/vehicle_preferences.py @@ -28,6 +28,7 @@ class VehicleStartupPreferences: tesla_screen: bool = False tesla_screen_brake: bool = False gm_camera_pedal: bool = True + toyota_filter: bool = True @classmethod def read(cls, params, *, enabled: bool): @@ -55,7 +56,7 @@ class VehicleStartupPreferences: readable and safe in (None, b"0")) except (OSError, TypeError, ValueError): return cls(disable_bolt_long=disable_bolt, honda_bosch_a_radar=honda_radar, tesla_preap_stock=preap_stock, - gm_camera_pedal=False) + gm_camera_pedal=False, toyota_filter=False) try: pitch, pitch_readable = read_saved(params, "LongPitch", 8) pitch_enabled = not (enabled and pitch_readable and pitch == b"0" and readable and safe in (None, b"0")) @@ -73,6 +74,7 @@ class VehicleStartupPreferences: return cls(tesla_screen=screen, tesla_screen_brake=screen_brake, toyota_auto_hold=toyota, volt_sng=volt_sng, gm_auto_hold=gm_auto_hold, volt_one_pedal=volt_one_pedal, gm_camera_pedal=bool(enabled and not disable_bolt and readable and safe in (None, b"0")), + toyota_filter=bool(enabled and not disable_bolt and readable and safe in (None, b"0")), turn_assist=bool(enabled and assist and readable and safe in (None, b"0")), gm_long_pitch=pitch_enabled, disable_bolt_long=disable_bolt, honda_bosch_a_radar=honda_radar, tesla_preap_stock=preap_stock) @@ -121,6 +123,9 @@ class VehicleStartupPreferences: from opendbc.car.tesla.screen_button import apply_screen_button apply_screen_button(cp, self.tesla_screen, self.tesla_screen_brake) self._prepare_honda_radar(cp) + if not self.toyota_filter: + from opendbc.car.toyota.prius_longitudinal import prepare_stock as prepare_prius_stock + prepare_prius_stock(cp) self._prepare_bolt(cp, fingerprints) prepare_disable_longitudinal(cp, self.disable_bolt_long) self._prepare_camera_pedal(cp) @@ -139,6 +144,9 @@ class VehicleStartupPreferences: from opendbc.car.tesla.screen_button import apply_screen_button apply_screen_button(cp, self.tesla_screen, self.tesla_screen_brake) self._prepare_honda_radar(cp) + if not self.toyota_filter: + from opendbc.car.toyota.prius_longitudinal import prepare_stock as prepare_prius_stock + prepare_prius_stock(cp) self._prepare_bolt(cp) prepare_disable_longitudinal(cp, self.disable_bolt_long) self._prepare_camera_pedal(cp) diff --git a/tools/ci/run_local_tests.py b/tools/ci/run_local_tests.py index 2ee12efb65..2dce7ecc4d 100755 --- a/tools/ci/run_local_tests.py +++ b/tools/ci/run_local_tests.py @@ -51,6 +51,7 @@ QUICK_TARGETS = ( 'opendbc_repo/opendbc/car/tesla/tests/test_screen_button.py', 'opendbc_repo/opendbc/safety/tests/test_tesla_screen_button.py', 'opendbc_repo/opendbc/safety/tests/test_toyota.py', + 'opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py', 'opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_stock_parser.py', 'opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_engagement_messages.py', 'opendbc_repo/opendbc/safety/tests/test_hyundai_ioniq6_long.py',