def steer_thread():
context = zmq.Context()
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
CI, CP = get_car(logcan, sendcan, None)
print("got car", CP.carName)
CC = car.CarControl.new_message()
i = 0
rate = 0.001
direction = 1
while True:
# send
CS = CI.update(CC)
actuators = car.CarControl.Actuators.new_message()
if i > 0.9 and direction == 1:
direction = -1
if i < -0.9 and direction == -1:
direction = 1
i += rate * direction
axis_3 = clip(i * 1.05, -1., 1.) # -1 to 1
actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg
print("steer", actuators.steer)
CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle
CC.enabled = True
CI.apply(CC)
# 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())
# Limit to 100 frames per second
time.sleep(0.01)
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 main(gctx=None):
# setup logentries. we forward log messages to it
le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17"
le_handler = LogentriesHandler(le_token, use_tls=False, verbose=False)
le_level = 20 #logging.INFO
ctx = zmq.Context().instance()
sock = ctx.socket(zmq.PULL)
sock.bind("ipc:///tmp/logmessage")
# and we publish them
pub_sock = messaging.pub_sock(service_list['logMessage'].port)
while True:
dat = b''.join(sock.recv_multipart())
dat = dat.decode('utf8')
# print "RECV", repr(dat)
levelnum = ord(dat[0])
dat = dat[1:]
if levelnum >= le_level:
# push to logentries
# TODO: push to athena instead
le_handler.emit_raw(dat)
# then we publish them
msg = messaging.new_message()
msg.logMessage = dat
pub_sock.send(msg.to_bytes())
def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked = CP.carName == "mock"
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(
'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
if can_sock is None:
can_sock = messaging.sub_sock(service_list['can'].port)
if sm is None:
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
# *** publish radarState and liveTracks
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(mocked)
has_radar = not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def update(self, cs, sm, can_index, can_count):
self.frame_count += 1
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.valid = cs.canValid
if cs.camLeft.frame != self.stock_cam_frame_prev and cs.camLeft.frame == cs.camFarRight.frame:
self.stock_cam_frame_prev = cs.camLeft.frame
gps = sm['gpsLocationExternal']
#sm.update(0)
cs.gpsLocation.longitude = gps.longitude
cs.gpsLocation.latitude = gps.latitude
cs.gpsLocation.altitude = gps.altitude
cs.gpsLocation.flags = gps.flags
vNED = [float(x) for x in gps.vNED]
if len(vNED) == 0:
vNED = [0, 0, 0]
cs.gpsLocation.vNED = vNED
cs.gpsLocation.bearing = gps.bearing
cs.gpsLocation.speed = gps.speed
cs.gpsLocation.accuracy = gps.accuracy
cs.gpsLocation.verticalAccuracy = gps.verticalAccuracy
cs.gpsLocation.bearingAccuracy = gps.bearingAccuracy
cs.gpsLocation.speedAccuracy = gps.speedAccuracy
cs.gpsLocation.timestamp = gps.timestamp
cs_send.carState = cs
self.cs_prev.append(cs_send.to_bytes())
self.carstate.send_multipart(self.cs_prev)
self.cs_prev.clear()
else:
cs_send.carState = cs
self.cs_prev.append(cs_send.to_bytes())
def main(gctx):
# setup logentries. we forward log messages to it
le_token = "bc65354a-b887-4ef4-8525-15dd51230e8c"
le_handler = LogentriesHandler(le_token, use_tls=False)
le_level = 20 #logging.INFO
ctx = zmq.Context()
sock = ctx.socket(zmq.PULL)
sock.bind("ipc:///tmp/logmessage")
# and we publish them
pub_sock = messaging.pub_sock(ctx, service_list['logMessage'].port)
while True:
dat = ''.join(sock.recv_multipart())
# print "RECV", repr(dat)
levelnum = ord(dat[0])
dat = dat[1:]
if levelnum >= le_level:
# push to logentries
le_handler.emit_raw(dat)
# then we publish them
msg = messaging.new_message()
msg.logMessage = dat
pub_sock.send(msg.to_bytes())
def run_following_distance_simulation(v_lead, t_end=200.0):
dt = 0.2
t = 0.
x_lead = 200.0
x_ego = 0.0
v_ego = v_lead
a_ego = 0.0
v_cruise_setpoint = v_lead + 10.
pm = FakePubMaster()
mpc = LongitudinalMpc(1)
first = True
while t < t_end:
# Run cruise control
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, False)
]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0], dt)
# Setup CarState
CS = messaging.new_message()
CS.init('carState')
CS.carState.vEgo = v_ego
CS.carState.aEgo = a_ego
# Setup lead packet
lead = log.RadarState.LeadData.new_message()
lead.status = True
lead.dRel = x_lead - x_ego
lead.vLead = v_lead
lead.aLeadK = 0.0
# Run MPC
mpc.set_cur_state(v_ego, a_ego)
if first: # Make sure MPC is converged on first timestep
for _ in range(20):
mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
# Choose slowest of two solutions
if v_cruise < mpc.v_mpc:
v_ego, a_ego = v_cruise, a_cruise
else:
v_ego, a_ego = mpc.v_mpc, mpc.a_mpc
# Update state
x_lead += v_lead * dt
x_ego += v_ego * dt
t += dt
first = False
return x_lead - x_ego
def plannerd_thread(gctx):
context = zmq.Context()
poller = zmq.Poller()
carstate = messaging.sub_sock(context, service_list['carState'].port, poller)
live20 = messaging.sub_sock(context, service_list['live20'].port)
model = messaging.sub_sock(context, service_list['model'].port)
plan = messaging.pub_sock(context, service_list['plan'].port)
# wait for stats about the car to come in from controls
cloudlog.info("plannerd is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("plannerd got CarParams")
CS = None
PP = PathPlanner(model)
AC = AdaptiveCruise(live20)
# start the loop
set_realtime_priority(2)
# this runs whenever we get a packet that can change the plan
while True:
polld = poller.poll(timeout=1000)
for sock, mode in polld:
if mode != zmq.POLLIN or sock != carstate:
continue
cur_time = sec_since_boot()
CS = messaging.recv_sock(carstate).carState
PP.update(cur_time, CS.vEgo)
# LoC.v_pid -> CS.vEgo
# TODO: is this change okay?
AC.update(cur_time, CS.vEgo, CS.steeringAngle, CS.vEgo, CP)
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
# lateral plan
plan_send.plan.lateralValid = not PP.dead
if plan_send.plan.lateralValid:
plan_send.plan.dPoly = map(float, PP.d_poly)
# longitudal plan
plan_send.plan.longitudinalValid = not AC.dead
if plan_send.plan.longitudinalValid:
plan_send.plan.vTarget = float(AC.v_target_lead)
plan_send.plan.aTargetMin = float(AC.a_target[0])
plan_send.plan.aTargetMax = float(AC.a_target[1])
plan_send.plan.jerkFactor = float(AC.jerk_factor)
plan_send.plan.hasLead = AC.has_lead
plan.send(plan_send.to_bytes())
def broadcast_data(self, status, speed, angle, time):
status = True if status == "true" or status is True else False
data = messaging.new_message()
data.init('phantomData')
data.phantomData.status = status
data.phantomData.speed = speed
data.phantomData.angle = angle
data.phantomData.time = time
self.phantomData_sock.send(data.to_bytes())
def can_list_to_can_capnp(can_msgs):
dat = messaging.new_message()
dat.init('can', len(can_msgs))
for i, can_msg in enumerate(can_msgs):
dat.can[i].address = can_msg[0]
dat.can[i].busTime = can_msg[1]
dat.can[i].dat = can_msg[2]
dat.can[i].src = can_msg[3]
return dat
def update(self, CS, LoC):
#print "===>>> File: controls/lib/planner.py; FUnction: update"
cur_time = sec_since_boot()
md = messaging.recv_sock(self.model)
#print "============ Planner"
#print md
if md is not None:
self.last_md_ts = md.logMonoTime
# print "========= frameId"
# print md.model.frameId
# else:
# print "======== None"
l20 = messaging.recv_sock(self.live20)
if l20 is not None:
self.last_l20_ts = l20.logMonoTime
self.PP.update(cur_time, CS.vEgo, md)
# LoC.v_pid -> CS.vEgo
# TODO: is this change okay?
self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP,
l20)
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.PP.dead
if plan_send.plan.lateralValid:
plan_send.plan.dPoly = map(float, self.PP.d_poly)
#else:
#print "====================No Vision Data========"
# longitudal plan
plan_send.plan.longitudinalValid = not self.AC.dead
if plan_send.plan.longitudinalValid:
plan_send.plan.vTarget = float(self.AC.v_target_lead)
plan_send.plan.aTargetMin = float(self.AC.a_target[0])
plan_send.plan.aTargetMax = float(self.AC.a_target[1])
plan_send.plan.jerkFactor = float(self.AC.jerk_factor)
plan_send.plan.hasLead = self.AC.has_lead
# compute risk of collision events: fcw
self.FCW.process(CS, self.AC)
plan_send.plan.fcw = bool(self.FCW.active)
self.plan.send(plan_send.to_bytes())
return plan_send
def joystick_thread():
context = zmq.Context()
joystick_sock = messaging.pub_sock(context,
service_list['testJoystick'].port)
pygame.init()
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
# Initialize the joysticks
pygame.joystick.init()
# Get count of joysticks
joystick_count = pygame.joystick.get_count()
if joystick_count > 1:
raise ValueError("More than one joystick attached")
elif joystick_count < 1:
raise ValueError("No joystick found")
# -------- Main Program Loop -----------
while True:
# EVENT PROCESSING STEP
for event in pygame.event.get(): # User did something
if event.type == pygame.QUIT: # If user clicked close
pass
# Available joystick events: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
if event.type == pygame.JOYBUTTONDOWN:
print("Joystick button pressed.")
if event.type == pygame.JOYBUTTONUP:
print("Joystick button released.")
joystick = pygame.joystick.Joystick(0)
joystick.init()
# Usually axis run in pairs, up/down for one, and left/right for
# the other.
axes = []
buttons = []
for a in range(joystick.get_numaxes()):
axes.append(joystick.get_axis(a))
for b in range(joystick.get_numbuttons()):
buttons.append(joystick.get_button(b))
dat = messaging.new_message()
dat.init('testJoystick')
dat.testJoystick.axes = axes
dat.testJoystick.buttons = map(bool, buttons)
joystick_sock.send(dat.to_bytes())
# Limit to 100 frames per second
clock.tick(100)
def calibrationd_thread(gctx):
context = zmq.Context()
features = messaging.sub_sock(context, service_list['features'].port)
live100 = messaging.sub_sock(context, service_list['live100'].port)
livecalibration = messaging.pub_sock(context,
service_list['liveCalibration'].port)
# subscribe to stats about the car
VP = VehicleParams(False)
v_ego = None
calib = load_calibration(gctx)
last_cal_cycle = calib.cal_cycle
while 1:
# calibration at the end so it does not delay radar processing above
ft = messaging.recv_sock(features, wait=True)
# get latest here
l100 = messaging.recv_sock(live100)
if l100 is not None:
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
if v_ego is None:
continue
p0 = ft.features.p0
p1 = ft.features.p1
st = ft.features.status
calib.calibration(p0, p1, st, v_ego, steer_angle, VP)
# write a new calibration every 100 cal cycle
if calib.cal_cycle - last_cal_cycle >= 100:
print "writing cal", calib.cal_cycle
with open(CALIBRATION_FILE, "w") as cal_file:
cal_file.write(str(calib.cal_cycle) + '\n')
cal_file.write(str(calib.cal_status) + '\n')
cal_file.write(str(calib.vp_f[0]) + '\n')
cal_file.write(str(calib.vp_f[1]) + '\n')
last_cal_cycle = calib.cal_cycle
warp_matrix = map(float, calib.warp_matrix.reshape(9).tolist())
dat = messaging.new_message()
dat.init('liveCalibration')
dat.liveCalibration.warpMatrix = warp_matrix
dat.liveCalibration.calStatus = calib.cal_status
dat.liveCalibration.calCycle = calib.cal_cycle
dat.liveCalibration.calPerc = calib.cal_perc
livecalibration.send(dat.to_bytes())
def gen_init_data(gctx):
msg = messaging.new_message()
kernel_args = open("/proc/cmdline", "r").read().strip().split(" ")
msg.initData.kernelArgs = kernel_args
msg.initData.gctx = json.dumps(gctx)
if os.getenv('DONGLE_ID'):
msg.initData.dongleId = os.getenv('DONGLE_ID')
return msg.to_bytes()
def read_thermal():
dat = messaging.new_message()
dat.init('thermal')
dat.thermal.cpu0 = read_tz(5)
dat.thermal.cpu1 = read_tz(7)
dat.thermal.cpu2 = read_tz(10)
dat.thermal.cpu3 = read_tz(12)
dat.thermal.mem = read_tz(2)
dat.thermal.gpu = read_tz(16)
dat.thermal.bat = read_tz(29)
return dat
def send_data(self, pm):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100)
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
pm.send('liveCalibration', cal_send)
def can_list_to_can_capnp(can_msgs, msgtype='can'):
dat = messaging.new_message()
dat.init(msgtype, len(can_msgs))
for i, can_msg in enumerate(can_msgs):
if msgtype == 'sendcan':
cc = dat.sendcan[i]
else:
cc = dat.can[i]
cc.address = can_msg[0]
cc.busTime = can_msg[1]
cc.dat = str(can_msg[2])
cc.src = can_msg[3]
return dat
def __init__(self, services):
self.data = {}
self.sock = {}
self.last_updated = None
for s in services:
data = messaging.new_message()
try:
data.init(s)
except:
data.init(s, 0)
self.data[s] = data.as_reader()
self.sock[s] = DumbSocket()
self.send_called = threading.Event()
self.get_called = threading.Event()
def send_data(self, livecalibration):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
ke = eon_intrinsics.dot(extrinsic_matrix)
warp_matrix = get_camera_frame_from_model_frame(ke, model_height)
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100)
cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten())
cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten())
livecalibration.send(cal_send.to_bytes())
def plannerd_thread():
context = zmq.Context()
params = Params()
# Get FCW toggle from settings
fcw_enabled = params.get("IsFcwEnabled") == "1"
cloudlog.info("plannerd is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("plannerd got CarParams: %s", CP.carName)
PL = Planner(CP, fcw_enabled)
PP = PathPlanner(CP)
VM = VehicleModel(CP)
poller = zmq.Poller()
car_state_sock = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller)
live100_sock = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller)
model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller)
live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller)
car_state = messaging.new_message()
car_state.init('carState')
live100 = messaging.new_message()
live100.init('live100')
model = messaging.new_message()
model.init('model')
live20 = messaging.new_message()
live20.init('live20')
live_map_data = messaging.new_message()
live_map_data.init('liveMapData')
live_parameters = messaging.new_message()
live_parameters.init('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
while True:
for socket, event in poller.poll():
if socket is live100_sock:
live100 = messaging.recv_one(socket)
elif socket is car_state_sock:
car_state = messaging.recv_one(socket)
elif socket is live_parameters_sock:
live_parameters = messaging.recv_one(socket)
elif socket is model_sock:
model = messaging.recv_one(socket)
PP.update(CP, VM, car_state, model, live100, live_parameters)
elif socket is live_map_data_sock:
live_map_data = messaging.recv_one(socket)
elif socket is live20_sock:
live20 = messaging.recv_one(socket)
PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data)
def send_mpc_solution(self, pm, qp_iterations, calculation_time):
qp_iterations = max(0, qp_iterations)
dat = messaging.new_message()
dat.init('liveLongitudinalMpc')
dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
dat.liveLongitudinalMpc.qpIterations = qp_iterations
dat.liveLongitudinalMpc.mpcId = self.mpc_id
dat.liveLongitudinalMpc.calculationTime = calculation_time
pm.send('liveLongitudinalMpc', dat)
def send_mpc_solution(self, qp_iterations, calculation_time):
qp_iterations = max(0, qp_iterations)
dat = messaging.new_message()
dat.init('liveLongitudinalMpc')
dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
dat.liveLongitudinalMpc.aLead = list(self.mpc_solution[0].a_l)
dat.liveLongitudinalMpc.aLeadTau = self.l
dat.liveLongitudinalMpc.qpIterations = qp_iterations
dat.liveLongitudinalMpc.mpcId = self.mpc_id
dat.liveLongitudinalMpc.calculationTime = calculation_time
self.live_longitudinal_mpc.send(dat.to_bytes())
def boardd_loop(rate=200):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** publishes can and health
logcan = messaging.pub_sock(context, service_list['can'].port)
health_sock = messaging.pub_sock(context, service_list['health'].port)
# *** subscribes to can send
sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# health packet @ 1hz
if (rk.frame % rate) == 0:
health = can_health()
msg = messaging.new_message()
msg.init('health')
# store the health to be logged
msg.health.voltage = health['voltage']
msg.health.current = health['current']
msg.health.started = health['started']
msg.health.controlsAllowed = True
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs).to_bytes()
logcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc.sendcan))
rk.keep_time()
def send_data(self, livecalibration):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(
0, calib[1], calib[2], model_height)
ke = eon_intrinsics.dot(extrinsic_matrix)
warp_matrix = get_camera_frame_from_model_frame(ke)
warp_matrix_big = get_camera_frame_from_bigmodel_frame(ke)
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(
len(self.vps) * 100 // INPUTS_NEEDED, 100)
cal_send.liveCalibration.warpMatrix2 = [
float(x) for x in warp_matrix.flatten()
]
cal_send.liveCalibration.warpMatrixBig = [
float(x) for x in warp_matrix_big.flatten()
]
cal_send.liveCalibration.extrinsicMatrix = [
float(x) for x in extrinsic_matrix.flatten()
]
livecalibration.send(cal_send.to_bytes())
def update(self, sm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
if not self.model_v_kf_ready:
self.model_v_kf.x = [[v_ego],[0.0]]
self.model_v_kf_ready = True
if len(sm['model'].speed):
self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
if self.params.get("LimitSetSpeedNeural") == "1":
model_speed = self.model_v_kf.x[0][0]
else:
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# accel and jerk up limits are higher here to make model not limiting accel
# mainly done to prevent flickering of slowdown icon
self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
model_speed,
2*accel_limits[1], 3*accel_limits[0],
2*jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(sm['carState'], lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
sm['controlsState'].active,
v_ego, sm['carState'].aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vStart = self.v_acc_start
plan_send.plan.aStart = self.a_acc_start
plan_send.plan.vTarget = self.v_acc
plan_send.plan.aTarget = self.a_acc
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
# Send out fcw
fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
"""Gets called when new live20 is available"""
cur_time = live20.logMonoTime / 1e9
v_ego = CS.carState.vEgo
gasbuttonstatus = CS.carState.gasbuttonstatus
long_control_state = live100.live100.longControlState
v_cruise_kph = live100.live100.vCruise
force_slow_decel = live100.live100.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
for socket, event in self.poller.poll(0):
if socket is self.lat_Control:
self.lastlat_Control = messaging.recv_one(socket).latControl
self.lead_1 = live20.live20.leadOne
self.lead_2 = live20.live20.leadTwo
lead_1 = live20.live20.leadOne
lead_2 = live20.live20.leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
v_speedlimit = NO_CURVATURE_SPEED
v_curvature = NO_CURVATURE_SPEED
map_valid = live_map_data.liveMapData.mapValid
# Speed limit and curvature
set_speed_limit_active = self.params.get(
"LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None
if set_speed_limit_active:
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(
a_y_max / max(1e-4, curvature)) / 1.3 * _brake_factor
v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
decel_for_turn = bool(
v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(
float,
calc_cruise_accel_limits(v_ego, following, gasbuttonstatus))
if gasbuttonstatus == 0:
accellimitmaxdynamic = -0.0018 * v_ego + 0.2
jerk_limits = [
min(-0.1, accel_limits[0] * 0.5),
max(accellimitmaxdynamic, accel_limits[1])
] # dynamic
elif gasbuttonstatus == 1:
accellimitmaxsport = -0.002 * v_ego + 0.4
jerk_limits = [
min(-0.25, accel_limits[0]),
max(accellimitmaxsport, accel_limits[1])
] # sport
elif gasbuttonstatus == 2:
accellimitmaxeco = -0.0015 * v_ego + 0.1
jerk_limits = [
min(-0.1, accel_limits[0] * 0.5),
max(accellimitmaxeco, accel_limits[1])
] # eco
if not CS.carState.leftBlinker and not CS.carState.rightBlinker:
steering_angle = CS.carState.steeringAngle
if self.lastlat_Control and v_ego > 11:
angle_later = self.lastlat_Control.anglelater
else:
angle_later = 0
else:
angle_later = 0
steering_angle = 0
accel_limits = limit_accel_in_turns(
v_ego, steering_angle, accel_limits, self.CP,
angle_later * self.CP.steerRatio)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# Change accel limits based on time remaining to turn
if decel_for_turn:
time_to_turn = max(
1.0, live_map_data.liveMapData.distToTurn /
max(self.v_cruise, 1.))
required_decel = min(0, (v_curvature - self.v_cruise) /
time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(CS.carState.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, lead_1, v_cruise_setpoint)
self.mpc2.update(CS, lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not CS.carState.brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
# TODO: Move all these events to controlsd. This has nothing to do with planning
events = []
if model_dead:
events.append(
create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
radar_errors = list(live20.live20.radarErrors)
if 'commIssue' in radar_errors:
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if 'fault' in radar_errors:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
plan_send.plan.events = events
plan_send.plan.mdMonoTime = md.logMonoTime
plan_send.plan.l20MonoTime = live20.logMonoTime
# longitudal plan
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vStart = self.v_acc_start
plan_send.plan.aStart = self.a_acc_start
plan_send.plan.vTarget = self.v_acc
plan_send.plan.aTarget = self.a_acc
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.hasrightLaneDepart = bool(
PP.r_poly[3] > -1.1 and not CS.carState.rightBlinker)
plan_send.plan.hasleftLaneDepart = bool(
PP.l_poly[3] < 1.05 and not CS.carState.leftBlinker)
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = v_curvature
plan_send.plan.decelForTurn = decel_for_turn
plan_send.plan.mapValid = map_valid
# Send out fcw
fcw = fcw and (self.fcw_enabled
or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
# Interpolate 0.05 seconds and save as starting point for next iteration
dt = 0.05 # s
a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc -
self.a_acc_start)
v_acc_sol = self.v_acc_start + dt * (a_acc_sol +
self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def radard_thread(gctx=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked= CP.radarName == "mock"
VM = VehicleModel(CP)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.radarName)
exec('from selfdrive.radar.'+CP.radarName+'.interface import RadarInterface')
context = zmq.Context()
# *** subscribe to features and model from visiond
poller = zmq.Poller()
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
PP = PathPlanner()
RI = RadarInterface()
last_md_ts = 0
last_l100_ts = 0
# *** publish live20 and liveTracks
live20 = messaging.pub_sock(context, service_list['live20'].port)
liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
# Time-alignment
rate = 20. # model and radar are both at 20Hz
tsv = 1./rate
v_len = 20 # how many speed data points to remember for t alignment with rdr data
active = 0
steer_angle = 0.
steer_override = False
tracks = defaultdict(dict)
# Kalman filter stuff:
ekfv = EKFV1D()
speedSensorV = SimpleSensor(XV, 1, 2)
# v_ego
v_ego = None
v_ego_array = np.zeros([2, v_len])
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1:
rr = RI.update()
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s
l100 = None
md = None
for socket, event in poller.poll(0):
if socket is live100:
l100 = messaging.recv_one(socket)
elif socket is model:
md = messaging.recv_one(socket)
if l100 is not None:
active = l100.live100.active
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
steer_override = l100.live100.steerOverride
v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
v_ego_array = v_ego_array[:, 1:]
last_l100_ts = l100.logMonoTime
if v_ego is None:
continue
if md is not None:
last_md_ts = md.logMonoTime
# *** get path prediction from the model ***
PP.update(v_ego, md)
# run kalman filter only if prob is high enough
if PP.lead_prob > 0.7:
ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
ekfv.predict(tsv)
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), False)
else:
ekfv.state[XV] = PP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0.
if VISION_POINT in ar_pts:
del ar_pts[VISION_POINT]
# *** compute the likely path_y ***
if (active and not steer_override) or mocked:
# use path from model (always when mocking as steering is too noisy)
path_y = np.polyval(PP.d_poly, path_x)
else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
# *** remove missing points from meta data ***
for ids in tracks.keys():
if ids not in ar_pts:
tracks.pop(ids, None)
# *** compute the tracks ***
for ids in ar_pts:
# ignore standalone vision point, unless we are mocking the radar
if ids == VISION_POINT and not mocked:
continue
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
# create the track if it doesn't exist or it's a new track
if ids not in tracks:
tracks[ids] = Track()
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
fused_id = None
best_score = NO_FUSION_SCORE
for ids in tracks:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
if best_score > tracks[ids].vision_score:
fused_id = ids
best_score = tracks[ids].vision_score
if fused_id is not None:
tracks[fused_id].vision_cnt += 1
tracks[fused_id].update_vision_fusion()
if DEBUG:
print "NEW CYCLE"
if VISION_POINT in ar_pts:
print "vision", ar_pts[VISION_POINT]
idens = tracks.keys()
track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
# If we have multiple points, cluster them
if len(track_pts) > 1:
link = linkage_vector(track_pts, method='centroid')
cluster_idxs = fcluster(link, 2.5, criterion='distance')
clusters = [None]*max(cluster_idxs)
for idx in xrange(len(track_pts)):
cluster_i = cluster_idxs[idx]-1
if clusters[cluster_i] == None:
clusters[cluster_i] = Cluster()
clusters[cluster_i].add(tracks[idens[idx]])
elif len(track_pts) == 1:
# TODO: why do we need this?
clusters = [Cluster()]
clusters[0].add(tracks[idens[0]])
else:
clusters = []
if DEBUG:
for i in clusters:
print i
# *** extract the lead car ***
lead_clusters = [c for c in clusters
if c.is_potential_lead(v_ego)]
lead_clusters.sort(key=lambda x: x.dRel)
lead_len = len(lead_clusters)
# *** extract the second lead from the whole set of leads ***
lead2_clusters = [c for c in lead_clusters
if c.is_potential_lead2(lead_clusters)]
lead2_clusters.sort(key=lambda x: x.dRel)
lead2_len = len(lead2_clusters)
# *** publish live20 ***
dat = messaging.new_message()
dat.init('live20')
dat.live20.mdMonoTime = last_md_ts
dat.live20.canMonoTimes = list(rr.canMonoTimes)
dat.live20.radarErrors = list(rr.errors)
dat.live20.l100MonoTime = last_l100_ts
if lead_len > 0:
lead_clusters[0].toLive20(dat.live20.leadOne)
if lead2_len > 0:
lead2_clusters[0].toLive20(dat.live20.leadTwo)
else:
dat.live20.leadTwo.status = False
else:
dat.live20.leadOne.status = False
dat.live20.cumLagMs = -rk.remaining*1000.
live20.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary)
dat.liveTracks[cnt].trackId = ids
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
dat.liveTracks[cnt].stationary = tracks[ids].stationary
dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
liveTracks.send(dat.to_bytes())
rk.monitor_time()
def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
LaC, LoC, angle_offset, passive):
# ***** control the car *****
CC = car.CarControl.new_message()
if not passive:
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
# always cancel if we have an interceptor
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
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, plan.aTarget, CS.vEgo, 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 = plan.hasLead
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC)
# ***** publish state to logger *****
# publish controls state at 100Hz
dat = messaging.new_message()
dat.init('live100')
# show rear view camera on phone if in reverse gear or when button is pressed
dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle
dat.live100.alertText1 = AM.alert_text_1
dat.live100.alertText2 = AM.alert_text_2
dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0
# what packets were used to process
dat.live100.canMonoTimes = list(CS.canMonoTimes)
dat.live100.planMonoTime = plan_ts
# if controls is enabled
dat.live100.enabled = isEnabled(state)
dat.live100.active = isActive(state)
# car state
dat.live100.vEgo = CS.vEgo
dat.live100.vEgoRaw = CS.vEgoRaw
dat.live100.angleSteers = CS.steeringAngle
dat.live100.steerOverride = CS.steeringPressed
# high level control state
dat.live100.state = state
# longitudinal control state
dat.live100.longControlState = LoC.long_control_state
dat.live100.vPid = float(LoC.v_pid)
dat.live100.vCruise = float(v_cruise_kph)
dat.live100.upAccelCmd = float(LoC.pid.p)
dat.live100.uiAccelCmd = float(LoC.pid.i)
dat.live100.ufAccelCmd = float(LoC.pid.f)
# lateral control state
dat.live100.angleSteersDes = float(LaC.angle_steers_des)
dat.live100.upSteer = float(LaC.pid.p)
dat.live100.uiSteer = float(LaC.pid.i)
dat.live100.ufSteer = float(LaC.pid.f)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(plan.vTarget)
dat.live100.aTarget = float(plan.aTarget)
dat.live100.jerkFactor = float(plan.jerkFactor)
# log learned angle offset
dat.live100.angleOffset = float(angle_offset)
# lag
dat.live100.cumLagMs = -rk.remaining*1000.
live100.send(dat.to_bytes())
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
# TODO: override CS.events with all the cumulated events
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())
#print [i.name for i in events]
# publish mpc state at 20Hz
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
dat = messaging.new_message()
dat.init('liveMpc')
dat.liveMpc.x = list(LaC.mpc_solution[0].x)
dat.liveMpc.y = list(LaC.mpc_solution[0].y)
dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
livempc.send(dat.to_bytes())
return CC
def update(self, CS, LoC, v_cruise_kph, user_distracted):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
md = None
l20 = None
for socket, event in self.poller.poll(0):
if socket is self.model:
md = messaging.recv_one(socket)
elif socket is self.live20:
l20 = messaging.recv_one(socket)
if md is not None:
self.last_md_ts = md.logMonoTime
self.last_model = cur_time
self.model_dead = False
self.PP.update(CS.vEgo, md)
if l20 is not None:
self.last_l20_ts = l20.logMonoTime
self.last_l20 = cur_time
self.radar_dead = False
self.radar_errors = list(l20.live20.radarErrors)
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
self.lead_1 = l20.live20.leadOne
self.lead_2 = l20.live20.leadTwo
enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following))
# TODO: make a separate lookup for jerk tuning
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP)
if user_distracted:
# if user is not responsive to awareness alerts, then start a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0],
_DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
if cur_time - self.last_model > 0.5:
self.model_dead = True
if cur_time - self.last_l20 > 0.5:
self.radar_dead = True
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
events = []
if self.model_dead:
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.radar_dead or 'commIssue' in self.radar_errors:
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Interpolation of trajectory
dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
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
plan_send.plan.events = events
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.model_dead
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.laneWidth = float(self.PP.lane_width)
# longitudal plan
plan_send.plan.longitudinalValid = not self.radar_dead
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vTarget = self.v_acc_sol
plan_send.plan.aTarget = self.a_acc_sol
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
# Send out fcw
fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
return plan_send
def pt_from_car(pt):
x,y = pt
rx = x * math.cos(heading) + y * -math.sin(heading)
ry = x * math.sin(heading) + y * math.cos(heading)
rx += carx
ry += cary
return rx, ry
while 1:
if plant.rk.frame%100 >= 20 and plant.rk.frame%100 <= 25:
cruise_buttons = CruiseButtons.RES_ACCEL
else:
cruise_buttons = 0
md = messaging.new_message()
md.init('model')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0]*50
x.prob = 0.0
x.std = 1.0
car_pts = map(pt_to_car, control_pts)
print(car_pts)
car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3)
md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist()
md.model.path.prob = 1.0
Plant.model.send(md.to_bytes())
