Skip to content

Commit

Permalink
December 7th, 2024 Patch
Browse files Browse the repository at this point in the history
  • Loading branch information
FrogAi committed Dec 7, 2024
1 parent 9751e9b commit e08da67
Show file tree
Hide file tree
Showing 59 changed files with 963 additions and 649 deletions.
1 change: 1 addition & 0 deletions cereal/log.capnp
Original file line number Diff line number Diff line change
Expand Up @@ -622,6 +622,7 @@ struct RadarState @0x9a185389d6fdd05f {
leadRight @14 :LeadData;
leadLeftFar @15 :LeadData;
leadRightFar @16 :LeadData;
leadsLead @17 :LeadData;
cumLagMs @5 :Float32;

struct LeadData {
Expand Down
103 changes: 51 additions & 52 deletions common/params.cc

Large diffs are not rendered by default.

22 changes: 16 additions & 6 deletions panda/board/safety/safety_toyota.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,18 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks

// Stock longitudinal
#define TOYOTA_COMMON_TX_MSGS \
{0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \
#define TOYOTA_BASE_TX_MSGS \
{0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \
{0x750, 0, 8}, /* white list 0x750 for Enhanced Diagnostic Request */ \

#define TOYOTA_COMMON_TX_MSGS \
TOYOTA_BASE_TX_MSGS \
{0x2E4, 0, 5}, \

#define TOYOTA_COMMON_SECOC_TX_MSGS \
TOYOTA_BASE_TX_MSGS \
{0x2E4, 0, 8}, {0x131, 0, 8}, \

#define TOYOTA_COMMON_LONG_TX_MSGS \
TOYOTA_COMMON_TX_MSGS \
{0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, /* DSU bus 0 */ \
Expand All @@ -66,7 +74,7 @@ const CanMsg TOYOTA_TX_MSGS[] = {
};

const CanMsg TOYOTA_SECOC_TX_MSGS[] = {
TOYOTA_COMMON_TX_MSGS
TOYOTA_COMMON_SECOC_TX_MSGS
};

const CanMsg TOYOTA_LONG_TX_MSGS[] = {
Expand All @@ -81,9 +89,11 @@ const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
#define TOYOTA_COMMON_RX_CHECKS(lta) \
{.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \
{.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \
{.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \
{0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}}, \
{.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, \
{0x176, 0, 8, .check_checksum = true, .frequency = 32U}, { 0 }}}, \
{.msg = {{0x101, 0, 8, .check_checksum = false, .frequency = 50U}, \
{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \
{0x226, 0, 8, .check_checksum = false, .frequency = 40U}}}, \

RxCheck toyota_lka_rx_checks[] = {
TOYOTA_COMMON_RX_CHECKS(false)
Expand Down
5 changes: 5 additions & 0 deletions selfdrive/car/__init__.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
# functions common among cars
import logging
from collections import namedtuple
from dataclasses import dataclass
from enum import IntFlag, ReprEnum, EnumType
Expand All @@ -11,6 +12,10 @@
from openpilot.common.utils import Freezable
from openpilot.selfdrive.car.docs_definitions import CarDocs

# set up logging
carlog = logging.getLogger('carlog')
carlog.setLevel(logging.INFO)
carlog.propagate = False

# kg of standard extra cargo to count for drive, gas, etc...
STD_CARGO_KG = 136.
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/interfaces.py
Original file line number Diff line number Diff line change
Expand Up @@ -295,7 +295,7 @@ def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_
eps_firmware = str(next((fw.fwVersion for fw in car_fw if fw.ecu == "eps"), ""))
model = get_nn_model_path(candidate, eps_firmware)
if model is not None:
params.put_nonblocking("NNFFModelName", candidate.replace("_", " "))
params.put("NNFFModelName", candidate.replace("_", " "))

# Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
if not ret.notCar:
Expand Down
230 changes: 165 additions & 65 deletions selfdrive/car/toyota/carcontroller.py

Large diffs are not rendered by default.

82 changes: 45 additions & 37 deletions selfdrive/car/toyota/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, SECOC_CAR

SteerControlType = car.CarParams.SteerControlType

Expand Down Expand Up @@ -45,11 +45,15 @@ class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]
self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100.
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
self.cluster_min_speed = CV.KPH_TO_MS / 2.

if CP.flags & ToyotaFlags.SECOC.value:
self.shifter_values = can_define.dv["GEAR_PACKET_HYBRID"]["GEAR"]
else:
self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]

# On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
# the signal is zeroed to where the steering angle is at start.
# Need to apply an offset as soon as the steering angle measurements are both received
Expand All @@ -64,7 +68,8 @@ def __init__(self, CP):
self.low_speed_lockout = False
self.acc_type = 1
self.lkas_hud = {}
self.pcm_accel_net = 0.0
self.gvc = 0.0
self.secoc_synchronization = None

# FrogPilot variables
self.latActive_previous = False
Expand All @@ -76,21 +81,10 @@ def __init__(self, CP):
def update(self, cp, cp_cam, CC, frogpilot_toggles):
ret = car.CarState.new_message()
fp_ret = custom.FrogPilotCarState.new_message()
cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp

# Describes the acceleration request from the PCM if on flat ground, may be higher or lower if pitched
# CLUTCH->ACCEL_NET is only accurate for gas, PCM_CRUISE->ACCEL_NET is only accurate for brake
# These signals only have meaning when ACC is active
if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0)

# Sometimes ACC_BRAKING can be 1 while showing we're applying gas already
if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]:
self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0)

# add creeping force at low speeds only for braking, CLUTCH->ACCEL_NET already shows this
neutral_accel = max(cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] / self.CP.mass, 0.0)
if self.pcm_accel_net + neutral_accel < 0.0:
self.pcm_accel_net += neutral_accel
if not self.CP.flags & ToyotaFlags.SECOC.value:
self.gvc = cp.vl["VSC1S07"]["GVC"]

ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
Expand All @@ -100,12 +94,22 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles):
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1

if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
ret.gasPressed = ret.gas > 805
if self.CP.flags & ToyotaFlags.SECOC.value:
self.secoc_synchronization = copy.copy(cp.vl["SECOC_SYNCHRONIZATION"])
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"]
ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0
can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"])
else:
# TODO: find a common gas pedal percentage signal
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
ret.gasPressed = ret.gas > 805
else:
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
if self.CP.carFingerprint != CAR.TOYOTA_MIRAI:
ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]

ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
Expand Down Expand Up @@ -136,14 +140,10 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles):
ret.steeringAngleOffsetDeg = self.angle_offset.x
ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x

