def __init__(self, CP, sendcan=None):
self.CP = CP
self.VM = VehicleModel(CP)
self.idx = 0
self.lanes = 0
self.lkas_request = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.cruise_enabled_prev = False
self.acc_enable_prev = False
self.low_speed_alert = False
self.lkas_button_on_prev = False
self.vEgo_prev = False
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp_cam, self.cp_cam2 = get_camera_parser(CP)
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint,
CP.enableCamera)
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
self.low_speed_alert = False
self.blinker_status = 0
self.blinker_timer = 0
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp2 = get_can2_parser(CP)
self.cp_cam = get_camera_parser(CP)
self.cp_AVM = get_AVM_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
self.traceLKA = trace1.Loger("LKA")
self.traceCLU = trace1.Loger("clu11")
self.traceSCC = trace1.Loger("scc12")
self.traceMDPS = trace1.Loger("mdps12")
self.traceCGW = trace1.Loger("CGW1")
self.params = Params()
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
self.low_speed_alert = False
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp_cam = get_camera_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.idx = 0
self.lanes = 0
self.lkas_request = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
self.low_speed_alert = False
self.lkas_button_on_prev = False
self.vEgo_prev = False
self.turning_indicator_alert = False
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp_cam = get_camera_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.idx = 0
self.lanes = 0
self.lkas_request = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
self.low_speed_alert = False
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp_cam = get_camera_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.hyundai
ret.enableCruise = True # stock acc
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerRateCost = 0.5
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1.
if candidate == CAR.SANTA_FE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.766
# Values from optimizer
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]]
ret.minSteerSpeed = 0.
elif candidate == CAR.KIA_SORENTO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1985. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.minSteerSpeed = 0.
elif candidate == CAR.ELANTRA:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.GENESIS:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
elif candidate == CAR.KIA_OPTIMA:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_STINGER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.minSteerSpeed = 0.
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [0.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.centerToFront = ret.wheelbase * 0.4
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
ret.steerControlType = car.CarParams.SteerControlType.torque
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.0]
ret.gasMaxBP = [0.]
ret.gasMaxV = [1.]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
ret.openpilotLongitudinalControl = False
ret.stoppingControl = False
ret.startAccel = 0.0
return ret
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
ret.gearShifter = self.CS.gear_shifter_cluster
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
ret.gearShifter = self.CS.gear_tcu
else:
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
events = []
if not ret.gearShifter == GearShifter.drive:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if self.low_speed_alert:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
ret.events = events
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def apply(self, c):
hud_alert = get_hud_alerts(c.hudControl.visualAlert)
can_sends = self.CC.update(c.enabled, self.CS, c.actuators,
c.cruiseControl.cancel, hud_alert)
return can_sends
class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
self.low_speed_alert = False
self.vEgo_prev = False
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp2 = get_can2_parser(CP)
self.cp_cam = get_camera_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.hyundai
ret.enableCruise = True # stock acc
ret.steerActuatorDelay = 0.15 # Default delay
ret.steerRateCost = 0.45
ret.steerLimitTimer = 0.8
tire_stiffness_factor = 0.7
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.minSteerSpeed = 0.
#ret.minSteerSpeed = 60 * CV.KPH_TO_MS
if candidate == CAR.GENESIS:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
elif candidate in [CAR.GENESIS_G90, CAR.GENESIS_G80]:
ret.mass = 2200. + STD_CARGO_KG
ret.wheelbase = 3.15
ret.steerRatio = 12.069
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate in [CAR.SONATA, CAR.SONATA_HEV]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1640. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate in [CAR.GRANDEUR, CAR.GRANDEUR_HEV]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1719. + STD_CARGO_KG
ret.wheelbase = 2.8
ret.steerRatio = 14.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.SANTA_FE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1870. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]]
elif candidate == CAR.IONIQ:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.IONIQ_EV:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1490. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KONA:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KONA_EV:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1685. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_HEV]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1525. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate in [CAR.KIA_CARDENZA, CAR.KIA_CARDENZA_HEV]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1575. + STD_CARGO_KG
ret.wheelbase = 2.85
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_FORTE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1613. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_SORENTO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1985. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_STINGER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_SELTOS:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1444. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.73 * 1.1
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_SOUL_EV:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1682. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.73
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_SPORTAGE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1626. + STD_CARGO_KG
ret.wheelbase = 2.67
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.centerToFront = ret.wheelbase * 0.4
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
ret.steerControlType = car.CarParams.SteerControlType.torque
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.0]
ret.gasMaxBP = [0.]
ret.gasMaxV = [0.5]
ret.brakeMaxBP = [0., 20.]
ret.brakeMaxV = [1., 0.8]
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
ret.openpilotLongitudinalControl = False
ret.stoppingControl = True
ret.startAccel = 0.0
# ignore CAN2 address if L-CAN on the same BUS
ret.mdpsBus = 1 if 593 in fingerprint[1] and 1296 not in fingerprint[1] else 0
ret.sasBus = 1 if 688 in fingerprint[1] and 1296 not in fingerprint[1] else 0
ret.sccBus = 0 if 1056 in fingerprint[0] else 1 if 1056 in fingerprint[1] and 1296 not in fingerprint[1] \
else 2 if 1056 in fingerprint[2] else -1
ret.autoLcaEnabled = 1
return ret
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp2.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp2, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
# most HKG cars has no long control, it is safer and easier to engage by main on
ret.cruiseState.enabled = (self.CS.pcm_acc_status != 0) if self.CC.longcontrol else bool(self.CS.main_on)
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
ret.lcaLeft = self.CS.lca_left != 0
ret.lcaRight = self.CS.lca_right != 0
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_flash != self.CS.prev_left_blinker_flash:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_flash != 0
buttonEvents.append(be)
if self.CS.right_blinker_flash != self.CS.prev_right_blinker_flash:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_flash != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_flash)
ret.rightBlinker = bool(self.CS.right_blinker_flash)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < self.CP.minSteerSpeed and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > self.CP.minSteerSpeed:
self.low_speed_alert = False
# turning indicator alert hysteresis logic
# self.turning_indicator_alert = True if (self.CS.left_blinker_flash or self.CS.right_blinker_flash) and self.CS.v_ego < 17.5 else False
self.turning_indicator_alert = True if self.CC.turning_signal_timer and self.CS.v_ego < 16.7 else False
# LKAS button alert logic
self.lkas_button_alert = True if not self.CC.lkas_button else False
events = []
if not ret.gearShifter == GearShifter.drive:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
#if ret.seatbeltUnlatched:
# events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if ((ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1))) and self.CC.longcontrol:
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed and self.CC.longcontrol:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if self.low_speed_alert and not self.CS.mdps_bus :
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
if self.turning_indicator_alert:
events.append(create_event('turningIndicatorOn', [ET.WARNING]))
if self.lkas_button_alert:
events.append(create_event('lkasButtonOff', [ET.WARNING]))
# TODO: Varible for min Speed for LCA
if ret.rightBlinker and ret.lcaRight and self.CS.v_ego > (35 * CV.MPH_TO_MS):
events.append(create_event('rightLCAbsm', [ET.WARNING]))
if ret.leftBlinker and ret.lcaLeft and self.CS.v_ego > (35 * CV.MPH_TO_MS):
events.append(create_event('leftLCAbsm', [ET.WARNING]))
ret.events = events
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
self.vEgo_prev = ret.vEgo
return ret.as_reader()
def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
self.frame += 1
return can_sends
class CarInterface(object):
def __init__(self, CP, sendcan=None):
self.CP = CP
self.VM = VehicleModel(CP)
self.idx = 0
self.lanes = 0
self.lkas_request = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.cruise_enabled_prev = False
self.low_speed_alert = False
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp_cam = get_camera_parser(CP)
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint,
CP.enableCamera)
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def calc_accel_override(a_ego, a_target, v_ego, v_target):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModels.hyundai
ret.enableCruise = True # stock acc
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923 * CV.LB_TO_KG + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 192150
tireStiffnessRear_civic = 202500
ret.steerActuatorDelay = 0.1 # Default delay
tire_stiffness_factor = 1.
if candidate == CAR.SANTA_FE:
ret.steerKf = 0.00005
ret.steerRateCost = 0.5
ret.mass = 3982 * CV.LB_TO_KG + std_cargo
ret.wheelbase = 2.766
# Values from optimizer
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
tire_stiffness_factor = 0.82
ret.steerKiBP, ret.steerKpBP = [[9., 22.], [9., 22.]]
ret.steerKpV, ret.steerKiV = [[0.2, 0.35], [0.05, 0.09]]
ret.minSteerSpeed = 0.
elif candidate == CAR.KIA_SORENTO:
ret.steerKf = 0.00005
ret.steerRateCost = 0.5
ret.mass = 1985 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
ret.minSteerSpeed = 0.
elif candidate == CAR.ELANTRA:
ret.steerKf = 0.00006
ret.steerRateCost = 0.5
ret.mass = 1275 + std_cargo
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
tire_stiffness_factor = 0.385
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.GENESIS:
ret.steerKf = 0.00005
ret.steerRateCost = 0.5
ret.mass = 2060 + std_cargo
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.16], [0.01]]
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
elif candidate == CAR.KIA_STINGER:
ret.steerKf = 0.00005
ret.steerRateCost = 0.5
ret.mass = 1825 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
ret.minSteerSpeed = 0.
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.longitudinalKpBP = [0.]
ret.longitudinalKpV = [0.]
ret.longitudinalKiBP = [0.]
ret.longitudinalKiV = [0.]
ret.centerToFront = ret.wheelbase * 0.4
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
ret.steerControlType = car.CarParams.SteerControlType.torque
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.0]
ret.gasMaxBP = [0.]
ret.gasMaxV = [1.]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.longPidDeadzoneBP = [0.]
ret.longPidDeadzoneV = [0.]
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
ret.openpilotLongitudinalControl = False
ret.steerLimitAlert = False
ret.stoppingControl = False
ret.startAccel = 0.0
return ret
# returns a car.CarState
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
if self.CP.carFingerprint == CAR.ELANTRA:
ret.gearShifter = self.CS.gear_shifter_cluster
else:
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed +
2.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if not ret.gearShifter == 'drive':
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if self.low_speed_alert:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
ret.events = events
ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def apply(self, c, perception_state=log.Live20Data.new_message()):
hud_alert = get_hud_alerts(c.hudControl.visualAlert,
c.hudControl.audibleAlert)
self.CC.update(self.sendcan, c.enabled, self.CS, c.actuators,
c.cruiseControl.cancel, hud_alert)
return False
class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
self.low_speed_alert = False
self.blinker_status = 0
self.blinker_timer = 0
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp2 = get_can2_parser(CP)
self.cp_cam = get_camera_parser(CP)
self.cp_AVM = get_AVM_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
self.traceLKA = trace1.Loger("LKA")
self.traceCLU = trace1.Loger("clu11")
self.traceSCC = trace1.Loger("scc12")
self.traceMDPS = trace1.Loger("mdps12")
self.traceCGW = trace1.Loger("CGW1")
self.params = Params()
#self.lane_change_enabled = self.params.get('LaneChangeEnabled') == b'1'
#self.speed_control_enabled = self.params.get('SpeedControlEnabled') == b'1'
#self.car_avoid_enable = self.params.get('CarAvoidanceEnabled') == b'1'
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def get_params(candidate,
fingerprint=gen_empty_fingerprint(),
has_relay=False,
car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.hyundai
ret.enableCruise = True # stock acc
ret.minSteerSpeed = 0 # 5 km/h
ret.steerActuatorDelay = 0.10 # Default delay 0.15
ret.steerRateCost = 0.45
ret.steerLimitTimer = 0.8
tire_stiffness_factor = 0.7
#ret.radarOffCan = False
"""
0.7.5
ret.steerActuatorDelay = 0.1 # Default delay 0.1
ret.steerRateCost = 0.5
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1
"""
"""
0.7.3
ret.steerActuatorDelay = 0.10 # Default delay 0.15
ret.steerRateCost = 0.45
ret.steerLimitTimer = 0.8
tire_stiffness_factor = 0.7
"""
if candidate == CAR.SANTAFE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1830. + STD_CARGO_KG
ret.wheelbase = 2.765
# Values from optimizer
ret.steerRatio = 13.8 # 13.8 is spec end-to-end
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.SORENTO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1950. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.GENESIS:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate in [CAR.K5, CAR.SONATA]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1470. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 12.75
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.SONATA_TURBO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1565. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.K5_HEV:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1595. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 12.75
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate in [CAR.GRANDEUR, CAR.K7]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1570. + STD_CARGO_KG
ret.wheelbase = 2.885
ret.steerRatio = 12.5
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate in [CAR.GRANDEUR_HEV, CAR.K7_HEV]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1675. + STD_CARGO_KG
ret.wheelbase = 2.845
ret.steerRatio = 12.0 #12.5
ret.steerRateCost = 0.4 #0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 32.4 KPH ~ 79.2 KPH
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.STINGER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.KONA:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.KONA_HEV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.KONA_EV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.NIRO_HEV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1425. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.NIRO_EV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1425. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.IONIQ_HEV:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.IONIQ_EV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1490. + STD_CARGO_KG #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
ret.steerRatio = 13.25 #Spec
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.NEXO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1885. + STD_CARGO_KG
ret.wheelbase = 2.79
ret.steerRatio = 12.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.MOHAVE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2250. + STD_CARGO_KG
ret.wheelbase = 2.895
ret.steerRatio = 14.1
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.I30:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1380. + STD_CARGO_KG
ret.wheelbase = 2.65
ret.steerRatio = 13.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
elif candidate == CAR.AVANTE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.5 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
tire_stiffness_factor = 0.385 # stiffnessFactor settled on 1.0081302973865127
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]] # 9m/s = 32.4km/h ~ 22m/s = 79.2 km/h
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.1, 0.11
], [0.02, 0.05]]
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.centerToFront = ret.wheelbase * 0.4
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(
ret.mass,
ret.wheelbase,
ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
ret.steerControlType = car.CarParams.SteerControlType.torque
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.0]
ret.gasMaxBP = [0.]
ret.gasMaxV = [0.5]
ret.brakeMaxBP = [0., 20.]
ret.brakeMaxV = [1., 0.8]
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS,
ECU_FINGERPRINT, candidate,
Ecu.fwdCamera) or has_relay
ret.openpilotLongitudinalControl = False
ret.stoppingControl = True
ret.startAccel = 0.0
# ignore CAN2 address if L-CAN on the same BUS
ret.mdpsBus = 1 if 593 in fingerprint[1] and 1296 not in fingerprint[
1] else 0
ret.sasBus = 1 if 688 in fingerprint[1] and 1296 not in fingerprint[
1] else 0
ret.sccBus = 0 if 1056 in fingerprint[0] else 1 if 1056 in fingerprint[1] and 1296 not in fingerprint[1] \
else 2 if 1056 in fingerprint[2] else -1
ret.autoLcaEnabled = 0
return ret
# returns a car.CarState
def update(self, c, can_strings): # c => CC
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp2.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.cp_AVM.update_strings(can_strings)
self.CS.update(self.cp, self.cp2, self.cp_cam, self.cp_AVM)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid #and self.cp_AVM.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# cruise state
# most HKG cars has no long control, it is safer and easier to engage by main on
ret.cruiseState.enabled = (
self.CS.pcm_acc_status != 0) if self.CC.longcontrol else bool(
self.CS.main_on)
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
#ret.cruise_set_mode = self.CS.cruise_set_mode
# Some HKG cars only have blinker flash signal
#if self.CP.carFingerprint not in [CAR.IONIQ_HEV, CAR.KONA, CAR.KONA_HEV]:
#self.CS.left_blinker_on = self.CS.left_blinker_flash or self.CS.prev_left_blinker_on and self.CC.turning_signal_timer
#self.CS.right_blinker_on = self.CS.right_blinker_flash or self.CS.prev_right_blinker_on and self.CC.turning_signal_timer
blinker_status = self.CS.blinker_status
if self.CS.left_blinker_flash or self.CS.right_blinker_flash:
self.blinker_timer = 50
elif self.blinker_timer:
self.blinker_timer -= 1
else:
blinker_status = 0
if blinker_status == 3:
ret.leftBlinker = bool(self.blinker_timer)
ret.rightBlinker = bool(self.blinker_timer)
elif blinker_status == 1:
ret.leftBlinker = False
ret.rightBlinker = bool(self.blinker_timer)
elif blinker_status == 2:
ret.leftBlinker = bool(self.blinker_timer)
ret.rightBlinker = False
else:
ret.leftBlinker = False
ret.rightBlinker = False
#ret.leftBlinker = bool(self.CS.left_blinker_flash)
#ret.rightBlinker = bool(self.CS.right_blinker_flash)
ret.lcaLeft = self.CS.lca_left != 0
ret.lcaRight = self.CS.lca_right != 0
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
# turning indicator alert hysteresis logic
self.turning_indicator_alert = self.CC.turning_indicator
# LKAS button alert logic
self.lkas_button_alert = not self.CC.lkas_button
self.low_speed_alert = self.CC.low_speed_car
self.steer_angle_over_alert = self.CC.streer_angle_over
events = []
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
elif ret.gearShifter == GearShifter.reverse:
events.append(
create_event('reverseGear', [ET.NO_ENTRY, ET.USER_DISABLE]))
elif not ret.gearShifter == GearShifter.drive:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.USER_DISABLE]))
elif self.steer_angle_over_alert or self.CS.steer_error:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
elif self.lkas_button_alert:
events.append(create_event('lkasButtonOff', [ET.WARNING]))
elif self.CS.lkas_LdwsLHWarning or self.CS.lkas_LdwsRHWarning:
events.append(create_event('ldwPermanent', [ET.WARNING]))
if ret.cruiseState.enabled != self.cruise_enabled_prev:
if ret.cruiseState.enabled:
events.append(create_event('pcmEnable', [ET.ENABLE]))
else:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
self.cruise_enabled_prev = ret.cruiseState.enabled
elif ret.cruiseState.enabled:
if self.turning_indicator_alert:
events.append(create_event('turningIndicatorOn', [ET.WARNING]))
elif self.CC.steer_torque_over:
events.append(create_event('steerTorqueOver', [ET.WARNING]))
elif self.CS.stopped:
if ret.cruiseState.standstill:
events.append(create_event('resumeRequired', [ET.WARNING]))
else:
events.append(create_event('preStoped', [ET.WARNING]))
#elif self.low_speed_alert and not self.CS.mdps_bus:
# events.append(create_event('belowSteerSpeed', [ET.WARNING]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if self.CC.longcontrol:
if ((ret.gasPressed and not self.gas_pressed_prev)
or (ret.brakePressed and
(not self.brake_pressed_prev or ret.vEgoRaw > 0.1))):
events.append(
create_event('pedalPressed',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
#TODO Varible for min Speed for LCA
if ret.rightBlinker and ret.lcaRight and self.CS.v_ego > LaneChangeParms.LANE_CHANGE_SPEED_MIN:
events.append(create_event('rightLCAbsm', [ET.WARNING]))
if ret.leftBlinker and ret.lcaLeft and self.CS.v_ego > LaneChangeParms.LANE_CHANGE_SPEED_MIN:
events.append(create_event('leftLCAbsm', [ET.WARNING]))
ret.events = events
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
#self.log_update( can_strings )
return ret.as_reader()
def apply(self, c, sm, LaC):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel,
c.hudControl.visualAlert,
c.hudControl.leftLaneVisible,
c.hudControl.rightLaneVisible, sm, LaC)
self.frame += 1
return can_sends
def log_update(self, can_string):
v_ego = self.CS.v_ego * CV.MS_TO_KPH
log_v_ego = ' v_ego={:5.0f} km/h '.format(v_ego)
if v_ego > 10:
log_data = log_v_ego + str(self.CS.lkas11)
self.traceLKA.add(log_data)
log_data = log_v_ego + str(self.CS.clu11)
self.traceCLU.add(log_data)
log_data = log_v_ego + str(self.CS.scc12)
self.traceSCC.add(log_data)
log_data = log_v_ego + str(self.CS.mdps12)
self.traceMDPS.add(log_data)
log_data = log_v_ego + str(self.CS.cgw1)
self.traceCGW.add(log_data)
class CarInterface(object):
def __init__(self, CP, sendcan=None):
self.CP = CP
self.VM = VehicleModel(CP)
self.idx = 0
self.lanes = 0
self.lkas_request = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.cruise_enabled_prev = False
self.acc_enable_prev = False
self.low_speed_alert = False
self.lkas_button_on_prev = False
self.vEgo_prev = False
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp_cam, self.cp_cam2 = get_camera_parser(CP)
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint,
CP.enableCamera)
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def calc_accel_override(a_ego, a_target, v_ego, v_target):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 200 # Comma use 136kg .. Fuel = 60kg, Driver = 80kg (assuming 70kg and not naked), Cargo = 20kg .. This is the minimum.. assume 50% of the time there is a passenger also 70kg and not naked, so 40kg.
weight_dist_rear = 0.45
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModels.hyundai
ret.enableCruise = True # stock acc
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923 * CV.LB_TO_KG + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * weight_dist_rear
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 192150
tireStiffnessRear_civic = 202500
ret.steerActuatorDelay = 0.10
ret.lateralTuning.pid.kf = 0.00006
ret.steerRateCost = 0.50
tire_stiffness_factor = 0.60
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [0.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.12], [0.06]]
ret.minSteerSpeed = 0.
if candidate == CAR.GENESIS:
ret.mass = 2060
ret.wheelbase = 3.01
ret.steerRatio = 12.069
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
else:
ret.mass = 1800
ret.wheelbase = 2.8
ret.steerRatio = 13.5
ret.mass += std_cargo
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.centerToFront = ret.wheelbase * weight_dist_rear
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
ret.steerControlType = car.CarParams.SteerControlType.torque
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.0]
ret.gasMaxBP = [0.]
ret.gasMaxV = [1.]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
ret.openpilotLongitudinalControl = True
ret.steerLimitAlert = True
ret.stoppingControl = False
ret.startAccel = 0.0
return ret
# returns a car.CarState
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
self.cp_cam2.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam, self.cp_cam2)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter_cluster
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed -
1.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 1.):
self.low_speed_alert = False
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
# Try all 3 gear selector locations, as some cars miss one or 2 of them inconsistently
if (self.CS.gear_shifter != 'drive') and (
self.CS.gear_tcu != 'drive') and (self.CS.gear_shifter_cluster
!= 'drive'):
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if self.low_speed_alert:
events.append(
create_event('speedTooLow',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.madMode == 1:
if self.CS.lkas_button_on:
if not self.lkas_button_on_prev or ret.vEgo > self.CP.minSteerSpeed >= self.vEgo_prev:
events.append(create_event('wrongCarMode', [ET.ENABLE]))
elif not self.CS.lkas_button_on and self.lkas_button_on_prev:
events.append(create_event('wrongCarMode', [ET.USER_DISABLE]))
elif self.CS.madMode == 2:
if not self.CS.acc_enable:
events.append(create_event('wrongCarMode', [ET.USER_DISABLE]))
elif self.CS.acc_enable and not self.acc_enable_prev:
events.append(create_event('wrongCarMode', [ET.ENABLE]))
else:
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)) and \
((self.CS.madMode == 1 and not self.CS.lkas_button_on) or \
(self.CS.madMode == 2 and not self.CS.acc_enable)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed and \
((self.CS.madMode == 1 and not self.CS.lkas_button_on) or \
(self.CS.madMode == 2 and not self.CS.acc_enable)):
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
ret.events = events
ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
self.acc_enable_prev = self.CS.acc_enable
self.lkas_button_on_prev = self.CS.lkas_button_on
self.vEgo_prev = ret.vEgo
return ret.as_reader()
def apply(self, c):
hud_alert = get_hud_alerts(c.hudControl.visualAlert,
c.hudControl.audibleAlert)
self.CC.update(self.sendcan, c.enabled, self.CS, c.actuators,
c.cruiseControl.cancel, hud_alert)
return False
class CarInterface(object):
def __init__(self, CP, sendcan=None):
self.CP = CP
self.VM = VehicleModel(CP)
self.idx = 0
self.lanes = 0
self.lkas_request = 0
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.cruise_enabled_prev = False
# *** init the major players ***
self.CS = CarState(CP)
self.CamS = CamState(CP)
self.cp = get_can_parser(CP)
self.cp2 = get_can_parser2(CP)
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint,
CP.enableCamera)
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def calc_accel_override(a_ego, a_target, v_ego, v_target):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 200 # Fuel = 70kg + Driver = 90kg + Roofracks, towball, etc = 40kg
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.safetyModel = car.CarParams.SafetyModels.hyundai
# pedal
ret.enableCruise = False
rotationalInertia = 2500
tireStiffnessFront = 85400
tireStiffnessRear = 90000
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerActuatorDelay = 0.1 # Default delay, Prius has larger delay
#borrowing a lot from corolla, given similar car size
ret.steerKf = 0.00008 # full torque for 20 deg at 80mph means 0.00007818594
ret.steerRateCost = 1.
stop_and_go = True
ret.mass = 1985 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 15.0
ret.steerKpV, ret.steerKiV = [[0.20], [0.007]]
ret.centerToFront = ret.wheelbase * 0.4
ret.longPidDeadzoneBP = [0., 9.]
ret.longPidDeadzoneV = [0., .15]
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter.
ret.minEnableSpeed = -1.
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia * ret.mass * ret.wheelbase**2
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = tireStiffnessFront * ret.mass * (
centerToRear / ret.wheelbase)
ret.tireStiffnessRear = tireStiffnessRear * ret.mass * (
ret.centerToFront / ret.wheelbase)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
ret.steerControlType = car.CarParams.SteerControlType.torque
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [16. * CV.KPH_TO_MS,
70. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph
ret.steerMaxV = [1.0, 1.0] # 2/3rd torque allowed above 45 kph
ret.gasMaxBP = [0.]
ret.gasMaxV = [0.5]
ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8]
ret.enableCamera = True
ret.steerLimitAlert = False
ret.stoppingControl = False
ret.startAccel = 0.0
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [3.6, 2.4, 1.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.54, 0.36]
return ret
# returns a car.CarState
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.cp2.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp)
self.CamS.update(self.cp2)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = 0. #self.CS.angle_steers_rate
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0.
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
#ret.genericToggle = self.CS.generic_toggle
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if not ret.gearShifter == 'drive':
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if False: #self.CS.low_speed_lockout:
events.append(
create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if c.actuators.gas > 0.1:
# some margin on the actuator to not false trigger cancellation while stopping
events.append(
create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
if ret.vEgo < 0.001:
# while in standstill, send a user alert
events.append(create_event('manualRestart', [ET.WARNING]))
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
ret.events = events
ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c, perception_state=log.Live20Data.new_message()):
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
c.actuators, self.CamS)
self.frame += 1
return False
class CarInterface(object):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.idx = 0
self.lanes = 0
self.lkas_request = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
self.low_speed_alert = False
# *** init the major players ***
self.CS = CarState(CP)
self.cp = get_can_parser(CP)
self.cp_cam = get_camera_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def calc_accel_override(a_ego, a_target, v_ego, v_target):
return 1.0
@staticmethod
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.hyundai
ret.enableCruise = True # stock acc
tire_stiffness_factor = 1.
# Hyundai Kia and Genesis Starting Values
# Auto Tune corrects anything from here on out.
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 0.5
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1985. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 15.0
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.minSteerSpeed = 0.
ret.minEnableSpeed = -1.
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [0.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.centerToFront = ret.wheelbase * 0.4
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(
ret.mass,
ret.wheelbase,
ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
ret.steerControlType = car.CarParams.SteerControlType.torque
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.0]
ret.gasMaxBP = [0.]
ret.gasMaxV = [1.]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.openpilotLongitudinalControl = False
ret.enableCamera = True
ret.steerLimitAlert = False
ret.stoppingControl = False
ret.startAccel = 0.0
return ret
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid # TODO: check cp_cam validity
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
events = []
if (self.CS.gear_shifter != 'drive') and (
self.CS.gear_tcu != 'drive') and (self.CS.gear_shifter_cluster
!= 'drive'):
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if self.CS.low_speed_alert:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
ret.events = events
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def apply(self, c):
hud_alert = get_hud_alerts(c.hudControl.visualAlert,
c.hudControl.audibleAlert)
can_sends = self.CC.update(c.enabled, self.CS, c.actuators,
c.cruiseControl.cancel, hud_alert)
return can_sends
