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.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 = get_can_parser(CP, mydbc)
self.epas_cp = None
if self.CS.useWithoutHarness:
self.epas_cp = get_epas_parser(CP, 0)
else:
self.epas_cp = get_epas_parser(CP, 2)
self.pedal_cp = get_pedal_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name)
self.compute_gb = tesla_compute_gb
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)
self.compute_gb = get_compute_gb_models
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():
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.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 = get_can_parser(CP, mydbc)
self.epas_cp = None
if self.CS.useWithoutHarness:
self.epas_cp = get_epas_parser(CP, 0)
else:
self.epas_cp = get_epas_parser(CP, 2)
self.pedal_cp = get_pedal_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name)
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
# 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, vin="", is_panda_black=False):
# Scaled tire stiffness
ts_factor = 8
ret = car.CarParams.new_message()
ret.carName = "tesla"
ret.carFingerprint = candidate
ret.isPandaBlack = is_panda_black
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 = vin
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")
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 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
