158 lines
7.9 KiB
Python
158 lines
7.9 KiB
Python
import copy
|
|
from iqdbc.can import CANDefine, CANParser
|
|
from iqdbc.car import Bus, structs
|
|
from iqdbc.car.common.conversions import Conversions as CV
|
|
from iqdbc.car.interfaces import CarStateBase
|
|
from iqdbc.car.subaru.values import DBC, CanBus, SubaruFlags
|
|
from iqdbc.car import CanSignalRateCalculator
|
|
|
|
from iqdbc.lvbs.car.subaru.aol import AolCarState
|
|
from iqdbc.lvbs.car.subaru.creep_assist import IQStopAndGoState
|
|
|
|
|
|
class CarState(CarStateBase, AolCarState, IQStopAndGoState):
|
|
def __init__(self, CP, CP_IQ):
|
|
CarStateBase.__init__(self, CP, CP_IQ)
|
|
AolCarState.__init__(self, CP, CP_IQ)
|
|
IQStopAndGoState.__init__(self, CP, CP_IQ)
|
|
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
|
self.shifter_values = can_define.dv["Transmission"]["Gear"]
|
|
|
|
self.angle_rate_calulator = CanSignalRateCalculator(50)
|
|
|
|
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
|
cp = can_parsers[Bus.pt]
|
|
cp_cam = can_parsers[Bus.cam]
|
|
cp_alt = can_parsers[Bus.alt]
|
|
ret = structs.CarState()
|
|
ret_iq = structs.IQCarState()
|
|
|
|
throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_alt.vl["Throttle_Hybrid"]
|
|
ret.gasPressed = throttle_msg["Throttle_Pedal"] > 1e-5
|
|
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
|
ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 0
|
|
else:
|
|
cp_brakes = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
|
ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1
|
|
|
|
cp_es_distance = cp_alt if self.CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID) else cp_cam
|
|
if not (self.CP.flags & SubaruFlags.HYBRID):
|
|
eyesight_fault = bool(cp_es_distance.vl["ES_Distance"]["Cruise_Fault"])
|
|
|
|
# if openpilot is controlling long, an eyesight fault is a non-critical fault. otherwise it's an ACC fault
|
|
if self.CP.openpilotLongitudinalControl:
|
|
ret.carFaultedNonCritical = eyesight_fault
|
|
else:
|
|
ret.accFaulted = eyesight_fault
|
|
|
|
cp_wheels = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
|
self.parse_wheel_speeds(ret,
|
|
cp_wheels.vl["Wheel_Speeds"]["FL"],
|
|
cp_wheels.vl["Wheel_Speeds"]["FR"],
|
|
cp_wheels.vl["Wheel_Speeds"]["RL"],
|
|
cp_wheels.vl["Wheel_Speeds"]["RR"],
|
|
)
|
|
ret.standstill = ret.vEgoRaw == 0
|
|
|
|
# continuous blinker signals for assisted lane change
|
|
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["Dashlights"]["LEFT_BLINKER"],
|
|
cp.vl["Dashlights"]["RIGHT_BLINKER"])
|
|
|
|
if self.CP.enableBsm:
|
|
ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
|
|
ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1)
|
|
|
|
cp_transmission = cp_alt if self.CP.flags & SubaruFlags.HYBRID else cp
|
|
can_gear = int(cp_transmission.vl["Transmission"]["Gear"])
|
|
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
|
|
|
if not (self.CP.flags & SubaruFlags.LKAS_ANGLE):
|
|
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
|
|
steering_updated = len(cp.vl_all["Steering_Torque"]["Steering_Angle"]) > 0
|
|
else:
|
|
# Steering_Torque->Steering_Angle exists on SUBARU_FORESTER_2022, SUBARU_OUTBACK_2023, SUBARU_ASCENT_2023 where
|
|
# it is identical to Steering_2's signal. However, it is always zero on newer LKAS_ANGLE cars
|
|
# such as 2024+ Crosstrek, 2023+ Ascent, etc. Use a universal signal for LKAS_ANGLE cars.
|
|
ret.steeringAngleDeg = cp.vl["Steering_2"]["Steering_Angle"]
|
|
steering_updated = len(cp.vl_all["Steering_2"]["Steering_Angle"]) > 0
|
|
|
|
if not (self.CP.flags & SubaruFlags.PREGLOBAL):
|
|
# ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
|
|
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, steering_updated)
|
|
|
|
ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"]
|
|
ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"]
|
|
|
|
steer_threshold = 75 if self.CP.flags & SubaruFlags.PREGLOBAL else 80
|
|
ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
|
|
|
|
cp_cruise = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
|
cp_es_brake = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam
|
|
|
|
if self.CP.flags & (SubaruFlags.HYBRID | SubaruFlags.LKAS_ANGLE):
|
|
# ES_DashStatus->Cruise_Activated_Dash is likely intended for the dash display only, as it falls
|
|
# during user gas override and at standstill. ES_Status is missing on hybrid, so we use ES_Brake instead
|
|
|
|
# TODO: ES_Brake->Cruise_Activated has been seen staying high when Crosstrek 2025 angle LKAS user pressed
|
|
# brake while engaged at a stop. ES_Status and ES_DashStatus->Signal7 correctly fell, but is either missing or
|
|
# always zero on hybrids. Probably need to split angle & hybrid. 0x27 and 0x225 on hybrids may work for them.
|
|
ret.cruiseState.enabled = cp_es_brake.vl["ES_Brake"]['Cruise_Activated'] != 0
|
|
ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
|
|
else:
|
|
ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
|
|
ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
|
|
ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS
|
|
|
|
if (self.CP.flags & SubaruFlags.PREGLOBAL and cp.vl["Dash_State2"]["UNITS"] == 1) or \
|
|
(not (self.CP.flags & SubaruFlags.PREGLOBAL) and cp.vl["Dashlights"]["UNITS"] == 1):
|
|
ret.cruiseState.speed *= CV.MPH_TO_KPH
|
|
|
|
ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1
|
|
ret.doorOpen = any([cp.vl["BodyInfo"]["DOOR_OPEN_RR"],
|
|
cp.vl["BodyInfo"]["DOOR_OPEN_RL"],
|
|
cp.vl["BodyInfo"]["DOOR_OPEN_FR"],
|
|
cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
|
|
ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
|
|
|
|
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
|
self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
|
|
self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
|
|
else:
|
|
ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
|
|
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
|
|
ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
|
|
ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \
|
|
(cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2)
|
|
|
|
self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
|
|
self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
|
|
|
|
# TODO: Hybrid cars don't have ES_Distance, need a replacement
|
|
if not (self.CP.flags & SubaruFlags.HYBRID):
|
|
# 8 is known AEB, there are a few other values related to AEB we ignore
|
|
ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
|
|
(cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
|
|
|
|
self.es_status_msg = copy.copy(cp_es_brake.vl["ES_Status"])
|
|
self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
|
|
|
|
if not (self.CP.flags & SubaruFlags.HYBRID):
|
|
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
|
|
|
|
self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
|
|
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
|
|
self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
|
|
|
|
AolCarState.update_aol(self, ret, can_parsers)
|
|
IQStopAndGoState.update(self, ret, can_parsers)
|
|
|
|
return ret, ret_iq
|
|
|
|
@staticmethod
|
|
def get_can_parsers(CP, CP_IQ):
|
|
return {
|
|
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main),
|
|
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.camera),
|
|
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt)
|
|
}
|