def __init__(self, CP, sendcan=None): self.CP = CP self.VM = VehicleModel(CP) self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.cam_can_valid_count = 0 self.cruise_enabled_prev = False # *** init the major players *** self.CS = CarState(CP) self.cp = get_can_parser(CP) self.cp_cam = get_cam_can_parser(CP) self.forwarding_camera = False # 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, CP.enableDsu, CP.enableApgs)
def __init__(self, CP, logcan, sendcan=None): self.logcan = logcan self.CP = CP self.frame = 0 self.can_invalid_count = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.cruise_enabled_prev = False # *** init the major players *** self.CS = CarState(CP, self.logcan) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
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 # *** init the major players *** self.CS = CarState(CP) self.cp = get_can_parser(CP) self.cp_cam = get_cam_can_parser(CP) self.CC = None if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
class CarInterface(object): 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 # *** init the major players *** self.CS = CarState(CP) self.cp = get_can_parser(CP) self.cp_cam = get_cam_can_parser(CP) self.forwarding_camera = False self.CC = None if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) @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=""): ret = car.CarParams.new_message() ret.carName = "toyota" ret.carFingerprint = candidate ret.carVin = vin ret.safetyModel = car.CarParams.SafetyModel.toyota # pedal ret.enableCruise = not ret.enableGasInterceptor ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay if candidate != CAR.PRIUS: ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] if candidate == CAR.PRIUS: stop_and_go = True ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 15.00 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 4.0 ret.lateralTuning.indi.outerLoopGain = 3.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.0 ret.steerActuatorDelay = 0.5 elif candidate in [CAR.RAV4, CAR.RAV4H]: stop_and_go = True if (candidate in CAR.RAV4H) else False ret.safetyParam = 73 ret.wheelbase = 2.65 ret.steerRatio = 16.30 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 elif candidate == CAR.COROLLA: stop_and_go = False ret.safetyParam = 100 ret.wheelbase = 2.70 ret.steerRatio = 17.8 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RXH: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.79 ret.steerRatio = 16. # 14.8 is spec end-to-end tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 elif candidate in [CAR.CHR, CAR.CHRH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.6 tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]] ret.lateralTuning.pid.kf = 0.00006 elif candidate in [CAR.CAMRY, CAR.CAMRYH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.82448 ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.78 ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid limited ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.18 ], [0.015]] # community tuning ret.lateralTuning.pid.kf = 0.00012 # community tuning elif candidate == CAR.AVALON: stop_and_go = False ret.safetyParam = 73 ret.wheelbase = 2.82 ret.steerRatio = 14.8 #Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]] ret.lateralTuning.pid.kf = 0.00006 elif candidate == CAR.RAV4_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.68986 ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.COROLLA_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.LEXUS_ESH_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.8702 ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.SIENNA: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 3.03 ret.steerRatio = 16.0 tire_stiffness_factor = 0.444 ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00007818594 ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 #detect the Pedal address ret.enableGasInterceptor = 0x201 in fingerprint # 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. if ( stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS # 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 = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU) ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS) ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs) cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.steerLimitAlert = False ret.longitudinalTuning.deadzoneBP = [0., 9.] ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kiBP = [0., 35.] ret.stoppingControl = False ret.startAccel = 0.0 if ret.enableGasInterceptor: ret.gasMaxBP = [0., 9., 35] ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiV = [0.18, 0.12] else: ret.gasMaxBP = [0.] ret.gasMaxV = [0.5] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiV = [0.54, 0.36] 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) # run the cam can update for 10s as we just need to know if the camera is alive if self.frame < 1000: self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.cp) # create message ret = car.CarState.new_message() ret.canValid = self.cp.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.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) 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 if self.CP.enableGasInterceptor: # use interceptor values to disengage on pedal press ret.gasPressed = self.CS.pedal_gas > 15 else: 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 = 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_active ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: # ignore standstill in hybrid vehicles, since pcm allows to restart without # receiving any special command # also if interceptor is detected ret.cruiseState.standstill = False else: ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 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 self.cp_cam.can_valid: self.forwarding_camera = True if not ret.gearShifter == 'drive' and self.CP.enableDsu: 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 and self.CP.enableDsu: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on and self.CP.enableDsu: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse' and self.CP.enableDsu: events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if (self.CS.left_blinker_on or self.CS.right_blinker_on ) and params.get("DragonEnableSteeringOnSignal") == "1": events.append( create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) elif self.CS.steer_error: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.low_speed_lockout and self.CP.enableDsu: events.append( create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: 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])) # DragonAllowGas if params.get("DragonAllowGas") == "0": # 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])) else: if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) 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() # pass in a car.CarControl # to be called @ 100hz 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.audibleAlert, self.forwarding_camera, c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible, 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.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.cp = get_can_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, CP.enableDsu, CP.enableApgs) @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 = "toyota" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.toyota # pedal ret.enableCruise = True # 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 = 85400 tireStiffnessRear_civic = 90000 ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 15.0 ret.mass = 3045 * CV.LB_TO_KG + std_cargo ret.steerKpV, ret.steerKiV = [[0.4], [0.01]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = 1.5 f = 1.43353663 tireStiffnessFront_civic *= f tireStiffnessRear_civic *= f # Prius has a very bad actuator ret.steerActuatorDelay = 0.25 elif candidate in [CAR.RAV4, CAR.RAV4H]: ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.65 ret.steerRatio = 14.5 # Rav4 2017 ret.mass = 3650 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = 1. elif candidate == CAR.COROLLA: ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 17.8 ret.mass = 2860 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.2], [0.05]] ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerRateCost = 1. elif candidate == CAR.LEXUS_RXH: ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.79 ret.steerRatio = 16. # official specs say 14.8, but it does not seem right ret.mass = 4481 * CV.LB_TO_KG + std_cargo # mean between min and max ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = .8 ret.centerToFront = ret.wheelbase * 0.44 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. if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH]: # rav4 hybrid can do stop and go ret.minEnableSpeed = -1. elif candidate in [CAR.RAV4, CAR.COROLLA]: # TODO: hack ICE to do stop and go ret.minEnableSpeed = 19. * CV.MPH_TO_MS 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 * \ ret.mass / mass_civic * \ (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) ret.tireStiffnessRear = tireStiffnessRear_civic * \ 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 = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph ret.steerMaxV = [1., 1.] # 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 = not check_ecu_msgs(fingerprint, candidate, ECU.CAM) ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU) ret.enableApgs = False #not check_ecu_msgs(fingerprint, candidate, ECU.APGS) print "ECU Camera Simulated: ", ret.enableCamera print "ECU DSU Simulated: ", ret.enableDsu print "ECU APGS Simulated: ", ret.enableApgs 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.CS.update(self.cp) # 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.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) 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 = 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 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. if self.CP.carFingerprint == CAR.RAV4H: # ignore standstill in hybrid rav4, since pcm allows to restart without # receiving any special command ret.cruiseState.standstill = False else: ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 # 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' and self.CP.enableDsu: 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 and self.CP.enableDsu: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on and self.CP.enableDsu: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse' and self.CP.enableDsu: 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.CS.low_speed_lockout and self.CP.enableDsu: events.append( create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: 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): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert) self.frame += 1 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 # *** init the major players *** self.CS = CarState(CP) self.cp = get_can_parser(CP) self.cp_cam = get_cam_can_parser(CP) self.CC = None if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) @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 = "toyota" ret.carFingerprint = candidate ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.toyota ret.enableCruise = True ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] if candidate == CAR.PRIUS: stop_and_go = True ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 15.74 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 4.0 ret.lateralTuning.indi.outerLoopGain = 3.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.0 # TODO: Determine if this is better than INDI # ret.lateralTuning.init('lqr') # ret.lateralTuning.lqr.scale = 1500.0 # ret.lateralTuning.lqr.ki = 0.01 # ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] # ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] # ret.lateralTuning.lqr.c = [1., 0.] # ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255] # ret.lateralTuning.lqr.l = [0.03233671, 0.03185757] # ret.lateralTuning.lqr.dcGain = 0.002237852961363602 ret.steerActuatorDelay = 0.5 elif candidate in [CAR.RAV4, CAR.RAV4H]: stop_and_go = True if (candidate in CAR.RAV4H) else False ret.safetyParam = 73 ret.wheelbase = 2.65 ret.steerRatio = 16.88 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1500.0 ret.lateralTuning.lqr.ki = 0.05 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255] ret.lateralTuning.lqr.l = [0.3233671, 0.3185757] ret.lateralTuning.lqr.dcGain = 0.002237852961363602 elif candidate == CAR.COROLLA: stop_and_go = False ret.safetyParam = 100 ret.wheelbase = 2.70 ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RX: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.79 ret.steerRatio = 14.8 tire_stiffness_factor = 0.5533 ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] ret.lateralTuning.pid.kf = 0.00006 elif candidate == CAR.LEXUS_RXH: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.79 ret.steerRatio = 16. # 14.8 is spec end-to-end tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RX_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.79 ret.steerRatio = 14.8 tire_stiffness_factor = 0.5533 # not optimized yet ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate in [CAR.CHR, CAR.CHRH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.6 tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]] ret.lateralTuning.pid.kf = 0.00006 elif candidate in [CAR.CAMRY, CAR.CAMRYH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.82448 ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 elif candidate == CAR.HIGHLANDER_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.84988 # 112.2 in = 2.84988 m ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.18 ], [0.015]] # community tuning ret.lateralTuning.pid.kf = 0.00012 # community tuning elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.78 ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid limited ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.18 ], [0.015]] # community tuning ret.lateralTuning.pid.kf = 0.00012 # community tuning elif candidate == CAR.AVALON: stop_and_go = False ret.safetyParam = 73 ret.wheelbase = 2.82 ret.steerRatio = 14.8 #Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]] ret.lateralTuning.pid.kf = 0.00006 elif candidate == CAR.RAV4_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.68986 ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.RAV4H_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.68986 ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.mass = 3800. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kf = 0.00007818594 elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.8702 ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.SIENNA: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 3.03 ret.steerRatio = 16.0 tire_stiffness_factor = 0.444 ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.LEXUS_IS: stop_and_go = False ret.safetyParam = 77 ret.wheelbase = 2.79908 ret.steerRatio = 13.3 tire_stiffness_factor = 0.444 ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00006 elif candidate == CAR.LEXUS_CTH: stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.60 ret.steerRatio = 18.6 tire_stiffness_factor = 0.517 ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00007 elif candidate == CAR.LEXUS_NXH: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.66 ret.steerRatio = 14.7 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 # 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 = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay # In TSS2 cars the camera does long control ret.enableDsu = is_ecu_disconnected( fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.apgs) ret.enableGasInterceptor = 0x201 in fingerprint[0] ret.openpilotLongitudinalControl = ret.enableCamera and ( ret.enableDsu or candidate in TSS2_CAR) cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs) cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) # 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. if ( stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS # removing the DSU disables AEB and it's considered a community maintained feature ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu ret.longitudinalTuning.deadzoneBP = [0., 9.] ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kiBP = [0., 35.] ret.stoppingControl = False ret.startAccel = 0.0 if ret.enableGasInterceptor: ret.gasMaxBP = [0., 9., 35] ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiV = [0.18, 0.12] else: ret.gasMaxBP = [0.] ret.gasMaxV = [0.5] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiV = [0.54, 0.36] 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.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) 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 if self.CP.enableGasInterceptor: # use interceptor values to disengage on pedal press ret.gasPressed = self.CS.pedal_gas > 15 else: 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 = self.CS.angle_steers_rate ret.steeringTorque = self.CS.steer_torque_driver ret.steeringTorqueEps = self.CS.steer_torque_motor ret.steeringPressed = self.CS.steer_override ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_active ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: # ignore standstill in hybrid vehicles, since pcm allows to restart without # receiving any special command # also if interceptor is detected ret.cruiseState.standstill = False else: ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 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 ret.genericToggle = self.CS.generic_toggle ret.stockAeb = self.CS.stock_aeb # events events = [] if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera: events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT])) if not ret.gearShifter == GearShifter.drive and self.CP.openpilotLongitudinalControl: 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 and self.CP.openpilotLongitudinalControl: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on and self.CP.openpilotLongitudinalControl: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl: 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.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: events.append( create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: 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 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): 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.leadVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 return can_sends
class CarInterface(object): def __init__(self, CP, logcan, sendcan=None): self.logcan = logcan self.CP = CP self.frame = 0 self.can_invalid_count = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.cruise_enabled_prev = False # *** init the major players *** self.CS = CarState(CP, self.logcan) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) @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 = "toyota" ret.radarName = "toyota" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.toyota ret.enableSteer = True ret.enableBrake = True # pedal ret.enableCruise = True # FIXME: hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars m_civic = 2923./2.205 + std_cargo l_civic = 2.70 aF_civic = l_civic * 0.4 aR_civic = l_civic - aF_civic j_civic = 2500 cF_civic = 85400 cR_civic = 90000 stop_and_go = True ret.m = 3045./2.205 + std_cargo ret.l = 2.70 ret.aF = ret.l * 0.44 ret.sR = 14.5 #Rav4 2017, TODO: find exact value for Prius if candidate == CAR.PRIUS: ret.steerKp, ret.steerKi = 0.2, 0.01 elif candidate == CAR.RAV4: # rav4 control seem to be ok with integrators ret.steerKp, ret.steerKi = 0.2, 0.05 ret.steerKf = 0.00007818594 # full torque for 10 deg at 80mph # 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. if candidate == CAR.PRIUS: ret.minEnableSpeed = -1. elif candidate == CAR.RAV4: # TODO: hack Rav4 to do stop and go ret.minEnableSpeed = 19. * CV.MPH_TO_MS ret.aR = ret.l - ret.aF # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_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.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic) ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic) # no rear steering, at least on the listed cars above ret.chi = 0. # steer, gas, brake limitations VS speed ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph ret.steerMaxV = [1., 1.] # 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 = not check_ecu_msgs(fingerprint, candidate, ECU.CAM) ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU) ret.enableApgs = False # not check_ecu_msgs(fingerprint, candidate, ECU.APGS) print "ECU Camera Simulated: ", ret.enableCamera print "ECU DSU Simulated: ", ret.enableDsu print "ECU APGS Simulated: ", ret.enableApgs ret.enableGas = True ret.steerLimitAlert = False return ret @staticmethod def compute_gb(accel, speed): # toyota interface is already in accelration cmd, so conversion to gas-brake it's a pass-through. return accel # returns a car.CarState def update(self, c): # ******************* do can recv ******************* can_pub_main = [] canMonoTimes = [] self.CS.update() # 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.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 / 256.0 ret.gasPressed = self.CS.pedal_gas > 0 # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate ret.steeringTorque = 0 ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 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 = '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 # 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' and self.CP.enableDsu: events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.door_all_closed: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.seatbelt: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled and self.CP.enableDsu: events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on and self.CP.enableDsu: events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse' and self.CP.enableDsu: 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.CS.low_speed_lockout: events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: 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): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert) self.frame += 1 return False
class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP self.VM = VehicleModel(CP) self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.cam_can_valid_count = 0 self.cruise_enabled_prev = False # *** init the major players *** self.CS = CarState(CP) self.cp = get_can_parser(CP) self.cp_cam = get_cam_can_parser(CP) self.forwarding_camera = False # 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, CP.enableDsu, CP.enableApgs) @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 = "toyota" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.toyota # pedal ret.enableCruise = not ret.enableGasInterceptor # 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.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.001 # Default delay, Prius has larger delay if candidate == CAR.PRIUS: stop_and_go = True ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 16.26 # 0.5.10 auto tune tire_stiffness_factor = 0.7549 # hand-tune by Gernby ret.mass = 3375 * CV.LB_TO_KG + std_cargo ret.steerKpV, ret.steerKiV = [[0.4], [0.05]] ret.steerKf = 0.0001 # full torque for 10 deg at 80mph means 0.00007818594 # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis ret.steerActuatorDelay = 0.018 #ret.steerKiBP, ret.steerKpBP = [[0.,27.,47.], [0.,27.,47.]] if ret.enableGasInterceptor: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [3.6, 2.4, 1.5] ret.longitudinalKiV = [0.54, 0.36] else: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [3.6, 2.4, 1.5] ret.longitudinalKiV = [0.54, 0.36] elif candidate in [CAR.RAV4]: ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.66 # 2.65 default ret.steerRatio = 17.28 # Rav4 0.5.10 tuning value ret.mass = 4100. / 2.205 + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.3], [0.03]] #0.6 0.05 default ret.wheelbase = 2.65 tire_stiffness_factor = 0.5533 ret.steerKf = 0.00001 # full torque for 10 deg at 80mph means 0.00007818594 if ret.enableGasInterceptor: stop_and_go = True ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [0.1, 0.8, 0.8] ret.longitudinalKiV = [0.06, 0.12] else: stop_and_go = False ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [3.6, 1.1, 1.0] ret.longitudinalKiV = [0.5, 0.24] elif candidate in [CAR.RAV4H]: stop_and_go = True ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.65 # 2.65 default ret.steerRatio = 16.53 # 0.5.10 tuning ret.mass = 4100. / 2.205 + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.3], [0.03]] #0.6 0.05 default ret.wheelbase = 2.65 tire_stiffness_factor = 0.5533 ret.steerKf = 0.0001 # full torque for 10 deg at 80mph means 0.00007818594 if ret.enableGasInterceptor: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiV = [0.18, 0.12] else: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [1.0, 0.5, 0.3] ret.longitudinalKiV = [0.20, 0.10] elif candidate == CAR.RAV4_2019: stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.68986 ret.steerRatio = 17.0 tire_stiffness_factor = 0.7933 ret.mass = 3370. * CV.LB_TO_KG + std_cargo ret.steerKpV, ret.steerKiV = [[0.3], [0.05]] ret.steerKf = 0.0001 ret.longitudinalKpV = [2.0, 1.0, 0.8] ret.longitudinalKiV = [0.25, 0.14] ret.gasMaxV = [0.2, 0.5, 0.7] elif candidate == CAR.COROLLA: stop_and_go = False ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 17.84 # 0.5.10 tire_stiffness_factor = 1.01794 ret.mass = 2860 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.2], [0.05]] ret.steerKf = 0.00003909297 # full torque for 20 deg at 80mph means 0.00007818594 if ret.enableGasInterceptor: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiV = [0.18, 0.12] else: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [3.6, 1.1, 1.0] ret.longitudinalKiV = [0.5, 0.24] elif candidate == CAR.LEXUS_RXH: stop_and_go = True ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.79 ret.steerRatio = 18.6 # 0.5.10 tire_stiffness_factor = 0.517 # 0.5.10 ret.mass = 4481 * CV.LB_TO_KG + std_cargo # mean between min and max ret.steerKpV, ret.steerKiV = [[0.2], [0.02]] ret.steerKf = 0.00007 # full torque for 10 deg at 80mph means 0.00007818594 if ret.enableGasInterceptor: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiV = [0.18, 0.12] else: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [3.6, 1.1, 1.0] ret.longitudinalKiV = [0.5, 0.24] elif candidate in [CAR.CHR, CAR.CHRH]: stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.63906 ret.steerRatio = 15.6 tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + std_cargo ret.steerKpV, ret.steerKiV = [[0.3], [0.03]] ret.steerKf = 0.0001 if ret.enableGasInterceptor: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiV = [0.18, 0.12] else: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [3.6, 1.1, 1.0] ret.longitudinalKiV = [0.5, 0.24] elif candidate in [CAR.CAMRY, CAR.CAMRYH]: stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.82448 ret.steerRatio = 17.7 # 0.5.10 tire_stiffness_factor = 0.7933 ret.mass = 3400 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.3], [0.03]] ret.steerKf = 0.0001 if ret.enableGasInterceptor: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiV = [0.18, 0.12] else: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [3.6, 1.1, 1.0] ret.longitudinalKiV = [0.5, 0.24] elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.78 ret.steerRatio = 17.36 # 0.5.10 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4607 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid limited ret.steerKpV, ret.steerKiV = [[0.18], [0.0075]] ret.steerKf = 0.00015 if ret.enableGasInterceptor: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiV = [0.18, 0.12] else: ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalKpV = [3.6, 1.1, 1.0] ret.longitudinalKiV = [0.5, 0.24] ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 ret.longPidDeadzoneBP = [0., 9.] ret.longPidDeadzoneV = [0., .15] #detect the Pedal address ret.enableGasInterceptor = 0x201 in fingerprint # 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. if ( stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS 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 = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph ret.gasMaxBP = [0., 9., 35.] #ret.gasMaxV = [0.2, 0.5, 0.7] ret.brakeMaxBP = [5., 20.] ret.brakeMaxV = [1., 0.8] ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU) ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS) ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs) cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.steerLimitAlert = False ret.longitudinalKpBP = [0., 5., 55.] ret.longitudinalKiBP = [0., 55.] ret.stoppingControl = False ret.startAccel = 0.0 ret.longitudinalKpBP = [0., 5., 55.] ret.longitudinalKiBP = [0., 55.] # returns a car.CarState return ret 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.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) 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 if self.CP.enableGasInterceptor: # use interceptor values to disengage on pedal press ret.gasPressed = self.CS.pedal_gas > 15 else: 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 + steeringAngleoffset ret.steeringRate = 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_active ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. if self.CP.carFingerprint in [ CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_2019 ] or self.CP.enableGasInterceptor: # ignore standstill in hybrid vehicles, since pcm allows to restart without # receiving any special command # also if interceptor is detected ret.cruiseState.standstill = False else: ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 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.leftline = self.CS.left_line #ret.rightline = self.CS.right_line #ret.lkasbarriers = self.CS.lkas_barriers ret.blindspot = self.CS.blind_spot_on ret.blindspotside = self.CS.blind_spot_side ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt ret.genericToggle = self.CS.generic_toggle ret.laneDepartureToggle = self.CS.lane_departure_toggle_on ret.distanceToggle = self.CS.distance_toggle ret.accSlowToggle = self.CS.acc_slow_on ret.readdistancelines = self.CS.read_distance_lines ret.gasbuttonstatus = self.CS.gasMode if self.CS.sport_on == 1: ret.gasbuttonstatus = 1 if self.CS.econ_on == 1: ret.gasbuttonstatus = 2 # 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 self.CS.cam_can_valid: self.cam_can_valid_count += 1 if self.cam_can_valid_count >= 5: self.forwarding_camera = True if ret.cruiseState.enabled and not self.cruise_enabled_prev: disengage_event = True else: disengage_event = False if not ret.gearShifter == 'drive' and self.CP.enableDsu: events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen and disengage_event: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched and disengage_event: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled and self.CP.enableDsu: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on and self.CP.enableDsu: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse' and self.CP.enableDsu: 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.CS.low_speed_lockout and self.CP.enableDsu: events.append( create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if self.CS.steer_unavailable: events.append( create_event('steerTempUnavailableNoCancel', [ET.WARNING])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: 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): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert, self.forwarding_camera, c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 return False