mouxangithub
2270c6d7f1
新增 `DistractionDetectionLevel` 参数以控制驾驶员分心检测的灵敏度等级，并在 `dmonitoringd.py` 和 `helpers.py` 中实现不同等级对应的时间阈值配置。同时更新了相关逻辑以支持动态调整该参数。 fix(toyota): 支持 Toyota Wildlander PHEV 车型接入与控制 增加对 Toyota Wildlander PHEV 的指纹识别、车辆规格定义及接口适配，确保其在 TSS2 平台下的正常运行，并修正部分雷达ACC判断条件。 feat(ui): 优化 Dragonpilot 设置界面选项显示语言一致性 将 Dragonpilot 设置页面中的多个下拉选项文本进行国际化处理，统一使用翻译函数包裹，提升多语言兼容性。 chore(config): 更新 launch 脚本 API 地址并切换 shell 解释器 修改 `launch_openpilot.sh` 使用 `/usr/bin/bash` 作为解释器，并设置自定义 API 与 Athena 服务地址。 refactor(key): 实现 ECU 秘钥提取脚本并写入参数存储 创建 `key.py` 脚本用于通过 UDS 协议从 ECU 提取 SecOC 密钥，并将其保存至系统参数中供后续使用。 docs(vscode): 移除不再使用的终端配置项 清理 `.vscode/settings.json` 文件中过时的 terminal 配置内容。 feat(fonts): 新增中文字体资源文件 添加 `china.ttf` 字体文件以增强 UI 在中文环境下的渲染效果。 build(payload): 添加二进制负载文件 引入新的二进制 payload 文件用于辅助密钥提取流程。
246 lines
12 KiB
Python
246 lines
12 KiB
Python
import copy
|
|
|
|
from opendbc.can import CANDefine, CANParser
|
|
from opendbc.car import Bus, DT_CTRL, create_button_events, structs
|
|
from opendbc.car.common.conversions import Conversions as CV
|
|
from opendbc.car.common.filter_simple import FirstOrderFilter
|
|
from opendbc.car.interfaces import CarStateBase
|
|
from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
|
|
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, \
|
|
SECOC_CAR
|
|
|
|
ButtonType = structs.CarState.ButtonEvent.Type
|
|
SteerControlType = structs.CarParams.SteerControlType
|
|
|
|
# These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
|
|
# - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds
|
|
# - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3.
|
|
# if using the other control command, goes directly to 3 after 1.5 seconds
|
|
# - initializing: LTA can report 0 as long as STEER_TORQUE_SENSOR->STEER_ANGLE_INITIALIZING is 1,
|
|
# and is a catch-all for LKA
|
|
TEMP_STEER_FAULTS = (0, 9, 11, 21, 25)
|
|
# - lka/lta msg drop out: 3 (recoverable)
|
|
# - prolonged high driver torque: 17 (permanent)
|
|
PERM_STEER_FAULTS = (3, 17)
|
|
|
|
|
|
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.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
|
|
self.accurate_steer_angle_seen = False
|
|
self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False)
|
|
|
|
self.lkas_button = 0
|
|
self.distance_button = 0
|
|
|
|
self.pcm_follow_distance = 0
|
|
|
|
self.acc_type = 1
|
|
self.lkas_hud = {}
|
|
self.gvc = 0.0
|
|
self.secoc_synchronization = None
|
|
|
|
# radar filter (mainly for CHR/Camry)
|
|
# the idea is to place a Panda in between Radar and camera/body (engine room) to block 0x343 (longitudinal)
|
|
# depends on the firmware, we should be able to read most CAN directly from cp (not cp_cam, its empty)
|
|
self.dp_radar_filter = bool(self.CP.flags & ToyotaFlags.RADAR_FILTER.value)
|
|
|
|
# rick - dsu_bypass from cydia2020: https://github.com/cydia2020/toyota-dsu-reroute-harness/
|
|
# the idea is to "re-route" the DSU to Panda CAN2 (Which connects to ADAS Camera)
|
|
# * when comma device is not available, the DSU message can still communicate with ADAS Camera, and over to car.
|
|
# * when comma device is active, CAN message of DSU and ADAS camera will the be blocked by Panda, only forward some CAN messages over to car (from CAN0).
|
|
self.dp_dsu_bypass = self.CP.flags & ToyotaFlags.DSU_BYPASS.value
|
|
|
|
from opendbc.car.toyota.zss import ZSS
|
|
self.zss = ZSS(CP.flags)
|
|
|
|
def update(self, can_parsers) -> structs.CarState:
|
|
cp = can_parsers[Bus.pt]
|
|
cp_cam = can_parsers[Bus.cam]
|
|
|
|
ret = structs.CarState()
|
|
if self.dp_dsu_bypass:
|
|
cp_acc = cp_cam
|
|
else:
|
|
cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp
|
|
|
|
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"]])
|
|
ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
|
|
ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1
|
|
|
|
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
|
|
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
|
|
|
|
if self.CP.flags & ToyotaFlags.SECOC.value:
|
|
self.secoc_synchronization = copy.copy(cp.vl["SECOC_SYNCHRONIZATION"])
|
|
ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0
|
|
can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"])
|
|
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) or self.dp_radar_filter:
|
|
ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
|
|
|
|
self.parse_wheel_speeds(ret,
|
|
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
|
|
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
|
|
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
|
|
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
|
|
)
|
|
ret.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars
|
|
|
|
ret.standstill = abs(ret.vEgoRaw) < 1e-3
|
|
|
|
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
|
|
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
|
|
torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
|
|
|
|
if self.zss.enabled:
|
|
self.zss.set_values(can_parsers[Bus.zss])
|
|
if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
|
|
main_on = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
|
|
else:
|
|
main_on = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
|
|
ret.steeringAngleDeg = self.zss.get_steering_angle_deg(main_on, cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"], ret.steeringAngleDeg)
|
|
|
|
# On some cars, the angle measurement is non-zero while initializing
|
|
if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]):
|
|
self.accurate_steer_angle_seen = True
|
|
|
|
if self.accurate_steer_angle_seen:
|
|
# Offset seems to be invalid for large steering angles and high angle rates
|
|
if abs(ret.steeringAngleDeg) < 90 and abs(ret.steeringRateDeg) < 100 and cp.can_valid:
|
|
self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg)
|
|
|
|
if self.angle_offset.initialized:
|
|
ret.steeringAngleOffsetDeg = self.angle_offset.x
|
|
ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x
|
|
|
|
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
|
|
|
|
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
|
|
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
|
|
|
# Check EPS LKA/LTA fault status
|
|
ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in TEMP_STEER_FAULTS
|
|
ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in PERM_STEER_FAULTS
|
|
|
|
if self.CP.steerControlType == SteerControlType.angle:
|
|
ret.steerFaultTemporary = ret.steerFaultTemporary or cp.vl["EPS_STATUS"]["LTA_STATE"] in TEMP_STEER_FAULTS
|
|
ret.steerFaultPermanent = ret.steerFaultPermanent or cp.vl["EPS_STATUS"]["LTA_STATE"] in PERM_STEER_FAULTS
|
|
|
|
# Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until
|
|
# the more accurate angle sensor signal is initialized
|
|
ret.vehicleSensorsInvalid = not self.accurate_steer_angle_seen
|
|
|
|
if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
|
|
# TODO: find the bit likely in DSU_CRUISE that describes an ACC fault. one may also exist in CLUTCH
|
|
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
|
|
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS
|
|
cluster_set_speed = cp.vl["PCM_CRUISE_ALT"]["UI_SET_SPEED"]
|
|
else:
|
|
ret.accFaulted = cp.vl["PCM_CRUISE_2"]["ACC_FAULTED"] != 0
|
|
ret.carFaultedNonCritical = cp.vl["PCM_CRUISE_SM"]["TEMP_ACC_FAULTED"] != 0
|
|
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
|
|
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS
|
|
cluster_set_speed = cp.vl["PCM_CRUISE_SM"]["UI_SET_SPEED"]
|
|
|
|
# UI_SET_SPEED is always non-zero when main is on, hide until first enable
|
|
is_metric = cp.vl["BODY_CONTROL_STATE_2"]["UNITS"] in (1, 2)
|
|
if ret.cruiseState.speed != 0:
|
|
conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
|
|
ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor
|
|
|
|
if (self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value) or \
|
|
self.dp_dsu_bypass:
|
|
if not (self.CP.flags & ToyotaFlags.SDSU.value):
|
|
self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
|
|
ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"])
|
|
|
|
# some TSS2 cars have low speed lockout permanently set, so ignore on those cars
|
|
# these cars are identified by an ACC_TYPE value of 2.
|
|
# TODO: it is possible to avoid the lockout and gain stop and go if you
|
|
# send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
|
|
if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR) or \
|
|
(self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
|
|
if self.CP.openpilotLongitudinalControl:
|
|
ret.accFaulted = ret.accFaulted or cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
|
|
|
|
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
|
|
if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR):
|
|
# ignore standstill state in certain vehicles, since pcm allows to restart with just an acceleration request
|
|
ret.cruiseState.standstill = self.pcm_acc_status == 7
|
|
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
|
|
ret.cruiseState.nonAdaptive = self.pcm_acc_status in (1, 2, 3, 4, 5, 6)
|
|
|
|
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
|
|
ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0
|
|
|
|
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)
|
|
|
|
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
|
|
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
|
|
|
|
if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR:
|
|
self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
|
|
|
|
buttonEvents = []
|
|
prev_distance_button = self.distance_button
|
|
if self.CP.carFingerprint in TSS2_CAR:
|
|
# lkas button is wired to the camera
|
|
prev_lkas_button = self.lkas_button
|
|
self.lkas_button = cp_cam.vl["LKAS_HUD"]["LDA_ON_MESSAGE"]
|
|
|
|
# Cycles between 1 and 2 when pressing the button, then rests back at 0 after ~3s
|
|
if self.lkas_button != 0 and self.lkas_button != prev_lkas_button:
|
|
buttonEvents.extend(create_button_events(1, 0, {1: ButtonType.lkas}) +
|
|
create_button_events(0, 1, {1: ButtonType.lkas}))
|
|
|
|
if self.CP.carFingerprint not in RADAR_ACC_CAR:
|
|
# distance button is wired to the ACC module (camera or radar)
|
|
self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"]
|
|
|
|
buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
|
|
|
|
ret.buttonEvents = buttonEvents
|
|
return ret
|
|
|
|
@staticmethod
|
|
def get_can_parsers(CP):
|
|
pt_messages = [
|
|
("BLINKERS_STATE", float('nan')),
|
|
]
|
|
|
|
parsers = {
|
|
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
|
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
|
|
}
|
|
|
|
if CP.flags & ToyotaFlags.ZSS:
|
|
parsers[Bus.zss] = CANParser("toyota_zss", [("SECONDARY_STEER_ANGLE", float('nan'))], 0)
|
|
|
|
return parsers
|