can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2

if self.CP.carFingerprint != CAR.TOYOTA_MIRAI:
ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]

ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale
# we could use the override bit from dbc, but it's triggered at too high torque values
Expand Down Expand Up @@ -174,8 +174,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles):
conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor

cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp

if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
if not (self.CP.flags & ToyotaFlags.SMART_DSU.value):
self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
Expand All @@ -199,9 +197,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles):
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0

if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)

if self.CP.enableBsm:
ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1)
ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1)
Expand Down Expand Up @@ -264,7 +259,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles):
@staticmethod
def get_can_parser(CP):
messages = [
("GEAR_PACKET", 1),
("LIGHT_STALK", 1),
("BLINKERS_STATE", 0.15),
("BODY_CONTROL_STATE", 3),
Expand All @@ -279,11 +273,20 @@ def get_can_parser(CP):
("STEER_TORQUE_SENSOR", 50),
]

if CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
messages.append(("CLUTCH", 15))
if CP.flags & ToyotaFlags.SECOC.value:
messages += [
("GEAR_PACKET_HYBRID", 60),
("SECOC_SYNCHRONIZATION", 10),
("GAS_PEDAL", 42),
]
else:
messages.append(("VSC1S07", 20))
if CP.carFingerprint not in [CAR.TOYOTA_MIRAI]:
messages.append(("ENGINE_RPM", 42))

if CP.carFingerprint != CAR.TOYOTA_MIRAI:
messages.append(("ENGINE_RPM", 42))
messages += [
("GEAR_PACKET", 1),
]

if CP.carFingerprint in UNSUPPORTED_DSU_CAR:
messages.append(("DSU_CRUISE", 5))
Expand Down Expand Up @@ -338,9 +341,14 @@ def get_cam_can_parser(CP):

if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
messages += [
("PRE_COLLISION", 33),
("ACC_CONTROL", 33),
("PCS_HUD", 1),
]

# TODO: Figure out new layout of the PRE_COLLISION message
if not CP.flags & ToyotaFlags.SECOC.value:
messages += [
("PRE_COLLISION", 33),
]

