@ -5,7 +5,7 @@ from cereal import car
from common . conversions import Conversions as CV
from opendbc . can . parser import CANParser
from opendbc . can . can_define import CANDefine
from selfdrive . car . hyundai . values import DBC , STEER_THRESHOLD , FEATURES , EV_CAR , HYBRID_CAR , Buttons
from selfdrive . car . hyundai . values import DBC , STEER_THRESHOLD , FEATURES , HDA2_CAR , EV_CAR , HYBRID_CAR , Buttons
from selfdrive . car . interfaces import CarStateBase
PREV_BUTTON_SAMPLES = 4
@ -19,14 +19,23 @@ class CarState(CarStateBase):
self . cruise_buttons = deque ( [ Buttons . NONE ] * PREV_BUTTON_SAMPLES , maxlen = PREV_BUTTON_SAMPLES )
self . main_buttons = deque ( [ Buttons . NONE ] * PREV_BUTTON_SAMPLES , maxlen = PREV_BUTTON_SAMPLES )
if self . CP . carFingerprint in FEATURES [ " use_cluster_gears " ] :
if CP . carFingerprint in HDA2_CAR :
self . shifter_values = can_define . dv [ " ACCELERATOR " ] [ " GEAR " ]
elif self . CP . carFingerprint in FEATURES [ " use_cluster_gears " ] :
self . shifter_values = can_define . dv [ " CLU15 " ] [ " CF_Clu_Gear " ]
elif self . CP . carFingerprint in FEATURES [ " use_tcu_gears " ] :
self . shifter_values = can_define . dv [ " TCU12 " ] [ " CUR_GR " ]
else : # preferred and elect gear methods use same definition
self . shifter_values = can_define . dv [ " LVR12 " ] [ " CF_Lvr_Gear " ]
self . brake_error = False
self . park_brake = False
self . buttons_counter = 0
def update ( self , cp , cp_cam ) :
if self . CP . carFingerprint in HDA2_CAR :
return self . update_hda2 ( cp , cp_cam )
ret = car . CarState . new_message ( )
ret . doorOpen = any ( [ cp . vl [ " CGW1 " ] [ " CF_Gway_DrvDrSw " ] , cp . vl [ " CGW1 " ] [ " CF_Gway_AstDrSw " ] ,
@ -120,10 +129,57 @@ class CarState(CarStateBase):
return ret
def update_hda2 ( self , cp , cp_cam ) :
ret = car . CarState . new_message ( )
ret . gas = cp . vl [ " ACCELERATOR " ] [ " ACCELERATOR_PEDAL " ] / 255.
ret . gasPressed = ret . gas > 1e-3
ret . brakePressed = cp . vl [ " BRAKE " ] [ " BRAKE_PRESSED " ] == 1
ret . doorOpen = cp . vl [ " DOORS_SEATBELTS " ] [ " DRIVER_DOOR_OPEN " ] == 1
ret . seatbeltUnlatched = cp . vl [ " DOORS_SEATBELTS " ] [ " DRIVER_SEATBELT_LATCHED " ] == 0
gear = cp . vl [ " ACCELERATOR " ] [ " GEAR " ]
ret . gearShifter = self . parse_gear_shifter ( self . shifter_values . get ( gear ) )
# TODO: figure out positions
ret . wheelSpeeds = self . get_wheel_speeds (
cp . vl [ " WHEEL_SPEEDS " ] [ " WHEEL_SPEED_1 " ] ,
cp . vl [ " WHEEL_SPEEDS " ] [ " WHEEL_SPEED_2 " ] ,
cp . vl [ " WHEEL_SPEEDS " ] [ " WHEEL_SPEED_3 " ] ,
cp . vl [ " WHEEL_SPEEDS " ] [ " WHEEL_SPEED_4 " ] ,
)
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 . steeringRateDeg = cp . vl [ " STEERING_SENSORS " ] [ " STEERING_RATE " ]
ret . steeringAngleDeg = cp . vl [ " STEERING_SENSORS " ] [ " STEERING_ANGLE " ] * - 1
ret . steeringTorque = cp . vl [ " MDPS " ] [ " STEERING_COL_TORQUE " ]
ret . steeringTorqueEps = cp . vl [ " MDPS " ] [ " STEERING_OUT_TORQUE " ]
ret . steeringPressed = abs ( ret . steeringTorque ) > STEER_THRESHOLD
ret . leftBlinker , ret . rightBlinker = self . update_blinker_from_lamp ( 50 , cp . vl [ " BLINKERS " ] [ " LEFT_LAMP " ] ,
cp . vl [ " BLINKERS " ] [ " RIGHT_LAMP " ] )
ret . cruiseState . available = True
ret . cruiseState . enabled = cp . vl [ " SCC1 " ] [ " CRUISE_ACTIVE " ] == 1
ret . cruiseState . standstill = cp . vl [ " CRUISE_INFO " ] [ " CRUISE_STANDSTILL " ] == 1
speed_factor = CV . MPH_TO_MS if cp . vl [ " CLUSTER_INFO " ] [ " DISTANCE_UNIT " ] == 1 else CV . KPH_TO_MS
ret . cruiseState . speed = cp . vl [ " CRUISE_INFO " ] [ " SET_SPEED " ] * speed_factor
self . buttons_counter = cp . vl [ " CRUISE_BUTTONS " ] [ " _COUNTER " ]
return ret
@staticmethod
def get_can_parser ( CP ) :
if CP . carFingerprint in HDA2_CAR :
return CarState . get_can_parser_hda2 ( CP )
signals = [
# sig_name, sig_address
# signal _name, signal _address
( " WHL_SPD_FL " , " WHL_SPD11 " ) ,
( " WHL_SPD_FR " , " WHL_SPD11 " ) ,
( " WHL_SPD_RL " , " WHL_SPD11 " ) ,
@ -135,9 +191,9 @@ class CarState(CarStateBase):
( " CF_Gway_DrvSeatBeltSw " , " CGW1 " ) ,
( " CF_Gway_DrvDrSw " , " CGW1 " ) , # Driver Door
( " CF_Gway_AstDrSw " , " CGW1 " ) , # Passenger d oor
( " CF_Gway_RLDrSw " , " CGW2 " ) , # Rear reft d oor
( " CF_Gway_RRDrSw " , " CGW2 " ) , # Rear right d oor
( " CF_Gway_AstDrSw " , " CGW1 " ) , # Passenger D oor
( " CF_Gway_RLDrSw " , " CGW2 " ) , # Rear left D oor
( " CF_Gway_RRDrSw " , " CGW2 " ) , # Rear right D oor
( " CF_Gway_TurnSigLh " , " CGW1 " ) ,
( " CF_Gway_TurnSigRh " , " CGW1 " ) ,
( " CF_Gway_ParkBrakeSw " , " CGW1 " ) ,
@ -175,7 +231,6 @@ class CarState(CarStateBase):
( " SAS_Angle " , " SAS11 " ) ,
( " SAS_Speed " , " SAS11 " ) ,
]
checks = [
# address, frequency
( " MDPS12 " , 50 ) ,
@ -198,7 +253,6 @@ class CarState(CarStateBase):
( " ACC_ObjDist " , " SCC11 " ) ,
( " ACCMode " , " SCC12 " ) ,
]
checks + = [
( " SCC11 " , 50 ) ,
( " SCC12 " , 50 ) ,
@ -256,8 +310,11 @@ class CarState(CarStateBase):
@staticmethod
def get_cam_can_parser ( CP ) :
if CP . carFingerprint in HDA2_CAR :
return None
signals = [
# sig_name, sig_address
# signal _name, signal _address
( " CF_Lkas_LdwsActivemode " , " LKAS11 " ) ,
( " CF_Lkas_LdwsSysState " , " LKAS11 " ) ,
( " CF_Lkas_SysWarning " , " LKAS11 " ) ,
@ -274,9 +331,55 @@ class CarState(CarStateBase):
( " CF_Lkas_FcwOpt_USM " , " LKAS11 " ) ,
( " CF_Lkas_LdwsOpt_USM " , " LKAS11 " ) ,
]
checks = [
( " LKAS11 " , 100 )
]
return CANParser ( DBC [ CP . carFingerprint ] [ " pt " ] , signals , checks , 2 )
@staticmethod
def get_can_parser_hda2 ( CP ) :
signals = [
( " WHEEL_SPEED_1 " , " WHEEL_SPEEDS " ) ,
( " WHEEL_SPEED_2 " , " WHEEL_SPEEDS " ) ,
( " WHEEL_SPEED_3 " , " WHEEL_SPEEDS " ) ,
( " WHEEL_SPEED_4 " , " WHEEL_SPEEDS " ) ,
( " ACCELERATOR_PEDAL " , " ACCELERATOR " ) ,
( " GEAR " , " ACCELERATOR " ) ,
( " BRAKE_PRESSED " , " BRAKE " ) ,
( " STEERING_RATE " , " STEERING_SENSORS " ) ,
( " STEERING_ANGLE " , " STEERING_SENSORS " ) ,
( " STEERING_COL_TORQUE " , " MDPS " ) ,
( " STEERING_OUT_TORQUE " , " MDPS " ) ,
( " CRUISE_ACTIVE " , " SCC1 " ) ,
( " SET_SPEED " , " CRUISE_INFO " ) ,
( " CRUISE_STANDSTILL " , " CRUISE_INFO " ) ,
( " _COUNTER " , " CRUISE_BUTTONS " ) ,
( " DISTANCE_UNIT " , " CLUSTER_INFO " ) ,
( " LEFT_LAMP " , " BLINKERS " ) ,
( " RIGHT_LAMP " , " BLINKERS " ) ,
( " DRIVER_DOOR_OPEN " , " DOORS_SEATBELTS " ) ,
( " DRIVER_SEATBELT_LATCHED " , " DOORS_SEATBELTS " ) ,
]
checks = [
( " WHEEL_SPEEDS " , 100 ) ,
( " ACCELERATOR " , 100 ) ,
( " BRAKE " , 100 ) ,
( " STEERING_SENSORS " , 100 ) ,
( " MDPS " , 100 ) ,
( " SCC1 " , 50 ) ,
( " CRUISE_INFO " , 50 ) ,
( " CRUISE_BUTTONS " , 50 ) ,
( " CLUSTER_INFO " , 4 ) ,
( " BLINKERS " , 4 ) ,
( " DOORS_SEATBELTS " , 4 ) ,
]
return CANParser ( DBC [ CP . carFingerprint ] [ " pt " ] , signals , checks , 5 )
Shane Smiskol
3 years ago