318 lines
11 KiB
Python
318 lines
11 KiB
Python
import copy
|
|
from cereal import car
|
|
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_HYBRID
|
|
from selfdrive.car.interfaces import CarStateBase
|
|
from opendbc.can.parser import CANParser
|
|
from selfdrive.config import Conversions as CV
|
|
|
|
GearShifter = car.CarState.GearShifter
|
|
|
|
|
|
class CarState(CarStateBase):
|
|
def update(self, cp, cp_cam):
|
|
ret = car.CarState.new_message()
|
|
|
|
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
|
|
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
|
|
|
|
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
|
|
|
|
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]["WHL_SPD_FL"] * CV.KPH_TO_MS
|
|
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]["WHL_SPD_FR"] * CV.KPH_TO_MS
|
|
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]["WHL_SPD_RL"] * CV.KPH_TO_MS
|
|
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]["WHL_SPD_RR"] * CV.KPH_TO_MS
|
|
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
|
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
|
|
|
ret.standstill = ret.vEgoRaw < 0.1
|
|
|
|
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
|
|
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
|
|
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
|
|
ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"],
|
|
cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
|
|
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
|
|
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
|
|
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
|
ret.steerWarning = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
|
|
|
|
# cruise state
|
|
if self.CP.openpilotLongitudinalControl:
|
|
ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
|
|
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
|
|
ret.cruiseState.standstill = cp.vl["TCS13"]["StandStill"] == 1
|
|
else:
|
|
ret.cruiseState.available = cp.vl["SCC11"]["MainMode_ACC"] == 1
|
|
ret.cruiseState.enabled = cp.vl["SCC12"]["ACCMode"] != 0
|
|
ret.cruiseState.standstill = cp.vl["SCC11"]["SCCInfoDisplay"] == 4.
|
|
|
|
if ret.cruiseState.enabled:
|
|
speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS
|
|
ret.cruiseState.speed = cp.vl["SCC11"]["VSetDis"] * speed_conv
|
|
else:
|
|
ret.cruiseState.speed = 0
|
|
|
|
# TODO: Find brake pressure
|
|
ret.brake = 0
|
|
ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
|
|
|
|
if self.CP.carFingerprint in EV_HYBRID:
|
|
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 256.
|
|
ret.gasPressed = ret.gas > 0
|
|
else:
|
|
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100
|
|
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
|
|
|
|
# TODO: refactor gear parsing in function
|
|
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
|
|
# as this seems to be standard over all cars, but is not the preferred method.
|
|
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
|
|
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
|
|
ret.gearShifter = GearShifter.drive
|
|
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
|
|
ret.gearShifter = GearShifter.neutral
|
|
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
|
|
ret.gearShifter = GearShifter.park
|
|
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
|
|
ret.gearShifter = GearShifter.reverse
|
|
else:
|
|
ret.gearShifter = GearShifter.unknown
|
|
# Gear Selecton via TCU12
|
|
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
|
|
gear = cp.vl["TCU12"]["CUR_GR"]
|
|
if gear == 0:
|
|
ret.gearShifter = GearShifter.park
|
|
elif gear == 14:
|
|
ret.gearShifter = GearShifter.reverse
|
|
elif gear > 0 and gear < 9: # unaware of anything over 8 currently
|
|
ret.gearShifter = GearShifter.drive
|
|
else:
|
|
ret.gearShifter = GearShifter.unknown
|
|
# Gear Selecton - This is only compatible with optima hybrid 2017
|
|
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
|
|
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
|
|
if gear in (5, 8): # 5: D, 8: sport mode
|
|
ret.gearShifter = GearShifter.drive
|
|
elif gear == 6:
|
|
ret.gearShifter = GearShifter.neutral
|
|
elif gear == 0:
|
|
ret.gearShifter = GearShifter.park
|
|
elif gear == 7:
|
|
ret.gearShifter = GearShifter.reverse
|
|
else:
|
|
ret.gearShifter = GearShifter.unknown
|
|
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
|
|
else:
|
|
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
|
|
if gear in (5, 8): # 5: D, 8: sport mode
|
|
ret.gearShifter = GearShifter.drive
|
|
elif gear == 6:
|
|
ret.gearShifter = GearShifter.neutral
|
|
elif gear == 0:
|
|
ret.gearShifter = GearShifter.park
|
|
elif gear == 7:
|
|
ret.gearShifter = GearShifter.reverse
|
|
else:
|
|
ret.gearShifter = GearShifter.unknown
|
|
|
|
if self.CP.carFingerprint in FEATURES["use_fca"]:
|
|
ret.stockAeb = cp.vl["FCA11"]["FCA_CmdAct"] != 0
|
|
ret.stockFcw = cp.vl["FCA11"]["CF_VSM_Warn"] == 2
|
|
else:
|
|
ret.stockAeb = cp.vl["SCC12"]["AEB_CmdAct"] != 0
|
|
ret.stockFcw = cp.vl["SCC12"]["CF_VSM_Warn"] == 2
|
|
|
|
if self.CP.enableBsm:
|
|
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
|
|
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
|
|
|
|
# save the entire LKAS11 and CLU11
|
|
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
|
|
self.clu11 = copy.copy(cp.vl["CLU11"])
|
|
self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
|
|
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
|
|
self.lead_distance = cp.vl["SCC11"]["ACC_ObjDist"]
|
|
self.brake_hold = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
|
|
self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
|
|
self.prev_cruise_buttons = self.cruise_buttons
|
|
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
|
|
|
|
return ret
|
|
|
|
@staticmethod
|
|
def get_can_parser(CP):
|
|
signals = [
|
|
# sig_name, sig_address, default
|
|
("WHL_SPD_FL", "WHL_SPD11", 0),
|
|
("WHL_SPD_FR", "WHL_SPD11", 0),
|
|
("WHL_SPD_RL", "WHL_SPD11", 0),
|
|
("WHL_SPD_RR", "WHL_SPD11", 0),
|
|
|
|
("YAW_RATE", "ESP12", 0),
|
|
|
|
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
|
|
|
|
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
|
|
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door
|
|
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door
|
|
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door
|
|
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door
|
|
("CF_Gway_TurnSigLh", "CGW1", 0),
|
|
("CF_Gway_TurnSigRh", "CGW1", 0),
|
|
("CF_Gway_ParkBrakeSw", "CGW1", 0),
|
|
|
|
("CYL_PRES", "ESP12", 0),
|
|
|
|
("CF_Clu_CruiseSwState", "CLU11", 0),
|
|
("CF_Clu_CruiseSwMain", "CLU11", 0),
|
|
("CF_Clu_SldMainSW", "CLU11", 0),
|
|
("CF_Clu_ParityBit1", "CLU11", 0),
|
|
("CF_Clu_VanzDecimal" , "CLU11", 0),
|
|
("CF_Clu_Vanz", "CLU11", 0),
|
|
("CF_Clu_SPEED_UNIT", "CLU11", 0),
|
|
("CF_Clu_DetentOut", "CLU11", 0),
|
|
("CF_Clu_RheostatLevel", "CLU11", 0),
|
|
("CF_Clu_CluInfo", "CLU11", 0),
|
|
("CF_Clu_AmpInfo", "CLU11", 0),
|
|
("CF_Clu_AliveCnt1", "CLU11", 0),
|
|
|
|
("ACCEnable", "TCS13", 0),
|
|
("ACC_REQ", "TCS13", 0),
|
|
("DriverBraking", "TCS13", 0),
|
|
("StandStill", "TCS13", 0),
|
|
("PBRAKE_ACT", "TCS13", 0),
|
|
|
|
("ESC_Off_Step", "TCS15", 0),
|
|
("AVH_LAMP", "TCS15", 0),
|
|
|
|
("CR_Mdps_StrColTq", "MDPS12", 0),
|
|
("CF_Mdps_ToiActive", "MDPS12", 0),
|
|
("CF_Mdps_ToiUnavail", "MDPS12", 0),
|
|
("CF_Mdps_ToiFlt", "MDPS12", 0),
|
|
("CR_Mdps_OutTq", "MDPS12", 0),
|
|
|
|
("SAS_Angle", "SAS11", 0),
|
|
("SAS_Speed", "SAS11", 0),
|
|
|
|
("MainMode_ACC", "SCC11", 0),
|
|
("VSetDis", "SCC11", 0),
|
|
("SCCInfoDisplay", "SCC11", 0),
|
|
("ACC_ObjDist", "SCC11", 0),
|
|
("ACCMode", "SCC12", 1),
|
|
]
|
|
|
|
checks = [
|
|
# address, frequency
|
|
("MDPS12", 50),
|
|
("TCS13", 50),
|
|
("TCS15", 10),
|
|
("CLU11", 50),
|
|
("ESP12", 100),
|
|
("CGW1", 10),
|
|
("CGW2", 5),
|
|
("CGW4", 5),
|
|
("WHL_SPD11", 50),
|
|
("SAS11", 100),
|
|
]
|
|
|
|
if not CP.openpilotLongitudinalControl:
|
|
checks += [
|
|
("SCC11", 50),
|
|
("SCC12", 50),
|
|
]
|
|
|
|
if CP.enableBsm:
|
|
signals += [
|
|
("CF_Lca_IndLeft", "LCA11", 0),
|
|
("CF_Lca_IndRight", "LCA11", 0),
|
|
]
|
|
checks += [("LCA11", 50)]
|
|
|
|
if CP.carFingerprint in EV_HYBRID:
|
|
signals += [
|
|
("Accel_Pedal_Pos", "E_EMS11", 0),
|
|
]
|
|
checks += [
|
|
("E_EMS11", 50),
|
|
]
|
|
else:
|
|
signals += [
|
|
("PV_AV_CAN", "EMS12", 0),
|
|
("CF_Ems_AclAct", "EMS16", 0),
|
|
]
|
|
checks += [
|
|
("EMS12", 100),
|
|
("EMS16", 100),
|
|
]
|
|
|
|
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
|
|
signals += [
|
|
("CF_Clu_InhibitD", "CLU15", 0),
|
|
("CF_Clu_InhibitP", "CLU15", 0),
|
|
("CF_Clu_InhibitN", "CLU15", 0),
|
|
("CF_Clu_InhibitR", "CLU15", 0),
|
|
]
|
|
checks += [
|
|
("CLU15", 5)
|
|
]
|
|
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
|
|
signals += [
|
|
("CUR_GR", "TCU12", 0)
|
|
]
|
|
checks += [
|
|
("TCU12", 100)
|
|
]
|
|
elif CP.carFingerprint in FEATURES["use_elect_gears"]:
|
|
signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)]
|
|
checks += [("ELECT_GEAR", 20)]
|
|
else:
|
|
signals += [
|
|
("CF_Lvr_Gear", "LVR12", 0)
|
|
]
|
|
checks += [
|
|
("LVR12", 100)
|
|
]
|
|
|
|
if CP.carFingerprint in FEATURES["use_fca"]:
|
|
signals += [
|
|
("FCA_CmdAct", "FCA11", 0),
|
|
("CF_VSM_Warn", "FCA11", 0),
|
|
]
|
|
if not CP.openpilotLongitudinalControl:
|
|
checks += [("FCA11", 50)]
|
|
else:
|
|
signals += [
|
|
("AEB_CmdAct", "SCC12", 0),
|
|
("CF_VSM_Warn", "SCC12", 0),
|
|
]
|
|
|
|
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
|
|
|
@staticmethod
|
|
def get_cam_can_parser(CP):
|
|
|
|
signals = [
|
|
# sig_name, sig_address, default
|
|
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
|
|
("CF_Lkas_LdwsSysState", "LKAS11", 0),
|
|
("CF_Lkas_SysWarning", "LKAS11", 0),
|
|
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
|
|
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
|
|
("CF_Lkas_HbaLamp", "LKAS11", 0),
|
|
("CF_Lkas_FcwBasReq", "LKAS11", 0),
|
|
("CF_Lkas_HbaSysState", "LKAS11", 0),
|
|
("CF_Lkas_FcwOpt", "LKAS11", 0),
|
|
("CF_Lkas_HbaOpt", "LKAS11", 0),
|
|
("CF_Lkas_FcwSysState", "LKAS11", 0),
|
|
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
|
|
("CF_Lkas_FusionState", "LKAS11", 0),
|
|
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
|
|
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0),
|
|
]
|
|
|
|
checks = [
|
|
("LKAS11", 100)
|
|
]
|
|
|
|
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|