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
