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(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(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