class TestVehicleModel(unittest.TestCase): def setUp(self): CP = CarInterface.get_params(CAR.CIVIC) self.VM = VehicleModel(CP) def test_round_trip_yaw_rate(self): # TODO: fix VM to work at zero speed for u in np.linspace(1, 30, num=10): for roll in np.linspace(math.radians(-20), math.radians(20), num=11): for sa in np.linspace(math.radians(-20), math.radians(20), num=11): yr = self.VM.yaw_rate(sa, u, roll) new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll) self.assertAlmostEqual(sa, new_sa) def test_dyn_ss_sol_against_yaw_rate(self): """Verify that the yaw_rate helper function matches the results from the state space model.""" for roll in np.linspace(math.radians(-20), math.radians(20), num=11): for u in np.linspace(1, 30, num=10): for sa in np.linspace(math.radians(-20), math.radians(20), num=11): # Compute yaw rate based on state space model _, yr1 = dyn_ss_sol(sa, u, roll, self.VM) # Compute yaw rate using direct computations yr2 = self.VM.yaw_rate(sa, u, roll) self.assertAlmostEqual(float(yr1), yr2) def test_syn_ss_sol_simulate(self): """Verifies that dyn_ss_sol mathes a simulation""" for roll in np.linspace(math.radians(-20), math.radians(20), num=11): for u in np.linspace(1, 30, num=10): A, B = create_dyn_state_matrices(u, self.VM) # Convert to discrete time system ss = StateSpace(A, B, np.eye(2), np.zeros((2, 2))) ss = ss.sample(0.01) for sa in np.linspace(math.radians(-20), math.radians(20), num=11): inp = np.array([[sa], [roll]]) # Simulate for 1 second x1 = np.zeros((2, 1)) for _ in range(100): x1 = ss.A @ x1 + ss.B @ inp # Compute steady state solution directly x2 = dyn_ss_sol(sa, u, roll, self.VM) np.testing.assert_almost_equal(x1, x2, decimal=3)
def test_convergence(self): # Setup vehicle model with wrong parameters VM_sim = VehicleModel(self.CP) x_target = 0.75 sr_target = self.CP.steerRatio - 0.5 ao_target = -1.0 VM_sim.update_params(x_target, sr_target) # Run simulation times = np.arange(0, 15 * 3600, 0.01) angle_offset = np.radians(ao_target) steering_angles = np.radians( 10 * np.sin(2 * np.pi * times / 100.)) + angle_offset speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25 for i, _ in enumerate(times): u = speeds[i] sa = steering_angles[i] psi = VM_sim.yaw_rate(sa - angle_offset, u) liblocationd.params_learner_update(self.params_learner, psi, u, sa) # Verify learned parameters sr = liblocationd.params_learner_get_sR(self.params_learner) ao_slow = np.degrees( liblocationd.params_learner_get_slow_ao(self.params_learner)) x = liblocationd.params_learner_get_x(self.params_learner) self.assertAlmostEqual(x_target, x, places=1) self.assertAlmostEqual(ao_target, ao_slow, places=1) self.assertAlmostEqual(sr_target, sr, places=1)
def controlsd_thread(gctx=None, rate=100): gc.disable() # start the loop set_realtime_priority(3) ##### AS context = zmq.Context() live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) carla_socket = context.socket(zmq.PAIR) carla_socket.bind("tcp://*:5560") is_metric = True passive = True ##### AS # No sendcan if passive if not passive: sendcan = messaging.pub_sock(context, service_list['sendcan'].port) else: sendcan = None # Sub sockets poller = zmq.Poller() #thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller) #health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller) cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller) #driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller) plan_sock = messaging.sub_sock(context, service_list['plan'].port, conflate=True, poller=poller) path_plan_sock = messaging.sub_sock(context, service_list['pathPlan'].port, conflate=True, poller=poller) #logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {}) CP.steerRatio = 1.0 CI = ToyotaInterface(CP, sendcan) if CI is None: raise Exception("unsupported car") # if stock camera is connected, then force passive behavior if not CP.enableCamera: passive = True sendcan = None if passive: CP.safetyModel = car.CarParams.SafetyModels.noOutput LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(CP) AM = AlertManager() if not passive: AM.add("startup", False) state = State.enabled soft_disable_timer = 0 v_cruise_kph = 50 ##### !!! change v_cruise_kph_last = 0 ##### !! change cal_status = Calibration.CALIBRATED cal_perc = 0 plan = messaging.new_message() plan.init('plan') path_plan = messaging.new_message() path_plan.init('pathPlan') rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) angle_offset = 0. prof = Profiler(False) # off by default startup = True ##### AS while True: start_time = int(sec_since_boot() * 1e9) prof.checkpoint("Ratekeeper", ignore=True) if cal_status != Calibration.CALIBRATED: assert (1 == 0), 'Got uncalibrated for some reason' stuff = recv_array(carla_socket) current_vel = float(stuff[0]) current_steer = float(stuff[1]) v_cruise_kph = float(stuff[2]) updateInternalCS(CI.CS, current_vel, current_steer, 0, v_cruise_kph) CS = returnNewCS(CI) events = list(CS.events) ##### AS since we dont do preenabled state if startup: LaC.reset() LoC.reset(v_pid=CS.vEgo) v_acc = 0 a_acc = 0 AM.process_alerts(0.0) startup = False ASsend_live100_CS(plan, path_plan, CS, VM, state, events, v_cruise_kph, AM, LaC, LoC, angle_offset, v_acc, a_acc, rk, start_time, live100, carstate) cal_status, cal_perc, plan, path_plan =\ ASdata_sample(plan_sock, path_plan_sock, cal, poller, cal_status, cal_perc, state, plan, path_plan) prof.checkpoint("Sample") path_plan_age = (start_time - path_plan.logMonoTime) / 1e9 plan_age = (start_time - plan.logMonoTime) / 1e9 if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5: print 'planner time too long or invalid' #events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events += list(plan.plan.events) # Only allow engagement with brake pressed when stopped behind another stopped car #if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: # events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not passive: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") v_acc, a_acc = \ ASstate_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc) prof.checkpoint("State Control") # Publish data yawrate = VM.yaw_rate(math.radians(path_plan.pathPlan.angleSteers), v_acc) beta = (VM.aR + VM.aF * VM.chi - VM.m * v_acc**2 / VM.cF / VM.cR / VM.l * (VM.cF * VM.aF - VM.cR * VM.aR * VM.chi)) beta *= yawrate / (1 - VM.chi) / v_acc send_array( carla_socket, np.array([ v_acc, path_plan.pathPlan.angleSteers, -3.3 * math.degrees(yawrate), -3.3 * math.degrees(beta) ])) prof.checkpoint("Sent") rk.monitor_time() # Run at 100Hz, no 20 Hz prof.display()
class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.acc_active_prev = 0 # *** init the major players *** canbus = CanBus() self.CS = CarState(CP, canbus) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP, canbus) self.ch_cp = get_chassis_can_parser(CP, canbus) self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] self.CC = None if CarController is not None: self.CC = CarController(canbus, CP.carFingerprint) @staticmethod def compute_gb(accel, speed): # Ripped from compute_gb_honda in Honda's interface.py. Works well off shelf but may need more tuning creep_brake = 0.0 creep_speed = 2.68 creep_brake_value = 0.10 if speed < creep_speed: creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value return float(accel) / 4.8 - creep_brake @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): # normalized max accel. Allowing max accel at low speed causes speed overshoots max_accel_bp = [10, 20] # m/s max_accel_v = [0.85, 1.0] # unit of max accel max_accel = interp(v_ego, max_accel_bp, max_accel_v) # limit the pcm accel cmd if: # - v_ego exceeds v_target, or # - a_ego exceeds a_target and v_ego is close to v_target eA = a_ego - a_target valuesA = [1.0, 0.1] bpA = [0.3, 1.1] eV = v_ego - v_target valuesV = [1.0, 0.1] bpV = [0.0, 0.5] valuesRangeV = [1., 0.] bpRangeV = [-1., 0.] # only limit if v_ego is close to v_target speedLimiter = interp(eV, bpV, valuesV) accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart return float(max(max_accel, a_target / FOLLOW_AGGRESSION)) * min( speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "gm" ret.carFingerprint = candidate ret.carVin = vin ret.isPandaBlack = has_relay ret.enableCruise = False # GM port is considered a community feature, since it disables AEB; # TODO: make a port that uses a car harness and it only intercepts the camera ret.communityFeature = True # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or \ has_relay or \ candidate == CAR.CADILLAC_CT6 ret.openpilotLongitudinalControl = ret.enableCamera tire_stiffness_factor = 0.444 # not optimized yet # same tuning for Volt and CT6 for now ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerRateCost = 1.0 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 8 * CV.MPH_TO_MS ret.mass = 1607. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.69 ret.steerRatio = 15.7 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.MALIBU: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1496. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 ret.steerRatio = 15.8 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.HOLDEN_ASTRA: ret.mass = 1363. + STD_CARGO_KG ret.wheelbase = 2.662 # Remaining parameters copied from Volt for now ret.centerToFront = ret.wheelbase * 0.4 ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.safetyModel = car.CarParams.SafetyModel.gm ret.steerRatio = 15.7 ret.steerRatioRear = 0. elif candidate == CAR.ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.86 ret.steerRatio = 14.4 #end to end is 13.46 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 elif candidate == CAR.BUICK_REGAL: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 #111.4 inches in meters ret.steerRatio = 14.4 # guess for tourx ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx elif candidate == CAR.CADILLAC_ATS: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1601. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.78 ret.steerRatio = 15.3 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.49 elif candidate == CAR.CADILLAC_CT6: # engage speed is decided by pcm ret.minEnableSpeed = -1. ret.mass = 4016. * CV.LB_TO_KG + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.cadillac ret.wheelbase = 3.11 ret.steerRatio = 14.6 # it's 16.3 without rear active steering ret.steerRatioRear = 0. # TODO: there is RAS on this car! ret.centerToFront = ret.wheelbase * 0.465 # 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) ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.gasMaxBP = [0.] ret.gasMaxV = [0.5] ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] ret.longitudinalTuning.kpBP = [5., 35.] ret.longitudinalTuning.kpV = [2.4, 1.5] ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.36] ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] ret.stoppingControl = True ret.startAccel = 0.8 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerLimitTimer = 0.4 ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz ret.steerControlType = car.CarParams.SteerControlType.torque return ret # returns a car.CarState def update(self, c, can_strings): self.pt_cp.update_strings(can_strings) self.ch_cp.update_strings(can_strings) self.CS.update(self.pt_cp, self.ch_cp) # create message ret = car.CarState.new_message() ret.canValid = self.pt_cp.can_valid # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal information. ret.gas = self.CS.pedal_gas / 254.0 ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake / 0xd0 ret.brakePressed = self.CS.brake_pressed ret.brakeLights = self.CS.frictionBrakesActive # steering wheel ret.steeringAngle = self.CS.angle_steers # torque and user override. Driver awareness # timer resets when the user uses the steering wheel. ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # cruise state ret.cruiseState.available = bool(self.CS.main_on) cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF ret.cruiseState.enabled = cruiseEnabled ret.cruiseState.standstill = False ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt ret.gearShifter = self.CS.gear_shifter ret.readdistancelines = self.CS.follow_level buttonEvents = [] # blinkers 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 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 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: if not (cruiseEnabled and self.CS.standstill): be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed. elif but == CruiseButtons.DECEL_SET: if not cruiseEnabled and not self.CS.lkMode: self.lkMode = True be.type = ButtonType.decelCruise elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) ret.buttonEvents = buttonEvents if cruiseEnabled and self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button: self.CS.lkMode = not self.CS.lkMode if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button: self.CS.follow_level -= 1 if self.CS.follow_level < 1: self.CS.follow_level = 3 events = [] if not self.CS.lkMode: events.append(create_event('manualSteeringRequired', [ET.WARNING])) #if cruiseEnabled and (self.CS.left_blinker_on or self.CS.right_blinker_on): # events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.steer_not_allowed: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) 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.car_fingerprint in SUPERCRUISE_CARS: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) else: if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not self.CS.gear_shifter_valid: events.append( create_event('wrongGear', [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 self.CS.gear_shifter == 3: events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, 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])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: events.append( create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons # The ECM will fault if resume triggers an enable while speed is set to 0 if b.type == ButtonType.accelCruise and c.hudControl.setSpeed > 0 and c.hudControl.setSpeed < 70 and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) if b.type == ButtonType.decelCruise and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == ButtonType.cancel and b.pressed: events.append( create_event('buttonCancel', [ET.USER_DISABLE])) # The ECM independently tracks a ‘speed is set’ state that is reset on main off. # To keep controlsd in sync with the ECM state, generate a RESET_V_CRUISE event on main cruise presses. if b.type == ButtonType.altButton3 and b.pressed: events.append( create_event('buttonCancel', [ET.RESET_V_CRUISE, ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: hud_v_cruise = 0 # For Openpilot, "enabled" includes pre-enable. # In GM, PCM faults out if ACC command overlaps user gas. enabled = c.enabled and not self.CS.user_gas_pressed can_sends = self.CC.update(enabled, self.CS, self.frame, \ c.actuators, hud_v_cruise, c.hudControl.lanesVisible, \ c.hudControl.leadVisible, c.hudControl.visualAlert) self.frame += 1 return can_sends
class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.cp = get_can_parser(CP) # 2018.09.10 2:53PM move to 108 # *** init the major playeselfdrive/car/kia/carstate.pyrs *** # canbus = CanBus() self.CS = CarState(CP) #2018.09.07 11:33AM remove copy from subaru add in canbus borrow from subaru interface.py self.VM = VehicleModel(CP) # self.cp = get_can_parser(CP) #2018.09.05 borrow from subaru delete powertrain # sending if read only is False if sendcan is not None: self.sendcan = sendcan #2018.09.05 11:41PM change dbc_name to canbus self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera) # 2018.09.10 add CP.carFingerprint #print("self.cc interface.py dp.dbc_name") #print(self.cp.dbc_name) #print("interface.py CP.carFingerprint") # print(CP.carFingerprint) # print(CP.enableCamera) # if self.CS.CP.carFingerprint == CAR.DUMMY: #2018.09.06 12:43AM dummy car for not use # self.compute_gb = get_compute_gb_acura() # else: self.compute_gb = compute_gb_honda #2018.09.06 2:56PM remove parentheses @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): # limit the pcm accel cmd if: # - v_ego exceeds v_target, or # - a_ego exceeds a_target and v_ego is close to v_target eA = a_ego - a_target valuesA = [1.0, 0.1] bpA = [0.3, 1.1] eV = v_ego - v_target valuesV = [1.0, 0.1] bpV = [0.0, 0.5] valuesRangeV = [1., 0.] bpRangeV = [-1., 0.] # only limit if v_ego is close to v_target speedLimiter = interp(eV, bpV, valuesV) accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart return float(max(0.714, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carName = "kia" ret.carFingerprint = candidate #remove #radar off can from base #2018.09.16 10:09PMEST set all output #panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) #ret.safetyModel = car.CarParams.SafetyModels.kia #2018.09.16 10:11PMEST set to all output ret.safetyModel = car.CarParams.SafetyModels.subaru #ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.enableCamera = not any(x for x in CAMERA_MSGS_SOUL if x in fingerprint) #2018.09.02 DV change for KIA message #ret.enableGasInterceptor = 0x201 in fingerprint ret.enableGasInterceptor = 0x93 in fingerprint #2018.09.02 DV change for gas interceptor to throttle report cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor # kg of standard extra cargo to count for drive, gas, etc... std_cargo = 136 # 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 # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle # model optimization process. Certain Hondas have an extra steering sensor at the bottom # of the steering rack, which improves controls quality as it removes the steering column # torsion from feedback. # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] # # 2018.09.04 # full torque for 20 deg at 80mph means 0.00007818594 comment from hyundai ret.steerKf = 0.00006 # conservative feed-forward #2018.09.02 DV add Kia soul #TODO need to modified paramater if candidate == CAR.SOUL: stop_and_go = False # ret.safetyParam = 8 # define in /boardd/boardd.cc ret.mass = 3410. * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] # 2018.09.04 from hyundai ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.SOUL1: stop_and_go = False #ret.safetyParam = 8 # define in /boardd/boardd.cc ret.mass = 3410. * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] # 2018.09.04 from hyundai ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.SOUL2: stop_and_go = False # ret.safetyParam = 8 # define in /boardd/boardd.cc ret.mass = 3410. * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] # 2018.09.04 from hyundai ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] else: raise ValueError("unsupported car %s" % candidate) #TODO 2018.09.04 should modified our steering control type to match kia angle ret.steerControlType = car.CarParams.SteerControlType.torque #ret.steerControlType = car.CarParams.SteerControlType.angle # 2018.09.06 11:19PM reference in cereal car.capnp # 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. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # TODO: 2018.09.02 need to scale this value for Kia soul and other car # 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. #2018.09.02 Add separation for Kia soul and other below if candidate == CAR.SOUL: # no max steer limit VS speed ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] # max steer allowed #Todo:2018.09.02 tune in car and change ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed #TODO: 2018.09.02 confirm in car ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.stoppingControl = True ret.steerLimitAlert = True ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 elif candidate == CAR.SOUL1: # no max steer limit VS speed ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] # max steer allowed #Todo:2018.09.02 tune in car and change ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed #TODO: 2018.09.02 confirm in car ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.stoppingControl = True ret.steerLimitAlert = True ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 elif candidate == CAR.SOUL2: # no max steer limit VS speed ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] # max steer allowed #Todo:2018.09.02 tune in car and change ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed #TODO: 2018.09.02 confirm in car ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.stoppingControl = True ret.steerLimitAlert = True ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 else: # no max steer limit VS speed ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.stoppingControl = True ret.steerLimitAlert = True ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 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() #2018.09.05 add generic toggle for testing steering max# #ret.genericToggle = self.CS.generic_toggle # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw #2018.09.04 change to vehicle yaw rate values ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ## ret.yawRate = self.CS.yaw_rate 2018.09.10 use yaw calculation from angle 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 / 256.0 ret.gas = self.CS.car_gas #2018.09.13 12:39AM change gas to car_gas without deviding #TODO 2018.09.05 check Throttle report operator override match this if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.SOUL else 0.1 #2018.09.03 change CIVIC to soul ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers # ret.steeringRate = self.CS.angle_steers_rate #2018.09.11 TODO some can duplication error? or not reading # gear shifter lever (#2018.09.04 define in carstate.py ret.gearShifter = self.CS.gear_shifter #2018.09.05 #TODO find steering torque value from steering wheel ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override #2018.09.02 this come values py when steering operator override =1 #2018.09.02 TODO need to check is matter, we not using kia cruise control # cruise state #ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.enabled = False #2018.09.11 if use Openpilot cruise, no need, this should set to 0, need set to False #ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS #2018.09.11 check in controlsd.py if not enable cruise, this not needed ret.cruiseState.speed = 0 # 2018.09.11 check in controlsd.py if not enable cruise, this not needed, set to 0 ret.cruiseState.available = bool(self.CS.main_on) #2018.09.11 not use anywhere else beside interface.py ret.cruiseState.speedOffset = self.CS.cruise_speed_offset #2018.09.11 finally not use in controlsd.py if we use interceptor and enablecruise=false ret.cruiseState.standstill = False # TODO: button presses 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 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) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time 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.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) 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])) #remove brake hold and #electric parking brake if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # 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])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c, perception_state=log.Live20Data.new_message()): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: hud_v_cruise = 255 hud_alert = { "none": AH.NONE, "fcw": AH.FCW, "steerRequired": AH.STEER, "brakePressed": AH.BRAKE_PRESSED, "wrongGear": AH.GEAR_NOT_D, "seatbeltUnbuckled": AH.SEATBELT, "speedTooHigh": AH.SPEED_TOO_HIGH}[str(c.hudControl.visualAlert)] snd_beep, snd_chime = { "none": (BP.MUTE, CM.MUTE), "beepSingle": (BP.SINGLE, CM.MUTE), "beepTriple": (BP.TRIPLE, CM.MUTE), "beepRepeated": (BP.REPEATED, CM.MUTE), "chimeSingle": (BP.MUTE, CM.SINGLE), "chimeDouble": (BP.MUTE, CM.DOUBLE), "chimeRepeated": (BP.MUTE, CM.REPEATED), "chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)] pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6) self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ c.actuators, \ c.cruiseControl.speedOverride, \ c.cruiseControl.override, \ c.cruiseControl.cancel, \ pcm_accel, \ perception_state.radarErrors, \ hud_v_cruise, c.hudControl.lanesVisible, \ hud_show_car = c.hudControl.leadVisible, \ hud_alert = hud_alert, \ snd_beep = snd_beep, \ snd_chime = snd_chime) self.frame += 1
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, sendcan=None): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.cp = get_can_parser(CP) # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(self.cp.dbc_name, CP.enableCamera) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: self.compute_gb = get_compute_gb_acura() else: self.compute_gb = compute_gb_honda @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): # limit the pcm accel cmd if: # - v_ego exceeds v_target, or # - a_ego exceeds a_target and v_ego is close to v_target eA = a_ego - a_target valuesA = [1.0, 0.1] bpA = [0.3, 1.1] eV = v_ego - v_target valuesV = [1.0, 0.1] bpV = [0.0, 0.5] valuesRangeV = [1., 0.] bpRangeV = [-1., 0.] # only limit if v_ego is close to v_target speedLimiter = interp(eV, bpV, valuesV) accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart return float(max(0.714, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint): # kg of standard extra cargo to count for drive, gas, etc... std_cargo = 136 # Ridgeline reqires scaled tire stiffness ts_factor = 1 ret = car.CarParams.new_message() ret.carName = "honda" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.honda ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.enableGasInterceptor = 0x201 in fingerprint print "ECU Camera Simulated: ", ret.enableCamera print "ECU Gas Interceptor: ", ret.enableGasInterceptor 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./2.205 + 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.]] if candidate == CAR.CIVIC: stop_and_go = True ret.mass = mass_civic ret.wheelbase = wheelbase_civic ret.centerToFront = centerToFront_civic ret.steerRatio = 13.0 # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [3.6, 2.4, 1.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.54, 0.36] elif candidate == CAR.ACURA_ILX: stop_and_go = False ret.mass = 3095./2.205 + std_cargo ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 15.3 # Acura at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['ff83f397542ab647'] ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.CRV: stop_and_go = False ret.mass = 3572./2.205 + std_cargo ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.3 ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ACURA_RDX: stop_and_go = False ret.mass = 3935./2.205 + std_cargo ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ODYSSEY: stop_and_go = False ret.mass = 4354./2.205 + std_cargo ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.PILOT: stop_and_go = False ret.mass = 4303./2.205 + std_cargo ret.wheelbase = 2.81 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.RIDGELINE: stop_and_go = False ts_factor = 1.4 ret.mass = 4515./2.205 + std_cargo ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.59 ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] else: raise ValueError("unsupported car %s" % candidate) ret.steerKf = 0. # TODO: investigate FF steer control for Honda ret.steerControlType = car.CarParams.SteerControlType.torque # 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. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * 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 * ts_factor) * \ ret.mass / mass_civic * \ (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) ret.tireStiffnessRear = (tireStiffnessRear_civic * ts_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. # no max steer limit VS speed ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.stoppingControl = True ret.steerLimitAlert = True ret.startAccel = 0.5 ret.steerRateCost = 0.5 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.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal ret.gas = self.CS.car_gas / 256.0 if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter 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 = self.CS.cruise_speed_offset ret.cruiseState.standstill = False # TODO: button presses 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 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) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time 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.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_not_allowed: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) 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.brake_hold: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # 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])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: print "enabled pressed at", cur_time self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: hud_v_cruise = 255 hud_alert = { "none": AH.NONE, "fcw": AH.FCW, "steerRequired": AH.STEER, "brakePressed": AH.BRAKE_PRESSED, "wrongGear": AH.GEAR_NOT_D, "seatbeltUnbuckled": AH.SEATBELT, "speedTooHigh": AH.SPEED_TOO_HIGH}[str(c.hudControl.visualAlert)] snd_beep, snd_chime = { "none": (BP.MUTE, CM.MUTE), "beepSingle": (BP.SINGLE, CM.MUTE), "beepTriple": (BP.TRIPLE, CM.MUTE), "beepRepeated": (BP.REPEATED, CM.MUTE), "chimeSingle": (BP.MUTE, CM.SINGLE), "chimeDouble": (BP.MUTE, CM.DOUBLE), "chimeRepeated": (BP.MUTE, CM.REPEATED), "chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)] pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6) self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ c.actuators, \ c.cruiseControl.speedOverride, \ c.cruiseControl.override, \ c.cruiseControl.cancel, \ pcm_accel, \ hud_v_cruise, c.hudControl.lanesVisible, \ hud_show_car = c.hudControl.leadVisible, \ hud_alert = hud_alert, \ snd_beep = snd_beep, \ snd_chime = snd_chime) self.frame += 1
class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.VM = VehicleModel(CP) 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, CP.enableCamera) @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 = "chrysler" ret.carFingerprint = candidate ret.carVin = vin ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.chrysler # pedal ret.enableCruise = True # Speed conversion: 20, 45 mph ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 ret.mass = 2858. + STD_CARGO_KG # kg curb weight Pacifica Hybrid 2017 ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15,0.30], [0.03,0.05]] ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): ret.wheelbase = 2.91 # in meters ret.steerRatio = 12.7 ret.steerActuatorDelay = 0.2 # in seconds ret.centerToFront = ret.wheelbase * 0.44 ret.minSteerSpeed = 3.8 # m/s ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. # TODO allow 2019 cars to steer down to 13 m/s if already engaged. # 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) # no rear steering, at least on the listed cars above ret.steerRatioRear = 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 = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay print("ECU Camera Simulated: {0}".format(ret.enableCamera)) ret.openpilotLongitudinalControl = False ret.stoppingControl = False ret.startAccel = 0.0 ret.longitudinalTuning.deadzoneBP = [0., 9.] ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiBP = [0., 35.] 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 ret.gasPressed = self.CS.pedal_gas > 0 # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed 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 ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status # same as main_on ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = 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 = 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 self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed) ret.genericToggle = self.CS.generic_toggle #ret.lkasCounter = self.CS.lkas_counter #ret.lkasCarModel = self.CS.lkas_car_model # events events = [] if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)): 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('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) 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 gas pedal and speed isn't zero. Gas pedal is used to resume ACC # from a 3+ second stop. if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) 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() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): if (self.CS.frame == -1): return [] # if we haven't seen a frame 220, then do not update. can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert) 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 # Double cruise stalk pull enable # 2 taps of on/off will enable/disable self.last_cruise_stalk_pull_time = 0 self.cruise_stalk_pull_time = 0 #self.cruise_stalk_pull = False in carstate self.last_cruise_stalk_pull = False self.double_stalk_pull = False self.user_enabled = False self.current_time = 0 self.last_angle_steers = 0 #CAN check between arduino and OP self.can_check = 0 self.last_can_check_time = 0 # *** 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 = "oldcar" 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 = 192150 tireStiffnessRear_civic = 202500 ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 #0.16 # Default delay, Prius has larger delay if candidate == CAR.COROLLA: ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 17.8 tire_stiffness_factor = 0.444 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 elif candidate == CAR.CAMRYH: ret.safetyParam = 100 ret.wheelbase = 2.77622 ret.steerRatio = 18.0 #14.8 tire_stiffness_factor = 0.7933 ret.mass = 3400 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 ret.steerRateCost = 1. 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. # hybrid models can't do stop and go even though the stock ACC can't if candidate in [CAR.CAMRYH]: ret.minEnableSpeed = -1. elif candidate in [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 * 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.angle #Imported through tesla no max steer limit VS speed ret.steerMaxBP = [0., 15.] # m/s ret.steerMaxV = [420., 420.] # max steer allowed # 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, 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) 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 # Double Stalk Pull Logic # 2 taps of on/off will enable/disable if (self.CS.cruise_stalk_pull != self.last_cruise_stalk_pull): self.cruise_stalk_pull_time = (sec_since_boot() * 1e3) if ((self.cruise_stalk_pull_time - self.last_cruise_stalk_pull_time) < 500): #Stalk pulled twice, enable or disable if (self.user_enabled == True): self.user_enabled = False else: self.user_enabled = True self.CS.enabled_time = (sec_since_boot() * 1e3) self.last_cruise_stalk_pull_time = self.cruise_stalk_pull_time self.last_cruise_stalk_pull = self.CS.cruise_stalk_pull # for safety loop self.current_time = (sec_since_boot() * 1e3) #safety CAN check if (self.can_check != self.CS.can_check): self.last_can_check_time = (sec_since_boot() * 1e3) self.can_check = self.CS.can_check # cruise state ret.cruiseState.enabled = self.user_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 in [CAR.CAMRYH]: # ignore standstill in hybrid vehicles, since pcm allows to restart without # receiving any special command 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 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 #Gear CAN command not known for camry #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])) self.user_enabled = False if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) #Disable if started for over 3 seconds and delta angle >5 degrees if (abs(self.CS.desired_angle - self.CS.angle_steers) > 5) and \ ((self.current_time - self.CS.enabled_time) > 3000) and (self.user_enabled): # disable if angle not moving towards desired angle if (self.CS.desired_angle > self.CS.angle_steers): if not (self.CS.angle_steers > (self.last_angle_steers - 0.2)): self.user_enabled = False events.append( create_event('motorIssue', [ET.IMMEDIATE_DISABLE])) # disable if angle not moving towards desired angle elif (self.CS.desired_angle < self.CS.angle_steers): if not (self.CS.angle_steers < (self.last_angle_steers + 0.2)): self.user_enabled = False events.append( create_event('motorIssue', [ET.IMMEDIATE_DISABLE])) #Disable if haven't heard from arduino in 0.5 seconds if ((self.current_time - self.last_can_check_time) > 500): events.append(create_event('steerCANerror', [ET.IMMEDIATE_DISABLE])) 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.last_angle_steers = self.CS.angle_steers 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, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert) self.frame += 1 return False
class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) mydbc = DBC[CP.carFingerprint]['pt'] if CP.carFingerprint == CAR.MODELS and self.CS.fix1916: mydbc = mydbc + "1916" self.cp = self.CS.get_can_parser2(CP,mydbc) self.epas_cp = None self.pedal_cp = None if self.CS.useWithoutHarness: self.epas_cp = self.CS.get_epas_parser(CP,0) self.pedal_cp = self.CS.get_pedal_parser(CP,0) else: self.epas_cp = self.CS.get_epas_parser(CP,2) self.pedal_cp = self.CS.get_pedal_parser(CP,2) self.CC = None if CarController is not None: self.CC = CarController(self.cp.dbc_name,CP,self.VM) @staticmethod def compute_gb(accel, speed): return float(accel) / 3. @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): # limit the pcm accel cmd if: # - v_ego exceeds v_target, or # - a_ego exceeds a_target and v_ego is close to v_target # normalized max accel. Allowing max accel at low speed causes speed overshoots max_accel_bp = [10, 20] # m/s max_accel_v = [0.714, 1.0] # unit of max accel max_accel = interp(v_ego, max_accel_bp, max_accel_v) eA = a_ego - a_target valuesA = [1.0, 0.1] bpA = [0.3, 1.1] eV = v_ego - v_target valuesV = [1.0, 0.1] bpV = [0.0, 0.5] valuesRangeV = [1., 0.] bpRangeV = [-1., 0.] # only limit if v_ego is close to v_target speedLimiter = interp(eV, bpV, valuesV) accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # Scaled tire stiffness ts_factor = 8 ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "tesla" ret.carFingerprint = candidate teslaModel = read_db('/data/params','TeslaModel') if teslaModel is not None: teslaModel = teslaModel.decode() if teslaModel is None: teslaModel = "S" ret.safetyModel = car.CarParams.SafetyModel.tesla ret.safetyParam = 1 ret.carVin = "TESLAFAKEVIN12345" ret.enableCamera = True ret.enableGasInterceptor = False #keep this False for now print ("ECU Camera Simulated: ", ret.enableCamera) print ("ECU Gas Interceptor: ", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor mass_models = 4722./2.205 + STD_CARGO_KG wheelbase_models = 2.959 # RC: I'm assuming center means center of mass, and I think Model S is pretty even between two axles centerToFront_models = wheelbase_models * 0.5 #BB was 0.48 centerToRear_models = wheelbase_models - centerToFront_models rotationalInertia_models = 2500 tireStiffnessFront_models = 85100 #BB was 85400 tireStiffnessRear_models = 90000 # will create Kp and Ki for 0, 20, 40, 60 mph ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 8.94, 17.88, 26.82 ], [0., 8.94, 17.88, 26.82]] if candidate == CAR.MODELS: stop_and_go = True ret.mass = mass_models ret.wheelbase = wheelbase_models ret.centerToFront = centerToFront_models ret.steerRatio = 11.5 # Kp and Ki for the lateral control for 0, 20, 40, 60 mph ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.20, 0.80, 0.60, 0.30], [0.16, 0.12, 0.08, 0.04]] ret.lateralTuning.pid.kf = 0.00006 # Initial test value TODO: investigate FF steer control for Model S? ret.steerActuatorDelay = 0.2 #ret.steerReactance = 1.0 #ret.steerInductance = 1.0 #ret.steerResistance = 1.0 # Kp and Ki for the longitudinal control if teslaModel == "S": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.01,0.01,0.01] elif teslaModel == "SP": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] elif teslaModel == "SD": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.01,0.01,0.01] elif teslaModel == "SPD": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] else: #use S numbers if we can't match anything ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.08,0.08,0.08] else: raise ValueError("unsupported car %s" % candidate) ret.steerControlType = car.CarParams.SteerControlType.angle # 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. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for Model S ret.rotationalInertia = rotationalInertia_models * \ ret.mass * ret.wheelbase**2 / (mass_models * wheelbase_models**2) # TODO: start from empirically derived lateral slip stiffness and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront = (tireStiffnessFront_models * ts_factor) * \ ret.mass / mass_models * \ (centerToRear / ret.wheelbase) / (centerToRear_models / wheelbase_models) ret.tireStiffnessRear = (tireStiffnessRear_models * ts_factor) * \ ret.mass / mass_models * \ (ret.centerToFront / ret.wheelbase) / (centerToFront_models / wheelbase_models) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. # no max steer limit VS speed ret.steerMaxBP = [0.,15.] # m/s ret.steerMaxV = [420.,420.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.3] #if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [0., 20.] # m/s ret.brakeMaxV = [1., 1.] # max brake allowed - BB: since we are using regen, make this even ret.longitudinalTuning.deadzoneBP = [0., 9.] #BB: added from Toyota to start pedal work; need to tune ret.longitudinalTuning.deadzoneV = [0., 0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now ret.stoppingControl = True ret.openpilotLongitudinalControl = True ret.steerLimitAlert = False ret.startAccel = 0.5 ret.steerRateCost = 1.0 ret.radarOffCan = not CarSettings().get_value("useTeslaRadar") ret.radarTimeStep = 0.05 #20Hz return ret # returns a car.CarState def update(self, c, can_strings): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update_strings(can_strings) ch_can_valid = self.cp.can_valid self.epas_cp.update_strings(can_strings) epas_can_valid = self.epas_cp.can_valid self.pedal_cp.update_strings(can_strings) pedal_can_valid = self.pedal_cp.can_valid can_rcv_error = not (ch_can_valid and epas_can_valid and pedal_can_valid) self.CS.update(self.cp, self.epas_cp, self.pedal_cp) # create message ret = car.CarState.new_message() ret.canValid = ch_can_valid #and epas_can_valid #and pedal_can_valid # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal, we don't use with with interceptor so it's always 0/False ret.gas = self.CS.user_gas if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.user_gas_pressed else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brakePressed =False # (self.CS.brake_pressed != 0) and (self.CS.cstm_btns.get_button_status("brake") == 0) # FIXME: read sendcan for brakelights brakelights_threshold = 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = True #self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS * (CV.MPH_TO_KPH if self.CS.imperial_speed_units else 1.) ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. ret.cruiseState.standstill = False # TODO: button presses buttonEvents = [] ret.leftBlinker = bool(self.CS.turn_signal_state_left == 1) ret.rightBlinker = bool(self.CS.turn_signal_state_right == 1) ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt if self.CS.prev_turn_signal_stalk_state != self.CS.turn_signal_stalk_state: if self.CS.turn_signal_stalk_state == 1 or self.CS.prev_turn_signal_stalk_state == 1: be = car.CarState.ButtonEvent.new_message() be.type = 'leftBlinker' be.pressed = self.CS.turn_signal_stalk_state == 1 buttonEvents.append(be) if self.CS.turn_signal_stalk_state == 2 or self.CS.prev_turn_signal_stalk_state == 2: be = car.CarState.ButtonEvent.new_message() be.type = 'rightBlinker' be.pressed = self.CS.turn_signal_stalk_state == 2 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' be.pressed = bool(self.CS.cruise_buttons) buttonEvents.append(be) ret.buttonEvents = buttonEvents # events events = [] #notification messages for DAS if (not c.enabled) and (self.CC.opState == 2): self.CC.opState = 0 if c.enabled and (self.CC.opState == 0): self.CC.opState = 1 if can_rcv_error: self.can_invalid_count += 1 if self.can_invalid_count >= 100: #BB increased to 100 to see if we still get the can error messages events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) self.CS.DAS_canErrors = 1 if self.CC.opState == 1: self.CC.opState = 2 else: self.can_invalid_count = 0 if self.CS.steer_error: if not self.CS.enableHSO: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.WARNING])) elif self.CS.steer_warning: if not self.CS.enableHSO: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CC.opState == 1: self.CC.opState = 2 if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CC.opState == 1: self.CC.opState = 2 if not ret.gearShifter == 'drive': events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if c.enabled: self.CC.DAS_222_accCameraBlind = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 if ret.doorOpen: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) self.CS.DAS_doorOpen = 1 if self.CC.opState == 1: self.CC.opState = 0 if ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if c.enabled: self.CC.DAS_211_accNoSeatBelt = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 if self.CC.opState == 1: self.CC.opState = 2 if self.CS.esp_disabled: events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CC.opState == 1: self.CC.opState = 2 if not self.CS.main_on: events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CC.opState == 1: self.CC.opState = 0 if ret.gearShifter == 'reverse': events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) self.CS.DAS_notInDrive = 1 if self.CC.opState == 1: self.CC.opState = 0 if ret.gearShifter == 'drive': self.CS.DAS_notInDrive =0 if self.CS.brake_hold: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CC.opState == 1: self.CC.opState = 0 if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CC.opState == 1: self.CC.opState = 0 if (not c.enabled) and (self.CC.opState == 1): self.CC.opState = 0 if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # Standard OP method to disengage: # 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('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) #if (self.CS.cstm_btns.get_button_status("brake")>0): # if ((self.CS.brake_pressed !=0) != self.brake_pressed_prev): #break not canceling when pressed # self.CS.cstm_btns.set_button_status("brake", 2 if self.CS.brake_pressed != 0 else 1) #else: # if ret.brakePressed: # events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = self.frame * DT_CTRL enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type == "altButton3" and not b.pressed: print ("enabled pressed at", cur_time) self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and # pylint: disable=chained-comparison (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): if ret.seatbeltUnlatched: self.CC.DAS_211_accNoSeatBelt = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 elif not ret.gearShifter == 'drive': self.CC.DAS_222_accCameraBlind = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 elif not self.CS.apEnabled: self.CC.DAS_206_apUnavailable = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 else: events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: if ret.seatbeltUnlatched: self.CC.DAS_211_accNoSeatBelt = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 elif not ret.gearShifter == 'drive': self.CC.DAS_222_accCameraBlind = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 elif not self.CS.apEnabled: self.CC.DAS_206_apUnavailable = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 else: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = self.CS.brake_pressed != 0 # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: hud_v_cruise = 255 VISUAL_HUD = { VisualAlert.none: AH.NONE, VisualAlert.fcw: AH.FCW, VisualAlert.steerRequired: AH.STEER, VisualAlert.brakePressed: AH.BRAKE_PRESSED, VisualAlert.wrongGear: AH.GEAR_NOT_D, VisualAlert.seatbeltUnbuckled: AH.SEATBELT, VisualAlert.speedTooHigh: AH.SPEED_TOO_HIGH} AUDIO_HUD = { AudibleAlert.none: (BP.MUTE, CM.MUTE), AudibleAlert.chimeEngage: (BP.SINGLE, CM.MUTE), AudibleAlert.chimeDisengage: (BP.SINGLE, CM.MUTE), AudibleAlert.chimeError: (BP.MUTE, CM.DOUBLE), AudibleAlert.chimePrompt: (BP.MUTE, CM.SINGLE), AudibleAlert.chimeWarning1: (BP.MUTE, CM.DOUBLE), AudibleAlert.chimeWarning2: (BP.MUTE, CM.REPEATED), AudibleAlert.chimeWarningRepeat: (BP.MUTE, CM.REPEATED)} hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw] snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw] pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6) can_sends = self.CC.update(c.enabled, self.CS, self.frame, \ c.actuators, \ c.cruiseControl.speedOverride, \ c.cruiseControl.override, \ c.cruiseControl.cancel, \ pcm_accel, \ hud_v_cruise, c.hudControl.lanesVisible, \ hud_show_car = c.hudControl.leadVisible, \ hud_alert = hud_alert, \ snd_beep = snd_beep, \ snd_chime = snd_chime, \ leftLaneVisible = c.hudControl.leftLaneVisible,\ rightLaneVisible = c.hudControl.rightLaneVisible) self.frame += 1 return can_sends
class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.cam_can_invalid_count = 0 self.cruise_enabled_prev = False self.cp = get_can_parser(CP) self.cp_cam = get_cam_can_parser(CP) # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(self.cp.dbc_name, CP.enableCamera) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: self.compute_gb = get_compute_gb_acura() else: self.compute_gb = compute_gb_honda @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): # normalized max accel. Allowing max accel at low speed causes speed overshoots max_accel_bp = [10, 20] # m/s max_accel_v = [0.714, 1.0] # unit of max accel max_accel = interp(v_ego, max_accel_bp, max_accel_v) # limit the pcm accel cmd if: # - v_ego exceeds v_target, or # - a_ego exceeds a_target and v_ego is close to v_target eA = a_ego - a_target valuesA = [1.0, 0.1] bpA = [0.3, 1.1] eV = v_ego - v_target valuesV = [1.0, 0.1] bpV = [0.0, 0.5] valuesRangeV = [1., 0.] bpRangeV = [-1., 0.] # only limit if v_ego is close to v_target speedLimiter = interp(eV, bpV, valuesV) accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carName = "honda" ret.carFingerprint = candidate if candidate in HONDA_BOSCH: ret.safetyModel = car.CarParams.SafetyModels.hondaBosch ret.enableCamera = True ret.radarOffCan = True ret.openpilotLongitudinalControl = False else: ret.safetyModel = car.CarParams.SafetyModels.honda ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.enableGasInterceptor = 0x201 in fingerprint ret.openpilotLongitudinalControl = ret.enableCamera cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor # kg of standard extra cargo to count for drive, gas, etc... std_cargo = 136 # 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 # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle # model optimization process. Certain Hondas have an extra steering sensor at the bottom # of the steering rack, which improves controls quality as it removes the steering column # torsion from feedback. # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerKf = 0.00006 # conservative feed-forward if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: stop_and_go = True ret.mass = mass_civic ret.wheelbase = wheelbase_civic ret.centerToFront = centerToFront_civic ret.steerRatio = 15.58 # 0.5.10 tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.4], [0.14]] if is_fw_modified: tire_stiffness_factor = 0.9 ret.steerKf = 0.00004 ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [3.6, 2.4, 1.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.54, 0.36] elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): stop_and_go = True if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3279. * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 17.11 # 0.5.10 tire_stiffness_factor = 0.8467 ret.steerKpV, ret.steerKiV = [[0.4], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ACURA_ILX: stop_and_go = False ret.mass = 3095 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 18.61 # 15.3 is spec end-to-end tire_stiffness_factor = 0.72 ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.CRV: stop_and_go = False ret.mass = 3572 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 0.5.10 tire_stiffness_factor = 0.444 # not optimized yet ret.steerKpV, ret.steerKiV = [[0.6], [0.14]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.CRV_5G: stop_and_go = True ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3410. * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ACURA_RDX: stop_and_go = False ret.mass = 3935 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec tire_stiffness_factor = 0.444 # not optimized yet ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ODYSSEY: stop_and_go = False ret.mass = 4471 * CV.LB_TO_KG + std_cargo ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 # as spec tire_stiffness_factor = 0.82 ret.steerKpV, ret.steerKiV = [[0.45], [0.135]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] elif candidate in (CAR.PILOT, CAR.PILOT_2019): stop_and_go = False ret.mass = 4303 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.81 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # as spec ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] #tire_stiffness_factor = 0.444 # not optimized yet tire_stiffness_factor = 0.82 # trying same as odyssey #ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] #original model: OS 30%, risetime: 1.75s, Max Output 0.8 of 1 #ret.steerKpV, ret.steerKiV = [[0.38], [0.23]] #model: OS 10%,risetime 2.75s - RESULT: Better than default stock values. #ret.steerKpV, ret.steerKiV = [[0.38], [0.3]] #model: OS 5%, risetime 3.5s - NOT TESTED #ret.steerKpV, ret.steerKiV = [[0.38], [0.25]] #model: OS 7%, risetime 3s - NOT TESTED #ret.steerKpV, ret.steerKiV = [[0.38], [0.35]] #model: OS 2%, risetime 4s - NOT TESTED ret.steerKpV, ret.steerKiV = [[0.5], [0.22]] #tweaking optimal result - RESULT: Seems to fix slow / fast lane hug! #ret.steerKpV, ret.steerKiV = [[0.5], [0.23]] #model: OS 8%, risetime 2.75s - RESULT: Great centering on fast turns. Fixes fast lane but slow lane still hugs. #ret.steerKpV, ret.steerKiV = [[0.5], [0.24]] #tweaking optimal result - RESULT: Does not fix slow lane hug #ret.steerKpV, ret.steerKiV = [[0.5], [0.3]] #model: OS 3%, risetime 3.5s - NOT TESTED #ret.steerKpV, ret.steerKiV = [[0.8], [0.23]] #model: OS 5%, risetime 3s - NOT TESTED #ret.steerKpV, ret.steerKiV = [[0.8], [0.3]] #model: OS 2%, risetime 3.8s - RESULT: Fast lane left hugging recurrence. Wheel jiggles after fast turn. #ret.steerKpV, ret.steerKiV = [[1], [0.14]] #model: OS 18%,risetime 2s - NOT TESTED - CAUTION Large kP #ret.steerKpV, ret.steerKiV = [[1.5], [0.2]] #model: OS 5%, risetime 2.3s - NOT TESTED - CAUTION Large kP #ret.steerKpV, ret.steerKiV = [[2], [0.24]] #model: OS 0%, risetime 3s - NOT TESTED - CAUTION Large kP #ret.steerKpV, ret.steerKiV = [[2], [0.21]] #model: OS 3%, risetime 2.3s - NOT TESTED - CAUTION Large kP #ret.steerKf = 0.00009 # - NOT TESTED #ret.steerKf = 0.00012 # - NOT TESTED - RESULT: 0.5,0.23 makes fast lane hug left side #ret.steerKf = 0.00015 # - NOT TESTED #ret.steerKf = 0.00018 # - NOT TESTED #ret.steerKf = 0.00021 # - NOT TESTED #ret.steerKf = 0.00024 # - NOT TESTED #ret.steerKf = 0.00027 # - NOT TESTED #ret.steerKf = 0.00030 # - NOT TESTED elif candidate == CAR.RIDGELINE: stop_and_go = False ret.mass = 4515 * CV.LB_TO_KG + std_cargo ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.59 # as spec tire_stiffness_factor = 0.444 # not optimized yet ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] else: raise ValueError("unsupported car %s" % candidate) ret.steerControlType = car.CarParams.SteerControlType.torque # 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. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * 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. # no max steer limit VS speed ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.stoppingControl = True ret.steerLimitAlert = True ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 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.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal ret.gas = self.CS.car_gas / 256.0 if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers + angleSteersoffset ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter 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 = self.CS.cruise_speed_offset ret.cruiseState.standstill = False ret.readdistancelines = self.CS.read_distance_lines # TODO: button presses 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 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) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents ret.gasbuttonstatus = self.CS.gasMode # events # TODO: event names aren't checked at compile time. # Maybe there is a way to use capnp enums directly 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 self.CS.cam_can_valid and self.CP.enableCamera: self.cam_can_invalid_count += 1 # wait 1.0s before throwing the alert to avoid it popping when you turn off the car if self.cam_can_invalid_count >= 100 and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) else: self.cam_can_invalid_count = 0 if ret.cruiseState.enabled and not self.cruise_enabled_prev: disengage_event = True else: disengage_event = False if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == 'drive': 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: 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.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # 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])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified self.cruise_enabled_prev = ret.cruiseState.enabled return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: hud_v_cruise = 255 hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw] snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw] pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6) self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.speedOverride, c.cruiseControl.override, c.cruiseControl.cancel, pcm_accel, hud_v_cruise, c.hudControl.lanesVisible, hud_show_car=c.hudControl.leadVisible, hud_alert=hud_alert, snd_beep=snd_beep, snd_chime=snd_chime) self.frame += 1
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
class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.acc_active_prev = 0 # *** init the major players *** canbus = CanBus() self.CS = CarState(CP) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP, canbus) self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] self.CC = None if CarController is not None: self.CC = CarController(canbus, CP.carFingerprint) @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): ret = car.CarParams.new_message() ret.carName = "gm" ret.carFingerprint = candidate ret.isPandaBlack = has_relay ret.enableCruise = False # GM port is considered a community feature, since it disables AEB; # TODO: make a port that uses a car harness and it only intercepts the camera ret.communityFeature = True # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or \ has_relay or \ candidate == CAR.CADILLAC_CT6 ret.openpilotLongitudinalControl = ret.enableCamera tire_stiffness_factor = 0.444 # not optimized yet # Start with a baseline lateral tuning for all GM vehicles. Override tuning as needed in each model section below. ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerRateCost = 1.0 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1607. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.69 ret.steerRatio = 15.7 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.MALIBU: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1496. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 ret.steerRatio = 15.8 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.HOLDEN_ASTRA: ret.mass = 1363. + STD_CARGO_KG ret.wheelbase = 2.662 # Remaining parameters copied from Volt for now ret.centerToFront = ret.wheelbase * 0.4 ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.safetyModel = car.CarParams.SafetyModel.gm ret.steerRatio = 15.7 ret.steerRatioRear = 0. elif candidate == CAR.ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.86 ret.steerRatio = 14.4 #end to end is 13.46 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 elif candidate == CAR.BUICK_REGAL: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 #111.4 inches in meters ret.steerRatio = 14.4 # guess for tourx ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx elif candidate == CAR.CADILLAC_ATS: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1601. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.78 ret.steerRatio = 15.3 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.49 elif candidate == CAR.CADILLAC_CT6: # engage speed is decided by pcm ret.minEnableSpeed = -1. ret.mass = 4016. * CV.LB_TO_KG + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.cadillac ret.wheelbase = 3.11 ret.steerRatio = 14.6 # it's 16.3 without rear active steering ret.steerRatioRear = 0. # TODO: there is RAS on this car! ret.centerToFront = ret.wheelbase * 0.465 # 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) ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.gasMaxBP = [0.] ret.gasMaxV = [.5] ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] ret.longitudinalTuning.kpBP = [5., 35.] ret.longitudinalTuning.kpV = [2.4, 1.5] ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.36] ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] ret.stoppingControl = True ret.startAccel = 0.8 ret.steerLimitTimer = 0.4 ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz ret.steerControlType = car.CarParams.SteerControlType.torque return ret # returns a car.CarState def update(self, c, can_strings): self.pt_cp.update_strings(can_strings) ret = self.CS.update(self.pt_cp) ret.canValid = self.pt_cp.can_valid ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False buttonEvents = [] if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: if not (ret.cruiseState.enabled and ret.standstill): be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed. elif but == CruiseButtons.DECEL_SET: be.type = ButtonType.decelCruise elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) ret.buttonEvents = buttonEvents events = [] if self.CS.steer_not_allowed: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) 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.car_fingerprint in SUPERCRUISE_CARS: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) else: if ret.gearShifter != car.CarState.GearShifter.drive: events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not ret.cruiseState.available: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == car.CarState.GearShifter.reverse: events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, 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])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: events.append( create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in [ButtonType.accelCruise, ButtonType.decelCruise ] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == ButtonType.cancel and b.pressed: events.append( create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # copy back carState packet to CS self.CS.out = ret.as_reader() return self.CS.out def apply(self, c): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: hud_v_cruise = 0 # For Openpilot, "enabled" includes pre-enable. # In GM, PCM faults out if ACC command overlaps user gas. enabled = c.enabled and not self.CS.out.gasPressed can_sends = self.CC.update(enabled, self.CS, self.frame, \ c.actuators, hud_v_cruise, c.hudControl.lanesVisible, \ c.hudControl.leadVisible, c.hudControl.visualAlert) self.frame += 1 return can_sends
class CarInterface(object): def __init__(self, CP, sendcan=None): self.angleSteersoffset = float( kegman.conf['angle_steers_offset']) # deg offset self.CP = CP self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.acc_active_prev = 0 self.cruise_enabled_prev = False # *** init the major players *** canbus = CanBus() self.CS = CarState(CP, canbus) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP, canbus) self.ch_cp = get_chassis_can_parser(CP, canbus) self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(canbus, CP.carFingerprint, CP.enableCamera) @staticmethod def compute_gb(accel, speed): # Ripped from compute_gb_honda in Honda's interface.py. Works well off shelf but may need more tuning creep_brake = 0.0 creep_speed = 2.68 creep_brake_value = 0.10 if speed < creep_speed: creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value return float(accel) / 4.8 - creep_brake @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): return 1.0 @staticmethod def get_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carName = "gm" ret.carFingerprint = candidate ret.enableCruise = False # Presence of a camera on the object bus is ok. # Have to go passive if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). ret.enableCamera = not any( x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) ret.openpilotLongitudinalControl = ret.enableCamera std_cargo = 136 if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 7 * CV.MPH_TO_MS # kg of standard extra cargo to count for driver, gas, etc... ret.mass = 1607. + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.69 ret.steerRatio = 16.17 #0.5.10 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.MALIBU: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 7 * CV.MPH_TO_MS ret.mass = 1496 + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.83 ret.steerRatio = 15.8 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.HOLDEN_ASTRA: # kg of standard extra cargo to count for driver, gas, etc... ret.mass = 1363. + std_cargo ret.wheelbase = 2.662 # Remaining parameters copied from Volt for now ret.centerToFront = ret.wheelbase * 0.4 ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.safetyModel = car.CarParams.SafetyModels.gm ret.steerRatio = 15.75 #0.5.10 ret.steerRatioRear = 0. elif candidate == CAR.ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 4353. * CV.LB_TO_KG + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.86 ret.steerRatio = 14.4 #end to end is 13.46 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 elif candidate == CAR.BUICK_REGAL: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 3779. * CV.LB_TO_KG + std_cargo # (3849+3708)/2 ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.83 #111.4 inches in meters ret.steerRatio = 14.4 # guess for tourx ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx elif candidate == CAR.CADILLAC_ATS: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1601. + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.78 ret.steerRatio = 15.3 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.49 elif candidate == CAR.CADILLAC_CT6: # engage speed is decided by pcm ret.minEnableSpeed = -1. # kg of standard extra cargo to count for driver, gas, etc... ret.mass = 4016. * CV.LB_TO_KG + std_cargo ret.safetyModel = car.CarParams.SafetyModels.cadillac ret.wheelbase = 3.11 ret.steerRatio = 14.6 # it's 16.3 without rear active steering ret.steerRatioRear = 0. # TODO: there is RAS on this car! ret.centerToFront = ret.wheelbase * 0.465 # 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 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) # same tuning for Volt and CT6 for now ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.gasMaxBP = [0.] ret.gasMaxV = [.5] ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] ret.longitudinalTuning.kpBP = [0., 5., 55.] ret.longitudinalTuning.kpV = [2., 1., 0.5] ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.3] ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] ret.steerLimitAlert = True ret.stoppingControl = True # Volt PID controller gets upset in heavy low speed stop-go traffic as startAccel interferes with it. ret.startAccel = 0.0 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 ret.steerControlType = car.CarParams.SteerControlType.torque return ret # returns a car.CarState def update(self, c): self.pt_cp.update(int(sec_since_boot() * 1e9), False) self.ch_cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.pt_cp, self.ch_cp) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal information. ret.gas = self.CS.pedal_gas / 254.0 ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake / 0xd0 ret.brakePressed = self.CS.brake_pressed ret.brakeLights = self.CS.frictionBrakesActive # steering wheel ret.steeringAngle = self.CS.angle_steers + self.angleSteersoffset # torque and user override. Driver awareness # timer resets when the user uses the steering wheel. ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver # cruise state ret.cruiseState.available = bool(self.CS.main_on) cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF ret.cruiseState.enabled = cruiseEnabled ret.cruiseState.standstill = False ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt ret.gearShifter = self.CS.gear_shifter ret.readdistancelines = self.CS.follow_level ret.genericToggle = False ret.laneDepartureToggle = False ret.distanceToggle = self.CS.follow_level ret.accSlowToggle = False ret.blindspot = self.CS.blind_spot_on buttonEvents = [] # blinkers 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 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 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: if not (cruiseEnabled and self.CS.standstill): be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed. elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' if not cruiseEnabled and not self.CS.lkMode: self.CS.lkMode = True elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) ret.buttonEvents = buttonEvents if self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button: if self.CS.lkMode: self.CS.lkMode = False else: self.CS.lkMode = True if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button: self.CS.follow_level -= 1 if self.CS.follow_level < 1: self.CS.follow_level = 3 kegman.save({'lastTrMode': int(self.CS.follow_level) }) # write last distance bar setting to file ret.gasbuttonstatus = self.CS.gasMode 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 ret.cruiseState.enabled and not self.cruise_enabled_prev: disengage_event = True else: disengage_event = False if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.steer_not_allowed: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) 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.car_fingerprint in SUPERCRUISE_CARS: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) else: if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not self.CS.gear_shifter_valid: events.append( create_event('wrongGear', [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 self.CS.gear_shifter == 3: events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, 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])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: events.append( create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == "cancel" and b.pressed: events.append( create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: hud_v_cruise = 0 chime, chime_count = AUDIO_HUD[c.hudControl.audibleAlert.raw] # For Openpilot, "enabled" includes pre-enable. # In GM, PCM faults out if ACC command overlaps user gas. enabled = c.enabled and not self.CS.user_gas_pressed self.CC.update(self.sendcan, enabled, self.CS, self.frame, \ c.actuators, hud_v_cruise, c.hudControl.lanesVisible, \ c.hudControl.leadVisible, \ chime, chime_count) self.frame += 1
class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 self.can_invalid_count = 0 self.acc_active_prev = 0 # *** init the major players *** canbus = CanBus() self.CS = CarState(CP, canbus) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP, canbus) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(canbus, CP.carFingerprint) @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): return 1.0 @staticmethod def get_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carName = "subaru" ret.carFingerprint = candidate ret.enableCruise = False # TODO: gate this on detection ret.enableCamera = True std_cargo = 136 if candidate in [CAR.OUTBACK, CAR.LEGACY]: ret.mass = 1568 + std_cargo ret.wheelbase = 2.75 ret.centerToFront = ret.wheelbase * 0.5 + 1 ret.steerRatio = 14 ret.steerActuatorDelay = 0.3 ret.steerRateCost = 0 ret.steerKf = 0.00006 ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] # m/s ret.steerKpV, ret.steerKiV = [[0.003], [0.00]] ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] elif candidate in [CAR.XV2018]: ret.mass = 1568 + std_cargo ret.wheelbase = 2.75 ret.centerToFront = ret.wheelbase * 0.5 + 1 ret.steerRatio = 7 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0 ret.steerKf = 0.00006 ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerKpV, ret.steerKiV = [[0.0], [0.00]] ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.safetyModel = car.CarParams.SafetyModels.subaru ret.steerControlType = car.CarParams.SteerControlType.torque ret.steerLimitAlert = False # testing tuning # FIXME: from gm ret.gasMaxBP = [0.] ret.gasMaxV = [.5] ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.longitudinalKpBP = [5., 35.] ret.longitudinalKpV = [2.4, 1.5] ret.longitudinalKiBP = [0.] ret.longitudinalKiV = [0.36] ret.stoppingControl = True ret.startAccel = 0.8 # end from gm # hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars mass_civic = 2923. / 2.205 + 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 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) return ret # returns a car.CarState def update(self, c): self.pt_cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.pt_cp) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ret.standstill = self.CS.standstill # steering wheel ret.steeringAngle = self.CS.angle_steers # torque and user override. Driver awareness # timer resets when the user uses the steering wheel. ret.steeringTorque = self.CS.steer_torque_driver # cruise state ret.cruiseState.available = bool(self.CS.main_on) ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on buttonEvents = [] # blinkers 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 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 buttonEvents.append(be) be = car.CarState.ButtonEvent.new_message() be.type = 'accelCruise' buttonEvents.append(be) 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.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active # cast to reader so it can't be modified return ret.as_reader() def apply(self, c, perception_state): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators) self.frame += 1
class CarInterface(object): def __init__(self, CP, CarController): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.cp = get_can_parser(CP) self.cp_cam = get_cam_can_parser(CP) # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) self.CC = None if CarController is not None: self.CC = CarController(self.cp.dbc_name) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: self.compute_gb = get_compute_gb_acura() else: self.compute_gb = compute_gb_honda @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): # normalized max accel. Allowing max accel at low speed causes speed overshoots max_accel_bp = [10, 20] # m/s max_accel_v = [0.714, 1.0] # unit of max accel max_accel = interp(v_ego, max_accel_bp, max_accel_v) # limit the pcm accel cmd if: # - v_ego exceeds v_target, or # - a_ego exceeds a_target and v_ego is close to v_target eA = a_ego - a_target valuesA = [1.0, 0.1] bpA = [0.3, 1.1] eV = v_ego - v_target valuesV = [1.0, 0.1] bpV = [0.0, 0.5] valuesRangeV = [1., 0.] bpRangeV = [-1., 0.] # only limit if v_ego is close to v_target speedLimiter = interp(eV, bpV, valuesV) accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart return float(max(max_accel, a_target / A_ACC_MAX)) * min( speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() ret.carName = "honda" ret.carFingerprint = candidate ret.carVin = vin ret.isPandaBlack = is_panda_black if candidate in HONDA_BOSCH: ret.safetyModel = car.CarParams.SafetyModel.hondaBosch ret.enableCamera = True ret.radarOffCan = True ret.openpilotLongitudinalControl = not any( x for x in BOSCH_RADAR_MSGS if x in fingerprint) ret.enableCruise = not ret.openpilotLongitudinalControl else: ret.safetyModel = car.CarParams.SafetyModel.honda ret.enableCamera = not any( x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black ret.enableGasInterceptor = 0x201 in fingerprint ret.openpilotLongitudinalControl = ret.enableCamera ret.enableCruise = not ret.enableGasInterceptor cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle # model optimization process. Certain Hondas have an extra steering sensor at the bottom # of the steering rack, which improves controls quality as it removes the steering column # torsion from feedback. # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: stop_and_go = True ret.mass = CivicParams.MASS ret.wheelbase = CivicParams.WHEELBASE ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 15.38 # 10.93 is end-to-end spec tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['5b7c365c50084530'] if is_fw_modified: ret.lateralTuning.pid.kf = 0.00004 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.4 ], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.54, 0.36] elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): stop_and_go = True if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 16.33 # 11.82 is spec end-to-end tire_stiffness_factor = 0.8467 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] 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] elif candidate == CAR.ACURA_ILX: stop_and_go = False ret.mass = 3095. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 18.61 # 15.3 is spec end-to-end tire_stiffness_factor = 0.72 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] 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] elif candidate == CAR.CRV: stop_and_go = False ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.89 # as spec tire_stiffness_factor = 0.444 # not optimized yet ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] 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] elif candidate == CAR.CRV_5G: stop_and_go = True ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] 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] elif candidate == CAR.CRV_HYBRID: stop_and_go = True ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] 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] elif candidate == CAR.ACURA_RDX: stop_and_go = False ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec tire_stiffness_factor = 0.444 # not optimized yet ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] 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] elif candidate == CAR.ODYSSEY: stop_and_go = False ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 # as spec tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] 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] elif candidate == CAR.ODYSSEY_CHN: stop_and_go = False ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg ret.wheelbase = 2.90 # spec ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY ret.steerRatio = 14.35 # from CAR.ODYSSEY tire_stiffness_factor = 0.82 # from CAR.ODYSSEY ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] 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] elif candidate in (CAR.PILOT, CAR.PILOT_2019): stop_and_go = False ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight ret.wheelbase = 2.82 ret.centerToFront = ret.wheelbase * 0.428 # average weight distribution ret.steerRatio = 17.25 # as spec tire_stiffness_factor = 0.444 # not optimized yet ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] 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] elif candidate == CAR.RIDGELINE: stop_and_go = False ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.59 # as spec tire_stiffness_factor = 0.444 # not optimized yet ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] 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] else: raise ValueError("unsupported car %s" % candidate) ret.steerControlType = car.CarParams.SteerControlType.torque # 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. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if ( stop_and_go or ret.enableGasInterceptor) else 25.5 * 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. # no max steer limit VS speed ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] # max steer allowed ret.gasMaxBP = [0.] # m/s # TODO: what is the correct way to handle this? ret.gasMaxV = [ 1. ] #if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] ret.stoppingControl = True ret.steerLimitAlert = True ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 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 # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal ret.gas = self.CS.car_gas / 256.0 if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = self.CS.steer_torque_driver ret.steeringTorqueEps = self.CS.steer_torque_motor 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) and not bool(self.CS.cruise_mode) ret.cruiseState.speedOffset = self.CS.cruise_speed_offset ret.cruiseState.standstill = False # TODO: button presses 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 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) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events events = [] # wait 1.0s before throwing the alert to avoid it popping when you turn off the car if self.cp_cam.can_invalid_cnt >= 100 and self.CS.CP.carFingerprint not in HONDA_BOSCH and self.CP.enableCamera: events.append( create_event( 'invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) 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 or self.CS.cruise_mode: 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.brake_hold and self.CS.CP.openpilotLongitudinalControl: events.append( create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # 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])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append( create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append( create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = self.frame * DT_CTRL enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: hud_v_cruise = 255 hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw] pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6) can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.speedOverride, c.cruiseControl.override, c.cruiseControl.cancel, pcm_accel, hud_v_cruise, c.hudControl.lanesVisible, hud_show_car=c.hudControl.leadVisible, hud_alert=hud_alert) self.frame += 1 return can_sends
class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.VM = VehicleModel(CP) self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.cruise_enabled_prev = False # *** init the major players *** self.CS = CarState(CP) self.cp = get_can_parser(CP) #failing here 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 get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): print("CarParams new message") ret = car.CarParams.new_message() print("CarParams setting fingerprint and safety") ret.carName = "mitsubishi" ret.carFingerprint = candidate ret.carVin = vin ret.isPandaBlack = False ret.safetyModel = car.CarParams.SafetyModel.toyota print("CarParams done settng safety model") # # pedal ret.enableCruise = True #not ret.enableGasInterceptor print("CarParams setting car tuning") ret.steerActuatorDelay = 0.1 # Default delay ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] print("CarParams set up PID") stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.55 ret.steerRatio = 17. tire_stiffness_factor = 0.444 ret.mass = 1080. + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] ret.lateralTuning.pid.kf = 0.001388889 # 1 / max angle print("CarParams done setting tuning") ret.steerRateCost = 1.0 print("CarParams steerRateCost set") #% ret.steerRateCost ret.centerToFront = ret.wheelbase * 0.44 print("CarParams centerToFront set") #% ret.centerToFront ret.enableGasInterceptor = True print("CarParams enableGasInterceptor set to True") ret.minEnableSpeed = -1. print("CarParams done setting ratecost and min enable speed") ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) ret.steerRatioRear = 0. ret.steerControlType = car.CarParams.SteerControlType.angle # 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 = True #not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black ret.enableDsu = True #not check_ecu_msgs(fingerprint, ECU.DSU) ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS) ret.openpilotLongitudinalControl = True #ret.enableCamera and ret.enableDsu cloudlog.warn("CarParams ECU DSU Simulated: %r", ret.enableDsu) 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 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] return ret # returns a car.CarState def update(self, c, can_strings): # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.CS.update(self.cp) # create message ret = car.CarState.new_message() ret.canValid = True # 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 # 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 = False #self.CS.brake_pressed != 0 ret.brakeLights = False #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. ret.cruiseState.standstill = False ret.doorOpen = False #not self.CS.door_all_closed ret.seatbeltUnlatched = False #not self.CS.seatbelt # events events = [] 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])) 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, 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.can_invalid_count = 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) # 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 = "chrysler" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.chrysler # 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./2.205 + std_cargo wheelbase_civic = 2.70 centerToFront_civic = wheelbase_civic * 0.4 centerToRear_civic = wheelbase_civic - centerToFront_civic rotationalInertia_civic = 2500 tireStiffnessFront_civic = 85400 * 2.0 tireStiffnessRear_civic = 90000 * 2.0 # Speed conversion: 20, 45 mph ret.steerKpBP, ret.steerKiBP = [[9., 20.], [9., 20.]] ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 ret.mass = 2858 + std_cargo # kg curb weight Pacifica Hybrid 2017 ret.steerKpV, ret.steerKiV = [[0.15,0.30], [0.03,0.05]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.7 if candidate == CAR.JEEP_CHEROKEE: ret.wheelbase = 2.91 # in meters ret.steerRatio = 12.7 ret.steerActuatorDelay = 0.2 # in seconds ret.centerToFront = ret.wheelbase * 0.44 ret.longPidDeadzoneBP = [0., 9.] ret.longPidDeadzoneV = [0., .15] ret.minSteerSpeed = 3.8 # m/s ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this 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. # 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, ECU.CAM) print "ECU Camera Simulated: ", ret.enableCamera ret.openpilotLongitudinalControl = False ret.steerLimitAlert = True 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 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 # same as main_on ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = self.CS.main_on ret.cruiseState.speedOffset = 0. # ignore standstill in hybrid rav4, since pcm allows to restart without # receiving any special command 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 self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed) 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('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) 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 gas pedal and speed isn't zero. Gas pedal is used to resume ACC # from a 3+ second stop. if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) 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() # pass in a car.CarControl # to be called @ 100hz def apply(self, c, perception_state=log.Live20Data.new_message()): if (self.CS.frame == -1): return False # if we haven't seen a frame 220, then do not update. self.frame = self.CS.frame self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert) return False
class CarInterface(object): def __init__(self, CP, CarController): self.CP = CP self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.acc_active_prev = 0 # *** init the major players *** canbus = CanBus() self.CS = CarState(CP, canbus) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP, canbus) self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] self.CC = None if CarController is not None: self.CC = CarController(canbus, CP.carFingerprint) @staticmethod def compute_gb(accel, speed): return float(accel) / 4.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 = "gm" ret.carFingerprint = candidate ret.carVin = vin ret.isPandaBlack = is_panda_black ret.enableCruise = False # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black ret.openpilotLongitudinalControl = ret.enableCamera ret.lateralTuning.pid.dampTime = 0.0 ret.lateralTuning.pid.reactMPC = 0.025 ret.lateralTuning.pid.dampMPC = 0.1 ret.lateralTuning.pid.rateFFGain = 0.4 ret.lateralTuning.pid.polyFactor = 0.005 ret.lateralTuning.pid.polyDampTime = 0.15 ret.lateralTuning.pid.polyReactTime = 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.01]] ret.lateralTuning.pid.kf = 0.000035 # full torque for 20 deg at 80mph means 0.00007818594 tire_stiffness_factor = 0.444 # not optimized yet if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1607. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.69 ret.steerRatio = 15.7 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.MALIBU: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1496. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 ret.steerRatio = 15.8 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.HOLDEN_ASTRA: ret.mass = 1363. + STD_CARGO_KG ret.wheelbase = 2.662 # Remaining parameters copied from Volt for now ret.centerToFront = ret.wheelbase * 0.4 ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.safetyModel = car.CarParams.SafetyModel.gm ret.steerRatio = 15.7 ret.steerRatioRear = 0. elif candidate == CAR.ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.86 ret.steerRatio = 14.4 #end to end is 13.46 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 elif candidate == CAR.BUICK_REGAL: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 #111.4 inches in meters ret.steerRatio = 14.4 # guess for tourx ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx elif candidate == CAR.CADILLAC_ATS: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1601. + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.78 ret.steerRatio = 15.3 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.49 elif candidate == CAR.CADILLAC_CT6: # engage speed is decided by pcm ret.minEnableSpeed = -1. ret.mass = 4016. * CV.LB_TO_KG + STD_CARGO_KG ret.safetyModel = car.CarParams.SafetyModel.cadillac ret.wheelbase = 3.11 ret.steerRatio = 14.6 # it's 16.3 without rear active steering ret.steerRatioRear = 0. # TODO: there is RAS on this car! ret.centerToFront = ret.wheelbase * 0.465 # 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) ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.gasMaxBP = [0.] ret.gasMaxV = [.5] ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] ret.longitudinalTuning.kpBP = [5., 35.] ret.longitudinalTuning.kpV = [2.4, 1.5] ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.36] ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] ret.steerLimitAlert = True ret.stoppingControl = True ret.startAccel = 0.8 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 ret.steerControlType = car.CarParams.SteerControlType.torque return ret # returns a car.CarState def update(self, c, can_strings): self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.pt_cp) # create message ret = car.CarState.new_message() ret.canValid = self.pt_cp.can_valid # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal information. ret.gas = self.CS.pedal_gas / 254.0 ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake / 0xd0 ret.brakePressed = self.CS.brake_pressed # steering wheel ret.steeringAngle = self.CS.angle_steers # torque and user override. Driver awareness # timer resets when the user uses the steering wheel. ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver # cruise state ret.cruiseState.available = bool(self.CS.main_on) cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF ret.cruiseState.enabled = cruiseEnabled ret.cruiseState.standstill = self.CS.pcm_acc_status == 4 ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt ret.gearShifter = self.CS.gear_shifter buttonEvents = [] # blinkers 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 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 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: if not (cruiseEnabled and self.CS.standstill): be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed. elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) ret.buttonEvents = buttonEvents events = [] if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.steer_not_allowed: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) 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.car_fingerprint in SUPERCRUISE_CARS: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) else: if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not self.CS.gear_shifter_valid: events.append( create_event('wrongGear', [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 self.CS.gear_shifter == 3: events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, 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])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: events.append( create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == "cancel" and b.pressed: events.append( create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: hud_v_cruise = 0 chime, chime_count = AUDIO_HUD[c.hudControl.audibleAlert.raw] # For Openpilot, "enabled" includes pre-enable. # In GM, PCM faults out if ACC command overlaps user gas. enabled = c.enabled and not self.CS.user_gas_pressed can_sends = self.CC.update(enabled, self.CS, self.frame, \ c.actuators, hud_v_cruise, c.hudControl.lanesVisible, \ c.hudControl.leadVisible, \ chime, chime_count, c.hudControl.visualAlert) self.frame += 1 return can_sends
class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 self.can_invalid_count = 0 self.acc_active_prev = 0 self.gas_pressed_prev = False # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP) self.cam_cp = get_camera_can_parser(CP) self.gas_pressed_prev = False # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(CP.carFingerprint) @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): return 1.0 @staticmethod def get_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carName = "subaru" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.subaru ret.enableCruise = True ret.steerLimitAlert = True ret.enableCamera = True std_cargo = 136 ret.steerRateCost = 0.7 if candidate in [CAR.IMPREZA]: ret.mass = 1568 + std_cargo ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 15 tire_stiffness_factor = 1.0 ret.steerActuatorDelay = 0.4 # end-to-end angle controller ret.steerKf = 0.00005 ret.steerKiBP, ret.steerKpBP = [[0., 20.], [0., 20.]] ret.steerKpV, ret.steerKiV = [[0.2, 0.3], [0.02, 0.03]] ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.steerControlType = car.CarParams.SteerControlType.torque ret.steerRatioRear = 0. # testing tuning # No long control in subaru ret.gasMaxBP = [0.] ret.gasMaxV = [0.] ret.brakeMaxBP = [0.] ret.brakeMaxV = [0.] ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.longitudinalKpBP = [0.] ret.longitudinalKpV = [0.] ret.longitudinalKiBP = [0.] ret.longitudinalKiV = [0.] # end from gm # hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars mass_civic = 2923./2.205 + 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 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) return ret # returns a car.CarState def update(self, c): self.pt_cp.update(int(sec_since_boot() * 1e9), False) self.cam_cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.pt_cp, self.cam_cp) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # steering wheel ret.steeringAngle = self.CS.angle_steers # torque and user override. Driver awareness # timer resets when the user uses the steering wheel. ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver ret.gas = self.CS.pedal_gas / 255. ret.gasPressed = self.CS.user_gas_pressed # cruise state ret.cruiseState.enabled = bool(self.CS.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. ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on ret.seatbeltUnlatched = self.CS.seatbelt_unlatched ret.doorOpen = self.CS.door_open buttonEvents = [] # blinkers 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 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 buttonEvents.append(be) be = car.CarState.ButtonEvent.new_message() be.type = 'accelCruise' buttonEvents.append(be) 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 ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on gas pedal rising edge if (ret.gasPressed and not self.gas_pressed_prev): 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 # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.acc_active_prev = self.CS.acc_active # cast to reader so it can't be modified return ret.as_reader() def apply(self, c): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert) self.frame += 1
class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.cp = get_can_parser(CP) self.epas_cp = get_epas_parser(CP) # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(self.cp.dbc_name, CP.enableCamera) self.compute_gb = tesla_compute_gb @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): # limit the pcm accel cmd if: # - v_ego exceeds v_target, or # - a_ego exceeds a_target and v_ego is close to v_target eA = a_ego - a_target valuesA = [1.0, 0.1] bpA = [0.3, 1.1] eV = v_ego - v_target valuesV = [1.0, 0.1] bpV = [0.0, 0.5] valuesRangeV = [1., 0.] bpRangeV = [-1., 0.] # only limit if v_ego is close to v_target speedLimiter = interp(eV, bpV, valuesV) accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart return float(max(0.714, a_target / A_ACC_MAX)) * min( speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint): # kg of standard extra cargo to count for drive, gas, etc... std_cargo = 136 # Scaled tire stiffness ts_factor = 8 ret = car.CarParams.new_message() ret.carName = "tesla" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.tesla ret.enableCamera = True ret.enableGasInterceptor = False #keep this False for now print "ECU Camera Simulated: ", ret.enableCamera print "ECU Gas Interceptor: ", ret.enableGasInterceptor ret.enableCruise = not ret.enableGasInterceptor mass_models = 4722. / 2.205 + std_cargo wheelbase_models = 2.959 # RC: I'm assuming center means center of mass, and I think Model S is pretty even between two axles centerToFront_models = wheelbase_models * 0.48 centerToRear_models = wheelbase_models - centerToFront_models rotationalInertia_models = 2500 tireStiffnessFront_models = 85400 tireStiffnessRear_models = 90000 # will create Kp and Ki for 0, 20, 40, 60 mph ret.steerKiBP, ret.steerKpBP = [[0., 8.94, 17.88, 26.82], [0., 8.94, 17.88, 26.82]] if candidate == CAR.MODELS: stop_and_go = True ret.mass = mass_models ret.wheelbase = wheelbase_models ret.centerToFront = centerToFront_models ret.steerRatio = 15.75 # Kp and Ki for the lateral control for 0, 20, 40, 60 mph ret.steerKpV, ret.steerKiV = [[1.20, 0.80, 0.60, 0.30], [0.16, 0.12, 0.08, 0.04]] ret.steerKf = 0.00006 # Initial test value TODO: investigate FF steer control for Model S? ret.steerActuatorDelay = 0.09 # Kp and Ki for the longitudinal control ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.27 / K_MULT, 1.05 / K_MULT, 0.85 / K_MULT] ret.longitudinalKiBP = [0., 5., 35.] ret.longitudinalKiV = [ 0.11 / K_MULTi, 0.09 / K_MULTi, 0.06 / K_MULTi ] #from honda #ret.longitudinalKpBP = [0., 5., 35.] #ret.longitudinalKpV = [1.2, 0.8, 0.5] #ret.longitudinalKiBP = [0., 35.] #ret.longitudinalKiV = [0.18, 0.12] # from toyota #ret.longitudinalKpBP = [0., 5., 35.] #ret.longitudinalKpV = [3.6, 2.4, 1.5] #ret.longitudinalKiBP = [0., 35.] #ret.longitudinalKiV = [0.54, 0.36] else: raise ValueError("unsupported car %s" % candidate) ret.steerControlType = car.CarParams.SteerControlType.angle # 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. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if ( stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for Model S ret.rotationalInertia = rotationalInertia_models * \ ret.mass * ret.wheelbase**2 / (mass_models * wheelbase_models**2) # TODO: start from empirically derived lateral slip stiffness and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront = (tireStiffnessFront_models * ts_factor) * \ ret.mass / mass_models * \ (centerToRear / ret.wheelbase) / (centerToRear_models / wheelbase_models) ret.tireStiffnessRear = (tireStiffnessRear_models * ts_factor) * \ ret.mass / mass_models * \ (ret.centerToFront / ret.wheelbase) / (centerToFront_models / wheelbase_models) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. # no max steer limit VS speed ret.steerMaxBP = [0., 15.] # m/s ret.steerMaxV = [420., 420.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [ 0.6 ] #if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [0., 20.] # m/s ret.brakeMaxV = [ 1., 1. ] # max brake allowed - BB: since we are using regen, make this even ret.longPidDeadzoneBP = [ 0., 9. ] #BB: added from Toyota to start pedal work; need to tune ret.longPidDeadzoneV = [ 0., 0. ] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now ret.stoppingControl = True ret.steerLimitAlert = False ret.startAccel = 0.5 ret.steerRateCost = 1. return ret # returns a car.CarState def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update(int(sec_since_boot() * 1e9), False) self.epas_cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp, self.epas_cp) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal, we don't use with with interceptor so it's always 0/False ret.gas = self.CS.user_gas if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.user_gas_pressed else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brakePressed = (self.CS.brake_pressed != 0) and ( self.CS.cstm_btns.get_button_status("brake") == 0) # FIXME: read sendcan for brakelights brakelights_threshold = 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = True #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 = self.CS.cruise_speed_offset ret.cruiseState.standstill = False # TODO: button presses 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 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) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True else: be.pressed = False buttonEvents.append(be) ret.buttonEvents = buttonEvents # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time 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.steer_error: if self.CS.cstm_btns.get_button_status("steer") == 0: events.append( create_event('steerUnavailable', [ET.NO_ENTRY, ET.WARNING])) elif self.CS.steer_warning: if self.CS.cstm_btns.get_button_status("steer") == 0: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) 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.brake_hold: events.append( create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # Standard OP method to disengage: # 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('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if (self.CS.cstm_btns.get_button_status("brake") > 0): if ((self.CS.brake_pressed != 0) != self.brake_pressed_prev ): #break not canceling when pressed self.CS.cstm_btns.set_button_status( "brake", 2 if self.CS.brake_pressed != 0 else 1) else: if ret.brakePressed: events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append( create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append( create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type == "altButton3" and not b.pressed: print "enabled pressed at", cur_time self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = self.CS.brake_pressed != 0 # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c, perception_state=log.Live20Data.new_message()): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: hud_v_cruise = 255 hud_alert = { "none": AH.NONE, "fcw": AH.FCW, "steerRequired": AH.STEER, "brakePressed": AH.BRAKE_PRESSED, "wrongGear": AH.GEAR_NOT_D, "seatbeltUnbuckled": AH.SEATBELT, "speedTooHigh": AH.SPEED_TOO_HIGH }[str(c.hudControl.visualAlert)] snd_beep, snd_chime = { "none": (BP.MUTE, CM.MUTE), "beepSingle": (BP.SINGLE, CM.MUTE), "beepTriple": (BP.TRIPLE, CM.MUTE), "beepRepeated": (BP.REPEATED, CM.MUTE), "chimeSingle": (BP.MUTE, CM.SINGLE), "chimeDouble": (BP.MUTE, CM.DOUBLE), "chimeRepeated": (BP.MUTE, CM.REPEATED), "chimeContinuous": (BP.MUTE, CM.CONTINUOUS) }[str(c.hudControl.audibleAlert)] pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6) self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ c.actuators, \ c.cruiseControl.speedOverride, \ c.cruiseControl.override, \ c.cruiseControl.cancel, \ pcm_accel, \ hud_v_cruise, c.hudControl.lanesVisible, \ hud_show_car = c.hudControl.leadVisible, \ hud_alert = hud_alert, \ snd_beep = snd_beep, \ snd_chime = snd_chime) self.frame += 1
class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.acc_active_prev = 0 # *** init the major players *** canbus = CanBus() self.CS = CarState(CP, canbus) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP, canbus) self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(canbus, CP.carFingerprint, CP.enableCamera) @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): return 1.0 @staticmethod def get_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carName = "gm" ret.carFingerprint = candidate ret.enableCruise = False # Presence of a camera on the object bus is ok. # Have to go passive if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) std_cargo = 136 if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS # kg of standard extra cargo to count for drive, gas, etc... ret.mass = 1607 + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.69 ret.steerRatio = 15.7 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.CADILLAC_CT6: # engage speed is decided by pcm ret.minEnableSpeed = -1 # kg of standard extra cargo to count for drive, gas, etc... ret.mass = 4016. * CV.LB_TO_KG + std_cargo ret.safetyModel = car.CarParams.SafetyModels.cadillac ret.wheelbase = 3.11 ret.steerRatio = 14.6 # it's 16.3 without rear active steering ret.steerRatioRear = 0. # TODO: there is RAS on this car! ret.centerToFront = ret.wheelbase * 0.465 # 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 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) # same tuning for Volt and CT6 for now ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerKpV, ret.steerKiV = [[0.2], [0.00]] ret.steerKf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.gasMaxBP = [0.] ret.gasMaxV = [.5] ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] ret.longitudinalKpBP = [5., 35.] ret.longitudinalKpV = [2.4, 1.5] ret.longitudinalKiBP = [0.] ret.longitudinalKiV = [0.36] ret.steerLimitAlert = True ret.stoppingControl = True ret.startAccel = 0.8 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 ret.steerControlType = car.CarParams.SteerControlType.torque return ret # returns a car.CarState def update(self, c): self.pt_cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.pt_cp) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal information. ret.gas = self.CS.pedal_gas / 254.0 ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake / 0xd0 ret.brakePressed = self.CS.brake_pressed # steering wheel ret.steeringAngle = self.CS.angle_steers # torque and user override. Driver awareness # timer resets when the user uses the steering wheel. ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver # cruise state ret.cruiseState.available = bool(self.CS.main_on) cruiseEnabled = self.CS.pcm_acc_status != 0 ret.cruiseState.enabled = cruiseEnabled ret.cruiseState.standstill = self.CS.pcm_acc_status == 4 ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt ret.gearShifter = self.CS.gear_shifter buttonEvents = [] # blinkers 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 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 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: if not (cruiseEnabled and self.CS.standstill): be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed. elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) ret.buttonEvents = buttonEvents 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.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.steer_not_allowed: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) 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.car_fingerprint == CAR.VOLT: if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not self.CS.gear_shifter_valid: events.append(create_event('wrongGear', [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 self.CS.gear_shifter == 3: events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, 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])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CS.car_fingerprint == CAR.CADILLAC_CT6: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c, perception_state=log.Live20Data.new_message()): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: hud_v_cruise = 0 chime, chime_count = { "none": (0, 0), "beepSingle": (CM.HIGH_CHIME, 1), "beepTriple": (CM.HIGH_CHIME, 3), "beepRepeated": (CM.LOW_CHIME, -1), "chimeSingle": (CM.LOW_CHIME, 1), "chimeDouble": (CM.LOW_CHIME, 2), "chimeRepeated": (CM.LOW_CHIME, -1), "chimeContinuous": (CM.LOW_CHIME, -1)}[str(c.hudControl.audibleAlert)] # For Openpilot, "enabled" includes pre-enable. # In GM, PCM faults out if ACC command overlaps user gas. enabled = c.enabled and not self.CS.user_gas_pressed self.CC.update(self.sendcan, enabled, self.CS, self.frame, \ c.actuators, hud_v_cruise, c.hudControl.lanesVisible, \ c.hudControl.leadVisible, \ chime, chime_count) self.frame += 1
class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.frame = 0 self.acc_active_prev = 0 self.gas_pressed_prev = False # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP) self.cam_cp = get_camera_can_parser(CP) self.gas_pressed_prev = False self.CC = None if CarController is not None: self.CC = CarController(CP.carFingerprint) @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): ret = car.CarParams.new_message() ret.carName = "subaru" ret.radarOffCan = True ret.carFingerprint = candidate ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.subaru ret.enableCruise = True # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches) # was never released ret.enableCamera = True ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 if candidate in [CAR.IMPREZA]: ret.mass = 1568. + STD_CARGO_KG ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 15 ret.steerActuatorDelay = 0.4 # end-to-end angle controller ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[ 0., 20. ], [0., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [ 0.02, 0.03 ]] ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.steerControlType = car.CarParams.SteerControlType.torque ret.steerRatioRear = 0. # testing tuning # No long control in subaru ret.gasMaxBP = [0.] ret.gasMaxV = [0.] ret.brakeMaxBP = [0.] ret.brakeMaxV = [0.] ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] ret.longitudinalTuning.kpBP = [0.] ret.longitudinalTuning.kpV = [0.] ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.] # end from gm # 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) return ret # returns a car.CarState def update(self, c, can_strings): self.pt_cp.update_strings(can_strings) self.cam_cp.update_strings(can_strings) self.CS.update(self.pt_cp, self.cam_cp) # create message ret = car.CarState.new_message() ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # steering wheel ret.steeringAngle = self.CS.angle_steers # torque and user override. Driver awareness # timer resets when the user uses the steering wheel. ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False ret.gas = self.CS.pedal_gas / 255. ret.gasPressed = self.CS.user_gas_pressed # cruise state ret.cruiseState.enabled = bool(self.CS.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. ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on ret.seatbeltUnlatched = self.CS.seatbelt_unlatched ret.doorOpen = self.CS.door_open buttonEvents = [] # blinkers 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 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 buttonEvents.append(be) be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.accelCruise buttonEvents.append(be) events = [] if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on gas pedal rising edge if (ret.gasPressed and not self.gas_pressed_prev): 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 # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.acc_active_prev = self.CS.acc_active # cast to reader so it can't be modified 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) self.frame += 1 return can_sends
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.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.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 = "chrysler" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.chrysler # 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. / 2.205 + std_cargo wheelbase_civic = 2.70 centerToFront_civic = wheelbase_civic * 0.4 centerToRear_civic = wheelbase_civic - centerToFront_civic rotationalInertia_civic = 2500 tireStiffnessFront_civic = 85400 * 2.0 tireStiffnessRear_civic = 90000 * 2.0 # Speed conversion: 0, 20, 45, 75 mph ret.steerKpBP, ret.steerKiBP = [[0., 9., 20., 34.], [0., 9., 20., 34.]] ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 ret.mass = 2858 + std_cargo # kg curb weight Pacifica Hybrid 2017 ret.steerKpV, ret.steerKiV = [[0.15, 0.15, 0.34, 0.34], [0.03, 0.03, 0.03, 0.03]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 1. 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. ret.minEnableSpeed = 5. * CV.MPH_TO_MS # -1 for stop-and-go 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. # 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 = False #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 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 # is this the same as main_on an issue? ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = self.CS.main_on ret.cruiseState.speedOffset = 0. # ignore standstill in hybrid rav4, since pcm allows to restart without # receiving any special command 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' 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, perception_state=log.Live20Data.new_message()): #! if our frame isn't synced with 220, then reset it. # this wasn't needed in the game, so could it be needed for OP?!?! # check if it's more than 3 away, accounting for 0x10 wrap-around. f1 = int(self.frame) % 0x10 f2 = int(self.CS.frame_220 ) % 0x10 # shouldn't need the mod, but just in case. fmin = min(f1, f2) fmax = max(f1, f2) if ((fmax - fmin) > 2) and ((fmin + 0x10 - fmax) > 2): # copy lower nibble from frame_220 to our frame so they match self.frame = (int(self.frame) & 0xfffffff0) | f2 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