def update(
self,
rate=40.43
): # in the future, pass in the current rate of long_mpc to accurate calculate disconnect time
phantomData = messaging.recv_one_or_none(self.phantom_Data_sock)
if phantomData is not None:
self.data = {
"status": phantomData.phantomData.status,
"speed": phantomData.phantomData.speed,
"angle": phantomData.phantomData.angle,
"time": phantomData.phantomData.time
}
self.last_phantom_data = dict(self.data)
self.last_receive_counter = 0
self.to_disable = not self.data["status"]
else:
if self.to_disable: # if last message is status: False, disable phantom mode, also disable by default
self.data = {"status": False, "speed": 0.0}
elif self.last_receive_counter > int(
rate * 1.0
) and self.timeout: # lost connection, don't disable. keep phantom on but set speed to 0
self.data = {
"status": True,
"speed": 0.0,
"angle": 0.0,
"time": 0.0
}
else: # if waiting between messages from app, message becomes none, this uses the data from last message
self.data = dict(self.last_phantom_data)
self.last_receive_counter = min(self.last_receive_counter + 1,
900) # don't infinitely increment
def calibrationd_thread(gctx=None, addr="127.0.0.1"):
context = zmq.Context()
live100 = messaging.sub_sock(context,
service_list['live100'].port,
addr=addr,
conflate=True)
orbfeatures = messaging.sub_sock(context,
service_list['orbFeatures'].port,
addr=addr,
conflate=True)
livecalibration = messaging.pub_sock(context,
service_list['liveCalibration'].port)
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
while 1:
of = messaging.recv_one(orbfeatures)
l100 = messaging.recv_one_or_none(live100)
new_vp = calibrator.handle_orb_features(of)
if DEBUG and new_vp is not None:
print 'got new vp', new_vp
if l100 is not None:
calibrator.handle_live100(l100)
if DEBUG:
calibrator.handle_debug()
calibrator.send_data(livecalibration)
def fingerprint(msgs, pub_socks, sub_socks):
print "start fingerprinting"
manager.prepare_managed_process("logmessaged")
manager.start_managed_process("logmessaged")
can = pub_socks["can"]
logMessage = messaging.sub_sock(service_list["logMessage"].port)
time.sleep(1)
messaging.drain_sock(logMessage)
# controlsd waits for a health packet before fingerprinting
msg = messaging.new_message()
msg.init("health")
pub_socks["health"].send(msg.to_bytes())
canmsgs = filter(lambda msg: msg.which() == "can", msgs)
for msg in canmsgs[:200]:
can.send(msg.as_builder().to_bytes())
time.sleep(0.005)
log = messaging.recv_one_or_none(logMessage)
if log is not None and "fingerprinted" in log.logMessage:
break
manager.kill_managed_process("logmessaged")
print "finished fingerprinting"
def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd,
hud_alert):
if not self.enable_camera:
return
if CS.camcan > 0:
if self.checksum == "NONE":
self.checksum = learn_checksum(self.packer, CS.lkas11)
print("Discovered Checksum", self.checksum)
if self.checksum == "NONE":
return
elif CS.steer_error == 1:
if self.checksum_learn_cnt > 200:
self.checksum_learn_cnt = 0
if self.checksum == "NONE":
print("Testing 6B Checksum")
self.checksum == "6B"
elif self.checksum == "6B":
print("Testing 7B Checksum")
self.checksum == "7B"
elif self.checksum == "7B":
print("Testing CRC8 Checksum")
self.checksum == "crc8"
else:
self.checksum == "NONE"
else:
self.checksum_learn_cnt += 1
force_enable = False
# I don't care about your opinion, deal with it!
if (CS.cstm_btns.get_button_status("alwon") > 0) and CS.acc_active:
enabled = True
force_enable = True
if (self.car_fingerprint in FEATURES["soft_disable"]
and CS.v_wheel < 16.8):
enabled = False
force_enable = False
if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1):
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (self.cnt % 100 == 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)
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, self.cnt, actuators)
if force_enable and not CS.acc_active:
apply_steer = int(
round(actuators.steer * SteerLimitParams.STEER_MAX))
else:
apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
# SPAS limit angle extremes for safety
apply_steer_ang_req = np.clip(actuators.steerAngle,
-1 * (SteerLimitParams.STEER_ANG_MAX),
SteerLimitParams.STEER_ANG_MAX)
# SPAS limit angle rate for safety
if abs(self.apply_steer_ang - apply_steer_ang_req) > 0.6:
if apply_steer_ang_req > self.apply_steer_ang:
self.apply_steer_ang += 0.5
else:
self.apply_steer_ang -= 0.5
else:
self.apply_steer_ang = apply_steer_ang_req
# Limit steer rate for safety
apply_steer = limit_steer_rate(apply_steer, self.apply_steer_last,
SteerLimitParams,
CS.steer_torque_driver)
if alca_enabled:
self.turning_signal_timer = 0
if self.turning_signal_timer > 0:
self.turning_signal_timer = self.turning_signal_timer - 1
turning_signal = 1
else:
turning_signal = 0
# Use LKAS or SPAS
if CS.mdps11_stat == 7 or CS.v_wheel > 2.7:
self.lkas = True
elif CS.v_wheel < 0.1:
self.lkas = False
if self.spas_present:
self.lkas = True
# If ALCA is disabled, and turning indicators are turned on, we do not want OP to steer,
if not enabled or (turning_signal and not alca_enabled):
if self.lkas:
apply_steer = 0
else:
self.apply_steer_ang = 0.0
self.en_cnt = 0
steer_req = 1 if enabled and self.lkas else 0
self.apply_steer_last = apply_steer
can_sends = []
self.lkas11_cnt = self.cnt % 0x10
self.clu11_cnt = self.cnt % 0x10
self.mdps12_cnt = self.cnt % 0x100
self.spas_cnt = self.cnt % 0x200
can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, \
enabled if self.lkas else False, False if CS.camcan == 0 else CS.lkas11, hud_alert, (CS.cstm_btns.get_button_status("cam") > 0), \
(False if CS.camcan == 0 else True), self.checksum))
if CS.camcan > 0:
can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \
CS.camcan, self.checksum))
# SPAS11 50hz
if (self.cnt % 2) == 0 and not self.spas_present:
if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7:
self.en_spas == 7
self.en_cnt = 0
if self.en_spas == 7 and self.en_cnt >= 8:
self.en_spas = 3
self.en_cnt = 0
if self.en_cnt < 8 and enabled and not self.lkas:
self.en_spas = 4
elif self.en_cnt >= 8 and enabled and not self.lkas:
self.en_spas = 5
if self.lkas or not enabled:
self.apply_steer_ang = CS.mdps11_strang
self.en_spas = 3
self.en_cnt = 0
self.mdps11_stat_last = CS.mdps11_stat
self.en_cnt += 1
can_sends.append(
create_spas11(self.packer, (self.spas_cnt / 2), self.en_spas,
self.apply_steer_ang, self.checksum))
# SPAS12 20Hz
if (self.cnt % 5) == 0 and not self.spas_present:
can_sends.append(create_spas12(self.packer))
# Force Disable
if pcm_cancel_cmd and (not force_enable):
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0))
elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5:
self.last_resume_cnt = self.cnt
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, 0))
# Speed Limit Related Stuff Lot's of comments for others to understand!
# Run this twice a second
if (self.cnt % 50) == 0:
if self.params.get("LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None:
# If Not Enabled, or cruise not set, allow auto speed adjustment again
if not (enabled and CS.acc_active_real):
self.speed_adjusted = False
# Attempt to read the speed limit from zmq
map_data = messaging.recv_one_or_none(self.map_data_sock)
# If we got a message
if map_data != None:
# See if we use Metric or dead kings extremeties for measurements, and set a variable to the conversion value
if bool(self.params.get("IsMetric")):
self.speed_conv = CV.MS_TO_KPH
else:
self.speed_conv = CV.MS_TO_MPH
# If the speed limit is valid
if map_data.liveMapData.speedLimitValid:
last_speed = self.map_speed
# Get the speed limit, and add the offset to it,
v_speed = (map_data.liveMapData.speedLimit +
float(self.params.get("SpeedLimitOffset")))
## Stolen curvature code from planner.py, and updated it for us
v_curvature = 45.0
if map_data.liveMapData.curvatureValid:
v_curvature = math.sqrt(
1.85 /
max(1e-4, abs(map_data.liveMapData.curvature)))
# Use the minimum between Speed Limit and Curve Limit, and convert it as needed
#self.map_speed = min(v_speed, v_curvature) * self.speed_conv
self.map_speed = v_speed * self.speed_conv
# Compare it to the last time the speed was read. If it is different, set the flag to allow it to auto set our speed
if last_speed != self.map_speed:
self.speed_adjusted = False
else:
# If it is not valid, set the flag so the cruise speed won't be changed.
self.map_speed = 0
self.speed_adjusted = True
else:
self.speed_adjusted = True
# Ensure we have cruise IN CONTROL, so we don't do anything dangerous, like turn cruise on
# Ensure the speed limit is within range of the stock cruise control capabilities
# Do the spamming 10 times a second, we might get from 0 to 10 successful
# Only do this if we have not yet set the cruise speed
if CS.acc_active_real and not self.speed_adjusted and self.map_speed > (
8.5 * self.speed_conv) and (self.cnt % 9 == 0
or self.cnt % 9 == 1):
# Use some tolerance because of Floats being what they are...
if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed *
1.005):
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL,
(1 if self.cnt % 9 == 1 else 0)))
elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed /
1.005):
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL,
(1 if self.cnt % 9 == 1 else 0)))
# If nothing needed adjusting, then the speed has been set, which will lock out this control
else:
self.speed_adjusted = True
### If Driver Overrides using accelerator (or gas for the antiquated), cancel auto speed adjustment
if CS.pedal_gas:
self.speed_adjusted = True
### Send messages to canbus
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
self.cnt += 1
def ui_thread(addr, frame_address):
context = zmq.Context()
# TODO: Detect car from replay and use that to select carparams
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {})
VM = VehicleModel(CP)
CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]])
vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]])
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
if HOR:
size = (640+384+640, 960)
write_x = 5
write_y = 680
else:
size = (640+384, 960+300)
write_x = 645
write_y = 970
pygame.display.set_caption("openpilot debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
alert1_font = pygame.font.SysFont("arial", 30)
alert2_font = pygame.font.SysFont("arial", 20)
info_font = pygame.font.SysFont("arial", 15)
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert()
cameraw_test_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24)
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y),0,8)
frame = context.socket(zmq.SUB)
frame.connect(frame_address or "tcp://%s:%d" % (addr, service_list['frame'].port))
frame.setsockopt(zmq.SUBSCRIBE, "")
carState = sub_sock(context, service_list['carState'].port, addr=addr, conflate=True)
plan = sub_sock(context, service_list['plan'].port, addr=addr, conflate=True)
carControl = sub_sock(context, service_list['carControl'].port, addr=addr, conflate=True)
radar_state_sock = sub_sock(context, service_list['radarState'].port, addr=addr, conflate=True)
liveCalibration = sub_sock(context, service_list['liveCalibration'].port, addr=addr, conflate=True)
controls_state_sock = sub_sock(context, service_list['controlsState'].port, addr=addr, conflate=True)
liveTracks = sub_sock(context, service_list['liveTracks'].port, addr=addr, conflate=True)
model = sub_sock(context, service_list['model'].port, addr=addr, conflate=True)
test_model = sub_sock(context, 8040, addr=addr, conflate=True)
liveMpc = sub_sock(context, service_list['liveMpc'].port, addr=addr, conflate=True)
liveParameters = sub_sock(context, service_list['liveParameters'].port, addr=addr, conflate=True)
v_ego, angle_steers, angle_steers_des, model_bias = 0., 0., 0., 0.
params_ao, params_ao_average, params_stiffness, params_sr = None, None, None, None
enabled = False
gas = 0.
accel_override = 0.
computer_gas = 0.
brake = 0.
steer_torque = 0.
curvature = 0.
computer_brake = 0.
plan_source = 'none'
long_control_state = 'none'
model_data = None
test_model_data = None
a_ego = 0.0
a_target = 0.0
d_rel, y_rel, lead_status = 0., 0., False
d_rel2, y_rel2, lead_status2 = 0., 0., False
v_ego, v_pid, v_cruise, v_override = 0., 0., 0., 0.
brake_lights = False
alert_text1, alert_text2 = "", ""
intrinsic_matrix = None
calibration = None
#img = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype='uint8')
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image
good_lt = None
lid_overlay_blank = get_blank_lid_overlay(UP)
img_offset = (0, 0)
if vision_test:
visiontest = VisionTest(FULL_FRAME_SIZE, MODEL_INPUT_SIZE, None)
# plots
name_to_arr_idx = { "gas": 0,
"computer_gas": 1,
"user_brake": 2,
"computer_brake": 3,
"v_ego": 4,
"v_pid": 5,
"angle_steers_des": 6,
"angle_steers": 7,
"steer_torque": 8,
"v_override": 9,
"v_cruise": 10,
"a_ego": 11,
"a_target": 12,
"accel_override": 13}
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
plot_ylims = [(-0.1, 1.1), (-5., 5.), (0., 75.), (-3.0, 2.0)]
plot_names = [["gas", "computer_gas", "user_brake", "computer_brake", "accel_override"],
["angle_steers", "angle_steers_des", "steer_torque"],
["v_ego", "v_override", "v_pid", "v_cruise"],
["a_ego", "a_target"]]
plot_colors = [["b", "b", "g", "r", "y"],
["b", "g", "r"],
["b", "g", "r", "y"],
["b", "r"]]
plot_styles = [["-", "-", "-", "-", "-"],
["-", "-", "-"],
["-", "-", "-", "-"],
["-", "-"]]
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True)
while 1:
list(pygame.event.get())
screen.fill((64,64,64))
lid_overlay = lid_overlay_blank.copy()
top_down = top_down_surface, lid_overlay
# ***** frame *****
fpkt = recv_one(frame)
yuv_img = fpkt.frame.image
if fpkt.frame.transform:
yuv_transform = np.array(fpkt.frame.transform).reshape(3,3)
else:
# assume frame is flipped
yuv_transform = np.array([
[-1.0, 0.0, FULL_FRAME_SIZE[0]-1],
[ 0.0, -1.0, FULL_FRAME_SIZE[1]-1],
[ 0.0, 0.0, 1.0]
])
if yuv_img and len(yuv_img) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3 // 2:
yuv_np = np.frombuffer(yuv_img, dtype=np.uint8).reshape(FULL_FRAME_SIZE[1] * 3 // 2, -1)
cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff)
cv2.warpAffine(imgff, np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2],
(img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
intrinsic_matrix = eon_intrinsics
else:
img.fill(0)
intrinsic_matrix = np.eye(3)
if calibration is not None and yuv_img and vision_test:
model_input_yuv = visiontest.transform_contiguous(yuv_img,
np.dot(yuv_transform, calibration.model_to_full_frame).reshape(-1).tolist())
cv2.cvtColor(
np.frombuffer(model_input_yuv, dtype=np.uint8).reshape(MODEL_INPUT_SIZE[1] * 3 // 2, -1),
cv2.COLOR_YUV2RGB_I420,
dst=imgw)
else:
imgw.fill(0)
imgw_test_model = imgw.copy()
# ***** controlsState *****
controls_state = recv_one_or_none(controls_state_sock)
if controls_state is not None:
v_ego = controls_state.controlsState.vEgo
angle_steers = controls_state.controlsState.angleSteers
model_bias = controls_state.controlsState.angleModelBias
curvature = controls_state.controlsState.curvature
v_pid = controls_state.controlsState.vPid
enabled = controls_state.controlsState.enabled
alert_text1 = controls_state.controlsState.alertText1
alert_text2 = controls_state.controlsState.alertText2
long_control_state = controls_state.controlsState.longControlState
cs = recv_one_or_none(carState)
if cs is not None:
gas = cs.carState.gas
brake_lights = cs.carState.brakeLights
a_ego = cs.carState.aEgo
brake = cs.carState.brake
v_cruise = cs.carState.cruiseState.speed
cc = recv_one_or_none(carControl)
if cc is not None:
v_override = cc.carControl.cruiseControl.speedOverride
computer_brake = cc.carControl.actuators.brake
computer_gas = cc.carControl.actuators.gas
steer_torque = cc.carControl.actuators.steer * 5.
angle_steers_des = cc.carControl.actuators.steerAngle
accel_override = cc.carControl.cruiseControl.accelOverride
p = recv_one_or_none(plan)
if p is not None:
a_target = p.plan.aTarget
plan_source = p.plan.longitudinalPlanSource
plot_arr[:-1] = plot_arr[1:]
plot_arr[-1, name_to_arr_idx['angle_steers']] = angle_steers
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = angle_steers_des
plot_arr[-1, name_to_arr_idx['gas']] = gas
plot_arr[-1, name_to_arr_idx['computer_gas']] = computer_gas
plot_arr[-1, name_to_arr_idx['user_brake']] = brake
plot_arr[-1, name_to_arr_idx['steer_torque']] = steer_torque
plot_arr[-1, name_to_arr_idx['computer_brake']] = computer_brake
plot_arr[-1, name_to_arr_idx['v_ego']] = v_ego
plot_arr[-1, name_to_arr_idx['v_pid']] = v_pid
plot_arr[-1, name_to_arr_idx['v_override']] = v_override
plot_arr[-1, name_to_arr_idx['v_cruise']] = v_cruise
plot_arr[-1, name_to_arr_idx['a_ego']] = a_ego
plot_arr[-1, name_to_arr_idx['a_target']] = a_target
plot_arr[-1, name_to_arr_idx['accel_override']] = accel_override
# ***** model ****
# live model
md = recv_one_or_none(model)
if md:
model_data = extract_model_data(md)
if model_data:
plot_model(model_data, VM, v_ego, curvature, imgw, calibration,
top_down)
if test_model is not None:
test_md = recv_one_or_none(test_model)
if test_md:
test_model_data = extract_model_data(test_md)
if test_model_data:
plot_model(test_model_data, VM, v_ego, curvature, imgw_test_model,
calibration, top_down, 215)
# MPC
mpc = recv_one_or_none(liveMpc)
if mpc:
draw_mpc(mpc, top_down)
# LiveParams
params = recv_one_or_none(liveParameters)
if params:
params_ao = params.liveParameters.angleOffset
params_ao_average = params.liveParameters.angleOffsetAverage
params_stiffness = params.liveParameters.stiffnessFactor
params_sr = params.liveParameters.steerRatio
# **** tracks *****
# draw all radar points
lt = recv_one_or_none(liveTracks)
if lt is not None:
good_lt = lt
if good_lt is not None:
maybe_update_radar_points(good_lt, top_down[1])
# ***** radarState *****
radar_state = recv_one_or_none(radar_state_sock)
if radar_state is not None:
d_rel = radar_state.radarState.leadOne.dRel + RDR_TO_LDR
y_rel = radar_state.radarState.leadOne.yRel
lead_status = radar_state.radarState.leadOne.status
d_rel2 = radar_state.radarState.leadTwo.dRel + RDR_TO_LDR
y_rel2 = radar_state.radarState.leadTwo.yRel
lead_status2 = radar_state.radarState.leadTwo.status
lcal = recv_one_or_none(liveCalibration)
if lcal is not None:
calibration_message = lcal.liveCalibration
extrinsic_matrix = np.asarray(calibration_message.extrinsicMatrix).reshape(3, 4)
warp_matrix = np.asarray(calibration_message.warpMatrix2).reshape(3, 3)
calibration = CalibrationTransformsForWarpMatrix(warp_matrix, intrinsic_matrix, extrinsic_matrix)
# draw red pt for lead car in the main img
if lead_status:
if calibration is not None:
dx, dy = draw_lead_on(img, d_rel, y_rel, img_offset, calibration, color=(192,0,0))
# draw red line for lead car
draw_lead_car(d_rel, top_down)
# draw red pt for lead car2 in the main img
if lead_status2:
if calibration is not None:
dx2, dy2 = draw_lead_on(img, d_rel2, y_rel2, img_offset, calibration, color=(192,0,0))
# draw red line for lead car
draw_lead_car(d_rel2, top_down)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0,1))
screen.blit(camera_surface, (0, 0))
# display alerts
alert_line1 = alert1_font.render(alert_text1, True, (255,0,0))
alert_line2 = alert2_font.render(alert_text2, True, (255,0,0))
screen.blit(alert_line1, (180, 150))
screen.blit(alert_line2, (180, 190))
if calibration is not None and img is not None:
cpw = warp_points(CalP, calibration.model_to_bb)
vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb)
pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1)
pygame.draw.circle(screen, BLUE, map(int, map(round, vanishing_pointw[0])), 2)
if HOR:
screen.blit(draw_plots(plot_arr), (640+384, 0))
else:
screen.blit(draw_plots(plot_arr), (0, 600))
pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1))
screen.blit(cameraw_surface, (320, 480))
pygame.surfarray.blit_array(cameraw_test_surface, imgw_test_model.swapaxes(0, 1))
screen.blit(cameraw_test_surface, (0, 480))
pygame.surfarray.blit_array(*top_down)
screen.blit(top_down[0], (640,0))
i = 0
SPACING = 25
# enabled
enabled_line = info_font.render("ENABLED", True, GREEN if enabled else BLACK)
screen.blit(enabled_line, (write_x, write_y + i * SPACING))
i += 1
# brake lights
brake_lights_line = info_font.render("BRAKE LIGHTS", True, RED if brake_lights else BLACK)
screen.blit(brake_lights_line, (write_x, write_y + i * SPACING))
i += 1
# speed
v_ego_line = info_font.render("SPEED: " + str(round(v_ego, 1)) + " m/s", True, YELLOW)
screen.blit(v_ego_line, (write_x, write_y + i * SPACING))
i += 1
# angle offset
model_bias_line = info_font.render("MODEL BIAS: " + str(round(model_bias, 2)) + " deg", True, YELLOW)
screen.blit(model_bias_line, (write_x, write_y + i * SPACING))
i += 1
# long control state
long_control_state_line = info_font.render("LONG CONTROL STATE: " + str(long_control_state), True, YELLOW)
screen.blit(long_control_state_line, (write_x, write_y + i * SPACING))
i += 1
# long mpc source
plan_source_line = info_font.render("LONG MPC SOURCE: " + str(plan_source), True, YELLOW)
screen.blit(plan_source_line, (write_x, write_y + i * SPACING))
i += 1
if params_ao is not None:
i += 1
angle_offset_avg_line = info_font.render("ANGLE OFFSET (AVG): " + str(round(params_ao_average, 2)) + " deg", True, YELLOW)
screen.blit(angle_offset_avg_line, (write_x, write_y + i * SPACING))
i += 1
angle_offset_line = info_font.render("ANGLE OFFSET (INSTANT): " + str(round(params_ao, 2)) + " deg", True, YELLOW)
screen.blit(angle_offset_line, (write_x, write_y + i * SPACING))
i += 1
angle_offset_line = info_font.render("STIFFNESS: " + str(round(params_stiffness * 100., 2)) + " %", True, YELLOW)
screen.blit(angle_offset_line, (write_x, write_y + i * SPACING))
i += 1
steer_ratio_line = info_font.render("STEER RATIO: " + str(round(params_sr, 2)), True, YELLOW)
screen.blit(steer_ratio_line, (write_x, write_y + i * SPACING))
i += 1
# this takes time...vsync or something
pygame.display.flip()
def mapsd_thread():
global last_gps
context = zmq.Context()
gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True)
gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True)
map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port)
cur_way = None
curvature_valid = False
curvature = None
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
while True:
gps = messaging.recv_one(gps_sock)
gps_ext = messaging.recv_one_or_none(gps_external_sock)
if gps_ext is not None:
gps = gps_ext.gpsLocationExternal
else:
gps = gps.gpsLocation
last_gps = gps
fix_ok = gps.flags & 1
if not fix_ok or last_query_result is None or not cache_valid:
cur_way = None
curvature = None
curvature_valid = False
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
map_valid = False
else:
map_valid = True
lat = gps.latitude
lon = gps.longitude
heading = gps.bearing
speed = gps.speed
query_lock.acquire()
cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
if cur_way is not None:
pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
xs = pnts[:, 0]
ys = pnts[:, 1]
road_points = [float(x) for x in xs], [float(y) for y in ys]
if speed < 10:
curvature_valid = False
if curvature_valid and pnts.shape[0] <= 3:
curvature_valid = False
# The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
if curvature_valid:
# Compute the curvature for each point
with np.errstate(divide='ignore'):
circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[radii < 10] = np.inf
curvature = 1. / radii
# Index of closest point
closest = np.argmin(np.linalg.norm(pnts, axis=1))
dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close
# Compute distance along path
dists = list()
dists.append(0)
for p, p_prev in zip(pnts, pnts[1:, :]):
dists.append(dists[-1] + np.linalg.norm(p - p_prev))
dists = np.asarray(dists)
dists = dists - dists[closest] + dist_to_closest
dists = dists[1:-1]
close_idx = np.logical_and(dists > 0, dists < 500)
dists = dists[close_idx]
curvature = curvature[close_idx]
if len(curvature):
# TODO: Determine left or right turn
curvature = np.nan_to_num(curvature)
# Outlier rejection
new_curvature = np.percentile(curvature, 90, interpolation='lower')
k = 0.6
upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature
in_turn_indices = curvature > 0.8 * new_curvature
if np.any(in_turn_indices):
dist_to_turn = np.min(dists[in_turn_indices])
else:
dist_to_turn = 999
else:
upcoming_curvature = 0.
dist_to_turn = 999
query_lock.release()
dat = messaging.new_message()
dat.init('liveMapData')
if last_gps is not None:
dat.liveMapData.lastGps = last_gps
if cur_way is not None:
dat.liveMapData.wayId = cur_way.id
# Seed limit
max_speed = cur_way.max_speed()
if max_speed is not None:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed
# TODO: use the function below to anticipate upcoming speed limits
#max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
#if max_speed_ahead is not None and max_speed_ahead_dist is not None:
# dat.liveMapData.speedLimitAheadValid = True
# dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
# dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist)
advisory_max_speed = cur_way.advisory_max_speed()
if advisory_max_speed is not None:
dat.liveMapData.speedAdvisoryValid = True
dat.liveMapData.speedAdvisory = advisory_max_speed
# Curvature
dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature)
dat.liveMapData.distToTurn = float(dist_to_turn)
if road_points is not None:
dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
if curvature is not None:
dat.liveMapData.roadCurvatureX = [float(x) for x in dists]
dat.liveMapData.roadCurvature = [float(x) for x in curvature]
dat.liveMapData.mapValid = map_valid
map_data_sock.send(dat.to_bytes())
def update(self, cp, cp_cam):
# copy can_valid
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
msg = messaging.recv_one_or_none(self.gps_location)
if msg is not None:
gps_pkt = msg.gpsLocationExternal
self.inaccuracy = gps_pkt.accuracy
self.prev_distance = self.distance
self.distance, self.includeradius, self.approachradius, self.speedlimit = gps_distance(
gps_pkt.latitude, gps_pkt.longitude, gps_pkt.altitude,
gps_pkt.accuracy)
#self.distance = 6371010*acos(sin(radians(gps_pkt.latitude))*sin(radians(48.12893908))+cos(radians(gps_pkt.latitude))*cos(radians(48.12893908))*cos(radians(gps_pkt.longitude-9.797879048)))
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.door_all_closed = not any([
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']
])
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
if self.CP.enableGasInterceptor:
self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
self.v_wheel = float(
np.mean([
self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl,
self.v_wheel_rr
]))
# Kalman filter
if abs(
self.v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
self.v_ego_raw = self.v_wheel
v_ego_x = self.v_ego_kf.update(self.v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
#self.standstill = not self.v_wheel > 0.001
self.standstill = False
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl[
"STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
self.blind_spot_side = cp.vl["DEBUG"]['BLINDSPOTSIDE']
if (cp.vl["DEBUG"]['BLINDSPOTD1'] >
10) or (cp.vl["DEBUG"]['BLINDSPOTD1'] > 10):
self.blind_spot_on = bool(1)
else:
self.blind_spot_on = bool(0)
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(
cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [
1, 5, 9, 17, 25
]
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_EPS']
if bool(cp.vl["JOEL_ID"]
['LANE_WARNING']) <> self.lane_departure_toggle_on_prev:
self.lane_departure_toggle_on = bool(
cp.vl["JOEL_ID"]['LANE_WARNING'])
if self.lane_departure_toggle_on:
self.cstm_btns.set_button_status("lka", 1)
else:
self.cstm_btns.set_button_status("lka", 0)
self.lane_departure_toggle_on_prev = self.lane_departure_toggle_on
else:
if self.cstm_btns.get_button_status("lka") == 0:
self.lane_departure_toggle_on = False
else:
self.lane_departure_toggle_on = True
self.distance_toggle = cp.vl["JOEL_ID"]['ACC_DISTANCE']
self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
if self.distance_toggle <> self.distance_toggle_prev:
if self.read_distance_lines == self.distance_toggle:
self.distance_toggle_prev = self.distance_toggle
else:
self.cstm_btns.set_button_status("tr", 1)
if self.read_distance_lines <> self.read_distance_lines_prev:
self.cstm_btns.set_button_status("tr", 0)
if self.read_distance_lines == 1:
self.UE.custom_alert_message(2,
"Following distance set to 0.9s",
200, 3)
if self.read_distance_lines == 2:
self.UE.custom_alert_message(2,
"Following distance set to 1.8s",
200, 3)
if self.read_distance_lines == 3:
self.UE.custom_alert_message(2,
"Following distance set to 2.7s",
200, 3)
self.read_distance_lines_prev = self.read_distance_lines
if bool(cp.vl["JOEL_ID"]['ACC_SLOW']) <> self.acc_slow_on_prev:
self.acc_slow_on = bool(cp.vl["JOEL_ID"]['ACC_SLOW'])
if self.acc_slow_on:
self.cstm_btns.set_button_status("slow", 1)
else:
self.cstm_btns.set_button_status("slow", 0)
self.acc_slow_on_prev = self.acc_slow_on
else:
if self.cstm_btns.get_button_status("slow") == 0:
self.acc_slow_on = False
else:
self.acc_slow_on = True
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = 0
if self.acc_slow_on:
self.v_cruise_pcm = max(7,
cp.vl["PCM_CRUISE_2"]['SET_SPEED'] - 34.0)
else:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.v_cruise_pcm = int(
min(self.v_cruise_pcm,
interp(abs(self.angle_steers), self.Angle, self.Angle_Speed)))
#print "distane"
#print self.distance
if self.distance < self.approachradius + self.includeradius:
print "speed"
print self.prev_distance - self.distance
#if speed is 5% higher than the speedlimit
if self.prev_distance - self.distance > self.speedlimit * 0.00263889:
if self.v_cruise_pcm > self.speedlimit:
self.v_cruise_pcm = self.speedlimit
if self.distance < self.includeradius:
print "inside"
if self.v_cruise_pcm > self.speedlimit:
self.v_cruise_pcm = self.speedlimit
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][
'LOW_SPEED_LOCKOUT'] == 2
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC']
or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
def manager_thread():
# now loop
global manager_sock
context = zmq.Context()
thermal_sock = messaging.sub_sock(context, service_list['thermal'].port)
gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True)
manager_sock = messaging.pub_sock(context, service_list['managerData'].port)
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
for p in persistent_processes:
start_managed_process(p)
# start frame
pm_apply_packages('enable')
system("am start -n ai.comma.plus.frame/.MainActivity")
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
params = Params()
logger_dead = False
while 1:
# get health of board, log this in "thermal"
msg = messaging.recv_sock(thermal_sock, wait=True)
gps = messaging.recv_one_or_none(gps_sock)
if gps:
if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839:
logger_dead = True
else:
logger_dead = False
# uploader is gated based on the phone temperature
if msg.thermal.thermalStatus >= ThermalStatus.yellow:
kill_managed_process("uploader")
else:
start_managed_process("uploader")
if msg.thermal.freeSpace < 0.18:
logger_dead = True
if msg.thermal.started:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
else:
start_managed_process(p)
else:
logger_dead = False
for p in car_started_processes:
kill_managed_process(p)
# check the status of all processes, did any of them die?
#for p in running:
# cloudlog.debug(" running %s %s" % (p, running[p]))
send_running_processes()
# is this still needed?
if params.get("DoUninstall") == "1":
break
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed,
speed_limit_ms, set_speed_limit_active, speed_limit_offset,
alca_enabled):
idx = self.pedal_idx
self.prev_speed_limit_kph = self.speed_limit_kph
######################################################################################
# Determine pedal "zero"
#
#save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH
######################################################################################
if (CS.torqueLevel < TORQUE_LEVEL_ACC
and CS.torqueLevel > TORQUE_LEVEL_DECEL
and CS.v_ego >= 10. * CV.MPH_TO_MS and
abs(CS.torqueLevel) < abs(self.lastTorqueForPedalForZeroTorque)
and self.prev_tesla_accel > 0.):
self.PedalForZeroTorque = self.prev_tesla_accel
self.lastTorqueForPedalForZeroTorque = CS.torqueLevel
#print ("Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque))
#print ("Torque level at detection %s" % (CS.torqueLevel))
#print ("Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH))
if set_speed_limit_active and speed_limit_ms > 0:
self.speed_limit_kph = (speed_limit_ms +
speed_limit_offset) * CV.MS_TO_KPH
if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph):
self.pedal_speed_kph = self.speed_limit_kph
# reset MovingAverage for fleet speed when speed limit changes
self.fleet_speed.reset_averager()
else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway)
self.speed_limit_kph = 0.
self.pedal_idx = (self.pedal_idx + 1) % 16
if not self.pcc_available or not enabled:
return 0., 0, idx
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the radarState feed
radSt = messaging.recv_one_or_none(self.radarState)
mapd = messaging.recv_one_or_none(self.live_map_data)
if radSt is not None:
self.lead_1 = radSt.radarState.leadOne
if _is_present(self.lead_1):
self.lead_last_seen_time_ms = _current_time_millis()
self.continuous_lead_sightings += 1
else:
self.continuous_lead_sightings = 0
v_ego = CS.v_ego
following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1)
accel_limits[0] = _decel_limit(accel_limits[0], CS.v_ego, self.lead_1,
CS, self.pedal_speed_kph)
jerk_limits = [
min(-0.1, accel_limits[0] / 2.),
max(0.1, accel_limits[1] / 2.)
] # TODO: make a separate lookup for jerk tuning
#accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP)
output_gb = 0
####################################################################
# this mode (Follow) uses the Follow logic created by JJ for ACC
#
# once the speed is detected we have to use our own PID to determine
# how much accel and break we have to do
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [
LongCtrlState.pid, LongCtrlState.stopping
]
# determine if pedal is pressed by human
self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed
self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 10
#reset PID if we just lifted foot of accelerator
if (not self.accelerator_pedal_pressed
) and self.prev_accelerator_pedal_pressed:
self.reset(CS.v_ego)
if self.enable_pedal_cruise and not self.accelerator_pedal_pressed:
self.v_pid = self.calc_follow_speed_ms(CS, alca_enabled)
if mapd is not None:
v_curve = max_v_in_mapped_curve_ms(mapd.liveMapData,
self.pedal_speed_kph)
if v_curve:
self.v_pid = min(self.v_pid, v_curve)
# take fleet speed into consideration
self.v_pid = min(
self.v_pid,
self.fleet_speed.adjust(
CS, self.pedal_speed_kph * CV.KPH_TO_MS, frame))
# cruise speed can't be negative even if user is distracted
self.v_pid = max(self.v_pid, 0.)
jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1,
self.v_pid * CV.MS_TO_KPH,
self.lead_last_seen_time_ms,
CS)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start,
self.a_acc_start,
self.v_pid,
accel_limits[1],
accel_limits[0],
jerk_limits[1],
jerk_limits[0], #jerk_max, jerk_min,
_DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = self.v_pid
# Interpolation of trajectory
self.a_acc_sol = self.a_acc_start + (_DT / _DT_MPC) * (
self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + _DT * (
self.a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
# we will try to feed forward the pedal position.... we might want to feed the last_output_gb....
# op feeds forward self.a_acc_sol
# it's all about testing now.
vTarget = clip(self.v_acc_sol, 0, self.v_cruise)
self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid)
feedforward = self.a_acc_sol
#feedforward = self.last_output_gb
t_go, t_brake = self.LoC.update(
self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0,
CS.standstill, False, self.v_cruise, vTarget,
self.vTargetFuture, feedforward, CS.CP)
output_gb = t_go - t_brake
#print ("Output GB Follow:", output_gb)
else:
self.reset(CS.v_ego)
#print ("PID reset")
output_gb = 0.
starting = self.LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.a_ego, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.v_ego
reset_accel = CS.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.v_pid = reset_speed
self.last_speed_kph = None
##############################################################
# This mode uses the longitudinal MPC built in OP
#
# we use the values from actuator.accel and actuator.brake
##############################################################
elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
output_gb = actuators.gas - actuators.brake
self.v_pid = -10.
self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(
output_gb, 0.,
_accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1,
self.prev_tesla_accel, CS))
MPC_BRAKE_MULTIPLIER = 6.
apply_brake = -clip(
output_gb * MPC_BRAKE_MULTIPLIER,
_brake_pedal_min(CS.v_ego, self.v_pid, self.lead_1, CS,
self.pedal_speed_kph), 0.)
# if speed is over 5mph, the "zero" is at PedalForZeroTorque; otherwise it is zero
pedal_zero = 0.
if CS.v_ego >= 5. * CV.MPH_TO_MS:
pedal_zero = self.PedalForZeroTorque
tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero)
tesla_accel = clip(apply_accel * (MAX_PEDAL_VALUE - pedal_zero), 0,
MAX_PEDAL_VALUE - pedal_zero)
tesla_pedal = tesla_brake + tesla_accel
tesla_pedal = self.pedal_hysteresis(tesla_pedal, enabled)
tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN,
self.prev_tesla_pedal + PEDAL_MAX_UP)
tesla_pedal = clip(tesla_pedal, 0.,
MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0.
enable_pedal = 1. if self.enable_pedal_cruise else 0.
self.torqueLevel_last = CS.torqueLevel
self.prev_tesla_pedal = tesla_pedal * enable_pedal
self.prev_tesla_accel = apply_accel * enable_pedal
self.prev_v_ego = CS.v_ego
return self.prev_tesla_pedal, enable_pedal, idx
y.append(np.ones(LEN)*np.nan)
lines.append(ax.plot(x[i], y[i])[0])
for l in lines:
l.set_marker("*")
cur_t = 0.
ax.legend(subs_name)
ax.set_xlabel('time [s]')
while 1:
print 1./(time.time() - cur_t)
cur_t = time.time()
for i, s in enumerate(subs):
#msg = messaging.recv_sock(s)
msg = messaging.recv_one_or_none(s)
if msg is not None:
x[i] = np.append(x[i], getattr(msg, 'logMonoTime') / float(1e9))
x[i] = np.delete(x[i], 0)
y[i] = np.append(y[i], recursive_getattr(msg, subs_name[i]))
y[i] = np.delete(y[i], 0)
lines[i].set_xdata(x[i])
lines[i].set_ydata(y[i])
ax.relim()
ax.autoscale_view(True, scaley=True, scalex=True)
fig.canvas.blit(ax.bbox)
fig.canvas.flush_events()
def mapsd_thread():
global last_gps
context = zmq.Context()
gps_sock = messaging.sub_sock(context,
service_list['gpsLocation'].port,
conflate=True)
gps_external_sock = messaging.sub_sock(
context, service_list['gpsLocationExternal'].port, conflate=True)
map_data_sock = messaging.pub_sock(context,
service_list['liveMapData'].port)
cur_way = None
curvature_valid = False
curvature = None
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
xx = np.arange(0, MAPS_LOOKAHEAD_DISTANCE, 10)
while True:
gps = messaging.recv_one(gps_sock)
gps_ext = messaging.recv_one_or_none(gps_external_sock)
if gps_ext is not None:
gps = gps_ext.gpsLocationExternal
else:
gps = gps.gpsLocation
last_gps = gps
fix_ok = gps.flags & 1
if not fix_ok or last_query_result is None:
cur_way = None
curvature = None
curvature_valid = False
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
else:
lat = gps.latitude
lon = gps.longitude
heading = gps.bearing
speed = gps.speed
query_lock.acquire()
cur_way = Way.closest(last_query_result, lat, lon, heading,
cur_way)
if cur_way is not None:
pnts, curvature_valid = cur_way.get_lookahead(
last_query_result, lat, lon, heading,
MAPS_LOOKAHEAD_DISTANCE)
xs = pnts[:, 0]
ys = pnts[:, 1]
road_points = map(float, xs), map(float, ys)
if speed < 10:
curvature_valid = False
if curvature_valid and pnts.shape[0] <= 3:
curvature_valid = False
# The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
if curvature_valid:
# Compute the curvature for each point
with np.errstate(divide='ignore'):
circles = [
circle_through_points(*p)
for p in zip(pnts, pnts[1:], pnts[2:])
]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[radii < 10] = np.inf
curvature = 1. / radii
# Index of closest point
closest = np.argmin(np.linalg.norm(pnts, axis=1))
dist_to_closest = pnts[
closest,
0] # We can use x distance here since it should be close
# Compute distance along path
dists = list()
dists.append(0)
for p, p_prev in zip(pnts, pnts[1:, :]):
dists.append(dists[-1] + np.linalg.norm(p - p_prev))
dists = np.asarray(dists)
dists = dists - dists[closest] + dist_to_closest
# TODO: Determine left or right turn
curvature = np.nan_to_num(curvature)
curvature_interp = np.interp(xx, dists[1:-1], curvature)
curvature_lookahead = curvature_interp[:int(
speed * LOOKAHEAD_TIME / 10)]
# Outlier rejection
new_curvature = np.percentile(curvature_lookahead, 90)
k = 0.9
upcoming_curvature = k * upcoming_curvature + (
1 - k) * new_curvature
in_turn_indices = curvature_interp > 0.8 * new_curvature
if np.any(in_turn_indices):
dist_to_turn = np.min(xx[in_turn_indices])
else:
dist_to_turn = 999
query_lock.release()
dat = messaging.new_message()
dat.init('liveMapData')
if last_gps is not None:
dat.liveMapData.lastGps = last_gps
if cur_way is not None:
dat.liveMapData.wayId = cur_way.id
# Seed limit
max_speed = cur_way.max_speed
if max_speed is not None:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed
# Curvature
dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature)
dat.liveMapData.distToTurn = float(dist_to_turn)
if road_points is not None:
dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
if curvature is not None:
dat.liveMapData.roadCurvatureX = map(float, xx)
dat.liveMapData.roadCurvature = map(float, curvature_interp)
map_data_sock.send(dat.to_bytes())
def update(self, CS, frame, ahbLead1):
tms_now = _current_time_millis()
ahbInfoMsg = self.ahbInfo.receive(non_blocking=True)
frameInfoMsg = messaging.recv_one_or_none(self.frameInfo)
if ahbInfoMsg is not None:
self.ahbInfoData = tesla.AHBinfo.from_bytes(ahbInfoMsg)
if frameInfoMsg is not None:
self.frameInfoData = frameInfoMsg.frame
frameInfoGain = self.frameInfoData.globalGain
exposureTime = self.frameInfoData.androidCaptureResult.exposureTime
frameDuration = self.frameInfoData.androidCaptureResult.frameDuration
if frameInfoGain != self.frameInfoGain:
self.frameInfoGain = frameInfoGain
_debug("AHB Camera has new data [ frame - " +
str(self.frameInfoData.frameId) + "] = " +
str(frameInfoGain) + ", exposure = " + str(exposureTime) +
", frame duration = " + str(frameDuration))
#if AHB not enabled, then return OFF
if CS.ahbEnabled != 1:
_debug("AHB not enabled in CID")
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if stalk is not in the high beam position, return UNDECIDED
if (CS.ahbHighBeamStalkPosition != 1):
_debug("High Beam not on")
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA
self.ahbIsEnabled = False
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
else:
self.ahbIsEnabled = True
#if moving below 10mph take no decision, then return undecided
if CS.v_ego < 4.47:
_debug("moving too slow for decision")
highLowBeamStatus = self.prev_highLowBeamStatus
highLowBeamReason = self.prev_highLowBeamReason
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
if self.ahbInfoData is None:
_debug("No radar info")
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
# if gain less than max, we might see incoming car
if self.frameInfoGain < 510:
_debug("OP camera gain < 510")
self.time_last_car_detected = tms_now
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if lead car detected by radarD, i.e. OP has Lead, then reset timer and return OFF
if False and ahbLead1 is not None:
_debug("OP detected car")
self.time_last_car_detected = tms_now
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if lead car detected by radar, then reset timer and return OFF
if self.ahbInfoData.radarCarDetected:
_debug("Radar detected car")
self.time_last_car_detected = tms_now
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if lead car detected by vision, then reset timer and return OFF
if self.ahbInfoData.cameraCarDetected:
_debug("Vision detected car")
self.time_last_car_detected = tms_now
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_VISION_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if still waiting for the delay after car detected, send off
if (tms_now - self.time_last_car_detected < 1000 * CS.ahbOffDuration):
_debug("Waiting for time delay since last car")
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#we got here, high beam should be on
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
highLowBeamReason = AHBReason.HIGH_BEAM_ON
_debug("All conditions met, turn lights ON")
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
def update_acc(self, enabled, CS, frame, actuators, pcm_speed,
speed_limit_kph, set_speed_limit_active,
speed_limit_offset):
# Adaptive cruise control
self.prev_speed_limit_kph = self.speed_limit_kph
if set_speed_limit_active and speed_limit_kph > 0:
self.speed_limit_kph = speed_limit_kph + speed_limit_offset
if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph):
self.acc_speed_kph = self.speed_limit_kph
self.fleet_speed.reset_averager()
else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway)
self.speed_limit_kph = 0.
current_time_ms = _current_time_millis()
if CruiseButtons.should_be_throttled(CS.cruise_buttons):
self.human_cruise_action_time = current_time_ms
button_to_press = None
# If ACC is disabled, disengage traditional cruise control.
if ((self.prev_enable_adaptive_cruise)
and (not self.enable_adaptive_cruise)
and (CS.pcm_acc_status == CruiseState.ENABLED)):
button_to_press = CruiseButtons.CANCEL
#if non addaptive and we just engaged ACC but pcm is not engaged, then engage
if (not self.adaptive) and self.enable_adaptive_cruise and (
CS.pcm_acc_status != CruiseState.ENABLED):
button_to_press = CruiseButtons.MAIN
#if plain cc, not adaptive, then just return None or Cancel
if (not self.adaptive) and self.enable_adaptive_cruise:
self.acc_speed_kph = CS.v_cruise_actual #if not CS.imperial_speed_units else CS.v_cruise_actual * CV.MPH_TO_KPH
return button_to_press
#disengage if cruise is canceled
if (not self.enable_adaptive_cruise) and (CS.pcm_acc_status >= 2) and (
CS.pcm_acc_status <= 4):
button_to_press = CruiseButtons.CANCEL
lead_1 = None
#if enabled:
lead = messaging.recv_one_or_none(self.radarState)
if lead is not None:
lead_1 = lead.radarState.leadOne
if lead_1.dRel:
self.lead_last_seen_time_ms = current_time_ms
if self.enable_adaptive_cruise and enabled: # and (button_to_press == None):
if CS.cstm_btns.get_button_label2(
ACCMode.BUTTON_NAME) in ["OP", "AutoOP"]:
button_to_press = self._calc_button(CS, pcm_speed)
self.new_speed = pcm_speed * CV.MS_TO_KPH
else:
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the radarState feed
button_to_press = self._calc_follow_button(
CS, lead_1, speed_limit_kph, set_speed_limit_active,
speed_limit_offset, frame)
if button_to_press:
self.automated_cruise_action_time = current_time_ms
# If trying to slow below the min cruise speed, just cancel cruise.
# This prevents a SCCM crash which is triggered by repeatedly pressing
# stalk-down when already at min cruise speed.
if (CruiseButtons.is_decel(button_to_press)
and CS.v_cruise_actual - 1 <
self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH):
button_to_press = CruiseButtons.CANCEL
if button_to_press == CruiseButtons.CANCEL:
self.fast_decel_time = current_time_ms
# Debug logging (disable in production to reduce latency of commands)
#print "***ACC command: %s***" % button_to_press
return button_to_press
speed_txt = ax.text(-500, 900, '')
curv_txt = ax.text(-500, 775, '')
ax = fig.add_subplot(2, 1, 2)
ax.set_title('Curvature')
curvature_plt, = ax.plot([0.0], [0.0], "--xk")
ax.set_xlim([0, 500])
ax.set_ylim([0, 1e-2])
ax.set_xlabel('Distance along path [m]')
ax.set_ylabel('Curvature [1/m]')
ax.grid(True)
plt.show()
while True:
m = messaging.recv_one_or_none(live_map_sock)
p = messaging.recv_one_or_none(plan_sock)
if p is not None:
v = p.plan.vCurvature * CV.MS_TO_MPH
speed_txt.set_text('Desired curvature speed: %.2f mph' % v)
if m is not None:
print("Current way id: %d" % m.liveMapData.wayId)
curv_txt.set_text('Curvature valid: %s Dist: %03.0f m\nSpeedlimit valid: %s Speed: %.0f mph' %
(str(m.liveMapData.curvatureValid),
m.liveMapData.distToTurn,
str(m.liveMapData.speedLimitValid),
m.liveMapData.speedLimit * CV.MS_TO_MPH))
points_plt.set_xdata(m.liveMapData.roadX)
points_plt.set_ydata(m.liveMapData.roadY)
def update(self, 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,leftLaneVisible,rightLaneVisible):
if (not enabled) and (self.ALCA.laneChange_cancelled):
self.ALCA.laneChange_cancelled = False
self.ALCA.laneChange_cancelled_counter = 0
self.warningNeeded = 1
if self.warningCounter > 0:
self.warningCounter = self.warningCounter - 1
if self.warningCounter == 0:
self.warningNeeded = 1
if self.warningCounter == 0 or not enabled:
# when zero reset all warnings
self.DAS_222_accCameraBlind = 0 #we will see what we can use this for
self.DAS_219_lcTempUnavailableSpeed = 0
self.DAS_220_lcTempUnavailableRoad = 0
self.DAS_221_lcAborting = 0
self.DAS_211_accNoSeatBelt = 0
self.DAS_207_lkasUnavailable = 0 #use for manual not in drive?
self.DAS_208_rackDetected = 0 #use for low battery?
self.DAS_202_noisyEnvironment = 0 #use for planner error?
self.DAS_025_steeringOverride = 0 #use for manual steer?
self.DAS_206_apUnavailable = 0 #Ap disabled from CID
if CS.keepEonOff:
if CS.cstm_btns.get_button_status("dsp") != 9:
CS.cstm_btns.set_button_status("dsp",9)
else:
if CS.cstm_btns.get_button_status("dsp") != 1:
CS.cstm_btns.set_button_status("dsp",1)
# """ Controls thread """
if not CS.useTeslaMapData:
if self.speedlimit is None:
self.speedlimit = messaging.sub_sock('liveMapData', conflate=True)
# *** 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 ***
# Prevent steering while stopped
MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 100 == 0):
CS.cstm_btns.send_button_info()
#read speed limit params
if CS.hasTeslaIcIntegration:
self.set_speed_limit_active = True
self.speed_limit_offset = CS.userSpeedLimitOffsetKph
else:
self.set_speed_limit_active = (self.params.get("SpeedLimitOffset") is not None) and (self.params.get("LimitSetSpeed") == "1")
if self.set_speed_limit_active:
self.speed_limit_offset = float(self.params.get("SpeedLimitOffset"))
if not self.isMetric:
self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS
else:
self.speed_limit_offset = 0.
if CS.useTeslaGPS and (frame % 10 == 0):
if self.gpsLocationExternal is None:
self.gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
sol = gen_solution(CS)
sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
self.gpsLocationExternal.send(sol.to_bytes())
#get pitch/roll/yaw every 0.1 sec
if (frame %10 == 0):
(self.accPitch, self.accRoll, self.accYaw),(self.magPitch, self.magRoll, self.magYaw),(self.gyroPitch, self.gyroRoll, self.gyroYaw) = self.GYRO.update(CS.v_ego,CS.a_ego,CS.angle_steers)
CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw,self.magPitch, self.magRoll, self.magYaw,self.gyroPitch, self.gyroRoll, self.gyroYaw)
# Update statuses for custom buttons every 0.1 sec.
if (frame % 10 == 0):
self.ALCA.update_status((CS.cstm_btns.get_button_status("alca") > 0) and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or (CS.hasTeslaIcIntegration and CS.alcaEnabled)))
self.blinker.update_state(CS, frame)
# update PCC module info
pedal_can_sends = self.PCC.update_stat(CS, frame)
if self.PCC.pcc_available:
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# update CS.v_cruise_pcm based on module selected.
speed_uom_kph = 1.
if CS.imperial_speed_units:
speed_uom_kph = CV.KPH_TO_MPH
if self.ACC.enable_adaptive_cruise:
CS.v_cruise_pcm = self.ACC.acc_speed_kph * speed_uom_kph
elif self.PCC.enable_pedal_cruise:
CS.v_cruise_pcm = self.PCC.pedal_speed_kph * speed_uom_kph
else:
CS.v_cruise_pcm = max(0., CS.v_ego * CV.MS_TO_KPH) * speed_uom_kph
self.alca_enabled = self.ALCA.update(enabled, CS, actuators, self.alcaStateData, frame, self.blinker)
self.should_ldw = self._should_ldw(CS, frame)
apply_angle = -actuators.steerAngle # Tesla is reversed vs OP.
# Update HSO module info.
human_control = self.HSO.update_stat(self,CS, enabled, actuators, frame)
human_lane_changing = CS.turn_signal_stalk_state > 0 and not self.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)
#BB disable limits to test 0.5.8
# 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
# Send CAN commands.
can_sends = []
#if using radar, we need to send the VIN
if CS.useTeslaRadar and (frame % 100 == 0):
useRadar=0
if CS.useTeslaRadar:
useRadar=1
can_sends.append(teslacan.create_radar_VIN_msg(self.radarVin_idx,CS.radarVIN,1,0x108,useRadar,CS.radarPosition,CS.radarEpasType))
self.radarVin_idx += 1
self.radarVin_idx = self.radarVin_idx % 3
#First we emulate DAS.
# DAS_longC_enabled (1),DAS_speed_override (1),DAS_apUnavailable (1), DAS_collision_warning (1), DAS_op_status (4)
# DAS_speed_kph(8),
# DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (3),
# DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5),
# DAS_acc_speed_limit (8),
# DAS_speed_limit_units(8)
#send fake_das data as 0x553
# TODO: forward collision warning
if frame % 10 == 0:
speedlimitMsg = None
if self.speedlimit is not None:
speedlimitMsg = messaging.recv_one_or_none(self.speedlimit)
icLeadsMsg = self.icLeads.receive(non_blocking=True)
radarStateMsg = messaging.recv_one_or_none(self.radarState)
alcaStateMsg = self.alcaState.receive(non_blocking=True)
pathPlanMsg = messaging.recv_one_or_none(self.pathPlan)
icCarLRMsg = self.icCarLR.receive(non_blocking=True)
trafficeventsMsgs = None
if self.trafficevents is not None:
trafficeventsMsgs = messaging.recv_sock(self.trafficevents)
if CS.hasTeslaIcIntegration:
self.speed_limit_ms = CS.speed_limit_ms
if (speedlimitMsg is not None) and not CS.useTeslaMapData:
lmd = speedlimitMsg.liveMapData
self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0
if icLeadsMsg is not None:
self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg)
if radarStateMsg is not None:
#to show lead car on IC
if self.icLeadsData is not None:
can_messages = self.showLeadCarOnICCanMessage(radarStateMsg = radarStateMsg)
can_sends.extend(can_messages)
if alcaStateMsg is not None:
self.alcaStateData = tesla.ALCAState.from_bytes(alcaStateMsg)
if pathPlanMsg is not None:
#to show curvature and lanes on IC
if self.alcaStateData is not None:
self.handlePathPlanSocketForCurvatureOnIC(pathPlanMsg = pathPlanMsg, alcaStateData = self.alcaStateData,CS = CS)
if icCarLRMsg is not None:
can_messages = self.showLeftAndRightCarsOnICCanMessages(icCarLRMsg = tesla.ICCarsLR.from_bytes(icCarLRMsg))
can_sends.extend(can_messages)
if trafficeventsMsgs is not None:
can_messages = self.handleTrafficEvents(trafficEventsMsgs = trafficeventsMsgs)
can_sends.extend(can_messages)
op_status = 0x02
hands_on_state = 0x00
forward_collision_warning = 0 #1 if needed
if hud_alert == AH.FCW:
forward_collision_warning = hud_alert[1]
if forward_collision_warning > 1:
forward_collision_warning = 1
#cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold
cc_state = 1
alca_state = 0x00
speed_override = 0
collision_warning = 0x00
speed_control_enabled = 0
accel_min = -15
accel_max = 5
acc_speed_kph = 0
send_fake_warning = False
send_fake_msg = False
if enabled:
#self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
alca_state = 0x01
if self.opState == 0:
op_status = 0x02
if self.opState == 1:
op_status = 0x03
if self.opState == 2:
op_status = 0x08
if self.opState == 3:
op_status = 0x01
if self.opState == 5:
op_status = 0x03
if self.blinker.override_direction > 0:
alca_state = 0x08 + self.blinker.override_direction
elif (self.lLine > 1) and (self.rLine > 1):
alca_state = 0x08
elif (self.lLine > 1):
alca_state = 0x06
elif (self.rLine > 1):
alca_state = 0x07
else:
alca_state = 0x01
#canceled by user
if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0):
alca_state = 0x14
#min speed for ALCA
if (CS.CL_MIN_V > CS.v_ego):
alca_state = 0x05
#max angle for ALCA
if (abs(actuators.steerAngle) >= CS.CL_MAX_A):
alca_state = 0x15
if not enable_steer_control:
#op_status = 0x08
hands_on_state = 0x02
if hud_alert == AH.STEER:
if snd_chime == CM.MUTE:
hands_on_state = 0x03
else:
hands_on_state = 0x05
if self.PCC.pcc_available:
acc_speed_kph = self.PCC.pedal_speed_kph
if hud_alert == AH.FCW:
collision_warning = hud_alert[1]
if collision_warning > 1:
collision_warning = 1
#use disabling for alerts/errors to make them aware someting is goin going on
if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW):
op_status = 0x08
if self.ACC.enable_adaptive_cruise:
acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH
if (self.PCC.pcc_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise):
speed_control_enabled = 1
cc_state = 2
if not self.ACC.adaptive:
cc_state = 3
CS.speed_control_enabled = 1
else:
CS.speed_control_enabled = 0
if (CS.pcm_acc_status == 4):
#car CC enabled but not OP, display the HOLD message
cc_state = 3
else:
if (CS.pcm_acc_status == 4):
cc_state = 3
if enabled:
if frame % 2 == 0:
send_fake_msg = True
if frame % 25 == 0:
send_fake_warning = True
else:
if frame % 23 == 0:
send_fake_msg = True
if frame % 60 == 0:
send_fake_warning = True
if frame % 10 == 0:
can_sends.append(teslacan.create_fake_DAS_obj_lane_msg(self.leadDx,self.leadDy,self.leadClass,self.rLine,self.lLine,self.curv0,self.curv1,self.curv2,self.curv3,self.laneRange,self.laneWidth))
speed_override = 0
if (CS.pedal_interceptor_value > 10) and (cc_state > 1):
speed_override = 0 #force zero for now
if (not enable_steer_control) and op_status == 3:
#hands_on_state = 0x03
self.DAS_219_lcTempUnavailableSpeed = 1
self.warningCounter = 100
self.warningNeeded = 1
if enabled and self.ALCA.laneChange_cancelled and (not CS.steer_override) and (CS.turn_signal_stalk_state == 0) and (self.ALCA.laneChange_cancelled_counter > 0):
self.DAS_221_lcAborting = 1
self.warningCounter = 300
self.warningNeeded = 1
if CS.hasTeslaIcIntegration:
highLowBeamStatus,highLowBeamReason,ahbIsEnabled = self.AHB.update(CS,frame,self.ahbLead1)
if frame % 5 == 0:
self.cc_counter = (self.cc_counter + 1) % 40 #use this to change status once a second
self.fleet_speed_state = 0x00 #fleet speed unavailable
if FleetSpeed.is_available(CS):
if self.ACC.fleet_speed.is_active(frame) or self.PCC.fleet_speed.is_active(frame):
self.fleet_speed_state = 0x02 #fleet speed enabled
else:
self.fleet_speed_state = 0x01 #fleet speed available
can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,self.fleet_speed_state))
if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02):
CS.v_cruise_pcm = CS.v_cruise_pcm + 1
send_fake_msg = True
if (self.cc_counter == 3):
send_fake_msg = True
if send_fake_msg:
if enable_steer_control and op_status == 3:
op_status = 0x5
park_brake_request = int(CS.ahbEnabled)
if park_brake_request == 1:
print("Park Brake Request received")
adaptive_cruise = 1 if (not self.PCC.pcc_available and self.ACC.adaptive) or self.PCC.pcc_available else 0
can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \
acc_speed_kph, \
self.blinker.override_direction,forward_collision_warning, adaptive_cruise, hands_on_state, \
cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \
CS.v_cruise_pcm,
CS.DAS_fusedSpeedLimit,
apply_angle,
1 if enable_steer_control else 0,
park_brake_request))
if send_fake_warning or (self.opState == 2) or (self.opState == 5) or (self.stopSignWarning != self.stopSignWarning_last) or (self.stopLightWarning != self.stopLightWarning_last) or (self.warningNeeded == 1) or (frame % 100 == 0):
#if it's time to send OR we have a warning or emergency disable
can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \
self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \
self.stopSignWarning, self.stopLightWarning, \
self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \
self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,0,CS.useWithoutHarness))
self.stopLightWarning_last = self.stopLightWarning
self.stopSignWarning_last = self.stopSignWarning
self.warningNeeded = 0
# end of DAS emulation """
if frame % 100 == 0: # and CS.hasTeslaIcIntegration:
#IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake
can_sends.append(teslacan.create_fake_IC_msg())
# send enabled ethernet every 0.2 sec
if frame % 20 == 0:
can_sends.append(teslacan.create_enabled_eth_msg(1))
if (not self.PCC.pcc_available) and frame % 5 == 0: # acc processed at 20Hz
cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \
self.speed_limit_ms * CV.MS_TO_KPH,
self.set_speed_limit_active, self.speed_limit_offset)
if cruise_btn:
cruise_msg = teslacan.create_cruise_adjust_msg(
spdCtrlLvr_stat=cruise_btn,
turnIndLvr_Stat= 0,
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.pcc_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, \
self.speed_limit_ms,
self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled)
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
return pedal_can_sends + can_sends
