forked from IQ.Lvbs/IQ.Pilot
140 lines
6.0 KiB
Python
140 lines
6.0 KiB
Python
import copy
|
|
|
|
from iqdbc.car import Bus, structs
|
|
from iqdbc.can.parser import CANParser
|
|
from iqdbc.car.common.conversions import Conversions as CV
|
|
from iqdbc.car.byd.values import DBC, CarControllerParams as CCP
|
|
from iqdbc.car.interfaces import CarStateBase
|
|
|
|
GearShifter = structs.CarState.GearShifter
|
|
|
|
GEAR_MAP = {
|
|
1: GearShifter.park,
|
|
2: GearShifter.reverse,
|
|
3: GearShifter.neutral,
|
|
4: GearShifter.drive,
|
|
}
|
|
|
|
# STEERING_TORQUE low nibble, rebuilt from LKS_PREPARED + CRUISE_ACTIVATED
|
|
EPS_STATE_OFF = 8
|
|
EPS_STATE_PREPARED = 9
|
|
EPS_STATE_ACTUATING = 10
|
|
EPS_STATE_LATCHED_FAULT = 11
|
|
|
|
# ACC_HUD_ADAS.CRUISE_STATE
|
|
CRUISE_STATE_AVAILABLE = 1
|
|
CRUISE_STATE_ENGAGED = 2
|
|
|
|
|
|
class CarState(CarStateBase):
|
|
def __init__(self, CP, CP_IQ):
|
|
super().__init__(CP, CP_IQ)
|
|
self.lkas_hud = {}
|
|
self.acc_cmd = {}
|
|
self.buttons = {}
|
|
self.eps_state = EPS_STATE_OFF
|
|
self.override_latched = False
|
|
self.lkas_btn_prev = False
|
|
|
|
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
|
cp = can_parsers[Bus.pt]
|
|
cp_cam = can_parsers[Bus.cam]
|
|
ret = structs.CarState()
|
|
ret_iq = structs.IQCarState()
|
|
|
|
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["FL"] * CV.KPH_TO_MS
|
|
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["FR"] * CV.KPH_TO_MS
|
|
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["RL"] * CV.KPH_TO_MS
|
|
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["RR"] * CV.KPH_TO_MS
|
|
self.parse_wheel_speeds(ret,
|
|
cp.vl["WHEEL_SPEEDS"]["FL"],
|
|
cp.vl["WHEEL_SPEEDS"]["FR"],
|
|
cp.vl["WHEEL_SPEEDS"]["RL"],
|
|
cp.vl["WHEEL_SPEEDS"]["RR"],
|
|
)
|
|
ret.standstill = ret.vEgoRaw < 0.01
|
|
ret.vEgoCluster = ret.vEgo
|
|
|
|
# both torques come from STEERING_TORQUE; 0x11F's 16|8 field is unsigned and reads as a
|
|
# steering rate in BYD's own firmware, so it is not used for override detection
|
|
ret.steeringAngleDeg = cp.vl["STEER_MODULE_2"]["STEER_ANGLE_2"]
|
|
ret.steeringTorque = cp.vl["STEERING_TORQUE"]["DRIVER_TORQUE"]
|
|
ret.steeringTorqueEps = cp.vl["STEERING_TORQUE"]["MAIN_TORQUE"]
|
|
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CCP.STEER_DRIVER_OVERRIDE, 5)
|
|
# Disengagement on override is handled by the latch below, which also decides when
|
|
# re-engagement is allowed, so no separate hard-disengage threshold here.
|
|
ret.steeringDisengage = False
|
|
|
|
# state 11 is a latched dropout: the command stream stopped while the EPS was actuating.
|
|
# It clears only on a STEER_REQ rising edge over a continuous stream.
|
|
lks_prepared = bool(cp.vl["STEERING_TORQUE"]["LKS_PREPARED"])
|
|
cruise_activated = bool(cp.vl["STEERING_TORQUE"]["CRUISE_ACTIVATED"])
|
|
self.eps_state = EPS_STATE_OFF + int(lks_prepared) + 2 * int(cruise_activated)
|
|
|
|
ret.steerFaultTemporary = self.eps_state == EPS_STATE_LATCHED_FAULT
|
|
ret.steerFaultTemporary |= int(cp_cam.vl["LKAS_HUD_ADAS"]["LKAS_STATE"]) == 4
|
|
ret.steerFaultPermanent = bool(cp.vl["STEERING_TORQUE"]["TORQUE_FAILED"])
|
|
|
|
# DRIVE_STATE.RAW_THROTTLE is powertrain torque demand, not the pedal
|
|
ret.gasPressed = cp.vl["PEDAL"]["GAS_PEDAL"] > 0.10
|
|
ret.brake = cp.vl["PEDAL"]["BRAKE_PEDAL"]
|
|
# must stay the same bit byd_rx_hook reads, or the two engage latches desync on a light
|
|
# brake graze and controlsd raises "Controls Mismatch"
|
|
ret.brakePressed = bool(cp.vl["DRIVE_STATE"]["BRAKE_PRESSED"])
|
|
|
|
ret.gearShifter = GEAR_MAP.get(int(cp.vl["DRIVE_STATE"]["GEAR"]), GearShifter.unknown)
|
|
|
|
ret.leftBlinker = bool(cp.vl["STALKS"]["LEFT_BLINKER"])
|
|
ret.rightBlinker = bool(cp.vl["STALKS"]["RIGHT_BLINKER"])
|
|
|
|
ret.leftBlindspot = cp.vl["BSD_RADAR"]["LEFT_APPROACH"] != 0
|
|
ret.rightBlindspot = cp.vl["BSD_RADAR"]["RIGHT_APPROACH"] != 0
|
|
|
|
ret.doorOpen = any((
|
|
cp.vl["METER_CLUSTER"]["FRONT_LEFT_DOOR"],
|
|
cp.vl["METER_CLUSTER"]["FRONT_RIGHT_DOOR"],
|
|
cp.vl["METER_CLUSTER"]["BACK_LEFT_DOOR"],
|
|
cp.vl["METER_CLUSTER"]["BACK_RIGHT_DOOR"],
|
|
))
|
|
ret.seatbeltUnlatched = not bool(cp.vl["METER_CLUSTER"]["SEATBELT_DRIVER"])
|
|
|
|
# The ADAS/ACC ECU is on the chassis bus, not behind the camera relay, so these come off
|
|
# bus 0. Bus 2 carries only the camera's own frames (0x1E2, 0x316, ...). This differs from
|
|
# the Atto 3, where PR #3337 reads both from the camera bus.
|
|
# CRUISE_STATE: 0=off, 1=available, 2=engaged, 3=engaged and commanding accel.
|
|
# Do NOT use PR #3337/#3352's ACC_STATE (19|3) - byte 2 is a constant 0x3c on this car, so
|
|
# it reads 7 (ERROR) forever and engagement can never happen.
|
|
ret.cruiseState.speed = cp.vl["ACC_HUD_ADAS"]["SET_SPEED"] * CV.KPH_TO_MS
|
|
cruise_state = int(cp.vl["ACC_HUD_ADAS"]["CRUISE_STATE"])
|
|
ret.cruiseState.available = cruise_state >= CRUISE_STATE_AVAILABLE
|
|
|
|
# A steering override fully disengages and stays disengaged. Re-arming is deliberate:
|
|
# either cycle stock cruise, or press the LKAS/ICC button. Suppressing cruiseState.enabled
|
|
# is what holds it off, and clearing the latch gives the rising edge that re-engages.
|
|
lkas_btn = bool(cp.vl["PCM_BUTTONS"]["LKAS_ON_BTN"])
|
|
lkas_rising = lkas_btn and not self.lkas_btn_prev
|
|
self.lkas_btn_prev = lkas_btn
|
|
|
|
# NOTE: latch on the instantaneous torque, not the debounced steeringPressed. byd_rx_hook
|
|
# latches on the same raw sample, and any skew between the two shows up as controlsMismatch.
|
|
if cruise_state < CRUISE_STATE_ENGAGED or lkas_rising:
|
|
self.override_latched = False
|
|
elif abs(ret.steeringTorque) > CCP.STEER_DRIVER_OVERRIDE:
|
|
self.override_latched = True
|
|
|
|
ret.cruiseState.enabled = cruise_state >= CRUISE_STATE_ENGAGED and not self.override_latched
|
|
ret.cruiseState.standstill = bool(cp.vl["ACC_CMD"]["STANDSTILL_STATE"])
|
|
|
|
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD_ADAS"])
|
|
self.acc_cmd = copy.copy(cp.vl["ACC_CMD"])
|
|
self.buttons = copy.copy(cp.vl["PCM_BUTTONS"])
|
|
|
|
return ret, ret_iq
|
|
|
|
@staticmethod
|
|
def get_can_parsers(CP, CP_IQ):
|
|
return {
|
|
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0),
|
|
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
|
|
}
|