return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
8 changes: 8 additions & 0 deletions selfdrive/car/toyota/fingerprints.py
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,7 @@
b'\x018966306Q6000\x00\x00\x00\x00',
b'\x018966333N1100\x00\x00\x00\x00',
b'\x018966333N4300\x00\x00\x00\x00',
b'\x018966333P3000\x00\x00\x00\x00',
b'\x018966333P3100\x00\x00\x00\x00',
b'\x018966333P3200\x00\x00\x00\x00',
b'\x018966333P4200\x00\x00\x00\x00',
Expand Down Expand Up @@ -160,6 +161,7 @@
b'8821F0605200 ',
b'8821F0606200 ',
b'8821F0607200 ',
b'8821F0607300 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0608300 ',
Expand Down Expand Up @@ -192,6 +194,7 @@
b'8965B33581\x00\x00\x00\x00\x00\x00',
b'8965B33611\x00\x00\x00\x00\x00\x00',
b'8965B33621\x00\x00\x00\x00\x00\x00',
b'8965B33630\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F0601200 ',
Expand All @@ -207,6 +210,7 @@
b'8821F0605200 ',
b'8821F0606200 ',
b'8821F0607200 ',
b'8821F0607300 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0608300 ',
Expand Down Expand Up @@ -754,6 +758,7 @@
b'\x018966353Q2300\x00\x00\x00\x00',
b'\x018966353Q4000\x00\x00\x00\x00',
b'\x018966353R1100\x00\x00\x00\x00',
b'\x018966353R7000\x00\x00\x00\x00',
b'\x018966353R7100\x00\x00\x00\x00',
b'\x018966353R8000\x00\x00\x00\x00',
b'\x018966353R8100\x00\x00\x00\x00',
Expand Down Expand Up @@ -1353,6 +1358,7 @@
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0610000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
b'\x028646F0610100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
Expand Down Expand Up @@ -1550,6 +1556,7 @@
b'\x01896630E36100\x00\x00\x00\x00',
b'\x01896630E36200\x00\x00\x00\x00',
b'\x01896630E36300\x00\x00\x00\x00',
b'\x01896630E36600\x00\x00\x00\x00',
b'\x01896630E37100\x00\x00\x00\x00',
b'\x01896630E37200\x00\x00\x00\x00',
b'\x01896630E37300\x00\x00\x00\x00',
Expand Down Expand Up @@ -1656,6 +1663,7 @@
b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x02348Y3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0234D15000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
Expand Down
33 changes: 20 additions & 13 deletions selfdrive/car/toyota/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp
if DBC[candidate]["pt"] == "toyota_new_mc_pt_generated":
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE

if ret.flags & ToyotaFlags.SECOC.value:
ret.secOcRequired = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_SECOC

if candidate in ANGLE_CONTROL_CAR:
ret.steerControlType = SteerControlType.angle
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
Expand Down Expand Up @@ -57,7 +61,11 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp
ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \
and not (ret.flags & ToyotaFlags.SMART_DSU)

if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus:
if Ecu.hybrid in found_ecus:
ret.flags |= ToyotaFlags.HYBRID.value

# TODO: expand to the rest of the cars
if candidate in (CAR.LEXUS_ES_TSS2,) and not (ret.flags & ToyotaFlags.HYBRID.value):
ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value

if candidate == CAR.TOYOTA_PRIUS:
Expand All @@ -73,7 +81,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2):
ret.wheelSpeedFactor = 1.035

elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023):
elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023, CAR.TOYOTA_RAV4_PRIME, CAR.TOYOTA_SIENNA_4TH_GEN):
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP = [0.0]
ret.lateralTuning.pid.kpBP = [0.0]
Expand Down Expand Up @@ -121,8 +129,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp
# - TSS2 radar ACC cars w/ smartDSU installed
# - TSS2 radar ACC cars w/o smartDSU installed (disables radar)
# - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet)
ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)

if ret.flags & ToyotaFlags.SECOC.value:
ret.openpilotLongitudinalControl = False
else:
ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
ret.openpilotLongitudinalControl &= not disable_openpilot_long

ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
ret.enableGasInterceptor = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl

Expand All @@ -136,27 +149,21 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp
# to a negative value, so it won't matter.
ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED

tune = ret.longitudinalTuning
if ret.enableGasInterceptor:
tune = ret.longitudinalTuning
tune.kiBP = [0., 5., 20.]
tune.kiV = [1.3, 1.0, 0.7]
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
elif candidate in TSS2_CAR:
tune.kpV = [0.0]
tune.kiV = [0.5]
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly

# Since we compensate for imprecise acceleration in carcontroller, we can be less aggressive with tuning
# This also prevents unnecessary request windup due to internal car jerk limits
if ret.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
tune.kiV = [0.25]
else:
tune.kiBP = [0., 5., 35.]
tune.kiV = [3.6, 2.4, 1.5]
# Hybrids have much quicker longitudinal actuator response
if ret.flags & ToyotaFlags.HYBRID.value:
ret.longitudinalActuatorDelay = 0.05

if params.get_bool("FrogsGoMoosTweak"):
ret.stoppingDecelRate = 0.1 # reach stopping target smoothly
Expand Down
Loading

0 comments on commit e08da67

Please sign in to comment.