forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Release Commit @ 0798119
This commit is contained in:
IQ.Lvbs history cleanup
139
iqdbc_repo/iqdbc/car/byd/carstate.py
Normal file
139
iqdbc_repo/iqdbc/car/byd/carstate.py
Normal file
@@ -0,0 +1,139 @@
|
||||
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),
|
||||
}
|
||||
Reference in New Issue
Block a user