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.can_invalid_count = 0
self.alca = messaging.pub_sock(service_list['alcaStatus'].port)
# *** 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
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 / max(_A_CRUISE_MAX_V_FOLLOWING))) * 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 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 = 12.
# 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.3]
ret.longitudinalTuning.kiBP = [0., 5., 35.]
ret.longitudinalTuning.kiV = [0.009, 0.008, 0.007]
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.50, 0.45, 0.4]
ret.longitudinalTuning.kiBP = [0., 5., 35.]
ret.longitudinalTuning.kiV = [0.009, 0.008, 0.007]
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 = 0.7
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(int(sec_since_boot() * 1e9), can_strings)
ch_can_valid = self.cp.can_valid
self.epas_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
epas_can_valid = self.epas_cp.can_valid
self.pedal_cp.update_strings(int(sec_since_boot() * 1e9), 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 or not self.CS.enableDriverMonitor
# 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_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
(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
#pass ALCA status
alca_status = tesla.ALCAStatus.new_message()
alca_status.alcaEnabled = bool(self.CS.ALCA_enabled)
alca_status.alcaTotalSteps = int(self.CS.ALCA_total_steps)
alca_status.alcaDirection = int(self.CS.ALCA_direction)
alca_status.alcaError = bool(self.CS.ALCA_error)
self.alca.send(alca_status.to_bytes())
# 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(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.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):
ret = car.CarParams.new_message()
ret.carName = "toyota"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.toyota
# pedal
ret.enableCruise = not ret.enableGasInterceptor
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
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_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
elif candidate == CAR.LEXUS_IS:
stop_and_go = False
ret.safetyParam = 66
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
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.CAM) or has_relay
ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.DSU) or (has_relay and candidate 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
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
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(can_strings)
self.cp_cam.update_strings(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.steeringTorqueEps = self.CS.steer_torque_motor
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 = 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
# events
events = []
if self.cp_cam.can_valid:
self.forwarding_camera = True
if self.cp_cam.can_invalid_cnt >= 100 and self.CP.enableCamera:
events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT]))
if not ret.gearShifter == 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 == 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
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
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric", encoding='utf8') == "1"
is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
passive = passive or not openpilot_enabled_toggle
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
'carParams'
])
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'plan', 'pathPlan', \
'model'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
car_recognized = CP.carName != 'mock'
#
# WARNING
#
# By removing or modifying this code you accept all responsibility and/or liability
# related to the use or misuse of this code.
#
# WARNING
#
vin_recognized = False
if car_recognized:
vin_hash = hashlib.md5(CP.carVin.encode('utf-8')).hexdigest()
print("vin hash: " + vin_hash)
if vin_hash in \
[
'69932544a73687c13e6963bedf2c3c57',
]:
print("vin recognized")
vin_recognized = True
panda_recognized = False
panda_dongle_id = params.get('PandaDongleId')
# If panda_dongle_id is None we may be running in docker/CI
if panda_dongle_id is not None:
# if dongle is detected, verify the ID.
panda_dongle_hash = hashlib.md5(panda_dongle_id).hexdigest()
print("panda dongle hash: " + panda_dongle_hash)
if panda_dongle_hash in \
[
'4305ef904aa31998f83431f61ab77b9e',
]:
print("panda dongle recognized")
if not panda_recognized and not vin_recognized:
car_recognized = False
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
community_feature_disallowed = CP.communityFeature and not community_feature_toggle
read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = CP.to_bytes()
params.put("CarParams", cp_bytes)
params.put("CarParamsCache", cp_bytes)
params.put("LongitudinalControl",
"1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP)
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
mismatch_counter = 0
can_error_counter = 0
last_blinker_frame = 0
events_prev = []
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
sm['thermal'].freeSpace = 1.
# detect sound card presence
sounds_available = not os.path.isfile('/EON') or (
os.path.isdir('/proc/asound/card0')
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
prof = Profiler(False) # off by default
while True:
start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(
CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter,
params)
prof.checkpoint("Sample")
# Create alerts
if not sm.alive['plan'] and sm.alive[
'pathPlan']: # only plan not being received: radar not communicating
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif not sm.all_alive_and_valid():
events.append(
create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid:
events.append(
create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(
create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
if not sm['plan'].radarValid:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(
create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
if internet_needed:
events.append(
create_event('internetConnectivityNeeded',
[ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
events.append(
create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if read_only and not passive:
events.append(create_event('carUnrecognized', [ET.PERMANENT]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm[
'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 read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, AM, LaC, LoC,
read_only, start_time, v_acc, a_acc,
lac_log, events_prev, last_blinker_frame,
is_ldw_enabled, can_error_counter)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
class Controls:
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster([
'thermal', 'health', 'model', 'liveCalibration', 'frontFrame',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
internet_needed = (params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None) and (
params.get("DisableUpdates") != b"1")
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def update_events(self, CS):
"""Compute carEvents from carState"""
self.events.clear()
self.events.add_from_msg(CS.events)
self.events.add_from_msg(self.sm['dMonitoringState'].events)
# Handle startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
# Create events for battery, temperature, disk space, and memory
if self.sm['thermal'].batteryPercent < 1 and self.sm[
'thermal'].chargingError:
# at zero percent battery, while discharging, OP should not allowed
self.events.add(EventName.lowBattery)
if self.sm['thermal'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['thermal'].freeSpace < 0.07:
# under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace)
if self.sm['thermal'].memUsedPercent > 90:
self.events.add(EventName.lowMemory)
# Alert if fan isn't spinning for 5 seconds
if self.sm['health'].hwType in [HwType.uno, HwType.dos]:
if self.sm['health'].fanSpeedRpm == 0 and self.sm[
'thermal'].fanSpeed > 50:
if (self.sm.frame -
self.last_functional_fan_frame) * DT_CTRL > 5.0:
self.events.add(EventName.fanMalfunction)
else:
self.last_functional_fan_frame = self.sm.frame
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
self.events.add(EventName.calibrationIncomplete)
else:
self.events.add(EventName.calibrationInvalid)
# Handle lane change
if self.sm[
'pathPlan'].laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['pathPlan'].laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['pathPlan'].laneChangeState in [
LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing
]:
self.events.add(EventName.laneChange)
if self.can_rcv_error or (not CS.canValid
and self.sm.frame > 5 / DT_CTRL):
self.events.add(EventName.canError)
if self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if not self.sm.alive['plan'] and self.sm.alive['pathPlan']:
# only plan not being received: radar not communicating
self.events.add(EventName.radarCommIssue)
elif not self.sm.all_alive_and_valid():
self.events.add(EventName.commIssue)
if not self.sm['pathPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid)
if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled
> 1000):
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
if not (SIMULATION or NOSENSOR): # TODO: send GPS in carla
self.events.add(EventName.noGps)
if not self.sm['pathPlan'].paramsValid:
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if not self.sm['plan'].radarValid:
self.events.add(EventName.radarFault)
if self.sm['plan'].radarCanError:
self.events.add(EventName.radarCanError)
if log.HealthData.FaultType.relayMalfunction in self.sm[
'health'].faults:
self.events.add(EventName.relayMalfunction)
if self.sm['plan'].fcw:
self.events.add(EventName.fcw)
if self.sm['model'].frameDropPerc > 1 and not SIMULATION:
self.events.add(EventName.modeldLagging)
if not self.sm.alive['frontFrame'] and (
self.sm.frame > 5 / DT_CTRL) and not SIMULATION:
self.events.add(EventName.cameraMalfunction)
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
self.events.add(EventName.noTarget)
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
if not self.sm['health'].controlsAllowed and self.enabled:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
return CS
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_kph_last = self.v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not self.CP.enableCruise:
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph,
CS.buttonEvents, self.enabled)
elif self.CP.enableCruise and CS.cruiseState.enabled:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = 300 # 3s
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(
ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_kph = initialize_v_cruise(
CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
if self.active:
self.current_alert_types.append(ET.WARNING)
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
plan = self.sm['plan']
path_plan = self.sm['pathPlan']
actuators = car.CarControl.Actuators.new_message()
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not self.active:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['plan'])
# no greater than dt mpc + dt, to prevent too high extraps
dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget -
plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
# Gas/Brake PID loop
actuators.gas, actuators.brake = self.LoC.update(
self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(
self.active, CS, self.CP, path_plan)
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
return actuators, v_acc_sol, a_acc_sol, lac_log
def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not self.CP.enableCruise or (
not self.enabled and CS.cruiseState.enabled)
# Some override values for Honda
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
speed_override = max(0.0,
(self.LoC.v_pid + CS.cruiseState.speedOffset) *
brake_discount)
CC.cruiseControl.speedOverride = float(
speed_override if self.CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = self.CI.calc_accel_override(
CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget)
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['plan'].hasLead
right_lane_visible = self.sm['pathPlan'].rProb > 0.5
left_lane_visible = self.sm['pathPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (self.sm.frame - self.last_blinker_frame
) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['model'].meta
if len(meta.desirePrediction) and ldw_allowed:
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft -
1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight -
1]
l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3]
< (1.08 - CAMERA_OFFSET))
r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3]
> -(1.08 + CAMERA_OFFSET))
CC.hudControl.leftLaneDepart = bool(
l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(
r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types,
[self.CP, self.sm, self.is_metric])
self.AM.add_many(self.sm.frame, alerts, self.enabled)
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
if not self.read_only:
# send car controls over can
can_sends = self.CI.apply(CC)
self.pm.send(
'sendcan',
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
steer_angle_rad = (CS.steeringAngle -
self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.alertText1 = self.AM.alert_text_1
controlsState.alertText2 = self.AM.alert_text_2
controlsState.alertSize = self.AM.alert_size
controlsState.alertStatus = self.AM.alert_status
controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert
controlsState.driverMonitoringOn = self.sm[
'dMonitoringState'].faceDetected
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.planMonoTime = self.sm.logMonoTime['plan']
controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.vEgo = CS.vEgo
controlsState.vEgoRaw = CS.vEgoRaw
controlsState.angleSteers = CS.steeringAngle
controlsState.curvature = self.VM.calc_curvature(
steer_angle_rad, CS.vEgo)
controlsState.steerOverride = CS.steeringPressed
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.angleSteersDes = float(self.LaC.angle_steers_des)
controlsState.vTargetLead = float(v_acc)
controlsState.aTarget = float(a_acc)
controlsState.jerkFactor = float(self.sm['plan'].jerkFactor)
controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive
controlsState.vCurvature = self.sm['plan'].vCurvature
controlsState.decelForModel = self.sm[
'plan'].longitudinalPlanSource == LongitudinalPlanSource.model
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.mapValid = self.sm['plan'].mapValid
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
if self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL)
== 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def step(self):
start_time = sec_since_boot()
self.prof.checkpoint("Ratekeeper", ignore=True)
# Sample data from sockets and get a carState
CS = self.data_sample()
self.prof.checkpoint("Sample")
self.update_events(CS)
if not self.read_only:
# Update control state
self.state_transition(CS)
self.prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_acc, a_acc, lac_log = self.state_control(CS)
self.prof.checkpoint("State Control")
# Publish data
self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
self.prof.checkpoint("Sent")
def controlsd_thread(self):
while True:
self.step()
self.rk.monitor_time()
self.prof.display()
def ui_thread(addr, frame_address):
# TODO: Detect car from replay and use that to select carparams
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017")
VM = VehicleModel(CP)
CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]])
vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]])
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
if HOR:
size = (640+384+640, 960)
write_x = 5
write_y = 680
else:
size = (640+384, 960+300)
write_x = 645
write_y = 970
pygame.display.set_caption("openpilot debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
alert1_font = pygame.font.SysFont("arial", 30)
alert2_font = pygame.font.SysFont("arial", 20)
info_font = pygame.font.SysFont("arial", 15)
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert()
cameraw_test_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24)
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y),0,8)
frame = messaging.sub_sock('frame', addr=addr, conflate=True)
sm = messaging.SubMaster(['carState', 'plan', 'carControl', 'radarState', 'liveCalibration', 'controlsState', 'liveTracks', 'model', 'liveMpc', 'liveParameters', 'pathPlan'], addr=addr)
calibration = None
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image
lid_overlay_blank = get_blank_lid_overlay(UP)
# plots
name_to_arr_idx = { "gas": 0,
"computer_gas": 1,
"user_brake": 2,
"computer_brake": 3,
"v_ego": 4,
"v_pid": 5,
"angle_steers_des": 6,
"angle_steers": 7,
"angle_steers_k": 8,
"steer_torque": 9,
"v_override": 10,
"v_cruise": 11,
"a_ego": 12,
"a_target": 13,
"accel_override": 14}
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.), (-3.0, 2.0)]
plot_names = [["gas", "computer_gas", "user_brake", "computer_brake", "accel_override"],
["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
["v_ego", "v_override", "v_pid", "v_cruise"],
["a_ego", "a_target"]]
plot_colors = [["b", "b", "g", "r", "y"],
["b", "g", "y", "r"],
["b", "g", "r", "y"],
["b", "r"]]
plot_styles = [["-", "-", "-", "-", "-"],
["-", "-", "-", "-"],
["-", "-", "-", "-"],
["-", "-"]]
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True)
counter = 0
while 1:
list(pygame.event.get())
screen.fill((64,64,64))
lid_overlay = lid_overlay_blank.copy()
top_down = top_down_surface, lid_overlay
# ***** frame *****
fpkt = messaging.recv_one(frame)
rgb_img_raw = fpkt.frame.image
if fpkt.frame.transform:
img_transform = np.array(fpkt.frame.transform).reshape(3,3)
else:
# assume frame is flipped
img_transform = np.array([
[-1.0, 0.0, FULL_FRAME_SIZE[0]-1],
[ 0.0, -1.0, FULL_FRAME_SIZE[1]-1],
[ 0.0, 0.0, 1.0]
])
if rgb_img_raw and len(rgb_img_raw) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3:
imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3))
imgff = imgff[:, :, ::-1] # Convert BGR to RGB
cv2.warpAffine(imgff, np.dot(img_transform, _BB_TO_FULL_FRAME)[:2],
(img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
intrinsic_matrix = eon_intrinsics
else:
img.fill(0)
intrinsic_matrix = np.eye(3)
if calibration is not None:
transform = np.dot(img_transform, calibration.model_to_full_frame)
imgw = cv2.warpAffine(imgff, transform[:2], (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]), flags=cv2.WARP_INVERSE_MAP)
else:
imgw.fill(0)
sm.update()
w = sm['controlsState'].lateralControlState.which()
if w == 'lqrState':
angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steerAngle
elif w == 'indiState':
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steerAngle
else:
angle_steers_k = np.inf
plot_arr[:-1] = plot_arr[1:]
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['controlsState'].angleSteers
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steerAngle
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas
plot_arr[-1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.steer * ANGLE_SCALE
plot_arr[-1, name_to_arr_idx['computer_brake']] = sm['carControl'].actuators.brake
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['controlsState'].vEgo
plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid
plot_arr[-1, name_to_arr_idx['v_override']] = sm['carControl'].cruiseControl.speedOverride
plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
plot_arr[-1, name_to_arr_idx['a_target']] = sm['plan'].aTarget
plot_arr[-1, name_to_arr_idx['accel_override']] = sm['carControl'].cruiseControl.accelOverride
# ***** model ****
# if len(sm['model'].path.poly) > 0:
model_data = extract_model_data(sm['model'])
imgw_copy = copy.deepcopy(imgw)
paths = plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration,
top_down, np.array(sm['pathPlan'].dPoly))
# interlock code HERE
if calibration:
left_fit = np.polyfit(_PATH_XD, model_data.lpath.y,2)
right_fit = np.polyfit(_PATH_XD, model_data.rpath.y,2)
imgw_copy = cv2.resize(imgw_copy, (int(imgw_copy.shape[1] * SCALE), int(imgw_copy.shape[0] * SCALE)))
interlock(imgw_copy, left_fit, right_fit, calibration.car_to_model, REG_THRESHOLDS)
# MPC
if sm.updated['liveMpc']:
draw_mpc(sm['liveMpc'], top_down)
# draw all radar points
maybe_update_radar_points(sm['liveTracks'], top_down[1])
if sm.updated['liveCalibration']:
extrinsic_matrix = np.asarray(sm['liveCalibration'].extrinsicMatrix).reshape(3, 4)
ke = intrinsic_matrix.dot(extrinsic_matrix)
warp_matrix = get_camera_frame_from_model_frame(ke)
calibration = CalibrationTransformsForWarpMatrix(warp_matrix, intrinsic_matrix, extrinsic_matrix)
# draw red pt for lead car in the main img
for lead in [sm['radarState'].leadOne, sm['radarState'].leadTwo]:
if lead.status:
if calibration is not None:
draw_lead_on(img, lead.dRel, lead.yRel, calibration, color=(192,0,0))
draw_lead_car(lead.dRel, top_down)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0,1))
screen.blit(camera_surface, (0, 0))
# display alerts
alert_line1 = alert1_font.render(sm['controlsState'].alertText1, True, (255,0,0))
alert_line2 = alert2_font.render(sm['controlsState'].alertText2, True, (255,0,0))
screen.blit(alert_line1, (180, 150))
screen.blit(alert_line2, (180, 190))
if calibration is not None and img is not None:
cpw = warp_points(CalP, calibration.model_to_bb)
vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb)
pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1)
pygame.draw.circle(screen, BLUE, list(map(int, map(round, vanishing_pointw[0]))), 2)
if HOR:
screen.blit(draw_plots(plot_arr), (640+384, 0))
else:
screen.blit(draw_plots(plot_arr), (0, 600))
pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1))
screen.blit(cameraw_surface, (320, 480))
pygame.surfarray.blit_array(*top_down)
screen.blit(top_down[0], (640,0))
i = 0
SPACING = 25
lines = [
info_font.render("ENABLED", True, GREEN if sm['controlsState'].enabled else BLACK),
info_font.render("BRAKE LIGHTS", True, RED if sm['carState'].brakeLights else BLACK),
info_font.render("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", True, YELLOW),
info_font.render("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), True, YELLOW),
info_font.render("LONG MPC SOURCE: " + str(sm['plan'].longitudinalPlanSource), True, YELLOW),
None,
info_font.render("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverage, 2)) + " deg", True, YELLOW),
info_font.render("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffset, 2)) + " deg", True, YELLOW),
info_font.render("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100., 2)) + " %", True, YELLOW),
info_font.render("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), True, YELLOW)
]
for i, line in enumerate(lines):
if line is not None:
screen.blit(line, (write_x, write_y + i * SPACING))
# this takes time...vsync or something
pygame.display.flip()
def controlsd_thread(gctx=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
# Pub Sockets
sendcan = messaging.pub_sock(service_list['sendcan'].port)
controlsstate = messaging.pub_sock(service_list['controlsState'].port)
carstate = messaging.pub_sock(service_list['carState'].port)
carcontrol = messaging.pub_sock(service_list['carControl'].port)
carevents = messaging.pub_sock(service_list['carEvents'].port)
carparams = messaging.pub_sock(service_list['carParams'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
sm = messaging.SubMaster([
'thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan',
'pathPlan'
])
logcan = messaging.sub_sock(service_list['can'].port)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
wait_for_can(logcan)
CI, CP = get_car(logcan, sendcan, is_panda_black)
logcan.close()
# TODO: Use the logcan socket from above, but that will currenly break the tests
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port,
timeout=can_timeout)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl",
"1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP)
driver_status = DriverStatus()
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.INVALID
cal_perc = 0
mismatch_counter = 0
low_battery = False
events_prev = []
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
# detect sound card presence
sounds_available = not os.path.isfile('/EON') or (
os.path.isdir('/proc/asound/card0')
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
prof = Profiler(False) # off by default
while True:
start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params)
prof.checkpoint("Sample")
# Create alerts
if not sm.all_alive_and_valid():
events.append(
create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid:
events.append(
create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(
create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['plan'].radarValid:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(
create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm[
'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 read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, carstate,
carcontrol, carevents, carparams,
controlsstate, sendcan, AM, driver_status,
LaC, LoC, read_only, start_time, v_acc,
a_acc, lac_log, events_prev)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
class Controls:
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster(
[
'deviceState', 'pandaStates', 'peripheralState', 'modelV2',
'liveCalibration', 'driverMonitoringState',
'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'
] + self.camera_packets + joystick_packet,
ignore_alive=ignore,
ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (
not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.button_timers = {
ButtonEvent.Type.decelCruise: 0,
ButtonEvent.Type.accelCruise: 0
}
# scc smoother
self.is_cruise_enabled = False
self.applyMaxSpeed = 0
self.apply_accel = 0.
self.fused_accel = 0.
self.lead_drel = 0.
self.aReqValue = 0.
self.aReqValueMin = 0.
self.aReqValueMax = 0.
self.sccStockCamStatus = 0
self.sccStockCamAct = 0
self.left_lane_visible = False
self.right_lane_visible = False
self.wide_camera = TICI and params.get_bool('EnableWideCamera')
self.disable_op_fcw = params.get_bool('DisableOpFcw')
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
set_offroad_alert("Offroad_CarUnrecognized", True)
else:
set_offroad_alert("Offroad_NoFirmware", True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def update_events(self, CS):
"""Compute carEvents from carState"""
self.events.clear()
self.events.add_from_msg(CS.events)
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Handle startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
# Don't add any more events if not initialized
if not self.initialized:
self.events.add(EventName.controlsInitializing)
return
# Create events for battery, temperature, disk space, and memory
if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \
self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
# at zero percent battery, while discharging, OP should not allowed
self.events.add(EventName.lowBattery)
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
# under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace)
# TODO: make tici threshold the same
if self.sm['deviceState'].memoryUsagePercent > (90 if TICI else
65) and not SIMULATION:
self.events.add(EventName.lowMemory)
# TODO: enable this once loggerd CPU usage is more reasonable
#cpus = list(self.sm['deviceState'].cpuUsagePercent)[:(-1 if EON else None)]
#if max(cpus, default=0) > 95 and not SIMULATION:
# self.events.add(EventName.highCpuUsage)
# Alert if fan isn't spinning for 5 seconds
if self.sm['peripheralState'].pandaType in [
PandaType.uno, PandaType.dos
]:
if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm[
'deviceState'].fanSpeedPercentDesired > 50:
if (self.sm.frame -
self.last_functional_fan_frame) * DT_CTRL > 5.0:
self.events.add(EventName.fanMalfunction)
else:
self.last_functional_fan_frame = self.sm.frame
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
self.events.add(EventName.calibrationIncomplete)
else:
self.events.add(EventName.calibrationInvalid)
# Handle lane change
if self.sm[
'lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['lateralPlan'].laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
elif self.sm['lateralPlan'].autoLaneChangeEnabled and self.sm[
'lateralPlan'].autoLaneChangeTimer > 0:
self.events.add(EventName.autoLaneChange)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['lateralPlan'].laneChangeState in [
LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing
]:
self.events.add(EventName.laneChange)
if self.can_rcv_error or not CS.canValid:
self.events.add(EventName.canError)
for i, pandaState in enumerate(self.sm['pandaStates']):
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
if i < len(self.CP.safetyConfigs):
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[
i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[
i].safetyParam
else:
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
if safety_mismatch or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
self.events.add(EventName.relayMalfunction)
# Check for HW or system issues
if len(self.sm['radarState'].radarErrors):
self.events.add(EventName.radarFault)
elif not self.sm.valid["pandaStates"]:
self.events.add(EventName.usbError)
elif not self.sm.all_alive_and_valid():
self.events.add(EventName.commIssue)
if not self.logged_comm_issue:
invalid = [
s for s, valid in self.sm.valid.items() if not valid
]
not_alive = [
s for s, alive in self.sm.alive.items() if not alive
]
cloudlog.event("commIssue",
invalid=invalid,
not_alive=not_alive)
self.logged_comm_issue = True
else:
self.logged_comm_issue = False
if not self.sm['liveParameters'].valid:
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['lateralPlan'].mpcSolutionValid and not (
EventName.turningIndicatorOn in self.events.names):
self.events.add(EventName.plannerError)
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid)
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
for pandaState in self.sm['pandaStates']:
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
self.events.add(EventName.relayMalfunction)
if not REPLAY:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabledAcc and (
not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
if self.cruise_mismatch_counter > int(1. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
# Check for FCW
stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.5
model_fcw = self.sm[
'modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
if not self.disable_op_fcw and (planner_fcw or model_fcw):
self.events.add(EventName.fcw)
if TICI:
logs = messaging.drain_sock(self.log_sock, wait_for_one=False)
messages = []
for m in logs:
try:
messages.append(m.androidLog.message)
except UnicodeDecodeError:
pass
for err in [
"ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW",
"APPLY FAILED"
]:
for m in messages:
if err not in m:
continue
csid = m.split("CSID:")[-1].split(" ")[0]
evt = {
"0": EventName.roadCameraError,
"1": EventName.wideRoadCameraError,
"2": EventName.driverCameraError
}.get(csid, None)
if evt is not None:
self.events.add(evt)
# TODO: fix simulator
if not SIMULATION:
#if not NOSENSOR:
# if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
# # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
# self.events.add(EventName.noGps)
if not self.sm.all_alive(self.camera_packets):
self.events.add(EventName.cameraMalfunction)
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
if self.sm['liveLocationKalman'].excessiveResets:
self.events.add(EventName.localizerMalfunction)
# Check if all manager processes are running
not_running = set(p.name for p in self.sm['managerState'].processes
if not p.running)
if self.sm.rcv_frame['managerState'] and (not_running -
IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning)
# Only allow engagement with brake pressed when stopped behind another stopped car
speeds = self.sm['longitudinalPlan'].speeds
if len(speeds) > 1:
v_future = speeds[-1]
else:
v_future = 100.0
#if CS.brakePressed and v_future >= self.CP.vEgoStarting \
# and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
# self.events.add(EventName.noTarget)
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
all_valid = CS.canValid and self.sm.all_alive_and_valid()
if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 3.5
or SIMULATION):
if not self.read_only:
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
self.initialized = True
Params().put_bool("ControlsReady", True)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if any(not ps.controlsAllowed and self.enabled
for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
return CS
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_kph_last = self.v_cruise_kph
self.CP.pcmCruise = self.CI.CP.pcmCruise
# if stock cruise is completely disabled, then we can use our own set speed logic
#if not self.CP.pcmCruise:
# self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.button_timers, self.enabled, self.is_metric)
#elif self.CP.pcmCruise and CS.cruiseState.enabled:
# self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
SccSmoother.update_cruise_buttons(self, CS,
self.CP.openpilotLongitudinalControl)
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(0.5 / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(
ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_kph = initialize_v_cruise(
CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
if self.active:
self.current_alert_types.append(ET.WARNING)
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
# Update VehicleModel
params = self.sm['liveParameters']
x = max(params.stiffnessFactor, 0.1)
#sr = max(params.steerRatio, 0.1)
if ntune_common_enabled('useLiveSteerRatio'):
sr = max(params.steerRatio, 0.1)
else:
sr = max(ntune_common_get('steerRatio'), 0.1)
self.VM.update_params(x, sr)
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
actuators = car.CarControl.Actuators.new_message()
actuators.longControlState = self.LoC.long_control_state
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not self.active:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
if not CS.cruiseState.enabledAcc:
self.LoC.reset(v_pid=CS.vEgo)
if not self.joystick_mode:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(
self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
actuators.accel = self.LoC.update(
self.active and CS.cruiseState.enabledAcc, CS, self.CP,
long_plan, pid_accel_limits, self.sm['radarState'])
# Steering PID loop and lateral MPC
lat_active = self.active and not CS.steerWarning and not CS.steerError and CS.vEgo > self.CP.minSteerSpeed
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(
self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures,
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(
lat_active, CS, self.CP, self.VM, params, desired_curvature,
desired_curvature_rate)
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
actuators.accel = 4.0 * clip(self.sm['testJoystick'].axes[0],
-1, 1)
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
# max angle is 45 for angle-based cars
actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
lac_log.active = True
lac_log.steeringAngleDeg = CS.steeringAngleDeg
lac_log.output = steer
lac_log.saturated = abs(steer) >= 0.9
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
if len(lat_plan.dPathPoints):
# Check if we deviated from the path
# TODO use desired vs actual curvature
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[
0] < -0.20
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[
0] > 0.20
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
if not isinstance(attr, Number):
continue
if not math.isfinite(attr):
cloudlog.error(
f"actuators.{p} not finite {actuators.to_dict()}")
setattr(actuators, p, 0.0)
return actuators, lac_log
def update_button_timers(self, buttonEvents):
# increment timer for buttons still pressed
for k in self.button_timers.keys():
if self.button_timers[k] > 0:
self.button_timers[k] += 1
for b in buttonEvents:
if b.type.raw in self.button_timers:
self.button_timers[b.type.raw] = 1 if b.pressed else 0
def publish_logs(self, CS, start_time, actuators, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.active = self.active
CC.actuators = actuators
if len(self.sm['liveLocationKalman'].orientationNED.value) > 2:
CC.roll = self.sm['liveLocationKalman'].orientationNED.value[0]
CC.pitch = self.sm['liveLocationKalman'].orientationNED.value[1]
CC.cruiseControl.cancel = self.CP.pcmCruise and not self.enabled and CS.cruiseState.enabled
if self.joystick_mode and self.sm.rcv_frame[
'testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
if self.sm.frame % 100 == 0:
self.right_lane_visible = right_lane_visible
self.left_lane_visible = left_lane_visible
CC.hudControl.rightLaneVisible = self.right_lane_visible
CC.hudControl.leftLaneVisible = self.left_lane_visible
recent_blinker = (self.sm.frame - self.last_blinker_frame
) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['modelV2'].meta
if len(meta.desirePrediction) and ldw_allowed:
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft -
1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight -
1]
cameraOffset = ntune_common_get(
"cameraOffset"
) + 0.08 if self.wide_camera else ntune_common_get("cameraOffset")
l_lane_close = left_lane_visible and (
self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + cameraOffset))
r_lane_close = right_lane_visible and (
self.sm['modelV2'].laneLines[2].y[0] < (1.08 - cameraOffset))
CC.hudControl.leftLaneDepart = bool(
l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(
r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types,
[self.CP, self.sm, self.is_metric])
self.AM.add_many(self.sm.frame, alerts, self.enabled)
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
if not self.read_only and self.initialized:
# send car controls over can
can_sends = self.CI.apply(CC, self)
self.pm.send(
'sendcan',
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
# Curvature & Steering angle
params = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg -
params.angleOffsetAverageDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset,
CS.vEgo)
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.alertText1 = self.AM.alert_text_1
controlsState.alertText2 = self.AM.alert_text_2
controlsState.alertSize = self.AM.alert_size
controlsState.alertStatus = self.AM.alert_status
controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime[
'longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = curvature
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(
self.applyMaxSpeed if self.CP.
openpilotLongitudinalControl else self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
controlsState.angleSteers = steer_angle_without_offset * CV.RAD_TO_DEG
controlsState.applyAccel = self.apply_accel
controlsState.aReqValue = self.aReqValue
controlsState.aReqValueMin = self.aReqValueMin
controlsState.aReqValueMax = self.aReqValueMax
controlsState.sccStockCamAct = self.sccStockCamAct
controlsState.sccStockCamStatus = self.sccStockCamStatus
controlsState.steerRatio = self.VM.sR
controlsState.steerRateCost = ntune_common_get('steerRateCost')
controlsState.steerActuatorDelay = ntune_common_get(
'steerActuatorDelay')
controlsState.sccGasFactor = ntune_scc_get('sccGasFactor')
controlsState.sccBrakeFactor = ntune_scc_get('sccBrakeFactor')
controlsState.sccCurvatureFactor = ntune_scc_get('sccCurvatureFactor')
controlsState.longitudinalActuatorDelayLowerBound = ntune_scc_get(
'longitudinalActuatorDelayLowerBound')
controlsState.longitudinalActuatorDelayUpperBound = ntune_scc_get(
'longitudinalActuatorDelayUpperBound')
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
elif self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL)
== 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def step(self):
start_time = sec_since_boot()
self.prof.checkpoint("Ratekeeper", ignore=True)
# Sample data from sockets and get a carState
CS = self.data_sample()
self.prof.checkpoint("Sample")
self.update_events(CS)
if not self.read_only and self.initialized:
# Update control state
self.state_transition(CS)
self.prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, lac_log = self.state_control(CS)
self.prof.checkpoint("State Control")
# Publish data
self.publish_logs(CS, start_time, actuators, lac_log)
self.prof.checkpoint("Sent")
self.update_button_timers(CS.buttonEvents)
def controlsd_thread(self):
while True:
self.step()
self.rk.monitor_time()
self.prof.display()
def controlsd_thread(gctx, rate=100):
# start the loop
set_realtime_priority(2)
context = zmq.Context()
# pub
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)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
# sub
thermal = messaging.sub_sock(context, service_list['thermal'].port)
health = messaging.sub_sock(context, service_list['health'].port)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan)
PL = Planner(CP)
LoC = LongControl(CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
# write CarParams
params = Params()
params.put("CarParams", CP.to_bytes())
state = State.DISABLED
soft_disable_timer = 0
v_cruise_kph = 255
cal_perc = 0
cal_status = Calibration.UNCALIBRATED
rear_view_toggle = False
rear_view_allowed = params.get("IsRearViewMirror") == "1"
# 0.0 - 1.0
awareness_status = 0.
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = 0.
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
prof = Profiler()
while 1:
prof.reset() # avoid memory leak
# sample data and compute car events
CS, events, cal_status, cal_perc = data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc)
prof.checkpoint("Sample")
# define plan
plan, plan_ts = calc_plan(CS, events, PL, LoC)
prof.checkpoint("Plan")
# update control state
state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM)
prof.checkpoint("State transition")
# compute actuators
actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
rear_view_allowed, rear_view_toggle)
prof.checkpoint("State Control")
# publish data
CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
rear_view_toggle, awareness_status, LaC, LoC, angle_offset)
prof.checkpoint("Sent")
# *** run loop at fixed rate ***
if rk.keep_time():
prof.display()
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
if 0x1ef in fingerprint:
ret.safetyModel = car.CarParams.SafetyModels.hondaBosch
ret.enableCamera = True
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
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.CIVIC_HATCH:
stop_and_go = True
ret.mass = 2961. / 2.205 + std_cargo
ret.wheelbase = wheelbase_civic
ret.centerToFront = centerToFront_civic
ret.steerRatio = 13.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.ACCORD:
stop_and_go = True
ret.safetyParam = 1 # Informs fw that this car uses an alternate user brake msg
ret.mass = 3298. / 2.205 + std_cargo
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 11.82
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_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.CRV_5G:
stop_and_go = True
ret.mass = 3358. / 2.205 + std_cargo
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 12.30
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
def locationd_thread(gctx, addr, disabled_logs):
ctx = zmq.Context()
poller = zmq.Poller()
live100_socket = messaging.sub_sock(ctx,
service_list['live100'].port,
poller,
addr=addr,
conflate=True)
sensor_events_socket = messaging.sub_sock(
ctx,
service_list['sensorEvents'].port,
poller,
addr=addr,
conflate=True)
camera_odometry_socket = messaging.sub_sock(
ctx,
service_list['cameraOdometry'].port,
poller,
addr=addr,
conflate=True)
kalman_odometry_socket = messaging.pub_sock(
ctx, service_list['kalmanOdometry'].port)
live_parameters_socket = messaging.pub_sock(
ctx, service_list['liveParameters'].port)
params_reader = Params()
cloudlog.info("Parameter learner is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
VM = VehicleModel(CP)
cloudlog.info("Parameter learner got CarParams: %s" % CP.carFingerprint)
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'angleOffsetAverage': 0.0,
'stiffnessFactor': 1.0,
'steerRatio': VM.sR,
}
cloudlog.info("Parameter learner resetting to default values")
cloudlog.info("Parameter starting with: %s" % str(params))
localizer = Localizer(disabled_logs=disabled_logs)
learner = ParamsLearner(VM,
angle_offset=params['angleOffsetAverage'],
stiffness_factor=params['stiffnessFactor'],
steer_ratio=params['steerRatio'],
learning_rate=LEARNING_RATE)
i = 0
while True:
for socket, event in poller.poll(timeout=1000):
log = messaging.recv_one(socket)
localizer.handle_log(log)
if socket is live100_socket:
if not localizer.kf.t:
continue
if i % LEARNING_RATE == 0:
# live100 is not updating the Kalman Filter, so update KF manually
localizer.kf.predict(1e-9 * log.logMonoTime)
predicted_state = localizer.kf.x
yaw_rate = -float(predicted_state[5])
steering_angle = math.radians(log.live100.angleSteers)
params_valid = learner.update(yaw_rate, log.live100.vEgo,
steering_angle)
params = messaging.new_message()
params.init('liveParameters')
params.liveParameters.valid = bool(params_valid)
params.liveParameters.angleOffset = float(
math.degrees(learner.ao))
params.liveParameters.angleOffsetAverage = float(
math.degrees(learner.slow_ao))
params.liveParameters.stiffnessFactor = float(learner.x)
params.liveParameters.steerRatio = float(learner.sR)
live_parameters_socket.send(params.to_bytes())
if i % 6000 == 0: # once a minute
params = learner.get_values()
params['carFingerprint'] = CP.carFingerprint
params_reader.put("LiveParameters", json.dumps(params))
params_reader.put(
"ControlsParams",
json.dumps(
{'angle_model_bias': log.live100.angleModelBias}))
i += 1
elif socket is camera_odometry_socket:
msg = messaging.new_message()
msg.init('kalmanOdometry')
msg.logMonoTime = log.logMonoTime
msg.kalmanOdometry = localizer.liveLocationMsg(
log.logMonoTime * 1e-9)
kalman_odometry_socket.send(msg.to_bytes())
elif socket is sensor_events_socket:
pass
def controlsd_thread(gctx=None, rate=100, default_bias=0.):
gc.disable()
# start the loop
set_realtime_priority(3)
context = zmq.Context()
params = Params()
# pub
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)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
passive = params.get("Passive") != "0"
if not passive:
while 1:
try:
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
break
except zmq.error.ZMQError:
kill_defaultd()
else:
sendcan = None
# sub
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)
gps_location = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
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
fcw_enabled = params.get("IsFcwEnabled") == "1"
geofence = None
try:
from selfdrive.controls.lib.geofence import Geofence
geofence = Geofence(params.get("IsGeofenceEnabled") == "1")
except ImportError:
# geofence not available
params.put("IsGeofenceEnabled", "-1")
PL = Planner(CP, fcw_enabled)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
driver_status = DriverStatus()
if not passive:
AM.add("startup", False)
# write CarParams
params.put("CarParams", CP.to_bytes())
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.UNCALIBRATED
mismatch_counter = 0
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = default_bias
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
angle_offset = calibration_params["angle_offset2"]
except (ValueError, KeyError):
pass
prof = Profiler(False) # off by default
while 1:
prof.checkpoint("Ratekeeper", ignore=True)
# sample data and compute car events
CS, events, cal_status, overtemp, free_space, mismatch_counter = data_sample(CI, CC, thermal, cal, health,
driver_monitor, gps_location, poller, cal_status, overtemp, free_space, driver_status, geofence, state, mismatch_counter, params)
prof.checkpoint("Sample")
# define plan
plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence)
prof.checkpoint("Plan")
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")
# compute actuators
actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive)
prof.checkpoint("State Control")
# publish data
CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive)
prof.checkpoint("Sent")
# *** run loop at fixed rate ***
rk.keep_time()
prof.display()
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 / max(_A_CRUISE_MAX_V_FOLLOWING))) * 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.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.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_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
# 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
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)
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
def __init__(self, sm=None, pm=None, can_sock=None):
gc.disable()
set_realtime_priority(3)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', \
'carControl', 'carEvents', 'carParams'])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
has_relay)
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = not os.path.isfile('/EON') or (os.path.isfile('/proc/asound/card0/state') \
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("LongitudinalControl",
"1" if self.CP.openpilotLongitudinalControl else "0")
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.consecutive_can_error_count = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.events_prev = []
self.current_alert_types = []
self.sm['liveCalibration'].calStatus = Calibration.INVALID
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available, hw_type)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if self.read_only and not passive:
self.events.add(EventName.carUnrecognized, static=True)
# if hw_type == HwType.whitePanda:
# self.events.add(EventName.whitePandaUnsupported, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
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):
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
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(3, Priority.CTRL_HIGH)
self.op_params = opParams()
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster([
'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
'liveLocationKalman', 'roadCameraState', 'driverCameraState',
'managerState', 'liveParameters', 'radarState'
],
ignore_alive=ignore)
self.sm_smiskol = messaging.SubMaster([
'radarState', 'dynamicFollowData', 'liveTracks',
'dynamicFollowButton', 'laneSpeed', 'dynamicCameraOffset',
'modelLongButton'
])
self.op_params = opParams()
self.df_manager = dfManager()
self.support_white_panda = self.op_params.get('support_white_panda')
self.last_model_long = False
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one pandaState and one CAN packet
panda_type = messaging.recv_one(
self.sm.sock['pandaState']).pandaState.pandaType
has_relay = panda_type in [
PandaType.blackPanda, PandaType.uno, PandaType.dos
]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP, candidate = get_car(self.can_sock,
self.pm.sock['sendcan'],
has_relay)
threading.Thread(target=log_fingerprint, args=[candidate]).start()
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get(
"Passive", encoding='utf8') == "1" or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb, candidate)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['deviceState'].freeSpacePercent = 100
self.sm['driverMonitoringState'].events = []
self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['driverMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
self.lead_rel_speed = 255
self.lead_long_dist = 255
def radard_thread(gctx=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked = CP.carName == "mock" or CP.carName == "tesla"
VM = VehicleModel(CP)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
context = zmq.Context()
# *** subscribe to features and model from visiond
poller = zmq.Poller()
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
MP = ModelParser()
RI = RadarInterface(CP)
last_md_ts = 0
last_l100_ts = 0
# *** publish live20 and liveTracks
live20 = messaging.pub_sock(context, service_list['live20'].port)
liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
# Time-alignment
rate = 20. # model and radar are both at 20Hz
tsv = 1./rate
v_len = 20 # how many speed data points to remember for t alignment with rdr data
active = 0
steer_angle = 0.
steer_override = False
tracks = defaultdict(dict)
# Kalman filter stuff:
ekfv = EKFV1D()
speedSensorV = SimpleSensor(XV, 1, 2)
# v_ego
v_ego = None
v_ego_array = np.zeros([2, v_len])
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1:
rr = RI.update()
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s
l100 = None
md = None
for socket, event in poller.poll(0):
if socket is live100:
l100 = messaging.recv_one(socket)
elif socket is model:
md = messaging.recv_one(socket)
if l100 is not None:
active = l100.live100.active
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
steer_override = l100.live100.steerOverride
v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
v_ego_array = v_ego_array[:, 1:]
last_l100_ts = l100.logMonoTime
if v_ego is None:
continue
if md is not None:
last_md_ts = md.logMonoTime
# *** get path prediction from the model ***
MP.update(v_ego, md)
# run kalman filter only if prob is high enough
if MP.lead_prob > 0.7:
reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var))
ekfv.update_scalar(reading)
ekfv.predict(tsv)
# When changing lanes the distance to the lead car can suddenly change,
# which makes the Kalman filter output large relative acceleration
if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0:
ekfv.state[XV] = MP.lead_dist
ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0.
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), False)
else:
ekfv.state[XV] = MP.lead_dist
ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0.
if VISION_POINT in ar_pts:
del ar_pts[VISION_POINT]
# *** compute the likely path_y ***
if (active and not steer_override) or mocked:
# use path from model (always when mocking as steering is too noisy)
path_y = np.polyval(MP.d_poly, path_x)
else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
# *** remove missing points from meta data ***
for ids in tracks.keys():
if ids not in ar_pts:
tracks.pop(ids, None)
# *** compute the tracks ***
for ids in ar_pts:
# ignore standalone vision point, unless we are mocking the radar
if ids == VISION_POINT and not mocked:
continue
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
# create the track if it doesn't exist or it's a new track
if ids not in tracks:
tracks[ids] = Track()
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
fused_id = None
best_score = NO_FUSION_SCORE
for ids in tracks:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
if best_score > tracks[ids].vision_score:
fused_id = ids
best_score = tracks[ids].vision_score
if fused_id is not None:
tracks[fused_id].vision_cnt += 1
tracks[fused_id].update_vision_fusion()
if DEBUG:
print("NEW CYCLE")
if VISION_POINT in ar_pts:
print("vision", ar_pts[VISION_POINT])
idens = tracks.keys()
track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
# If we have multiple points, cluster them
if len(track_pts) > 1:
link = linkage_vector(track_pts, method='centroid')
cluster_idxs = fcluster(link, 2.5, criterion='distance')
clusters = [None]*max(cluster_idxs)
for idx in xrange(len(track_pts)):
cluster_i = cluster_idxs[idx]-1
if clusters[cluster_i] == None:
clusters[cluster_i] = Cluster()
clusters[cluster_i].add(tracks[idens[idx]])
elif len(track_pts) == 1:
# TODO: why do we need this?
clusters = [Cluster()]
clusters[0].add(tracks[idens[0]])
else:
clusters = []
if DEBUG:
for i in clusters:
print(i)
# *** extract the lead car ***
lead_clusters = [c for c in clusters
if c.is_potential_lead(v_ego)]
lead_clusters.sort(key=lambda x: x.dRel)
lead_len = len(lead_clusters)
# *** extract the second lead from the whole set of leads ***
lead2_clusters = [c for c in lead_clusters
if c.is_potential_lead2(lead_clusters)]
lead2_clusters.sort(key=lambda x: x.dRel)
lead2_len = len(lead2_clusters)
# *** publish live20 ***
dat = messaging.new_message()
dat.init('live20')
dat.live20.mdMonoTime = last_md_ts
dat.live20.canMonoTimes = list(rr.canMonoTimes)
dat.live20.radarErrors = list(rr.errors)
dat.live20.l100MonoTime = last_l100_ts
if lead_len > 0:
dat.live20.leadOne = lead_clusters[0].toLive20()
if lead2_len > 0:
dat.live20.leadTwo = lead2_clusters[0].toLive20()
else:
dat.live20.leadTwo.status = False
else:
dat.live20.leadOne.status = False
dat.live20.cumLagMs = -rk.remaining*1000.
live20.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary,
tracks[ids].measured))
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
"aRel": float(tracks[ids].aRel),
"stationary": bool(tracks[ids].stationary),
"oncoming": bool(tracks[ids].oncoming),
}
liveTracks.send(dat.to_bytes())
rk.monitor_time()
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 = "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.]]
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
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
def controlsd_thread(gctx=None, rate=100, default_bias=0.):
# start the loop
set_realtime_priority(3)
context = zmq.Context()
params = Params()
# pub
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)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
passive = params.get("Passive") != "0"
if not passive:
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
else:
sendcan = None
# sub
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)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
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.honda
fcw_enabled = params.get("IsFcwEnabled") == "1"
PL = Planner(CP, fcw_enabled)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
if not passive:
AM.add("startup", False)
# write CarParams
params.put("CarParams", CP.to_bytes())
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
overtemp = False
free_space = False
cal_status = Calibration.UNCALIBRATED
rear_view_toggle = False
rear_view_allowed = params.get("IsRearViewMirror") == "1"
# 0.0 - 1.0
awareness_status = 1.
v_cruise_kph_last = 0
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = default_bias
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
angle_offset = calibration_params["angle_offset2"]
except (ValueError, KeyError):
pass
prof = Profiler(False) # off by default
while 1:
prof.checkpoint("Ratekeeper", ignore=True)
# sample data and compute car events
CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status,
overtemp, free_space)
prof.checkpoint("Sample")
# define plan
plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, awareness_status)
prof.checkpoint("Plan")
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")
# compute actuators
actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM,
angle_offset, rear_view_allowed, rear_view_toggle, passive)
prof.checkpoint("State Control")
# publish data
CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph,
rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive)
prof.checkpoint("Sent")
# *** run loop at fixed rate ***
rk.keep_time()
prof.display()
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)
@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
# TODO: gate this on detection
ret.enableCamera = True
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)
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
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:
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)
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):
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)]
self.CC.update(self.sendcan, c.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 == CAR.OUTBACK_15:
ret.mass = 1568 + std_cargo
ret.wheelbase = 2.75
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 14
ret.steerActuatorDelay = 0.3
ret.steerRateCost = 0
ret.steerKf = 0.000002
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 == CAR.OUTBACK_17:
ret.mass = 1568 + std_cargo
ret.wheelbase = 2.75
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 14
ret.steerActuatorDelay = 0.3
ret.steerRateCost = 0
ret.steerKf = 0.000002
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 == CAR.LEGACY_15:
ret.mass = 1568 + std_cargo
ret.wheelbase = 2.75
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 14
ret.steerActuatorDelay = 0.3
ret.steerRateCost = 0
ret.steerKf = 0.000002
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 == CAR.LEGACY_17:
ret.mass = 1568 + std_cargo
ret.wheelbase = 2.75
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 14
ret.steerActuatorDelay = 0.3
ret.steerRateCost = 0
ret.steerKf = 0.000002
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
ret.steerRatio = 14
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 1
ret.steerKf = 0.00002
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.003], [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
ret.steerRatioRear = 0.
# 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
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
# 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
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster(
[
'deviceState', 'pandaStates', 'peripheralState', 'modelV2',
'liveCalibration', 'driverMonitoringState',
'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'
] + self.camera_packets + joystick_packet,
ignore_alive=ignore,
ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (
not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.button_timers = {
ButtonEvent.Type.decelCruise: 0,
ButtonEvent.Type.accelCruise: 0
}
# scc smoother
self.is_cruise_enabled = False
self.applyMaxSpeed = 0
self.apply_accel = 0.
self.fused_accel = 0.
self.lead_drel = 0.
self.aReqValue = 0.
self.aReqValueMin = 0.
self.aReqValueMax = 0.
self.sccStockCamStatus = 0
self.sccStockCamAct = 0
self.left_lane_visible = False
self.right_lane_visible = False
self.wide_camera = TICI and params.get_bool('EnableWideCamera')
self.disable_op_fcw = params.get_bool('DisableOpFcw')
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
set_offroad_alert("Offroad_CarUnrecognized", True)
else:
set_offroad_alert("Offroad_NoFirmware", True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
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_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(),
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
# 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
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)
# 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 = [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(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 = 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:
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_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 [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
# 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
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster([
'thermal', 'health', 'model', 'liveCalibration', 'frontFrame',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
internet_needed = (params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None) and (
params.get("DisableUpdates") != b"1")
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
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
self.left_blinker_prev = False
self.right_blinker_prev = 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(),
has_relay=False,
car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "chrysler"
ret.carFingerprint = candidate
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.PACIFICA_2020_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.fwdCamera) 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)
ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
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
# TODO: button presses
buttonEvents = []
if ret.leftBlinker != self.left_blinker_prev:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = ret.leftBlinker != 0
buttonEvents.append(be)
if ret.rightBlinker != self.right_blinker_prev:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = ret.rightBlinker != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
# 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 ret.cruiseState.available:
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
self.left_blinker_prev = ret.leftBlinker
self.right_blinker_prev = ret.rightBlinker
# copy back carState packet to CS
self.CS.out = ret.as_reader()
return self.CS.out
# 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, 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 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 = "subaru"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModel.subaru
ret.enableCruise = True
ret.steerLimitAlert = True
ret.enableCamera = True
ret.steerRateCost = 0.7
if candidate in [CAR.IMPREZA]:
ret.mass = 1568. + STD_CARGO_KG
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.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
# 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_KG
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):
can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_valid, _ = 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()
ret.canValid = can_rcv_valid and cam_rcv_valid and 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.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 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
def controlsd_thread(gctx=None, rate=100, default_bias=0.):
gc.disable()
# start the loop
set_realtime_priority(3)
context = zmq.Context()
params = Params()
# Pub Sockets
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)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
# 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)
gps_location = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
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
# Get FCW toggle from settings
fcw_enabled = params.get("IsFcwEnabled") == "1"
geofence = None
PL = Planner(CP, fcw_enabled)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(CP)
AM = AlertManager()
driver_status = DriverStatus()
if not passive:
AM.add("startup", False)
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.INVALID
cal_perc = 0
mismatch_counter = 0
low_battery = False
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# Read angle offset from previous drive, fallback to default
angle_offset = default_bias
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
angle_offset = calibration_params["angle_offset2"]
except (ValueError, KeyError):
pass
prof = Profiler(False) # off by default
while True:
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter = data_sample(CI, CC, thermal, cal, health,
driver_monitor, gps_location, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params)
prof.checkpoint("Sample")
# Define longitudinal plan (MPC)
plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence)
prof.checkpoint("Plan")
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")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz
prof.display()
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.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 = 14.63 # 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 ['99c94dc769b5d96e']
ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
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 = 15.96 # 11.82 is spec end-to-end
tire_stiffness_factor = 0.8467
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_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 = 15.3 # 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.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
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]
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
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: 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 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]))
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
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 = 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, \
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 Controls:
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(3, Priority.CTRL_HIGH)
self.op_params = opParams()
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster([
'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
'liveLocationKalman', 'roadCameraState', 'driverCameraState',
'managerState', 'liveParameters', 'radarState'
],
ignore_alive=ignore)
self.sm_smiskol = messaging.SubMaster([
'radarState', 'dynamicFollowData', 'liveTracks',
'dynamicFollowButton', 'laneSpeed', 'dynamicCameraOffset',
'modelLongButton'
])
self.op_params = opParams()
self.df_manager = dfManager()
self.support_white_panda = self.op_params.get('support_white_panda')
self.last_model_long = False
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one pandaState and one CAN packet
panda_type = messaging.recv_one(
self.sm.sock['pandaState']).pandaState.pandaType
has_relay = panda_type in [
PandaType.blackPanda, PandaType.uno, PandaType.dos
]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP, candidate = get_car(self.can_sock,
self.pm.sock['sendcan'],
has_relay)
threading.Thread(target=log_fingerprint, args=[candidate]).start()
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get(
"Passive", encoding='utf8') == "1" or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb, candidate)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['deviceState'].freeSpacePercent = 100
self.sm['driverMonitoringState'].events = []
self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['driverMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
self.lead_rel_speed = 255
self.lead_long_dist = 255
def update_events(self, CS):
"""Compute carEvents from carState"""
self.events.clear()
self.events.add_from_msg(CS.events)
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Handle startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
# Create events for battery, temperature, disk space, and memory
if self.sm['deviceState'].batteryPercent < 1 and self.sm[
'deviceState'].chargingError:
# at zero percent battery, while discharging, OP should not allowed
self.events.add(EventName.lowBattery)
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7:
# under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace)
if self.sm['deviceState'].memoryUsagePercent > 90:
self.events.add(EventName.lowMemory)
# Alert if fan isn't spinning for 5 seconds
if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]:
if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm[
'deviceState'].fanSpeedPercentDesired > 50:
if (self.sm.frame -
self.last_functional_fan_frame) * DT_CTRL > 5.0:
self.events.add(EventName.fanMalfunction)
else:
self.last_functional_fan_frame = self.sm.frame
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
self.events.add(EventName.calibrationIncomplete)
else:
self.events.add(EventName.calibrationInvalid)
# Handle lane change
if self.sm[
'lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['lateralPlan'].laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['lateralPlan'].laneChangeState in [
LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing
]:
self.events.add(EventName.laneChange)
if self.can_rcv_error or (not CS.canValid
and self.sm.frame > 5 / DT_CTRL):
self.events.add(EventName.canError)
if (self.sm['pandaState'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \
self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if len(self.sm['radarState'].radarErrors):
self.events.add(EventName.radarFault)
elif not self.sm.valid['liveParameters']:
self.events.add(EventName.vehicleModelInvalid)
elif not self.sm.all_alive_and_valid():
self.events.add(EventName.commIssue)
if not self.logged_comm_issue:
cloudlog.error(
f"commIssue - valid: {self.sm.valid} - alive: {self.sm.alive}"
)
self.logged_comm_issue = True
else:
self.logged_comm_issue = False
if not self.sm['lateralPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid)
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if log.PandaState.FaultType.relayMalfunction in self.sm[
'pandaState'].faults:
self.events.add(EventName.relayMalfunction)
if self.sm['longitudinalPlan'].fcw:
self.events.add(EventName.fcw)
# TODO: fix simulator
# if not SIMULATION:
# if not NOSENSOR and not self.support_white_panda:
# if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and not TICI:
# # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
# self.events.add(EventName.noGps)
if not self.sm.all_alive(['roadCameraState', 'driverCameraState'
]) and (self.sm.frame > 5 / DT_CTRL):
self.events.add(EventName.cameraMalfunction)
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
# Check if all manager processes are running
not_running = set(p.name for p in self.sm['managerState'].processes
if not p.running)
if self.sm.rcv_frame['managerState'] and (not_running -
IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning)
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \
and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3 and not self.last_model_long:
self.events.add(EventName.noTarget)
self.add_stock_additions_alerts(CS)
# vision-only fcw, can be disabled if radar is present
if self.sm.updated['radarState']:
self.lead_rel_speed = self.sm['radarState'].leadOne.vRel
self.lead_long_dist = self.sm['radarState'].leadOne.dRel
#if CS.cruiseState.enabled and self.lead_long_dist > 5 and self.lead_long_dist < 100 and self.lead_rel_speed <= -0.5 and CS.vEgo >= 5 and \
# ((self.lead_long_dist/abs(self.lead_rel_speed) < 2.) or (self.lead_long_dist/abs(self.lead_rel_speed) < 4. and self.lead_rel_speed < -10) or \
# (self.lead_long_dist/abs(self.lead_rel_speed) < 5. and self.lead_long_dist/CS.vEgo < 1.5)):
# self.events.add(EventName.fcw)
def add_stock_additions_alerts(self, CS):
self.AM.SA_set_frame(self.sm.frame)
self.AM.SA_set_enabled(self.enabled)
# alert priority is defined by code location, keeping is highest, then lane speed alert, then auto-df alert
if self.sm_smiskol['modelLongButton'].enabled != self.last_model_long:
extra_text_1 = 'disabled!' if self.last_model_long else 'enabled!'
extra_text_2 = '' if self.last_model_long else ', model may behave unexpectedly'
self.AM.SA_add('modelLongAlert',
extra_text_1=extra_text_1,
extra_text_2=extra_text_2)
return
if self.sm_smiskol['dynamicCameraOffset'].keepingLeft:
self.AM.SA_add('laneSpeedKeeping',
extra_text_1='LEFT',
extra_text_2='Oncoming traffic in right lane')
return
elif self.sm_smiskol['dynamicCameraOffset'].keepingRight:
self.AM.SA_add('laneSpeedKeeping',
extra_text_1='RIGHT',
extra_text_2='Oncoming traffic in left lane')
return
ls_state = self.sm_smiskol['laneSpeed'].state
if ls_state != '':
self.AM.SA_add('lsButtonAlert', extra_text_1=ls_state)
return
faster_lane = self.sm_smiskol['laneSpeed'].fastestLane
if faster_lane in ['left', 'right']:
ls_alert = 'laneSpeedAlert'
if not self.sm_smiskol['laneSpeed'].new:
ls_alert += 'Silent'
self.AM.SA_add(
ls_alert,
extra_text_1='{} lane faster'.format(faster_lane).upper(),
extra_text_2='Change lanes to faster {} lane'.format(
faster_lane))
return
df_out = self.df_manager.update()
if df_out.changed:
df_alert = 'dfButtonAlert'
if df_out.is_auto and df_out.last_is_auto:
# only show auto alert if engaged, not hiding auto, and time since lane speed alert not showing
if CS.cruiseState.enabled and not self.op_params.get(
'hide_auto_df_alerts'):
df_alert += 'Silent'
self.AM.SA_add(df_alert,
extra_text_1=df_out.model_profile_text +
' (auto)')
return
else:
self.AM.SA_add(
df_alert,
extra_text_1=df_out.user_profile_text,
extra_text_2='Dynamic follow: {} profile active'.format(
df_out.user_profile_text))
return
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
self.sm_smiskol.update(0)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
if not self.sm['pandaState'].controlsAllowed and self.enabled:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
return CS
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_kph_last = self.v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not self.CP.enableCruise:
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph,
CS.buttonEvents, self.enabled)
elif self.CP.enableCruise and CS.cruiseState.enabled:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = 300 # 3s
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(
ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_kph = initialize_v_cruise(
CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
if self.active:
self.current_alert_types.append(ET.WARNING)
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
actuators = car.CarControl.Actuators.new_message()
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not self.active:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
long_plan_age = DT_CTRL * (self.sm.frame -
self.sm.rcv_frame['longitudinalPlan'])
# no greater than dt mpc + dt, to prevent too high extraps
dt = min(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (
long_plan.aTarget - long_plan.aStart)
v_acc_sol = long_plan.vStart + dt * (a_acc_sol +
long_plan.aStart) / 2.0
extras_loc = {
'lead_one': self.sm_smiskol['radarState'].leadOne,
'mpc_TR': self.sm_smiskol['dynamicFollowData'].mpcTR,
'live_tracks': self.sm_smiskol['liveTracks'],
'has_lead': long_plan.hasLead
}
# Gas/Brake PID loop
actuators.gas, actuators.brake = self.LoC.update(
self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol,
self.CP, extras_loc)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(
self.active, CS, self.CP, lat_plan)
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[
0] < -0.1
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[
0] > 0.1
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
return actuators, v_acc_sol, a_acc_sol, lac_log
def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not self.CP.enableCruise or (
not self.enabled and CS.cruiseState.enabled)
# Some override values for Honda
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
speed_override = max(0.0,
(self.LoC.v_pid + CS.cruiseState.speedOffset) *
brake_discount)
CC.cruiseControl.speedOverride = float(
speed_override if self.CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = self.CI.calc_accel_override(
CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo,
self.sm['longitudinalPlan'].vTarget)
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (self.sm.frame - self.last_blinker_frame
) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and (not self.active or CS.epsDisabled == True) and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['modelV2'].meta
if len(meta.desirePrediction) and ldw_allowed:
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft -
1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight -
1]
CAMERA_OFFSET = self.sm['lateralPlan'].cameraOffset
ldw_average_car_width = 1.750483672001016 # from sedans, suvs, and minivans (todo: find from all openpilot Toyotas instead)
ldw_m_from_wheel = 0.15
ldw_threshold = ldw_average_car_width / 2 + ldw_m_from_wheel
l_lane_close = left_lane_visible and (
self.sm['modelV2'].laneLines[1].y[0] >
-(ldw_threshold + CAMERA_OFFSET))
r_lane_close = right_lane_visible and (
self.sm['modelV2'].laneLines[2].y[0] <
(ldw_threshold - CAMERA_OFFSET))
CC.hudControl.leftLaneDepart = bool(
l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(
r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types,
[self.CP, self.sm, self.is_metric])
self.AM.add_many(self.sm.frame, alerts, self.enabled)
self.last_model_long = self.sm_smiskol['modelLongButton'].enabled
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
if not self.read_only:
# send car controls over can
can_sends = self.CI.apply(CC)
self.pm.send(
'sendcan',
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
steer_angle_rad = (
CS.steeringAngleDeg -
self.sm['lateralPlan'].angleOffsetDeg) * CV.DEG_TO_RAD
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.alertText1 = self.AM.alert_text_1
controlsState.alertText2 = self.AM.alert_text_2
controlsState.alertSize = self.AM.alert_size
controlsState.alertStatus = self.AM.alert_status
controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime[
'longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = self.VM.calc_curvature(
steer_angle_rad, CS.vEgo)
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.id)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.steeringAngleDesiredDeg = float(
self.LaC.angle_steers_des)
controlsState.vTargetLead = float(v_acc)
controlsState.aTarget = float(a_acc)
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
if self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL)
== 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def step(self):
start_time = sec_since_boot()
self.prof.checkpoint("Ratekeeper", ignore=True)
# Sample data from sockets and get a carState
CS = self.data_sample()
self.prof.checkpoint("Sample")
self.update_events(CS)
if not self.read_only:
# Update control state
self.state_transition(CS)
self.prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_acc, a_acc, lac_log = self.state_control(CS)
self.prof.checkpoint("State Control")
# Publish data
self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
self.prof.checkpoint("Sent")
def controlsd_thread(self):
while True:
self.step()
self.rk.monitor_time()
self.prof.display()
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 = 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):
# 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):
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 = 8 * 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 = 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
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.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
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.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 = [0.5]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.longPidDeadzoneBP = [0.]
ret.longPidDeadzoneV = [0.]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [3.3, 2.425, 2.2]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 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), True)
self.ch_cp.update(int(sec_since_boot() * 1e9), True)
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
# 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 = 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 = '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.lkMode = True
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
elif but == CruiseButtons.MAIN:
be.type = '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.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 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]))
# 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
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 Controls:
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster(
[
'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
'liveLocationKalman', 'managerState', 'liveParameters',
'radarState'
] + self.camera_packets + joystick_packet,
ignore_alive=ignore,
ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
panda_type = messaging.recv_one(
self.sm.sock['pandaState']).pandaState.pandaType
has_relay = panda_type in [
PandaType.blackPanda, PandaType.uno, PandaType.dos
]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
has_relay)
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (
not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.curve_speed_ms = 255.
self.v_cruise_kph_limit = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.v_target = 0.0
self.a_target = 0.0
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available,
self.CP.fuzzyFingerprint,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def update_events(self, CS):
"""Compute carEvents from carState"""
self.events.clear()
self.events.add_from_msg(CS.events)
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Handle startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
# Don't add any more events if not initialized
if not self.initialized:
self.events.add(EventName.controlsInitializing)
return
# Create events for battery, temperature, disk space, and memory
if self.sm['deviceState'].batteryPercent < 1 and self.sm[
'deviceState'].chargingError:
# at zero percent battery, while discharging, OP should not allowed
self.events.add(EventName.lowBattery)
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
# under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace)
# TODO: make tici threshold the same
if self.sm['deviceState'].memoryUsagePercent > (90 if TICI else
65) and not SIMULATION:
self.events.add(EventName.lowMemory)
# Alert if fan isn't spinning for 5 seconds
if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]:
if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm[
'deviceState'].fanSpeedPercentDesired > 50:
if (self.sm.frame -
self.last_functional_fan_frame) * DT_CTRL > 5.0:
self.events.add(EventName.fanMalfunction)
else:
self.last_functional_fan_frame = self.sm.frame
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
self.events.add(EventName.calibrationIncomplete)
else:
self.events.add(EventName.calibrationInvalid)
# Handle lane change
if self.sm[
'lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['lateralPlan'].laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['lateralPlan'].laneChangeState in [
LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing
]:
self.events.add(EventName.laneChange)
if self.can_rcv_error or not CS.canValid:
self.events.add(EventName.canError)
safety_mismatch = self.sm[
'pandaState'].safetyModel != self.CP.safetyModel or self.sm[
'pandaState'].safetyParam != self.CP.safetyParam
if safety_mismatch or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if not self.sm['liveParameters'].valid:
self.events.add(EventName.vehicleModelInvalid)
if len(self.sm['radarState'].radarErrors):
self.events.add(EventName.radarFault)
elif not self.sm.valid["pandaState"]:
self.events.add(EventName.usbError)
elif not self.sm.all_alive_and_valid():
self.events.add(EventName.commIssue)
if not self.logged_comm_issue:
invalid = [
s for s, valid in self.sm.valid.items() if not valid
]
not_alive = [
s for s, alive in self.sm.alive.items() if not alive
]
cloudlog.event("commIssue",
invalid=invalid,
not_alive=not_alive)
self.logged_comm_issue = True
else:
self.logged_comm_issue = False
if not self.sm['lateralPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid)
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if log.PandaState.FaultType.relayMalfunction in self.sm[
'pandaState'].faults:
self.events.add(EventName.relayMalfunction)
if self.sm['longitudinalPlan'].fcw or (
self.enabled and self.sm['modelV2'].meta.hardBrakePredicted):
self.events.add(EventName.fcw)
if TICI:
logs = messaging.drain_sock(self.log_sock, wait_for_one=False)
messages = []
for m in logs:
try:
messages.append(m.androidLog.message)
except UnicodeDecodeError:
pass
for err in [
"ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW",
"APPLY FAILED"
]:
for m in messages:
if err not in m:
continue
csid = m.split("CSID:")[-1].split(" ")[0]
evt = {
"0": EventName.roadCameraError,
"1": EventName.wideRoadCameraError,
"2": EventName.driverCameraError
}.get(csid, None)
if evt is not None:
self.events.add(evt)
# TODO: fix simulator
if not SIMULATION:
#if not NOSENSOR:
#if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
#self.events.add(EventName.noGps)
if not self.sm.all_alive(self.camera_packets):
self.events.add(EventName.cameraMalfunction)
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
if self.sm['liveLocationKalman'].excessiveResets:
self.events.add(EventName.localizerMalfunction)
# Check if all manager processes are running
not_running = set(p.name for p in self.sm['managerState'].processes
if not p.running)
if self.sm.rcv_frame['managerState'] and (not_running -
IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning)
# Only allow engagement with brake pressed when stopped behind another stopped car
speeds = self.sm['longitudinalPlan'].speeds
if len(speeds) > 1:
v_future = speeds[-1]
else:
v_future = 100.0
#if CS.brakePressed and v_future >= STARTING_TARGET_SPEED \
#and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
#self.events.add(EventName.noTarget)
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
all_valid = CS.canValid and self.sm.all_alive_and_valid()
if not self.initialized and (all_valid
or self.sm.frame * DT_CTRL > 2.0):
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
self.initialized = True
Params().put_bool("ControlsReady", True)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
if not self.sm['pandaState'].controlsAllowed and self.enabled:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
return CS
def cal_curve_speed(self, sm, v_ego, frame):
if frame % 10 == 0:
md = sm['modelV2']
if md is not None and len(
md.position.x) == TRAJECTORY_SIZE and len(
md.position.y) == TRAJECTORY_SIZE:
x = md.position.x
y = md.position.y
dy = np.gradient(y, x)
d2y = np.gradient(dy, x)
curv = d2y / (1 + dy**2)**1.5
curv = curv[5:TRAJECTORY_SIZE - 10]
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = np.sqrt(a_y_max /
np.clip(np.abs(curv), 1e-4, None))
model_speed = np.mean(v_curvature) * 0.95
if model_speed < v_ego:
self.curve_speed_ms = float(
max(model_speed, 32. * CV.KPH_TO_MS))
else:
self.curve_speed_ms = 255.
if np.isnan(self.curve_speed_ms):
self.curve_speed_ms = 255.
else:
self.curve_speed_ms = 255.
return self.curve_speed_ms
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_kph_last = self.v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not self.CP.pcmCruise:
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph,
CS.buttonEvents, self.enabled)
elif self.CP.pcmCruise and CS.cruiseState.enabled:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = 300 # 3s
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(
ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_kph = initialize_v_cruise(
CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
if self.active:
self.current_alert_types.append(ET.WARNING)
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
# Update VehicleModel
params = self.sm['liveParameters']
x = max(params.stiffnessFactor, 0.1)
sr = max(params.steerRatio, 0.1)
self.VM.update_params(x, sr)
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
actuators = car.CarControl.Actuators.new_message()
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not self.active:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
if not self.joystick_mode:
# Gas/Brake PID loop
actuators.gas, actuators.brake, self.v_target, self.a_target = self.LoC.update(
self.active, CS, self.CP, long_plan)
# Steering PID loop and lateral MPC
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(
self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures,
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(
self.active, CS, self.CP, self.VM, params, desired_curvature,
desired_curvature_rate)
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
gb = clip(self.sm['testJoystick'].axes[0], -1, 1)
actuators.gas, actuators.brake = max(gb, 0), max(-gb, 0)
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
# max angle is 45 for angle-based cars
actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
lac_log.active = True
lac_log.steeringAngleDeg = CS.steeringAngleDeg
lac_log.output = steer
lac_log.saturated = abs(steer) >= 0.9
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
if len(lat_plan.dPathPoints):
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[
0] < -0.115
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[
0] > 0.115
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
return actuators, lac_log
def publish_logs(self, CS, start_time, actuators, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not self.CP.pcmCruise or (
not self.enabled and CS.cruiseState.enabled)
if self.joystick_mode and self.sm.rcv_frame[
'testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
# TODO remove car specific stuff in controls
# Some override values for Honda
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
speed_override = max(0.0,
(self.LoC.v_pid + CS.cruiseState.speedOffset) *
brake_discount)
CC.cruiseControl.speedOverride = float(
speed_override if self.CP.pcmCruise else 0.0)
CC.cruiseControl.accelOverride = float(
self.CI.calc_accel_override(CS.aEgo, self.a_target, CS.vEgo,
self.v_target))
CC.hudControl.setSpeed = float(self.v_cruise_kph_limit * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (self.sm.frame - self.last_blinker_frame
) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['modelV2'].meta
if len(meta.desirePrediction) and ldw_allowed:
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft -
1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight -
1]
l_lane_close = left_lane_visible and (
self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET))
r_lane_close = right_lane_visible and (
self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET))
CC.hudControl.leftLaneDepart = bool(
l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(
r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types,
[self.CP, self.sm, self.is_metric])
self.AM.add_many(self.sm.frame, alerts, self.enabled)
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
if not self.read_only and self.initialized:
# send car controls over can
can_sends = self.CI.apply(CC)
self.pm.send(
'sendcan',
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
# Curvature & Steering angle
params = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg -
params.angleOffsetAverageDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset,
CS.vEgo)
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.alertText1 = self.AM.alert_text_1
controlsState.alertText2 = self.AM.alert_text_2
controlsState.alertSize = self.AM.alert_size
controlsState.alertStatus = self.AM.alert_status
controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime[
'longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = curvature
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_kph_limit)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
elif self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL)
== 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def step(self):
start_time = sec_since_boot()
self.prof.checkpoint("Ratekeeper", ignore=True)
# Sample data from sockets and get a carState
CS = self.data_sample()
self.prof.checkpoint("Sample")
self.update_events(CS)
if not self.read_only and self.initialized:
# Update control state
self.state_transition(CS)
self.prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, lac_log = self.state_control(CS)
self.prof.checkpoint("State Control")
# Publish data
self.publish_logs(CS, start_time, actuators, lac_log)
self.prof.checkpoint("Sent")
def controlsd_thread(self):
while True:
self.step()
self.rk.monitor_time()
self.prof.display()
