mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-04 19:43:49 +08:00
Prius EPS Retrofit
This commit is contained in:
firestar5683
@@ -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:
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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)):
|
||||
|
||||
@@ -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"),
|
||||
)
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
CM_ "IMPORT _community.dbc";
|
||||
CM_ "IMPORT _toyota_2017.dbc";
|
||||
CM_ "IMPORT _toyota_adas_standard.dbc";
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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',
|
||||
|
||||
Reference in New Issue
Block a user