def update(self, sendcan, enabled, CS, frame, actuators):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = final_steer * P.STEER_MAX
# limits due to driver torque
driver_max_torque = P.STEER_MAX + (
P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver *
P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER
driver_min_torque = -P.STEER_MAX + (
-P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver *
P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER
max_steer_allowed = max(min(P.STEER_MAX, driver_max_torque), 0)
min_steer_allowed = min(max(-P.STEER_MAX, driver_min_torque), 0)
apply_steer = clip(apply_steer, min_steer_allowed,
max_steer_allowed)
# slow rate if steer torque increases in magnitude
if self.apply_steer_last > 0:
apply_steer = clip(
apply_steer,
max(self.apply_steer_last - P.STEER_DELTA_DOWN,
-P.STEER_DELTA_UP),
self.apply_steer_last + P.STEER_DELTA_UP)
else:
apply_steer = clip(
apply_steer, self.apply_steer_last - P.STEER_DELTA_UP,
min(self.apply_steer_last + P.STEER_DELTA_DOWN,
P.STEER_DELTA_UP))
apply_steer = int(round(apply_steer))
self.apply_steer_last = apply_steer
lkas_enabled = enabled and not CS.steer_not_allowed
if not lkas_enabled:
apply_steer = 0
if self.car_fingerprint == CAR.CX5:
#counts from 0 to 15 then back to 0
ctr = (frame / P.STEER_STEP) % 16
#ctr = CS.CAM_LKAS.ctr #(frame / P.STEER_STEP) % 16
if ctr != -1 and self.last_cam_ctr != ctr:
self.last_cam_ctr = ctr
#line_not_visible = CS.CAM_LKAS.lnv
if CS.v_ego_raw > 4:
line_not_visible = 0
else:
line_not_visible = 1
e1 = CS.CAM_LKAS.err1
e2 = CS.CAM_LKAS.err2
# assuming controlsd runs at 83 hz
# 2000ms => 166.6
# 1000ms => 83.3
# 500ms => 41.65
tsec = 167
osec = 83
hsec = 42
qsec = 21
q3sec = 62
if CS.steer_lkas.handsoff == 1 and self.ldw == 0:
self.handsoff_ctr += 1
if self.handsoff_ctr > tsec:
self.ldw_ctr = osec
self.handsoff_ctr = 0
self.ldw = 1
if apply_steer > 0:
self.ldwr = 1
self.ldwl = 0
else:
self.ldwr = 0
self.ldwl = 1
#else:
# self.handsoff_ctr = 0
if self.ldw == 0 and CS.steer_lkas.block == 1 and CS.v_ego_raw > 54 and apply_steer != 0:
self.ldw = 1
self.ldw_ctr = osec
if apply_steer > 0:
self.ldwr = 1
self.ldwl = 0
else:
self.ldwr = 0
self.ldwl = 1
ldw = 0
if self.ldw_ctr > 0:
self.ldw_ctr -= 1
if self.ldw_ctr == 0:
self.ldw = 0
self.ldwl = 0
self.ldwr = 0
elif self.ldw_ctr < q3sec and self.ldw_ctr > qsec:
ldw = 1
can_sends.append(
mazdacan.create_steering_control(
self.packer_pt, canbus.powertrain,
CS.CP.carFingerprint, ctr, apply_steer,
line_not_visible, 1, 1, e1, e2, ldw))
# send lane info msgs at 1/8 rate of steer msgs
if (ctr % 8 == 0):
can_sends.append(
mazdacan.create_cam_lane_info(
self.packer_pt, canbus.powertrain,
CS.CP.carFingerprint, line_not_visible,
CS.cam_laneinfo, CS.steer_lkas, self.ldwr,
self.ldwl))
#can_sends.append(mazdacan.create_lkas_msg(self.packer_pt, canbus.powertrain, CS.CP.carFingerprint, CS.CAM_LKAS))
#can_sends.append(mazdacan.create_lane_track(self.packer_pt, canbus.powertrain, CS.CP.carFingerprint, CS.CAM_LT))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
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.accel * (3.0 / 4.0), 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 * 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)
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 update_stat(self, CS, enabled, sendcan):
if not self.LoC:
self.LoC = LongControl(CS.CP, tesla_compute_gb)
can_sends = []
if CS.pedal_interceptor_available and not CS.cstm_btns.get_button_status(
"pedal"):
# pedal hardware, enable button
CS.cstm_btns.set_button_status("pedal", 1)
print "enabling pedal"
elif not CS.pedal_interceptor_available:
if CS.cstm_btns.get_button_status("pedal"):
# no pedal hardware, disable button
CS.cstm_btns.set_button_status("pedal", 0)
print "disabling pedal"
print "Pedal unavailable."
return
# check if we had error before
if self.pedal_interceptor_state != CS.pedal_interceptor_state:
self.pedal_interceptor_state = CS.pedal_interceptor_state
CS.cstm_btns.set_button_status(
"pedal", 1 if self.pedal_interceptor_state > 0 else 0)
if self.pedal_interceptor_state > 0:
# send reset command
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx))
sendcan.send(
can_list_to_can_capnp(can_sends,
msgtype='sendcan').to_bytes())
CS.UE.custom_alert_message(
3, "Pedal Interceptor Off (state %s)" %
self.pedal_interceptor_state, 150, 3)
else:
CS.UE.custom_alert_message(3, "Pedal Interceptor On", 150, 3)
# disable on brake
if CS.brake_pressed and self.enable_pedal_cruise:
self.enable_pedal_cruise = False
self.reset(0.)
CS.UE.custom_alert_message(3, "PDL Disabled", 150, 4)
CS.cstm_btns.set_button_status("pedal", 1)
print "brake pressed"
prev_enable_pedal_cruise = self.enable_pedal_cruise
# process any stalk movement
curr_time_ms = _current_time_millis()
speed_uom_kph = 1.
if CS.imperial_speed_units:
speed_uom_kph = CV.MPH_TO_KPH
if (CS.cruise_buttons == CruiseButtons.MAIN
and self.prev_cruise_buttons != CruiseButtons.MAIN):
double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750
self.last_cruise_stalk_pull_time = curr_time_ms
ready = (CS.cstm_btns.get_button_status("pedal") > PCCState.OFF
and enabled and CruiseState.is_off(CS.pcm_acc_status))
if ready and double_pull:
# A double pull enables ACC. updating the max ACC speed if necessary.
self.enable_pedal_cruise = True
self.LoC.reset(CS.v_ego)
# Increase PCC speed to match current, if applicable.
self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH,
self.pedal_speed_kph)
else:
# A single pull disables PCC (falling back to just steering).
self.enable_pedal_cruise = False
# Handle pressing the cancel button.
elif CS.cruise_buttons == CruiseButtons.CANCEL:
self.enable_pedal_cruise = False
self.pedal_speed_kph = 0.
self.last_cruise_stalk_pull_time = 0
# Handle pressing up and down buttons.
elif (self.enable_pedal_cruise
and CS.cruise_buttons != self.prev_cruise_buttons):
# Real stalk command while PCC is already enabled. Adjust the max PCC
# speed if necessary.
actual_speed_kph = CS.v_ego * CV.MS_TO_KPH
if CS.cruise_buttons == CruiseButtons.RES_ACCEL:
self.pedal_speed_kph = max(self.pedal_speed_kph,
actual_speed_kph) + speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND:
self.pedal_speed_kph = max(
self.pedal_speed_kph, actual_speed_kph) + 5 * speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.DECEL_SET:
self.pedal_speed_kph = min(self.pedal_speed_kph,
actual_speed_kph) - speed_uom_kph
elif CS.cruise_buttons == CruiseButtons.DECEL_2ND:
self.pedal_speed_kph = min(
self.pedal_speed_kph, actual_speed_kph) - 5 * speed_uom_kph
# Clip PCC speed between 0 and 170 KPH.
self.pedal_speed_kph = clip(self.pedal_speed_kph, MIN_PCC_V_KPH,
MAX_PCC_V_KPH)
# If something disabled cruise control, disable PCC too
elif self.enable_pedal_cruise and CS.pcm_acc_status:
self.enable_pedal_cruise = False
# Notify if PCC was toggled
if prev_enable_pedal_cruise and not self.enable_pedal_cruise:
CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4)
CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY)
elif self.enable_pedal_cruise and not prev_enable_pedal_cruise:
CS.UE.custom_alert_message(2, "PCC Enabled", 150)
CS.cstm_btns.set_button_status("pedal", PCCState.ENABLED)
# Update the UI to show whether the current car state allows PCC.
if CS.cstm_btns.get_button_status("pedal") in [
PCCState.STANDBY, PCCState.NOT_READY
]:
if enabled and CruiseState.is_off(CS.pcm_acc_status):
CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY)
else:
CS.cstm_btns.set_button_status("pedal", PCCState.NOT_READY)
# Update prev state after all other actions.
self.prev_cruise_buttons = CS.cruise_buttons
self.prev_pcm_acc_status = CS.pcm_acc_status
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert):
# *** compute control surfaces ***
ts = sec_since_boot()
# steer torque is converted back to CAN reference (positive when steering right)
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = int(
round(clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)))
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(round(actuators.steer * STEER_MAX))
max_lim = min(
max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX),
STEER_MAX)
min_lim = max(
min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX),
-STEER_MAX)
apply_steer = clip(apply_steer, min_lim, max_lim)
# slow rate if steer torque increases in magnitude
if self.last_steer > 0:
apply_steer = clip(
apply_steer,
max(self.last_steer - STEER_DELTA_DOWN, -STEER_DELTA_UP),
self.last_steer + STEER_DELTA_UP)
else:
apply_steer = clip(
apply_steer, self.last_steer - STEER_DELTA_UP,
min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
# cuts steer torque immediately anyway TODO: monitor if this is a real issue
if not enabled or CS.steer_error:
apply_steer = 0
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
can_sends.append(create_steer_command(apply_steer, frame))
if ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(apply_steer))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(apply_accel, pcm_cancel_cmd,
self.standstill_req))
else:
can_sends.append(create_accel_command(0, pcm_cancel_cmd,
False))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame / 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(steer, sound1, sound2))
can_sends.append(create_fcw_command(fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame / 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
""" Controls thread """
# TODO: Make the accord work.
if CS.accord or not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.civic)
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
#TODO: use enum!!
if hud_show_lanes:
hud_lanes = 0x04
else:
hud_lanes = 0x00
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 0xe0
else:
hud_car = 0xd0
else:
hud_car = 0xc0
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 0x01,
hud_car, 0xc1, 0x41, hud_lanes + steer_required,
int(snd_beep), 0x48, (snd_chime << 5) + fcw_display,
acc_alert)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x41, 0x40, 0, 0x48, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
tt = sec_since_boot()
GAS_MAX = 1004
BRAKE_MAX = 1024 / 4
if CS.civic:
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
elif CS.crv:
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value
else:
STEER_MAX = 0xF00
GAS_OFFSET = 328
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
if CS.steer_not_allowed:
apply_steer = 0
# Send CAN commands.
can_sends = []
# Send steering command.
if CS.accord:
idx = frame % 2
can_sends.append(
hondacan.create_accord_steering_control(apply_steer, idx))
else:
idx = frame % 4
can_sends.extend(
hondacan.create_steering_control(apply_steer, CS.crv, idx))
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame / 2) % 4
can_sends.append(
hondacan.create_brake_command(apply_brake, pcm_override,
pcm_cancel_cmd, hud.chime, idx))
if not CS.brake_only:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
gas_amount = (apply_gas + GAS_OFFSET) * (apply_gas > 0)
can_sends.append(hondacan.create_gas_command(gas_amount, idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame / 10) % 4
can_sends.extend(
hondacan.create_ui_commands(pcm_speed, hud, CS.civic,
CS.accord, CS.crv, idx))
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
if CS.civic or CS.accord or CS.crv:
radar_send_step = 5
else:
radar_send_step = 2
if (frame % radar_send_step) == 0:
idx = (frame / radar_send_step) % 4
can_sends.extend(
hondacan.create_radar_commands(CS.v_ego, CS.civic, CS.accord,
CS.crv, idx))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def _can_tx(self, tx_addr, dat, bus):
"""Helper function to send single message"""
msg = [tx_addr, 0, dat, bus]
self.sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan'))
def 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):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# Some override values for Honda
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget)
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = sm['plan'].hasLead
right_lane_visible = sm['pathPlan'].rProb > 0.5
left_lane_visible = sm['pathPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
calibrated = sm['liveCalibration'].calStatus == Calibration.CALIBRATED
ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state) and calibrated
md = sm['model']
if len(md.meta.desirePrediction):
l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1]
r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1]
l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))
if ldw_allowed:
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:
AM.add(sm.frame, 'ldwPermanent', False)
events.append(create_event('ldw', [ET.PERMANENT]))
AM.process_alerts(sm.frame)
CC.hudControl.visualAlert = AM.visual_alert
if not read_only:
# send car controls over can
can_sends = CI.apply(CC)
pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = (sm['dMonitoringState'].awarenessStatus < 0.) or (state == State.softDisabling)
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
dat.controlsState = {
"alertText1": AM.alert_text_1,
"alertText2": AM.alert_text_2,
"alertSize": AM.alert_size,
"alertStatus": AM.alert_status,
"alertBlinkingRate": AM.alert_rate,
"alertType": AM.alert_type,
"alertSound": AM.audible_alert,
"driverMonitoringOn": sm['dMonitoringState'].faceDetected,
"canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": sm.logMonoTime['plan'],
"pathPlanMonoTime": sm.logMonoTime['pathPlan'],
"enabled": isEnabled(state),
"active": isActive(state),
"vEgo": CS.vEgo,
"vEgoRaw": CS.vEgoRaw,
"angleSteers": CS.steeringAngle,
"curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
"steerOverride": CS.steeringPressed,
"state": state,
"engageable": not bool(get_events(events, [ET.NO_ENTRY])),
"longControlState": LoC.long_control_state,
"vPid": float(LoC.v_pid),
"vCruise": float(v_cruise_kph),
"upAccelCmd": float(LoC.pid.p),
"uiAccelCmd": float(LoC.pid.i),
"ufAccelCmd": float(LoC.pid.f),
"angleSteersDes": float(LaC.angle_steers_des),
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(sm['plan'].jerkFactor),
"gpsPlannerActive": sm['plan'].gpsPlannerActive,
"vCurvature": sm['plan'].vCurvature,
"decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
"cumLagMs": -rk.remaining * 1000.,
"startMonoTime": int(start_time * 1e9),
"mapValid": sm['plan'].mapValid,
"forceDecel": bool(force_decel),
"canErrorCounter": can_error_counter,
}
if CP.lateralTuning.which() == 'pid':
dat.controlsState.lateralControlState.pidState = lac_log
elif CP.lateralTuning.which() == 'lqr':
dat.controlsState.lateralControlState.lqrState = lac_log
elif CP.lateralTuning.which() == 'indi':
dat.controlsState.lateralControlState.indiState = lac_log
pm.send('controlsState', dat)
# carState
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = events
pm.send('carState', cs_send)
# carEvents - logged every second or on change
events_bytes = events_to_bytes(events)
if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev):
ce_send = messaging.new_message('carEvents', len(events))
ce_send.carEvents = events
pm.send('carEvents', ce_send)
# carParams - logged every 50 seconds (> 1 per segment)
if (sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = CP
pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
pm.send('carControl', cc_send)
return CC, events_bytes
def update(self, sendcan, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera):
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder,CS.cstm_btns.car_name)
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators)
apply_steer = int(round(alca_steer * STEER_MAX))
max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX)
min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX)
apply_steer = clip(apply_steer, min_lim, max_lim)
# slow rate if steer torque increases in magnitude
if self.last_steer > 0:
apply_steer = clip(apply_steer, max(self.last_steer - STEER_DELTA_DOWN, - STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP)
else:
apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
# cuts steer torque immediately anyway TODO: monitor if this is a real issue
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if self.angle_control:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
if ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame/10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame/100)%0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep != 0 else snd_chime
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required, CS.read_distance_lines)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 / 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
else:
STEER_MAX = 0x1000
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-alca_steer * STEER_MAX, -STEER_MAX, STEER_MAX))
if not CS.lane_departure_toggle_on:
apply_steer = 0
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint,
idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame / 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, idx))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx))
elif CS.stopped:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15,
CAR.ACCORDH):
if CS.lead_distance > (self.prev_lead_distance +
float(kegman.conf['leadDistance'])):
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx))
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH):
if CS.hud_lead == 1:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx))
else:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx))
else:
self.prev_lead_distance = CS.lead_distance
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame / 2
pump_on, self.last_pump_ts = brake_pump_hysteresys(
apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.chime,
hud.fcw, idx))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def test_honda_lkas_hud(self):
self.longMessage = True
sendcan = messaging.pub_sock('sendcan')
car_name = HONDA.CIVIC
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('SET_ME_X41', 'LKAS_HUD', 0),
('SET_ME_X48', 'LKAS_HUD', 0),
('STEERING_REQUIRED', 'LKAS_HUD', 0),
('SOLID_LANES', 'LKAS_HUD', 0),
('LEAD_SPEED', 'RADAR_HUD', 0),
('LEAD_STATE', 'RADAR_HUD', 0),
('LEAD_DISTANCE', 'RADAR_HUD', 0),
('ACC_ALERTS', 'RADAR_HUD', 0),
]
VA = car.CarControl.HUDControl.VisualAlert
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
alerts = {
VA.none: 0,
VA.brakePressed: 10,
VA.wrongGear: 6,
VA.seatbeltUnbuckled: 5,
VA.speedTooHigh: 8,
}
for steer_required in [True, False]:
for lanes in [True, False]:
for alert in alerts.keys():
control = car.CarControl.new_message()
hud = car.CarControl.HUDControl.new_message()
control.enabled = True
if steer_required:
hud.visualAlert = VA.steerRequired
else:
hud.visualAlert = alert
hud.lanesVisible = lanes
control.hudControl = hud
CI.update(control)
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertEqual(0x41, parser.vl['LKAS_HUD']['SET_ME_X41'])
self.assertEqual(0x48, parser.vl['LKAS_HUD']['SET_ME_X48'])
self.assertEqual(steer_required, parser.vl['LKAS_HUD']['STEERING_REQUIRED'])
self.assertEqual(lanes, parser.vl['LKAS_HUD']['SOLID_LANES'])
self.assertEqual(0x1fe, parser.vl['RADAR_HUD']['LEAD_SPEED'])
self.assertEqual(0x7, parser.vl['RADAR_HUD']['LEAD_STATE'])
self.assertEqual(0x1e, parser.vl['RADAR_HUD']['LEAD_DISTANCE'])
self.assertEqual(alerts[alert] if not steer_required else 0, parser.vl['RADAR_HUD']['ACC_ALERTS'])
def test_honda_brake(self):
self.longMessage = True
sendcan = messaging.pub_sock('sendcan')
car_name = HONDA.CIVIC
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('COMPUTER_BRAKE', 'BRAKE_COMMAND', 0),
('BRAKE_PUMP_REQUEST', 'BRAKE_COMMAND', 0), # pump_on
('CRUISE_OVERRIDE', 'BRAKE_COMMAND', 0), # pcm_override
('CRUISE_FAULT_CMD', 'BRAKE_COMMAND', 0), # pcm_fault_cmd
('CRUISE_CANCEL_CMD', 'BRAKE_COMMAND', 0), # pcm_cancel_cmd
('COMPUTER_BRAKE_REQUEST', 'BRAKE_COMMAND', 0), # brake_rq
('SET_ME_0X80', 'BRAKE_COMMAND', 0),
('BRAKE_LIGHTS', 'BRAKE_COMMAND', 0), # brakelights
('FCW', 'BRAKE_COMMAND', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
VA = car.CarControl.HUDControl.VisualAlert
for override in [True, False]:
for cancel in [True, False]:
for fcw in [True, False]:
steps = 25 if not override and not cancel else 2
for brake in np.linspace(0., 0.95, steps):
control = car.CarControl.new_message()
hud = car.CarControl.HUDControl.new_message()
if fcw:
hud.visualAlert = VA.fcw
cruise = car.CarControl.CruiseControl.new_message()
cruise.cancel = cancel
cruise.override = override
actuators = car.CarControl.Actuators.new_message()
actuators.brake = float(brake)
control.enabled = True
control.actuators = actuators
control.hudControl = hud
control.cruiseControl = cruise
CI.update(control)
CI.CS.steer_not_allowed = False
for _ in range(20):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
brake_command = parser.vl['BRAKE_COMMAND']['COMPUTER_BRAKE']
min_expected_brake = int(1024 / 4 * (actuators.brake - 0.02))
max_expected_brake = int(1024 / 4 * (actuators.brake + 0.02))
braking = actuators.brake > 0
braking_ok = min_expected_brake <= brake_command <= max_expected_brake
if steps == 2:
braking_ok = True
self.assertTrue(braking_ok, msg="Car: %s, brake %.2f" % (car_name, brake))
self.assertEqual(0x80, parser.vl['BRAKE_COMMAND']['SET_ME_0X80'])
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['BRAKE_PUMP_REQUEST'])
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['COMPUTER_BRAKE_REQUEST'])
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['BRAKE_LIGHTS'])
self.assertFalse(parser.vl['BRAKE_COMMAND']['CRUISE_FAULT_CMD'])
self.assertEqual(override, parser.vl['BRAKE_COMMAND']['CRUISE_OVERRIDE'])
self.assertEqual(cancel, parser.vl['BRAKE_COMMAND']['CRUISE_CANCEL_CMD'])
self.assertEqual(fcw, bool(parser.vl['BRAKE_COMMAND']['FCW']))
sendcan = messaging.pub_sock('sendcan')
BEFORE = [
"\x10\x15\x30\x0B\x00\x00\x00\x00",
"\x21\x00\x00\x00\x00\x00\x00\x00",
]
LEFT = "\x22\x00\x00\x08\x00\x00\x00\x00"
RIGHT = "\x22\x00\x00\x04\x00\x00\x00\x00"
OFF = "\x22\x00\x00\x00\x00\x00\x00\x00"
AFTER = "\x23\x00\x00\x00\x00\x00\x00\x00"
i = 0
j = 0
while True:
i += 1
if i % 10 == 0:
j += 1
cur = RIGHT if j % 2 == 0 else OFF
can_list = [make_can_msg(1984, d, 0, False) for d in BEFORE]
can_list.append(make_can_msg(1984, cur, 0, False))
can_list.append(make_can_msg(1984, AFTER, 0, False))
for m in can_list:
sendcan.send(can_list_to_can_capnp([m], msgtype='sendcan').to_bytes())
time.sleep(0.01)
def update(self, sendcan, enabled, CS, frame, actuators):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = final_steer * P.STEER_MAX
# limits due to driver torque
driver_max_torque = P.STEER_MAX + (
P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver *
P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER
driver_min_torque = -P.STEER_MAX + (
-P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver *
P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER
max_steer_allowed = max(min(P.STEER_MAX, driver_max_torque), 0)
min_steer_allowed = min(max(-P.STEER_MAX, driver_min_torque), 0)
apply_steer = clip(apply_steer, min_steer_allowed,
max_steer_allowed)
# slow rate if steer torque increases in magnitude
if self.apply_steer_last > 0:
apply_steer = clip(
apply_steer,
max(self.apply_steer_last - P.STEER_DELTA_DOWN,
-P.STEER_DELTA_UP),
self.apply_steer_last + P.STEER_DELTA_UP)
else:
apply_steer = clip(
apply_steer, self.apply_steer_last - P.STEER_DELTA_UP,
min(self.apply_steer_last + P.STEER_DELTA_DOWN,
P.STEER_DELTA_UP))
apply_steer = int(round(apply_steer))
self.apply_steer_last = apply_steer
lkas_enabled = enabled and not CS.steer_not_allowed
if not lkas_enabled:
apply_steer = 0
if self.car_fingerprint == CAR.OUTBACK:
if apply_steer != 0:
chksm_steer = apply_steer * -1
chksm_engage = 1
else:
chksm_steer = 0
chksm_engage = 0
#counts from 0 to 7 then back to 0
idx = (frame / P.STEER_STEP) % 8
steer2 = (chksm_steer >> 8) & 0x1F
steer1 = chksm_steer - (steer2 << 8)
byte2 = steer2
checksum = (idx + steer2 + steer1 + chksm_engage) % 256
if (self.car_fingerprint == CAR.XV2018):
#counts from 0 to 15 then back to 0 + 16 for enable bit
chksm_steer = apply_steer * -1
if chksm_steer != 0:
left3 = 32
else:
left3 = 0
idx = ((frame / P.STEER_STEP) % 16) + 16
steer2 = (chksm_steer >> 8) & 0x1F
steer1 = chksm_steer - (steer2 << 8)
byte2 = steer2 + left3
checksum = ((idx + steer1 + byte2) % 256) + 35
can_sends.append(
subarucan.create_steering_control(self.packer_pt,
canbus.powertrain,
CS.CP.carFingerprint, idx,
steer1, byte2, checksum))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def fingerprint(logcan, sendcan):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None, "")
elif os.getenv("SIMULATOR") is not None:
return ("simulator", None, "")
finger = {0: {}, 2: {}} # collect on bus 0 or 2
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
can_seen_frame = None
can_seen = False
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
vin_query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
vin_cnts = [1, 2] # number of messages to wait for at each iteration
vin_step = 0
vin_cnt = 0
vin_responded = False
vin_never_responded = True
vin_dat = []
vin = ""
frame = 0
while True:
a = messaging.recv_one(logcan)
for can in a.can:
can_seen = True
# have we got a VIN query response?
if can.src == 0 and can.address == 0x7e8:
vin_never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(can.dat, vin_step, vin_cnt):
vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
vin_cnt += 1
if vin_cnt == vin_cnts[vin_step]:
vin_responded = True
vin_step += 1
# ignore everything not on bus 0 and with more than 11 bits,
# which are ussually sporadic and hard to include in fingerprints.
# also exclude VIN query response on 0x7e8.
# Include bus 2 for toyotas to disambiguate cars using camera messages
# (ideally should be done for all cars but we can't for Honda Bosch)
if (can.src == 0 or (only_toyota_left(candidate_cars) and can.src == 2)) and \
can.address < 0x800 and can.address != 0x7e8:
finger[can.src][can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(
can, candidate_cars)
if can_seen_frame is None and can_seen:
can_seen_frame = frame
# if we only have one car choice and the time_fingerprint since we got our first
# message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
# broadcast immediately
if len(candidate_cars) == 1 and can_seen_frame is not None:
time_fingerprint = 1.0 if only_toyota_left(candidate_cars) else 0.1
if (frame - can_seen_frame) > (time_fingerprint * 100):
break
# bail if no cars left or we've been waiting for more than 2s since can_seen
elif len(candidate_cars) == 0 or (can_seen_frame is not None and
(frame - can_seen_frame) > 200):
return None, finger, ""
# keep sending VIN qury if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
# TODO: VIN query temporarily disabled until we have the harness
if False and can_seen and (vin_never_responded or
(vin_responded
and vin_step < len(vin_cnts))):
sendcan.send(
can_list_to_can_capnp([vin_query_msg[vin_step]],
msgtype='sendcan'))
vin_responded = False
vin_cnt = 0
frame += 1
# only report vin if procedure is finished
if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
vin = "".join(vin_dat[3:])
cloudlog.warning("fingerprinted %s", candidate_cars[0])
cloudlog.warning("VIN %s", vin)
return candidate_cars[0], finger, vin
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.safetyModel == car.CarParams.SafetyModels.hondaBosch:
snd_beep = snd_beep if snd_beep is not 0 else snd_chime
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024/4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
else:
is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
if CS.steer_not_allowed:
apply_steer = 0
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, enabled, CS.CP.carFingerprint, idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame/10) % 4
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx))
if CS.CP.safetyModel == car.CarParams.SafetyModels.hondaBosch:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(hondacan.create_cancel_command(idx))
if CS.stopped:
can_sends.append(hondacan.create_resume_command(idx))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame / 2) % 4
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake, pcm_override,
pcm_cancel_cmd, hud.chime, hud.fcw, idx))
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx))
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
if CS.CP.carFingerprint == CAR.ACURA_ILX:
radar_send_step = 2
else:
radar_send_step = 5
if (frame % radar_send_step) == 0:
idx = (frame/radar_send_step) % 4
can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, idx))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
""" Controls thread """
## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars)
if not self.enable_camera:
return
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
#if CS.CP.radarOffCan:
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
STEER_MAX = 420
# Prevent steering while stopped
MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
# Basic highway lane change logic
changing_lanes = CS.right_blinker_on or CS.left_blinker_on
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
# Update statuses for custom buttons every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
#print CS.cstm_btns.get_button_status("alca")
if CS.pedal_interceptor_available:
#update PCC module info
self.PCC.update_stat(CS, True, sendcan)
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# Update HSO module info.
human_control = False
# update CS.v_cruise_pcm based on module selected.
if self.ACC.enable_adaptive_cruise:
CS.v_cruise_pcm = self.ACC.acc_speed_kph
elif self.PCC.enable_pedal_cruise:
CS.v_cruise_pcm = self.PCC.pedal_speed_kph
else:
CS.v_cruise_pcm = CS.v_cruise_actual
# Get the angle from ALCA.
alca_enabled = False
turn_signal_needed = 0
alca_steer = 0.
apply_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
apply_angle = -apply_angle # Tesla is reversed vs OP.
human_control = self.HSO.update_stat(CS, enabled, actuators, frame)
human_lane_changing = changing_lanes and not alca_enabled
enable_steer_control = (enabled and not human_lane_changing
and not human_control and vehicle_moving)
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# Windup slower.
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
# If human control, send the steering angle as read at steering wheel.
if human_control:
apply_angle = CS.angle_steers
# If blinker is on send the actual angle.
#if (changing_lanes and (CS.laneChange_enabled < 2)):
# apply_angle = CS.angle_steers
# Send CAN commands.
can_sends = []
send_step = 5
if (True):
#First we emulate DAS.
#send DAS_bootID
if not self.sent_DAS_bootID:
can_sends.append(teslacan.create_DAS_bootID_msg())
self.sent_DAS_bootID = True
else:
#get speed limit
for socket, _ in self.poller.poll(0):
if socket is self.speedlimit:
self.speedlimit_mph = ui.SpeedLimitData.from_bytes(
socket.recv()).speed * CV.MS_TO_MPH
#send DAS_info
if frame % 100 == 0:
can_sends.append(
teslacan.create_DAS_info_msg(CS.DAS_info_msg))
CS.DAS_info_msg += 1
CS.DAS_info_msg = CS.DAS_info_msg % 10
#send DAS_status
# TODO: forward collission warning
if frame % 50 == 0:
op_status = 0x02
hands_on_state = 0x00
forward_collission_warning = 0 #1 if needed
if hud_alert == AH.FCW:
forward_collission_warning = 0x01
cc_state = 0 #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold
speed_limit_kph = int(self.speedlimit_mph)
alca_state = 0x08
if enabled:
op_status = 0x03
alca_state = 0x08 + turn_signal_needed
if self.ALCA.laneChange_cancelled:
alca_state = 0x14
#if not enable_steer_control:
#op_status = 0x04
#hands_on_state = 0x03
if hud_alert == AH.STEER:
if snd_chime == CM.MUTE:
hands_on_state = 0x03
else:
hands_on_state = 0x05
can_sends.append(
teslacan.create_DAS_status_msg(
CS.DAS_status_idx, op_status, speed_limit_kph,
alca_state, hands_on_state,
forward_collission_warning, cc_state))
CS.DAS_status_idx += 1
CS.DAS_status_idx = CS.DAS_status_idx % 16
#send DAS_status2
if frame % 50 == 0:
collision_warning = 0x00
acc_speed_limit_mph = CS.v_cruise_pcm * CV.KPH_TO_MPH
if hud_alert == AH.FCW:
collision_warning = 0x01
can_sends.append(
teslacan.create_DAS_status2_msg(
CS.DAS_status2_idx, max(1, acc_speed_limit_mph),
collision_warning))
CS.DAS_status2_idx += 1
CS.DAS_status2_idx = CS.DAS_status2_idx % 16
#send DAS_bodyControl
if frame % 50 == 0:
can_sends.append(
teslacan.create_DAS_bodyControls_msg(
CS.DAS_bodyControls_idx, turn_signal_needed))
CS.DAS_bodyControls_idx += 1
CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16
#send DAS_control
if frame % 4 == 0:
acc_speed_limit_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH
accel_min = -15
accel_max = 5
speed_control_enabled = enabled and (acc_speed_limit_kph >
0)
can_sends.append(
teslacan.create_DAS_control(CS.DAS_control_idx,
speed_control_enabled,
acc_speed_limit_kph,
accel_min, accel_max))
CS.DAS_control_idx += 1
CS.DAS_control_idx = CS.DAS_control_idx % 8
#send DAS_lanes
if frame % 10 == 0:
can_sends.append(
teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx))
CS.DAS_lanes_idx += 1
CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16
#send DAS_pscControl
if frame % 4 == 0:
can_sends.append(
teslacan.create_DAS_pscControl_msg(
CS.DAS_pscControl_idx))
CS.DAS_pscControl_idx += 1
CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16
#send DAS_telemetryPeriodic
if frame % 4 == 0:
can_sends.append(
teslacan.create_DAS_telemetryPeriodic(
CS.DAS_telemetryPeriodic1_idx,
CS.DAS_telemetryPeriodic2_idx))
CS.DAS_telemetryPeriodic2_idx += 1
CS.DAS_telemetryPeriodic2_idx = CS.DAS_telemetryPeriodic2_idx % 10
if CS.DAS_telemetryPeriodic2_idx == 0:
CS.DAS_telemetryPeriodic1_idx += 2
CS.DAS_telemetryPeriodic1_idx = CS.DAS_telemetryPeriodic1_idx % 16
#send DAS_telemetryEvent
if frame % 10 == 0:
#can_sends.append(teslacan.create_DAS_telemetryEvent(CS.DAS_telemetryEvent1_idx,CS.DAS_telemetryEvent2_idx))
CS.DAS_telemetryEvent2_idx += 1
CS.DAS_telemetryEvent2_idx = CS.DAS_telemetryEvent2_idx % 10
if CS.DAS_telemetryEvent2_idx == 0:
CS.DAS_telemetryEvent1_idx += 2
CS.DAS_telemetryEvent1_idx = CS.DAS_telemetryEvent1_idx % 16
#send DAS_visualDebug
if (frame + 1) % 10 == 0:
can_sends.append(teslacan.create_DAS_visualDebug_msg())
#send DAS_chNm
if (frame + 2) % 10 == 0:
can_sends.append(teslacan.create_DAS_chNm())
#send DAS_objects
if frame % 3 == 0:
can_sends.append(
teslacan.create_DAS_objects_msg(CS.DAS_objects_idx))
CS.DAS_objects_idx += 1
CS.DAS_objects_idx = CS.DAS_objects_idx % 16
#send DAS_warningMatrix0
if frame % 6 == 0:
can_sends.append(
teslacan.create_DAS_warningMatrix0(
CS.DAS_warningMatrix0_idx))
CS.DAS_warningMatrix0_idx += 1
CS.DAS_warningMatrix0_idx = CS.DAS_warningMatrix0_idx % 16
#send DAS_warningMatrix3
if (frame + 3) % 6 == 0:
apUnavailable = 0
gas_to_resume = 0
alca_cancelled = 0
if enabled and not enable_steer_control:
apUnavailable = 1
if self.ALCA.laneChange_cancelled:
alca_cancelled = 1
can_sends.append(
teslacan.create_DAS_warningMatrix3(
CS.DAS_warningMatrix3_idx, apUnavailable,
alca_cancelled, gas_to_resume))
CS.DAS_warningMatrix3_idx += 1
CS.DAS_warningMatrix3_idx = CS.DAS_warningMatrix3_idx % 16
#send DAS_warningMatrix1
if frame % 100 == 0:
can_sends.append(
teslacan.create_DAS_warningMatrix1(
CS.DAS_warningMatrix1_idx))
CS.DAS_warningMatrix1_idx += 1
CS.DAS_warningMatrix1_idx = CS.DAS_warningMatrix1_idx % 16
# end of DAS emulation """
idx = frame % 16
can_sends.append(
teslacan.create_steering_control(enable_steer_control,
apply_angle, idx))
can_sends.append(teslacan.create_epb_enable_signal(idx))
cruise_btn = None
if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available:
cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators,
pcm_speed)
#add fake carConfig to trigger IC to display AP
if frame % 2 == 0:
carConfig_msg = teslacan.create_GTW_carConfig_msg(
real_carConfig_data=CS.real_carConfig,
dasHw=1,
autoPilot=1,
fRadarHw=1)
#can_sends.append(carConfig_msg)
if cruise_btn or (turn_signal_needed > 0 and frame % 2 == 0):
cruise_msg = teslacan.create_cruise_adjust_msg(
spdCtrlLvr_stat=cruise_btn,
turnIndLvr_Stat=0, #turn_signal_needed,
real_steering_wheel_stalk=CS.steering_wheel_stalk)
# Send this CAN msg first because it is racing against the real stalk.
can_sends.insert(0, cruise_msg)
apply_accel = 0.
if CS.pedal_interceptor_available and frame % 5 == 0: # pedal processed at 20Hz
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(
enabled, CS, frame, actuators, pcm_speed)
can_sends.append(
teslacan.create_pedal_command_msg(apply_accel,
int(accel_needed),
accel_idx))
self.last_angle = apply_angle
self.last_accel = apply_accel
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def fingerprint(logcan, sendcan):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None, "")
elif os.getenv("SIMULATOR") is not None:
return ("simulator", None, "")
finger = {}
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
can_seen_ts = None
can_seen = False
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
vin_query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
vin_cnts = [1, 2] # number of messages to wait for at each iteration
vin_step = 0
vin_cnt = 0
vin_responded = False
vin_never_responded = True
vin_dat = []
vin = ""
while 1:
for a in messaging.drain_sock(logcan):
for can in a.can:
can_seen = True
# have we got a VIN query response?
if can.src == 0 and can.address == 0x7e8:
vin_never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(can.dat, vin_step, vin_cnt):
vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
vin_cnt += 1
if vin_cnt == vin_cnts[vin_step]:
vin_responded = True
vin_step += 1
# ignore everything not on bus 0 and with more than 11 bits,
# which are ussually sporadic and hard to include in fingerprints.
# also exclude VIN query response on 0x7e8
if can.src == 0 and can.address < 0x800 and can.address != 0x7e8:
finger[can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(
can, candidate_cars)
if can_seen_ts is None and can_seen:
can_seen_ts = sec_since_boot() # start time
ts = sec_since_boot()
# if we only have one car choice and the time_fingerprint since we got our first
# message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
# broadcast immediately
if len(candidate_cars) == 1 and can_seen_ts is not None:
time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS"
in candidate_cars[0]) else 0.1
if (ts - can_seen_ts) > time_fingerprint:
break
# bail if no cars left or we've been waiting for more than 2s since can_seen
elif len(candidate_cars) == 0 or (can_seen_ts is not None and
(ts - can_seen_ts) > 2.):
return None, finger, ""
# keep sending VIN qury if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
if can_seen and (vin_never_responded or
(vin_responded and vin_step < len(vin_cnts))):
sendcan.send(
can_list_to_can_capnp([vin_query_msg[vin_step]],
msgtype='sendcan'))
vin_responded = False
vin_cnt = 0
time.sleep(0.01)
# only report vin if procedure is finished
if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
vin = "".join(vin_dat[3:])
cloudlog.warning("fingerprinted %s", candidate_cars[0])
cloudlog.warning("VIN %s", vin)
return candidate_cars[0], finger, vin
def update(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = final_steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
if not lkas_enabled:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4
if self.car_fingerprint == CAR.VOLT:
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
canbus.powertrain,
apply_steer, idx,
lkas_enabled))
if self.car_fingerprint == CAR.CADILLAC_CT6:
can_sends += gmcan.create_steering_control_ct6(
self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
lkas_enabled)
### GAS/BRAKE ###
if self.car_fingerprint == CAR.VOLT:
# no output if not enabled, but keep sending keepalive messages
# threat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
apply_gas = P.MAX_ACC_REGEN # TODO: do we really need to send max regen when not enabled?
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
apply_brake = int(
round(
interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame / 4) % 4
at_full_stop = enabled and CS.standstill
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, canbus.chassis, apply_brake, idx,
near_stop, at_full_stop))
at_full_stop = enabled and CS.standstill
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
canbus.powertrain,
apply_gas, idx, enabled,
at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
can_sends.append(
gmcan.create_acc_dashboard_command(
canbus.powertrain, enabled,
hud_v_cruise / CV.MS_TO_KPH, hud_show_car))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
if frame % time_and_headlights_step == 0:
idx = (frame / time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(
canbus.obstacle, int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame / speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
canbus.obstacle, CS.v_ego, idx))
# Send ADAS keepalive, 10hz
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Send chimes
if self.chime != chime:
duration = 0x3c
# There is no 'repeat forever' chime command
# TODO: Manage periodic re-issuing of chime command
# and chime cancellation
if chime_cnt == -1:
chime_cnt = 10
if chime != 0:
can_sends.append(
gmcan.create_chime_command(canbus.sw_gmlan, chime,
duration, chime_cnt))
# If canceling a repeated chime, cancel command must be
# issued for the same chime type and duration
self.chime = chime
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
# Sanity check.
if not self.allow_controls:
return
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.lkMode and CS.v_ego > P.MIN_STEER_SPEED and not CS.left_blinker_on and not CS.right_blinker_on
if lkas_enabled:
apply_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver,
P)
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4
if self.car_fingerprint in SUPERCRUISE_CARS:
can_sends += gmcan.create_steering_control_ct6(
self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
lkas_enabled)
else:
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
canbus.powertrain,
apply_steer, idx,
lkas_enabled))
### GAS/BRAKE ###
if self.car_fingerprint not in SUPERCRUISE_CARS:
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
apply_brake = int(
round(
interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame / 4) % 4
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = enabled and standstill and car_stopping
near_stop = enabled and (
CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, canbus.chassis, apply_brake, idx,
near_stop, at_full_stop))
# Auto-resume from full stop by resetting ACC control
acc_enabled = enabled
if standstill and not car_stopping:
acc_enabled = False
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
canbus.powertrain,
apply_gas, idx, acc_enabled,
at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
follow_level = CS.get_follow_level()
if (frame % 4) == 0:
can_sends.append(
gmcan.create_acc_dashboard_command(
self.packer_pt, canbus.powertrain, enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_show_car,
follow_level))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
if frame % time_and_headlights_step == 0:
idx = (frame / time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(
canbus.obstacle, int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame / speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
canbus.obstacle, CS.v_ego, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(
gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
lka_critical))
self.lka_icon_status_last = lka_icon_status
# Send chimes
if self.chime != chime:
duration = 0x3c
# There is no 'repeat forever' chime command
# TODO: Manage periodic re-issuing of chime command
# and chime cancellation
if chime_cnt == -1:
chime_cnt = 10
if chime != 0:
can_sends.append(
gmcan.create_chime_command(canbus.sw_gmlan, chime,
duration, chime_cnt))
# If canceling a repeated chime, cancel command must be
# issued for the same chime type and duration
self.chime = chime
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def steer_thread():
poller = messaging.Poller()
logcan = messaging.sub_sock('can')
health = messaging.sub_sock('health')
joystick_sock = messaging.sub_sock('testJoystick', conflate=True, poller=poller)
carstate = messaging.pub_sock('carState')
carcontrol = messaging.pub_sock('carControl')
sendcan = messaging.pub_sock('sendcan')
button_1_last = 0
enabled = False
# wait for health and CAN packets
hw_type = messaging.recv_one(health).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(logcan)
CI, CP = get_car(logcan, sendcan, has_relay)
Params().put("CarParams", CP.to_bytes())
CC = car.CarControl.new_message()
while True:
# send
joystick = messaging.recv_one(joystick_sock)
can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True)
CS = CI.update(CC, can_strs)
# Usually axis run in pairs, up/down for one, and left/right for
# the other.
actuators = car.CarControl.Actuators.new_message()
if joystick is not None:
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.) # -1 to 1
actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.) # -1 to 1
actuators.gas = max(axis_1, 0.)
actuators.brake = max(-axis_1, 0.)
pcm_cancel_cmd = joystick.testJoystick.buttons[0]
button_1 = joystick.testJoystick.buttons[1]
if button_1 and not button_1_last:
enabled = not enabled
button_1_last = button_1
print("enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake)
hud_alert = 0
audible_alert = 0
if joystick.testJoystick.buttons[2]:
audible_alert = "beepSingle"
if joystick.testJoystick.buttons[3]:
audible_alert = "chimeRepeated"
hud_alert = "steerRequired"
CC.actuators.gas = actuators.gas
CC.actuators.brake = actuators.brake
CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle
CC.hudControl.visualAlert = hud_alert
CC.hudControl.setSpeed = 20
CC.cruiseControl.cancel = pcm_cancel_cmd
CC.enabled = enabled
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep is not 0 else snd_chime
# Do not send audible alert when steering is disabled or blinkers on
#if not CS.lkMode or CS.left_blinker_on or CS.right_blinker_on:
# snd_chime = 0
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required, CS.read_distance_lines, CS.lkMode,
self.speed_units)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 / 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
else:
STEER_MAX = 0x1000
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on # add LKAS button to toggle steering
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint,
idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame / 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, idx))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx))
elif CS.stopped:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame / 2) % 4
pump_on, self.last_pump_ts = brake_pump_hysteresys(
apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.chime,
hud.fcw, idx))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
hondacan.create_gas_command(self.packer, apply_gas,
idx))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
""" Controls thread """
## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars)
if not self.enable_camera:
return
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
#if CS.CP.radarOffCan:
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
STEER_MAX = 420
# Prevent steering while stopped
MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
# Basic highway lane change logic
changing_lanes = CS.right_blinker_on or CS.left_blinker_on
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
# Update statuses for custom buttons every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
#print CS.cstm_btns.get_button_status("alca")
if CS.pedal_hardware_present:
#update PCC module info
self.PCC.update_stat(CS, True, sendcan)
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# Update HSO module info.
human_control = False
# update CS.v_cruise_pcm based on module selected.
if self.ACC.enable_adaptive_cruise:
CS.v_cruise_pcm = self.ACC.acc_speed_kph
elif self.PCC.enable_pedal_cruise:
CS.v_cruise_pcm = self.PCC.pedal_speed_kph
else:
CS.v_cruise_pcm = CS.v_cruise_actual
# Get the angle from ALCA.
alca_enabled = False
turn_signal_needed = 0
alca_steer = 0.
apply_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
apply_angle = -apply_angle # Tesla is reversed vs OP.
human_control = self.HSO.update_stat(CS, enabled, actuators, frame)
human_lane_changing = changing_lanes and not alca_enabled
enable_steer_control = (enabled and not human_lane_changing
and not human_control)
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# Windup slower.
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
# If human control, send the steering angle as read at steering wheel.
if human_control:
apply_angle = CS.angle_steers
# If blinker is on send the actual angle.
#if (changing_lanes and (CS.laneChange_enabled < 2)):
# apply_angle = CS.angle_steers
# Send CAN commands.
can_sends = []
send_step = 5
if (True):
"""#First we emulate DAS.
if (CS.DAS_info_frm == -1):
#initialize all frames
CS.DAS_info_frm = frame # 1.00 s interval
CS.DAS_status_frm = (frame + 10) % 100 # 0.50 s interval
CS.DAS_status2_frm = (frame + 35) % 100 # 0.50 s interval in between DAS_status
CS.DAS_bodyControls_frm = (frame + 40) % 100 # 0.50 s interval
CS.DAS_lanes_frm = (frame + 5) % 100 # 0.10 s interval
CS.DAS_objects_frm = (frame + 2) % 100 # 0.03 s interval
CS.DAS_pscControl_frm = (frame + 3) % 100 # 0.04 s interval
if (CS.DAS_info_frm == frame):
can_sends.append(teslacan.create_DAS_info_msg(CS.DAS_info_msg))
CS.DAS_info_msg += 1
CS.DAS_info_msg = CS.DAS_info_msg % 10
if (CS.DAS_status_frm == frame):
can_sends.append(teslacan.create_DAS_status_msg(CS.DAS_status_idx))
CS.DAS_status_idx += 1
CS.DAS_status_idx = CS.DAS_status_idx % 16
CS.DAS_status_frm = (CS.DAS_status_frm + 50) % 100
if (CS.DAS_status2_frm == frame):
can_sends.append(teslacan.create_DAS_status2_msg(CS.DAS_status2_idx))
CS.DAS_status2_idx += 1
CS.DAS_status2_idx = CS.DAS_status2_idx % 16
CS.DAS_status2_frm = (CS.DAS_status2_frm + 50) % 100
if (CS.DAS_bodyControls_frm == frame):
can_sends.append(teslacan.create_DAS_bodyControls_msg(CS.DAS_bodyControls_idx))
CS.DAS_bodyControls_idx += 1
CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16
CS.DAS_bodyControls_frm = (CS.DAS_bodyControls_frm + 50) % 100
if (CS.DAS_lanes_frm == frame):
can_sends.append(teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx))
CS.DAS_lanes_idx += 1
CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16
CS.DAS_lanes_frm = (CS.DAS_lanes_frm + 10) % 100
if (CS.DAS_pscControl_frm == frame):
can_sends.append(teslacan.create_DAS_pscControl_msg(CS.DAS_pscControl_idx))
CS.DAS_pscControl_idx += 1
CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16
CS.DAS_pscControl_frm = (CS.DAS_pscControl_frm + 4) % 100
if (CS.DAS_objects_frm == frame):
can_sends.append(teslacan.create_DAS_objects_msg(CS.DAS_objects_idx))
CS.DAS_objects_idx += 1
CS.DAS_objects_idx = CS.DAS_objects_idx % 16
CS.DAS_objects_frm = (CS.DAS_objects_frm + 3) % 100
# end of DAS emulation """
idx = frame % 16
can_sends.append(
teslacan.create_steering_control(enable_steer_control,
apply_angle, idx))
can_sends.append(teslacan.create_epb_enable_signal(idx))
cruise_btn = None
if self.ACC.enable_adaptive_cruise and not self.PCC.pedal_hardware_present:
cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators,
pcm_speed)
if (cruise_btn != None) or ((turn_signal_needed > 0) and
(frame % 2 == 0)):
cruise_msg = teslacan.create_cruise_adjust_msg(
spdCtrlLvr_stat=cruise_btn,
turnIndLvr_Stat=turn_signal_needed,
real_steering_wheel_stalk=CS.steering_wheel_stalk)
# Send this CAN msg first because it is racing against the real stalk.
can_sends.insert(0, cruise_msg)
apply_accel = 0.
if self.PCC.pedal_hardware_present: # and (frame % 10) == 0:
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(
enabled, CS, frame, actuators, pcm_speed)
can_sends.append(
teslacan.create_pedal_command_msg(apply_accel,
int(accel_needed),
accel_idx))
self.last_angle = apply_angle
self.last_accel = apply_accel
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert, forwarding_camera, left_line,
right_line, lead, left_lane_depart, right_lane_depart):
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None and not CS.indi_toggle:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
#apply_steer = int(round(alca_steer * STEER_MAX))
self.phantom.update()
# steer torque
if self.phantom.data["status"]:
apply_steer = int(round(self.phantom.data["angle"]))
if abs(CS.angle_steers) > 400:
apply_steer = 0
else:
apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
if abs(CS.angle_steers) > 100:
apply_steer = 0
if not CS.lane_departure_toggle_on:
apply_steer = 0
# only cut torque when steer state is a known fault
if CS.steer_state in [3, 7, 9, 11, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
cutout_time = 100 if self.phantom.data["status"] else 200
if not enabled or (frame - self.last_fault_frame < cutout_time):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
if apply_steer == 0 and self.last_steer == 0:
apply_steer_req = 0
if not enabled and right_lane_depart and CS.v_ego > 12.5 and not CS.right_blinker_on:
apply_steer = self.last_steer + 3
apply_steer = min(apply_steer, 800)
#print "right"
#print apply_steer
apply_steer_req = 1
if not enabled and left_lane_depart and CS.v_ego > 12.5 and not CS.left_blinker_on:
apply_steer = self.last_steer - 3
apply_steer = max(apply_steer, -800)
#print "left"
#print apply_steer
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
#apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
#if CS.standstill and not self.last_standstill:
# self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
# Enable blindspot debug mode once
if BLINDSPOTDEBUG:
self.blindspot_poll_counter += 1
if self.blindspot_poll_counter > 1000: # 10 seconds after start
if CS.left_blinker_on:
self.blindspot_blink_counter_left += 1
#print "debug Left Blinker on"
elif CS.right_blinker_on:
self.blindspot_blink_counter_right += 1
else:
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
#print "debug Left Blinker off"
if self.blindspot_debug_enabled_left:
can_sends.append(
set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
self.blindspot_debug_enabled_left = False
#print "debug Left blindspot debug disabled"
if self.blindspot_debug_enabled_right:
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self.blindspot_debug_enabled_right = False
#print "debug Right blindspot debug disabled"
if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds
can_sends.append(set_blindspot_debug_mode(
LEFT_BLINDSPOT, True))
#print "debug Left blindspot debug enabled"
self.blindspot_debug_enabled_left = True
if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode
if CS.v_ego > 6: #polling at low speeds switches camera off
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
#print "debug Right blindspot debug enabled"
self.blindspot_debug_enabled_right = True
if self.blindspot_debug_enabled_left:
if self.blindspot_poll_counter % 20 == 0 and self.blindspot_poll_counter > 1001: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
if self.blindspot_debug_enabled_right:
if self.blindspot_poll_counter % 20 == 10 and self.blindspot_poll_counter > 1005: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
if CS.cstm_btns.get_button_status("tr") > 0:
distance = 1
else:
distance = 0
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead, distance))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, distance))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, frame // 2))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame // 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2,
left_line, right_line, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSSP2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame // 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame // 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
def step(self,
v_lead=0.0,
cruise_buttons=None,
grade=0.0,
publish_model=True):
gen_dbc, gen_signals, gen_checks = get_can_signals(CP)
sgs = [s[0] for s in gen_signals]
msgs = [s[1] for s in gen_signals]
cks_msgs = set(check[0] for check in gen_checks)
cks_msgs.add(0x18F)
cks_msgs.add(0x30C)
# ******** get messages sent to the car ********
can_msgs = []
for a in messaging.drain_sock(Plant.sendcan):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0, 2]))
self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting ***
live100_msgs = []
for a in messaging.drain_sock(Plant.live100):
live100_msgs.append(a.live100)
fcw = None
for a in messaging.drain_sock(Plant.plan):
if a.plan.fcw:
fcw = True
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
else:
brake = 0.0
if self.cp.vl[0x200]['GAS_COMMAND'] > 0:
gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0
else:
gas = 0.0
if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
steer_torque = self.cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0xf00
else:
steer_torque = 0.0
distance_lead = self.distance_lead_prev + v_lead * self.ts
# ******** run the car ********
speed, acceleration = car_plant(self.distance_prev, self.speed_prev,
grade, gas, brake)
distance = self.distance_prev + speed * self.ts
speed = self.speed_prev + self.ts * acceleration
if speed <= 0:
speed = 0
acceleration = 0
# ******** lateral ********
self.angle_steer -= (steer_torque / 10.0) * self.ts
# *** radar model ***
if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance)
v_rel = v_lead - speed
else:
d_rel = 200.
v_rel = 0.
lateral_pos_rel = 0.
# print at 5hz
if (self.rk.frame % (self.rate / 5)) == 0:
print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (
distance, speed, acceleration, self.angle_steer, gas, brake,
steer_torque, d_rel, v_rel)
# ******** publish the car ********
# TODO: the order is this list should not matter, but currently everytime we change carstate we break this test. Fix it!
vls = [
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.angle_steer,
self.angle_steer_rate,
0,
0,
0,
0,
0, # Doors
0,
0, # Blinkers
0, # Cruise speed offset
self.gear_choice,
speed != 0,
self.brake_error,
self.brake_error,
self.v_cruise,
not self.seatbelt,
self.seatbelt, # Seatbelt
self.brake_pressed,
0.,
cruise_buttons,
self.esp_disabled,
0, # HUD lead
self.user_brake,
self.steer_error,
self.gear_shifter,
self.pedal_gas,
self.cruise_setting,
self.acc_status,
self.user_gas,
self.main_on,
0, # EPB State
0, # Brake hold
0, # Interceptor feedback
# 0,
]
# TODO: publish each message at proper frequency
can_msgs = []
for msg in set(msgs):
msg_struct = {}
indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
for i in indxs:
msg_struct[sgs[i]] = vls[i]
if "COUNTER" in honda.get_signals(msg):
msg_struct["COUNTER"] = self.rk.frame % 4
msg = honda.lookup_msg_id(msg)
msg_data = honda.encode(msg, msg_struct)
if "CHECKSUM" in honda.get_signals(msg):
msg_data = fix(msg_data, msg)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message
# TODO: use the DBC
if self.rk.frame % 5 == 0:
radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00'
radar_msg = to_3_byte(d_rel*16.0) + \
to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
to_3s_byte(int(v_rel*32.0)) + \
"0f00000"
can_msgs.append([0x400, 0, radar_state_msg, 1])
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
# ******** publish a fake model going straight and fake calibration ********
# note that this is worst case for MPC, since model will delay long mpc by one time step
if publish_model and self.rk.frame % 5 == 0:
md = messaging.new_message()
cal = messaging.new_message()
md.init('model')
cal.init('liveCalibration')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0] * 50
x.prob = 1.0
x.std = 1.0
md.model.lead.dist = float(d_rel)
md.model.lead.prob = 1.
md.model.lead.std = 0.1
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
Plant.model.send(md.to_bytes())
Plant.cal.send(cal.to_bytes())
# ******** update prevs ********
self.speed = speed
self.distance = distance
self.distance_lead = distance_lead
self.speed_prev = speed
self.distance_prev = distance
self.distance_lead_prev = distance_lead
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas,
steer_torque, fcw, live100_msgs)
def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True):
gen_signals, gen_checks = get_can_signals(CP)
sgs = [s[0] for s in gen_signals]
msgs = [s[1] for s in gen_signals]
cks_msgs = set(check[0] for check in gen_checks)
cks_msgs.add(0x18F)
cks_msgs.add(0x30C)
# ******** get messages sent to the car ********
can_msgs = []
for a in messaging.drain_sock(Plant.sendcan):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0,2]))
self.cp.update_can(can_msgs)
# ******** get controlsState messages for plotting ***
controls_state_msgs = []
for a in messaging.drain_sock(Plant.controls_state):
controls_state_msgs.append(a.controlsState)
fcw = None
for a in messaging.drain_sock(Plant.plan):
if a.plan.fcw:
fcw = True
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
else:
brake = 0.0
if self.cp.vl[0x200]['GAS_COMMAND'] > 0:
gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0
else:
gas = 0.0
if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
steer_torque = self.cp.vl[0xe4]['STEER_TORQUE']*1.0/0xf00
else:
steer_torque = 0.0
distance_lead = self.distance_lead_prev + v_lead * self.ts
# ******** run the car ********
speed, acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake)
distance = self.distance_prev + speed * self.ts
speed = self.speed_prev + self.ts * acceleration
if speed <= 0:
speed = 0
acceleration = 0
# ******** lateral ********
self.angle_steer -= (steer_torque/10.0) * self.ts
# *** radar model ***
if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance)
v_rel = v_lead - speed
else:
d_rel = 200.
v_rel = 0.
lateral_pos_rel = 0.
# print at 5hz
if (self.rk.frame % (self.rate//5)) == 0:
print("%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel))
# ******** publish the car ********
vls_tuple = namedtuple('vls', [
'XMISSION_SPEED',
'WHEEL_SPEED_FL', 'WHEEL_SPEED_FR', 'WHEEL_SPEED_RL', 'WHEEL_SPEED_RR',
'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR',
'LEFT_BLINKER', 'RIGHT_BLINKER',
'GEAR',
'WHEELS_MOVING',
'BRAKE_ERROR_1', 'BRAKE_ERROR_2',
'SEATBELT_DRIVER_LAMP', 'SEATBELT_DRIVER_LATCHED',
'BRAKE_PRESSED', 'BRAKE_SWITCH',
'CRUISE_BUTTONS',
'ESP_DISABLED',
'HUD_LEAD',
'USER_BRAKE',
'STEER_STATUS',
'GEAR_SHIFTER',
'PEDAL_GAS',
'CRUISE_SETTING',
'ACC_STATUS',
'CRUISE_SPEED_PCM',
'CRUISE_SPEED_OFFSET',
'DOOR_OPEN_FL', 'DOOR_OPEN_FR', 'DOOR_OPEN_RL', 'DOOR_OPEN_RR',
'CAR_GAS',
'MAIN_ON',
'EPB_STATE',
'BRAKE_HOLD_ACTIVE',
'INTERCEPTOR_GAS',
'IMPERIAL_UNIT',
])
vls = vls_tuple(
self.speed_sensor(speed),
self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
self.angle_steer, self.angle_steer_rate, 0, #Steer torque sensor
0, 0, # Blinkers
self.gear_choice,
speed != 0,
self.brake_error, self.brake_error,
not self.seatbelt, self.seatbelt, # Seatbelt
self.brake_pressed, 0., #Brake pressed, Brake switch
cruise_buttons,
self.esp_disabled,
0, # HUD lead
self.user_brake,
self.steer_error,
self.gear_shifter,
self.pedal_gas,
self.cruise_setting,
self.acc_status,
self.v_cruise,
0, # Cruise speed offset
0, 0, 0, 0, # Doors
self.user_gas,
self.main_on,
0, # EPB State
0, # Brake hold
0, # Interceptor feedback
False
)
# TODO: publish each message at proper frequency
can_msgs = []
for msg in set(msgs):
msg_struct = {}
indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
for i in indxs:
msg_struct[sgs[i]] = getattr(vls, sgs[i])
if "COUNTER" in honda.get_signals(msg):
msg_struct["COUNTER"] = self.rk.frame % 4
if "COUNTER_PEDAL" in honda.get_signals(msg):
msg_struct["COUNTER_PEDAL"] = self.rk.frame % 0xf
msg = honda.lookup_msg_id(msg)
msg_data = honda.encode(msg, msg_struct)
if "CHECKSUM" in honda.get_signals(msg):
msg_data = fix(msg_data, msg)
if "CHECKSUM_PEDAL" in honda.get_signals(msg):
msg_struct["CHECKSUM_PEDAL"] = crc8_pedal([ord(i) for i in msg_data][:-1])
msg_data = honda.encode(msg, msg_struct)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message
# TODO: use the DBC
if self.rk.frame % 5 == 0:
radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00'
radar_msg = to_3_byte(d_rel*16.0) + \
to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
to_3s_byte(int(v_rel*32.0)) + \
"0f00000"
can_msgs.append([0x400, 0, radar_state_msg, 1])
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
# add camera msg so controlsd thinks it's alive
msg_struct["COUNTER"] = self.rk.frame % 4
msg = honda.lookup_msg_id(0xe4)
msg_data = honda.encode(msg, msg_struct)
msg_data = fix(msg_data, 0xe4)
can_msgs.append([0xe4, 0, msg_data, 2])
Plant.logcan.send(can_list_to_can_capnp(can_msgs))
# Fake sockets that controlsd subscribes to
live_parameters = messaging.new_message()
live_parameters.init('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.sensorValid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
Plant.live_params.send(live_parameters.to_bytes())
driver_monitoring = messaging.new_message()
driver_monitoring.init('driverMonitoring')
driver_monitoring.driverMonitoring.descriptor = [0.] * 7
Plant.driverMonitoring.send(driver_monitoring.to_bytes())
health = messaging.new_message()
health.init('health')
health.health.controlsAllowed = True
Plant.health.send(health.to_bytes())
thermal = messaging.new_message()
thermal.init('thermal')
thermal.thermal.freeSpace = 1.
thermal.thermal.batteryPercent = 100
Plant.thermal.send(thermal.to_bytes())
# ******** publish a fake model going straight and fake calibration ********
# note that this is worst case for MPC, since model will delay long mpc by one time step
if publish_model and self.rk.frame % 5 == 0:
md = messaging.new_message()
cal = messaging.new_message()
md.init('model')
cal.init('liveCalibration')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0]*50
x.prob = 1.0
x.std = 1.0
md.model.lead.dist = float(d_rel)
md.model.lead.prob = 1.
md.model.lead.std = 0.1
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
Plant.model.send(md.to_bytes())
Plant.cal.send(cal.to_bytes())
# ******** update prevs ********
self.speed = speed
self.distance = distance
self.distance_lead = distance_lead
self.speed_prev = speed
self.distance_prev = distance
self.distance_lead_prev = distance_lead
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state_msgs)
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert, forwarding_camera, left_line,
right_line, lead):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only send brake values if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead))
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame / 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2,
left_line, right_line))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame / 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def publish_logs(self, CS, start_time, CC, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
# Orientation and angle rates can be useful for carcontroller
# Only calibrated (car) frame is relevant for the carcontroller
orientation_value = list(
self.sm['liveLocationKalman'].calibratedOrientationNED.value)
if len(orientation_value) > 2:
CC.orientationNED = orientation_value
angular_rate_value = list(
self.sm['liveLocationKalman'].angularVelocityCalibrated.value)
if len(angular_rate_value) > 2:
CC.angularVelocity = angular_rate_value
CC.cruiseControl.cancel = CS.cruiseState.enabled and (
not self.enabled or not self.CP.pcmCruise)
if self.joystick_mode and self.sm.rcv_frame[
'testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
hudControl = CC.hudControl
hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
hudControl.speedVisible = self.enabled
hudControl.lanesVisible = self.enabled
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
hudControl.rightLaneVisible = True
hudControl.leftLaneVisible = True
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 CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
model_v2 = self.sm['modelV2']
desire_prediction = model_v2.meta.desirePrediction
if len(desire_prediction) and ldw_allowed:
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1]
lane_lines = model_v2.laneLines
l_lane_close = left_lane_visible and (lane_lines[1].y[0] >
-(1.08 + CAMERA_OFFSET))
r_lane_close = right_lane_visible and (lane_lines[2].y[0] <
(1.08 - CAMERA_OFFSET))
hudControl.leftLaneDepart = bool(
l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
hudControl.rightLaneDepart = bool(
r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if hudControl.rightLaneDepart or hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event_types = set()
if ET.WARNING not in self.current_alert_types:
clear_event_types.add(ET.WARNING)
if self.enabled:
clear_event_types.add(ET.NO_ENTRY)
alerts = self.events.create_alerts(
self.current_alert_types,
[self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer])
self.AM.add_many(self.sm.frame, alerts)
current_alert = self.AM.process_alerts(self.sm.frame,
clear_event_types)
if current_alert:
hudControl.visualAlert = current_alert.visual_alert
if not self.read_only and self.initialized:
# send car controls over can
self.last_actuators, can_sends = self.CI.apply(CC)
self.pm.send(
'sendcan',
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
CC.actuatorsOutput = self.last_actuators
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.angleOffsetDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset,
CS.vEgo, params.roll)
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
if current_alert:
controlsState.alertText1 = current_alert.alert_text_1
controlsState.alertText2 = current_alert.alert_text_2
controlsState.alertSize = current_alert.alert_size
controlsState.alertStatus = current_alert.alert_status
controlsState.alertBlinkingRate = current_alert.alert_rate
controlsState.alertType = current_alert.alert_type
controlsState.alertSound = current_alert.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.desiredCurvature = self.desired_curvature
controlsState.desiredCurvatureRate = self.desired_curvature_rate
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.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_rcv_error_counter
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
elif lat_tuning == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif lat_tuning == 'torque':
controlsState.lateralControlState.torqueState = lac_log
elif lat_tuning == '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 update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert):
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
apply_steer = int(round(actuators.steer * STEER_MAX))
# TODO use these values to decide if we should use apply_steer or apply_angle
outp = 'carcontroller apply_steer %s actuators.steerAngle %s' % (
apply_steer, actuators.steerAngle)
# print outp
logging.info(outp)
# max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX)
# min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX)
# apply_steer = clip(apply_steer, min_lim, max_lim)
apply_steer = clip(apply_steer, -STEER_MAX, STEER_MAX)
# slow rate if steer torque increases in magnitude
if self.last_steer > 0:
apply_steer = clip(
apply_steer,
max(self.last_steer - STEER_DELTA_DOWN, -STEER_DELTA_UP),
self.last_steer + STEER_DELTA_UP)
else:
apply_steer = clip(
apply_steer, self.last_steer - STEER_DELTA_UP,
min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
#self.steer_angle_enabled, self.ipas_reset_counter = \
# ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle
self.steer_angle_enabled = True #!!! TODO use if we are doing apply_angle (instead of apply_steer)
if self.steer_angle_enabled:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
outp = ' apply_angle:%s angle_lim:%s angle_rate_lim:%s apply_steer:%s' % (
apply_angle, angle_lim, angle_rate_lim, apply_steer)
# print outp
logging.info(outp)
outp = ' CS.angle_steers:%s CS.v_ego:%s' % (CS.angle_steers,
CS.v_ego)
# print outp
logging.info(outp)
# else:
# apply_angle = CS.angle_steers # just sets it to the current steering angle
self.standstill_req = False #?
moving_fast = True # for status message
if CS.v_ego < 5: # don't steer if going under 6mph to not lock out LKAS (was < 3)
apply_angle = 0
apply_steer = 0
moving_fast = False
if self.last_steer == 0 and apply_steer != 0:
self.send_new_status = True
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# can_sends.append(create_steer_command(self.packer, apply_steer, frame))
# TODO verify units and see if we want apply_steer or apply_angle
# frame is 100Hz (0.01s period)
if (self.ccframe % 10 == 0): # 0.1s period
new_msg = create_2d9(self.car_fingerprint)
sendcan.send(
can_list_to_can_capnp([new_msg], msgtype='sendcan').to_bytes())
can_sends.append(new_msg)
if (self.ccframe % 25 == 0) or self.send_new_status: # 0.25s period
if (self.ccframe - self.prev_2a6
) < 20: # at least 200ms (20 frames) since last 2a6.
self.send_new_status = True # will not send, so send next time.
apply_steer = 0 # cannot steer yet, waiting for 2a6 to be sent.
last_steer = 0
last_angle = 0
else:
new_msg = create_2a6(CS.gear_shifter, apply_steer, moving_fast,
self.car_fingerprint)
sendcan.send(
can_list_to_can_capnp([new_msg],
msgtype='sendcan').to_bytes())
can_sends.append(new_msg)
self.send_new_status = False
self.prev_2a6 = self.ccframe
new_msg = create_292(int(apply_steer * CAR_UNITS_PER_DEGREE), frame,
moving_fast)
sendcan.send(
can_list_to_can_capnp([new_msg], msgtype='sendcan').to_bytes())
can_sends.append(new_msg) # degrees * 5.1 -> car steering units
for msg in can_sends:
[addr, _, dat, _] = msg
outp = ('make_can_msg:%s len:%d %s' %
('0x{:02x}'.format(addr), len(dat), ' '.join(
'{:02x}'.format(ord(c)) for c in dat)))
logging.info(outp)
self.ccframe += 1
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)
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)
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 test_panda_safety_carstate(self):
"""
Assert that panda safety matches openpilot's carState
"""
if self.CP.dashcamOnly:
self.skipTest("no need to check panda safety for dashcamOnly")
CC = car.CarControl.new_message()
# warm up pass, as initial states may be different
for can in self.can_msgs[:300]:
for msg in can_capnp_to_can_list(can.can, src_filter=range(64)):
to_send = package_can_msg(msg)
self.safety.safety_rx_hook(to_send)
self.CI.update(CC, (can_list_to_can_capnp([
msg,
]), ))
if not self.CP.pcmCruise:
self.safety.set_controls_allowed(0)
controls_allowed_prev = False
CS_prev = car.CarState.new_message()
checks = defaultdict(lambda: 0)
for can in self.can_msgs:
CS = self.CI.update(CC, (can.as_builder().to_bytes(), ))
for msg in can_capnp_to_can_list(can.can, src_filter=range(64)):
to_send = package_can_msg(msg)
ret = self.safety.safety_rx_hook(to_send)
self.assertEqual(1, ret,
f"safety rx failed ({ret=}): {to_send}")
# TODO: check rest of panda's carstate (steering, ACC main on, etc.)
# TODO: make the interceptor thresholds in openpilot and panda match, then remove this exception
gas_pressed = CS.gasPressed
if self.CP.enableGasInterceptor and gas_pressed and not self.safety.get_gas_pressed_prev(
):
# panda intentionally has a higher threshold
if self.CP.carName == "toyota" and 15 < CS.gas < 15 * 1.5:
gas_pressed = False
checks[
'gasPressed'] += gas_pressed != self.safety.get_gas_pressed_prev(
)
# TODO: remove this exception once this mismatch is resolved
brake_pressed = CS.brakePressed
if CS.brakePressed and not self.safety.get_brake_pressed_prev():
if self.CP.carFingerprint in (
HONDA.PILOT, HONDA.PASSPORT,
HONDA.RIDGELINE) and CS.brake > 0.05:
brake_pressed = False
checks[
'brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev(
)
if self.CP.pcmCruise:
# On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state.
# On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but
# openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages).
if self.CP.carName == "honda" and self.CP.carFingerprint not in HONDA_BOSCH:
# only the rising edges are expected to match
if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled:
checks[
'controlsAllowed'] += not self.safety.get_controls_allowed(
)
else:
checks[
'controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed(
)
else:
# Check for enable events on rising edge of controls allowed
button_enable = any(evt.enable for evt in CS.events)
mismatch = button_enable != (
self.safety.get_controls_allowed()
and not controls_allowed_prev)
checks['controlsAllowed'] += mismatch
controls_allowed_prev = self.safety.get_controls_allowed()
if button_enable and not mismatch:
self.safety.set_controls_allowed(False)
if self.CP.carName == "honda":
checks[
'mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on(
)
CS_prev = CS
# TODO: add flag to toyota safety
if self.CP.carFingerprint == TOYOTA.SIENNA and checks[
'brakePressed'] < 25:
checks['brakePressed'] = 0
failed_checks = {k: v for k, v in checks.items() if v > 0}
self.assertFalse(
len(failed_checks),
f"panda safety doesn't agree with openpilot: {failed_checks}")
