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 boardd_proxy_loop(rate=100, address="192.168.2.251"):
rk = Ratekeeper(rate)
can_init()
# *** subscribes can
logcan = messaging.sub_sock('can', addr=address)
# *** publishes to can send
sendcan = messaging.pub_sock('sendcan')
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
# print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs, "sendcan")
sendcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(logcan)
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
# print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
can_send_many(cl)
rk.keep_time()
def __init__(self,
lead_relevancy=False,
rate=100,
speed=0.0,
distance_lead=2.0):
self.rate = rate
if not Plant.messaging_initialized:
Plant.logcan = messaging.pub_sock(service_list['can'].port)
Plant.sendcan = messaging.sub_sock(service_list['sendcan'].port)
Plant.model = messaging.pub_sock(service_list['model'].port)
Plant.live_params = messaging.pub_sock(
service_list['liveParameters'].port)
Plant.health = messaging.pub_sock(service_list['health'].port)
Plant.thermal = messaging.pub_sock(service_list['thermal'].port)
Plant.driverMonitoring = messaging.pub_sock(
service_list['driverMonitoring'].port)
Plant.cal = messaging.pub_sock(
service_list['liveCalibration'].port)
Plant.controls_state = messaging.sub_sock(
service_list['controlsState'].port)
Plant.plan = messaging.sub_sock(service_list['plan'].port)
Plant.messaging_initialized = True
self.frame = 0
self.angle_steer = 0.
self.gear_choice = 0
self.speed, self.speed_prev = 0., 0.
self.esp_disabled = 0
self.main_on = 1
self.user_gas = 0
self.computer_brake, self.user_brake = 0, 0
self.brake_pressed = 0
self.angle_steer_rate = 0
self.distance, self.distance_prev = 0., 0.
self.speed, self.speed_prev = speed, speed
self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0
self.gear_shifter = 8 # D gear
self.pedal_gas = 0
self.cruise_setting = 0
self.seatbelt, self.door_all_closed = True, True
self.steer_torque, self.v_cruise, self.acc_status = 0, 0, 0 # v_cruise is reported from can, not the one used for controls
self.lead_relevancy = lead_relevancy
# lead car
self.distance_lead, self.distance_lead_prev = distance_lead, distance_lead
self.rk = Ratekeeper(rate, print_delay_threshold=100)
self.ts = 1. / rate
self.cp = get_car_can_parser()
self.response_seen = False
time.sleep(1)
messaging.drain_sock(Plant.sendcan)
messaging.drain_sock(Plant.controls_state)
def can_printer(bus=0, max_msg=None, addr="127.0.0.1"):
logcan = messaging.sub_sock(service_list['can'].port, addr=addr)
start = sec_since_boot()
lp = sec_since_boot()
msgs = defaultdict(list)
canbus = int(os.getenv("CAN", bus))
while 1:
can_recv = messaging.drain_sock(logcan, wait_for_one=True)
for x in can_recv:
for y in x.can:
if y.src == canbus:
msgs[y.address].append(y.dat)
if sec_since_boot() - lp > 0.1:
dd = chr(27) + "[2J"
dd += "%5.2f\n" % (sec_since_boot() - start)
for k, v in sorted(
zip(msgs.keys(),
map(lambda x: binascii.hexlify(x[-1]),
msgs.values()))):
if max_msg is None or k < max_msg:
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k, k), len(
msgs[k]), v.decode('ascii'))
print(dd)
lp = sec_since_boot()
def boardd_mock_loop():
can_init()
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
logcan = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc)
snd = []
for s in snds:
snd += s
snd = list(filter(lambda x: x[-1] <= 2, snd))
snd_0 = len(list(filter(lambda x: x[-1] == 0, snd)))
snd_1 = len(list(filter(lambda x: x[-1] == 1, snd)))
snd_2 = len(list(filter(lambda x: x[-1] == 2, snd)))
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
got_0 = len(list(filter(lambda x: x[-1] == 0 + 0x80, can_msgs)))
got_1 = len(list(filter(lambda x: x[-1] == 1 + 0x80, can_msgs)))
got_2 = len(list(filter(lambda x: x[-1] == 2 + 0x80, can_msgs)))
print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" %
(len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1,
got_2))
m = can_list_to_can_capnp(can_msgs, msgtype='sendcan')
sendcan.send(m.to_bytes())
def read(self, n):
for msg in messaging.drain_sock(self.ubloxRaw,
len(self.buf) < n):
self.buf += msg.ubloxRaw
ret = self.buf[:n]
self.buf = self.buf[n:]
return ret
def fingerprint(logcan, timeout):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None)
finger_st = sec_since_boot()
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
finger = {}
st = None
while 1:
for a in messaging.drain_sock(logcan, wait_for_one=True):
if st is None:
st = sec_since_boot()
for can in a.can:
if can.src == 0:
finger[can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
ts = sec_since_boot()
# if we only have one car choice and the time_fingerprint since we got our first
# message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
# broadcast immediately
if len(candidate_cars) == 1 and st is not None:
# TODO: better way to decide to wait more if Toyota
time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1
if (ts-st) > time_fingerprint:
break
# bail if no cars left or we've been waiting too long
elif len(candidate_cars) == 0 or (timeout and ts-finger_st > timeout):
return None, finger
cloudlog.warning("fingerprinted %s", candidate_cars[0])
return (candidate_cars[0], finger)
def fingerprint(logcan, timeout):
if os.getenv("SIMULATOR") is not None or logcan is None:
return ("simulator", None)
elif os.getenv("SIMULATOR2") is not None:
return ("simulator2", None)
finger_st = sec_since_boot()
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
finger = {}
st = None
while 1:
for a in messaging.drain_sock(logcan, wait_for_one=True):
if st is None:
st = sec_since_boot()
for can in a.can:
if can.src == 0:
finger[can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(
can, candidate_cars)
ts = sec_since_boot()
# if we only have one car choice and it's been 100ms since we got our first message, exit
if len(candidate_cars) == 1 and st is not None and (ts - st) > 0.1:
break
# bail if no cars left or we've been waiting too long
elif len(candidate_cars) == 0 or (timeout
and ts - finger_st > timeout):
return None, finger
cloudlog.warning("fingerprinted %s", candidate_cars[0])
return (candidate_cars[0], finger)
def fingerprint(logcan):
import selfdrive.messaging as messaging
from cereal import car
from common.realtime import sec_since_boot
if os.getenv("SIMULATOR") is not None or logcan is None:
return ("simulator", None)
elif os.getenv("SIMULATOR2") is not None:
return ("simulator2", None)
print "waiting for fingerprint..."
candidate_cars = all_known_cars()
finger = {}
st = None
while 1:
for a in messaging.drain_sock(logcan, wait_for_one=True):
if st is None:
st = sec_since_boot()
for can in a.can:
if can.src == 0:
finger[can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(
can, candidate_cars)
# if we only have one car choice and it's been 100ms since we got our first message, exit
if len(candidate_cars) == 1 and st is not None and (sec_since_boot() -
st) > 0.1:
break
elif len(candidate_cars) == 0:
print map(hex, finger.keys())
raise Exception("car doesn't match any fingerprints")
print "fingerprinted", candidate_cars[0]
return (candidate_cars[0], finger)
def get_vin(logcan, sendcan):
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
cnts = [1, 2] # Number of messages to wait for at each iteration
vin_valid = True
dat = []
for i in range(len(query_msg)):
cnt = 0
sendcan.send(can_list_to_can_capnp([query_msg[i]], msgtype='sendcan'))
got_response = False
t_start = sec_since_boot()
while sec_since_boot() - t_start < 0.05 and not got_response:
for a in messaging.drain_sock(logcan):
for can in a.can:
if can.src == 0 and can.address == 0x7e8:
vin_valid = vin_valid and is_vin_response_valid(
can.dat, i, cnt)
dat += can.dat[2:] if i == 0 else can.dat[1:]
cnt += 1
if cnt == cnts[i]:
got_response = True
time.sleep(0.01)
return "".join(dat[3:]) if vin_valid else ""
def main():
context = zmq.Context()
rcv = sub_sock(context, service_list['can'].port) # port 8006
snd = pub_sock(context, service_list['sendcan'].port) # port 8017
time.sleep(0.3) # wait to bind before send/recv
for _ in range(10):
at = random.randint(1024, 2000)
st = get_test_string()[0:8]
snd.send(can_list_to_can_capnp([[at, 0, st, 0]], msgtype='sendcan').to_bytes())
time.sleep(0.1)
res = drain_sock(rcv, True)
assert len(res) == 1
res = res[0].can
assert len(res) == 2
msg0, msg1 = res
assert msg0.dat == st
assert msg1.dat == st
assert msg0.address == at
assert msg1.address == at
assert msg0.src == 0x80 | BUS
assert msg1.src == BUS
print("Success")
def can_printer(bus=0, max_msg=0x10000, addr="127.0.0.1"):
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port, addr=addr)
start = sec_since_boot()
lp = sec_since_boot()
msgs = defaultdict(list)
canbus = int(os.getenv("CAN", bus))
f = open("can1_dsu_off_frc_off.txt", "w")
while 1:
can_recv = messaging.drain_sock(logcan, wait_for_one=True)
for x in can_recv:
for y in x.can:
if y.src == canbus:
msgs[y.address].append(y.dat)
if sec_since_boot() - lp > 0.1:
dd = chr(27) + "[2J"
dd += "%5.2f\n" % (sec_since_boot() - start)
for k, v in sorted(
zip(msgs.keys(),
map(lambda x: x[-1].encode("hex"), msgs.values()))):
if k < max_msg:
dd += "%s(%6d) %s\n" % ("%04X(%4d)" %
(k, k), len(msgs[k]), v)
print dd
f.write(dd)
lp = sec_since_boot()
f.close()
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 get_car_params(msgs, pub_socks, sub_socks):
sendcan = pub_socks.get("sendcan", None)
if sendcan is None:
sendcan = messaging.pub_sock(service_list["sendcan"].port)
logcan = sub_socks.get("can", None)
if logcan is None:
logcan = messaging.sub_sock(service_list["can"].port)
can = pub_socks.get("can", None)
if can is None:
can = messaging.pub_sock(service_list["can"].port)
time.sleep(0.5)
canmsgs = filter(lambda msg: msg.which() == "can", msgs)
for m in canmsgs[:200]:
can.send(m.as_builder().to_bytes())
_, CP = get_car(logcan, sendcan)
Params().put("CarParams", CP.to_bytes())
time.sleep(0.5)
messaging.drain_sock(logcan)
def update(self):
canMonoTimes = []
if NEW_CAN:
#print "================================ new_can" # didn't enter during simulation -- Hasnat
updated_messages = set()
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
if 0x445 in updated_messages:
break
else:
can_pub_radar = []
# TODO: can hang if no packets show up
while 1:
for a in messaging.drain_sock(self.logcan, wait_for_one=True):
canMonoTimes.append(a.logMonoTime)
can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3])) # receiving radar message from CAN (sent from Plant.py) -- Hasnat
# only run on the 0x445 packets, used for timing
if any(x[0] == 0x445 for x in can_pub_radar):
break
updated_messages = self.rcp.update_can(can_pub_radar)
ret = car.RadarState.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("notValid")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
for ii in updated_messages:
cpt = self.rcp.vl[ii]
#print cpt
if cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
else:
if ii in self.pts:
del self.pts[ii]
ret.points = self.pts.values()
return ret
def run(self):
print 'Listening for FlexRay frames'
try:
context = zmq.Context()
log_flexray = messaging.sub_sock(context,
service_list['flexRay'].port)
frames_count = 0
while not self.stopped():
for a in messaging.drain_sock(log_flexray):
for f in a.flexRay:
print 'Frame', frames_count, ', ID:', f.frameId, ', Payload Len:', len(
f.dat), [hex(ord(x)) for x in f.dat]
frames_count += 1
finally:
pass
def fingerprint(logcan, timeout):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None)
elif os.getenv("SIMULATOR") is not None:
return ("simulator", None)
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
finger = {}
st = None
st_passive = sec_since_boot() # only relevant when passive
can_seen = False
while 1:
for a in messaging.drain_sock(logcan):
for can in a.can:
can_seen = True
# ignore everything not on bus 0 and with more than 11 bits,
# which are ussually sporadic and hard to include in fingerprints
if can.src == 0 and can.address < 0x800:
finger[can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(
can, candidate_cars)
if st is None and can_seen:
st = sec_since_boot() # start time
ts = sec_since_boot()
# if we only have one car choice and the time_fingerprint since we got our first
# message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
# broadcast immediately
if len(candidate_cars) == 1 and st is not None:
# TODO: better way to decide to wait more if Toyota
time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS"
in candidate_cars[0]) else 0.1
if (ts - st) > time_fingerprint:
break
# bail if no cars left or we've been waiting too long
elif len(candidate_cars) == 0 or (timeout and
(ts - st_passive) > timeout):
#return None, finger
print "Fingerprinting Failed: Returning Tesla (based on branch)"
return "TESLA MODEL S", finger
time.sleep(0.01)
cloudlog.warning("fingerprinted %s", candidate_cars[0])
return (candidate_cars[0], finger)
def boardd_mock_loop():
context = zmq.Context()
can_init()
logcan = messaging.sub_sock(context, service_list['can'].port)
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc)
snd = []
for s in snds:
snd += s
snd = filter(lambda x: x[-1] <= 1, snd)
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
print "sent %d got %d" % (len(snd), len(can_msgs))
def main(gctx=None):
poller = zmq.Poller()
if not CarSettings().get_value("useTeslaGPS"):
gpsLocationExternal = messaging.pub_sock(
service_list['gpsLocationExternal'].port)
ubloxGnss = messaging.pub_sock(service_list['ubloxGnss'].port)
# ubloxRaw = messaging.sub_sock(service_list['ubloxRaw'].port, poller)
# buffer with all the messages that still need to be input into the kalman
while 1:
polld = poller.poll(timeout=1000)
for sock, mode in polld:
if mode != zmq.POLLIN:
continue
logs = messaging.drain_sock(sock)
for log in logs:
buff = log.ubloxRaw
ttime = log.logMonoTime
msg = ublox.UBloxMessage()
msg.add(buff)
if msg.valid():
if msg.name() == 'NAV_PVT':
sol = gen_solution(msg)
if unlogger:
sol.logMonoTime = ttime
else:
sol.logMonoTime = int(
realtime.sec_since_boot() * 1e9)
gpsLocationExternal.send(sol.to_bytes())
elif msg.name() == 'RXM_RAW':
raw = gen_raw(msg)
if unlogger:
raw.logMonoTime = ttime
else:
raw.logMonoTime = int(
realtime.sec_since_boot() * 1e9)
ubloxGnss.send(raw.to_bytes())
else:
print "INVALID MESSAGE"
else:
while True:
time.sleep(1.1)
def boardd_mock_loop():
context = zmq.Context()
can_init()
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
logcan = messaging.sub_sock(context, service_list['can'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = [can_capnp_to_can_list(x.can) for x in tsc]
snds = [x for x in snds if x[-1] <= 1]
can_send_many(snds)
# recv @ 100hz
can_msgs = can_recv()
print("sent %d got %d" % (len(snds), len(can_msgs)))
m = can_list_to_can_capnp(can_msgs)
sendcan.send(m.to_bytes())
def update(self):
canMonoTimes = []
can_pub_radar = []
# TODO: can hang if no packets show up
while 1:
for a in messaging.drain_sock(self.logcan, wait_for_one=True):
canMonoTimes.append(a.logMonoTime)
can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3]))
# only run on the 0x445 packets, used for timing
if any(x[0] == 0x445 for x in can_pub_radar):
break
self.rcp.update_can(can_pub_radar)
ret = car.RadarState.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("notValid")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
for ii in self.rcp.msgs_upd:
cpt = self.rcp.vl[ii]
if cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt[
'LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
else:
if ii in self.pts:
del self.pts[ii]
ret.points = self.pts.values()
return ret
def fingerprint(logcan):
print "waiting for fingerprint..."
brake_only = True
finger = {}
st = None
while 1:
possible_cars = []
for a in messaging.drain_sock(logcan, wait_for_one=True):
if st is None:
st = sec_since_boot()
for adr, _, msg, idx in can_capnp_to_can_list(a.can):
# pedal
if adr == 0x201 and idx == 0:
brake_only = False
if idx == 0:
finger[adr] = len(msg)
# check for a single match
for f in fingerprints:
is_possible = True
for adr in finger:
# confirm all messages we have seen match
if adr not in fingerprints[
f] or fingerprints[f][adr] != finger[adr]:
#print "mismatch", f, adr
is_possible = False
break
if is_possible:
possible_cars.append(f)
# if we only have one car choice and it's been 100ms since we got our first message, exit
if len(possible_cars) == 1 and st is not None and (sec_since_boot() -
st) > 0.1:
break
elif len(possible_cars) == 0:
print finger
raise Exception("car doesn't match any fingerprints")
print "fingerprinted", possible_cars[0]
return brake_only, possible_cars[0]
def boardd_mock_loop():
can_init()
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
logcan = messaging.sub_sock(service_list['can'].port)
sendcan = messaging.pub_sock(service_list['sendcan'].port)
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc)
snd = []
for s in snds:
snd += s
snd = filter(lambda x: x[-1] <= 1, snd)
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
print("sent %d got %d" % (len(snd), len(can_msgs)))
m = can_list_to_can_capnp(can_msgs)
sendcan.send(m.to_bytes())
def can_printer():
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
start = sec_since_boot()
lp = sec_since_boot()
msgs = defaultdict(list)
canbus = int(os.getenv("CAN", 0))
while 1:
can_recv = messaging.drain_sock(logcan, wait_for_one=True)
for x in can_recv:
for y in x.can:
if y.src == canbus:
msgs[y.address].append(y.dat)
if sec_since_boot() - lp > 0.1:
dd = chr(27) + "[2J"
dd += "%5.2f\n" % (sec_since_boot() - start)
for k, v in sorted(
zip(msgs.keys(),
map(lambda x: x[-1].encode("hex"), msgs.values()))):
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k, k), len(msgs[k]), v)
print dd
lp = sec_since_boot()
def step(self,
v_lead=0.0,
cruise_buttons=None,
grade=0.0,
publish_model=True):
# dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)
cp2 = get_can_parser(fake_car_params())
sgs = cp2._sgs
msgs = cp2._msgs
cks_msgs = cp2.msgs_ck
# ******** get messages sent to the car ********
can_msgs = []
for a in messaging.drain_sock(Plant.sendcan):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0, 2]))
self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting ***
live_msgs = []
for a in messaging.drain_sock(Plant.live100):
live_msgs.append(a.live100)
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE']
else:
brake = 0.0
if self.cp.vl[0x200]['GAS_COMMAND'] > 0:
gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0
else:
gas = 0.0
if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
steer_torque = self.cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0xf00
else:
steer_torque = 0.0
distance_lead = self.distance_lead_prev + v_lead * self.ts
# ******** run the car ********
speed, acceleration = car_plant(self.distance_prev, self.speed_prev,
grade, gas, brake)
standstill = (speed == 0)
distance = self.distance_prev + speed * self.ts
speed = self.speed_prev + self.ts * acceleration
if speed <= 0:
speed = 0
acceleration = 0
# ******** lateral ********
self.angle_steer -= (steer_torque / 10.0) * self.ts
# *** radar model ***
if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance)
v_rel = v_lead - speed
else:
d_rel = 200.
v_rel = 0.
a_rel = 0
lateral_pos_rel = 0.
# print at 5hz
if (self.rk.frame % (self.rate / 5)) == 0:
print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (
distance, speed, acceleration, self.angle_steer, gas, brake,
steer_torque, d_rel, v_rel)
# ******** publish the car ********
vls = [
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.angle_steer,
0,
self.gear_choice,
speed != 0,
0,
0,
0,
0,
self.v_cruise,
not self.seatbelt,
self.seatbelt,
self.brake_pressed,
self.user_gas,
cruise_buttons,
self.esp_disabled,
0,
self.user_brake,
self.steer_error,
self.speed_sensor(speed),
self.brake_error,
self.brake_error,
self.gear_shifter,
self.main_on,
self.acc_status,
self.pedal_gas,
self.cruise_setting,
# left_blinker, right_blinker, counter
0,
0,
0,
# interceptor_gas
0,
0
]
# TODO: publish each message at proper frequency
can_msgs = []
for msg in set(msgs):
msg_struct = {}
indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
for i in indxs:
msg_struct[sgs[i]] = vls[i]
if msg in cks_msgs:
msg_struct["COUNTER"] = self.rk.frame % 4
msg_data = acura.encode(msg, msg_struct)
if msg in cks_msgs:
msg_data = fix(msg_data, msg)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message
# TODO: use the DBC
def to_3_byte(x):
return struct.pack("!H", int(x)).encode("hex")[1:]
def to_3s_byte(x):
return struct.pack("!h", int(x)).encode("hex")[1:]
radar_msg = to_3_byte(d_rel*16.0) + \
to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
to_3s_byte(int(v_rel*32.0)) + \
"0f00000"
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
# ******** publish a fake model going straight ********
if publish_model:
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 = 1.0
x.std = 1.0
# fake values?
Plant.model.send(md.to_bytes())
# ******** update prevs ********
self.speed = speed
self.distance = distance
self.distance_lead = distance_lead
self.speed_prev = speed
self.distance_prev = distance
self.distance_lead_prev = distance_lead
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas,
steer_torque, live_msgs)
def update(self):
# ******************* do recv *******************
for a in messaging.drain_sock(self.logcan):
self.CS.angle_steers = a.carState.steeringAngle
# create message
ret = car.CarState.new_message()
ret.steeringAngle = self.CS.angle_steers
return ret.as_reader()
# speeds
ret.vEgo = self.CS.v_ego
ret.wheelSpeeds.fl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL']
ret.wheelSpeeds.fr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR']
ret.wheelSpeeds.rl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL']
ret.wheelSpeeds.rr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR']
# gas pedal
ret.gas = self.CS.car_gas / 256.0
if self.CS.VP.brake_only:
ret.gasPressed = self.CS.pedal_gas > 0
else:
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
# steering wheel
# TODO: units
ret.steeringAngle = self.CS.angle_steers
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
if self.CS.cruise_buttons != 0:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
be.type = 'accelCruise'
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
if self.CS.cruise_setting != 0:
be.pressed = True
but = self.CS.cruise_setting
else:
be.pressed = False
but = self.CS.prev_cruise_setting
if but == 1:
be.type = 'altButton1'
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# errors
# TODO: I don't like the way capnp does enums
# These strings aren't checked at compile time
errors = []
if self.CS.steer_error:
errors.append('steerUnavailable')
elif self.CS.steer_not_allowed:
errors.append('steerTemporarilyUnavailable')
if self.CS.brake_error:
errors.append('brakeUnavailable')
if not self.CS.gear_shifter_valid:
errors.append('wrongGear')
if not self.CS.door_all_closed:
errors.append('doorOpen')
if not self.CS.seatbelt:
errors.append('seatbeltNotLatched')
if self.CS.esp_disabled:
errors.append('espDisabled')
if not self.CS.main_on:
errors.append('wrongCarMode')
if self.CS.gear_shifter == 2:
errors.append('reverseGear')
ret.errors = errors
ret.canMonoTimes = canMonoTimes
# cast to reader so it can't be modified
#print ret
return ret.as_reader()
def step(self,
v_lead=0.0,
cruise_buttons=None,
grade=0.0,
publish_model=True):
gen_dbc, gen_signals, gen_checks = get_can_signals(CP)
sgs = [s[0] for s in gen_signals]
msgs = [s[1] for s in gen_signals]
cks_msgs = set(check[0] for check in gen_checks)
cks_msgs.add(0x18F)
cks_msgs.add(0x30C)
# ******** get messages sent to the car ********
can_msgs = []
for a in messaging.drain_sock(Plant.sendcan):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0, 2]))
self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting ***
live100_msgs = []
for a in messaging.drain_sock(Plant.live100):
live100_msgs.append(a.live100)
fcw = None
for a in messaging.drain_sock(Plant.plan):
if a.plan.fcw:
fcw = True
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
else:
brake = 0.0
if self.cp.vl[0x200]['GAS_COMMAND'] > 0:
gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0
else:
gas = 0.0
if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
steer_torque = self.cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0xf00
else:
steer_torque = 0.0
distance_lead = self.distance_lead_prev + v_lead * self.ts
# ******** run the car ********
speed, acceleration = car_plant(self.distance_prev, self.speed_prev,
grade, gas, brake)
distance = self.distance_prev + speed * self.ts
speed = self.speed_prev + self.ts * acceleration
if speed <= 0:
speed = 0
acceleration = 0
# ******** lateral ********
self.angle_steer -= (steer_torque / 10.0) * self.ts
# *** radar model ***
if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance)
v_rel = v_lead - speed
else:
d_rel = 200.
v_rel = 0.
lateral_pos_rel = 0.
# print at 5hz
if (self.rk.frame % (self.rate / 5)) == 0:
print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (
distance, speed, acceleration, self.angle_steer, gas, brake,
steer_torque, d_rel, v_rel)
# ******** publish the car ********
vls = [
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.angle_steer,
self.angle_steer_rate,
0,
0,
0,
0,
0, # Doors
0,
0, # Blinkers
0, # Cruise speed offset
self.gear_choice,
speed != 0,
self.brake_error,
self.brake_error,
self.v_cruise,
not self.seatbelt,
self.seatbelt, # Seatbelt
self.brake_pressed,
0.,
cruise_buttons,
self.esp_disabled,
0, # HUD lead
self.user_brake,
self.steer_error,
self.gear_shifter,
self.pedal_gas,
self.cruise_setting,
self.acc_status,
self.user_gas,
self.main_on,
0, # EPB State
0, # Brake hold
0, # Interceptor feedback
# 0,
]
# TODO: publish each message at proper frequency
can_msgs = []
for msg in set(msgs):
msg_struct = {}
indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
for i in indxs:
msg_struct[sgs[i]] = vls[i]
if "COUNTER" in honda.get_signals(msg):
msg_struct["COUNTER"] = self.rk.frame % 4
msg = honda.lookup_msg_id(msg)
msg_data = honda.encode(msg, msg_struct)
if "CHECKSUM" in honda.get_signals(msg):
msg_data = fix(msg_data, msg)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message
# TODO: use the DBC
if self.rk.frame % 5 == 0:
radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00'
radar_msg = to_3_byte(d_rel*16.0) + \
to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
to_3s_byte(int(v_rel*32.0)) + \
"0f00000"
can_msgs.append([0x400, 0, radar_state_msg, 1])
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
# ******** publish a fake model going straight and fake calibration ********
# note that this is worst case for MPC, since model will delay long mpc by one time step
if publish_model and self.rk.frame % 5 == 0:
md = messaging.new_message()
cal = messaging.new_message()
md.init('model')
cal.init('liveCalibration')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0] * 50
x.prob = 1.0
x.std = 1.0
md.model.lead.dist = float(d_rel)
md.model.lead.prob = 1.
md.model.lead.std = 0.1
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
Plant.model.send(md.to_bytes())
Plant.cal.send(cal.to_bytes())
# ******** update prevs ********
self.speed = speed
self.distance = distance
self.distance_lead = distance_lead
self.speed_prev = speed
self.distance_prev = distance
self.distance_lead_prev = distance_lead
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas,
steer_torque, fcw, live100_msgs)
def controlsd_thread(gctx, rate=100): #rate in Hz
# *** log ***
context = zmq.Context()
live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
thermal = messaging.sub_sock(context, service_list['thermal'].port)
live20 = messaging.sub_sock(context, service_list['live20'].port)
model = messaging.sub_sock(context, service_list['model'].port)
health = messaging.sub_sock(context, service_list['health'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
# connects to can and sendcan
CI = CarInterface()
VP = CI.getVehicleParams()
PP = PathPlanner(model)
AC = AdaptiveCruise(live20)
AM = AlertManager()
LoC = LongControl()
LaC = LatControl()
# controls enabled state
enabled = False
last_enable_request = 0
# learned angle offset
angle_offset = 0
# rear view camera state
rear_view_toggle = False
v_cruise_kph = 255
# 0.0 - 1.0
awareness_status = 0.0
soft_disable_timer = None
# Is cpu temp too high to enable?
overtemp = False
free_space = 1.0
# start the loop
set_realtime_priority(2)
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
while 1:
prof = Profiler()
cur_time = sec_since_boot()
# read CAN
# CS = CI.update()
CS = car.CarState.new_message()
CS.vEgo = 13
for a in messaging.drain_sock(logcan):
CS.steeringAngle = a.carState.steeringAngle
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = CS # copy?
carstate.send(cs_send.to_bytes())
prof.checkpoint("CarInterface")
# did it request to enable?
enable_request, enable_condition = False, False
if enabled:
# gives the user 6 minutes
# awareness_status -= 1.0/(100*60*6)
if awareness_status <= 0.:
# AM.add("driverDistracted", enabled)
awareness_status = 1.0
# reset awareness status on steering
if CS.steeringPressed:
awareness_status = 1.0
# handle button presses
for b in CS.buttonEvents:
print b
# reset awareness on any user action
awareness_status = 1.0
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed:
rear_view_toggle = True
else:
rear_view_toggle = False
if b.type == "altButton1" and b.pressed:
rear_view_toggle = not rear_view_toggle
if not VP.brake_only and enabled and not b.pressed:
if b.type == "accelCruise":
v_cruise_kph = v_cruise_kph - (
v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
elif b.type == "decelCruise":
v_cruise_kph = v_cruise_kph - (
v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
if not enabled and b.type in ["accelCruise", "decelCruise"
] and not b.pressed:
enable_request = True
# do disable on button down
if b.type == "cancel" and b.pressed:
AM.add("disable", enabled)
# Hack-hack
if not enabled:
enable_request = True
prof.checkpoint("Buttons")
# *** health checking logic ***
hh = messaging.recv_sock(health)
if hh is not None:
# if the board isn't allowing controls but somehow we are enabled!
if not hh.health.controlsAllowed and enabled:
AM.add("controlsMismatch", enabled)
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(thermal)
if False and td is not None:
# Check temperature.
overtemp = any(t > 950 for t in (td.thermal.cpu0, td.thermal.cpu1,
td.thermal.cpu2, td.thermal.cpu3,
td.thermal.mem, td.thermal.gpu))
# under 15% of space free
free_space = td.thermal.freeSpace
prof.checkpoint("Health")
# *** getting model logic ***
PP.update(cur_time, CS.vEgo)
if rk.frame % 5 == 2:
# *** run this at 20hz again ***
angle_offset = learn_angle_offset(enabled, CS.vEgo, angle_offset,
np.asarray(PP.d_poly), LaC.y_des,
CS.steeringPressed)
# disable if the pedals are pressed while engaged, this is a user disable
if enabled:
if CS.gasPressed or CS.brakePressed:
AM.add("disable", enabled)
if enable_request:
# check for pressed pedals
if CS.gasPressed or CS.brakePressed:
AM.add("pedalPressed", enabled)
enable_request = False
else:
print "enabled pressed at", cur_time
last_enable_request = cur_time
# don't engage with less than 15% free
if free_space < 0.15:
AM.add("outOfSpace", enabled)
enable_request = False
if VP.brake_only:
enable_condition = ((cur_time - last_enable_request) <
0.2) and CS.cruiseState.enabled
else:
enable_condition = enable_request
if enable_request or enable_condition or enabled:
# add all alerts from car
for alert in CS.errors:
AM.add(alert, enabled)
if False and AC.dead:
AM.add("radarCommIssue", enabled)
if PP.dead:
AM.add("modelCommIssue", enabled)
if overtemp:
AM.add("overheat", enabled)
prof.checkpoint("Model")
if enable_condition and not enabled and not AM.alertPresent():
print "*** enabling controls"
# beep for enabling
AM.add("enable", enabled)
# enable both lateral and longitudinal controls
enabled = True
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
v_cruise_kph = int(
round(
max(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge,
V_CRUISE_ENABLE_MIN)))
# 6 minutes driver you're on
awareness_status = 1.0
# reset the PID loops
LaC.reset()
# start long control at actual speed
LoC.reset(v_pid=CS.vEgo)
if VP.brake_only and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# *** put the adaptive in adaptive cruise control ***
AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid,
awareness_status, VP)
prof.checkpoint("AdaptiveCruise")
# *** gas/brake PID loop ***
final_gas, final_brake = LoC.update(enabled, CS.vEgo, v_cruise_kph,
AC.v_target_lead, AC.a_target,
AC.jerk_factor, VP)
# *** steering PID loop ***
final_steer, sat_flag = LaC.update(enabled, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, PP.d_poly,
angle_offset, VP)
prof.checkpoint("PID")
# ***** handle alerts ****
# send a "steering required alert" if saturation count has reached the limit
if False and sat_flag:
AM.add("steerSaturated", enabled)
if enabled and AM.alertShouldDisable():
print "DISABLING IMMEDIATELY ON ALERT"
enabled = False
if enabled and AM.alertShouldSoftDisable():
if soft_disable_timer is None:
soft_disable_timer = 3 * rate
elif soft_disable_timer == 0:
print "SOFT DISABLING ON ALERT"
enabled = False
else:
soft_disable_timer -= 1
else:
soft_disable_timer = None
# *** push the alerts to current ***
alert_text_1, alert_text_2, visual_alert, audible_alert = AM.process_alerts(
cur_time)
# ***** control the car *****
CC = car.CarControl.new_message()
CC.enabled = enabled
CC.gas = float(final_gas)
CC.brake = float(final_brake)
CC.steeringTorque = float(final_steer)
CC.cruiseControl.override = True
CC.cruiseControl.cancel = bool(
(not VP.brake_only)
or (not enabled and CS.cruiseState.enabled
)) # always cancel if we have an interceptor
CC.cruiseControl.speedOverride = float((LoC.v_pid - .3) if (
VP.brake_only and final_brake == 0.) else 0.0)
CC.cruiseControl.accelOverride = float(AC.a_pcm)
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = enabled
CC.hudControl.lanesVisible = enabled
CC.hudControl.leadVisible = bool(AC.has_lead)
CC.hudControl.visualAlert = visual_alert
CC.hudControl.audibleAlert = audible_alert
# this alert will apply next controls cycle
#if not CI.apply(CC):
# AM.add("controlsFailed", enabled)
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = CC # copy?
#carcontrol.send(cc_send.to_bytes())
sendcan.send(cc_send.to_bytes())
prof.checkpoint("CarControl")
# ***** 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 CS.errors) or rear_view_toggle
dat.live100.alertText1 = alert_text_1
dat.live100.alertText2 = alert_text_2
dat.live100.awarenessStatus = max(awareness_status,
0.0) if enabled else 0.0
# what packets were used to process
dat.live100.canMonoTimes = list(CS.canMonoTimes)
dat.live100.mdMonoTime = PP.logMonoTime
dat.live100.l20MonoTime = AC.logMonoTime
# if controls is enabled
dat.live100.enabled = enabled
# car state
dat.live100.vEgo = CS.vEgo
dat.live100.angleSteers = CS.steeringAngle
dat.live100.steerOverride = CS.steeringPressed
# longitudinal control state
dat.live100.vPid = float(LoC.v_pid)
dat.live100.vCruise = float(v_cruise_kph)
dat.live100.upAccelCmd = float(LoC.Up_accel_cmd)
dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd)
# lateral control state
dat.live100.yActual = float(LaC.y_actual)
dat.live100.yDes = float(LaC.y_des)
dat.live100.upSteer = float(LaC.Up_steer)
dat.live100.uiSteer = float(LaC.Ui_steer)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(AC.v_target_lead)
dat.live100.aTargetMin = float(AC.a_target[0])
dat.live100.aTargetMax = float(AC.a_target[1])
dat.live100.jerkFactor = float(AC.jerk_factor)
# lag
dat.live100.cumLagMs = -rk.remaining * 1000.
live100.send(dat.to_bytes())
prof.checkpoint("Live100")
# *** run loop at fixed rate ***
if rk.keep_time():
prof.display()
def update(self, logcan):
# ******************* do can recv *******************
can_pub_main = []
canMonoTimes = []
for a in messaging.drain_sock(logcan):
canMonoTimes.append(a.logMonoTime)
can_pub_main.extend(can_capnp_to_can_list_old(a, [0,2]))
cp = self.cp
cp.update_can(can_pub_main)
# copy can_valid
self.can_valid = cp.can_valid
# car params
v_weight_v = [0., 1. ] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting
self.prev_blinker_on = self.blinker_on
# ******************* parse out can *******************
self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
self.seatbelt = not cp.vl[0x305]['SEATBELT_DRIVER_LAMP'] and cp.vl[0x305]['SEATBELT_DRIVER_LATCHED']
# error 2 = temporary
# error 4 = temporary, hit a bump
# error 5 (permanent)
# error 6 = temporary
# error 7 (permanent)
#self.steer_error = cp.vl[0x18F]['STEER_STATUS'] in [5,7]
# whitelist instead of blacklist, safer at the expense of disengages
self.steer_error = cp.vl[0x18F]['STEER_STATUS'] not in [0,2,4,6]
self.steer_not_allowed = cp.vl[0x18F]['STEER_STATUS'] != 0
if cp.vl[0x18F]['STEER_STATUS'] != 0:
print cp.vl[0x18F]['STEER_STATUS']
self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel = (
cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
if not self.brake_only:
self.user_gas = cp.vl[0x201]['INTERCEPTOR_GAS']
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
#print user_gas, user_gas_pressed
if self.civic:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
self.gear = 0 # TODO: civic has CVT... needs rev engineering
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x326]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
else:
self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = cp.vl[0x1A3]['GEAR']
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,4] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.car_gas = cp.vl[0x130]['CAR_GAS']
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED']
self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']
self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
self.counter_pcm = cp.vl[0x324]['COUNTER']
return canMonoTimes
def dashboard_thread(rate=100):
kegman_valid = True
#url_string = 'http://192.168.1.61:8086/write?db=carDB'
#url_string = 'http://192.168.43.221:8086/write?db=carDB'
#url_string = 'http://192.168.137.1:8086/write?db=carDB'
#url_string = 'http://kevo.live:8086/write?db=carDB'
context = zmq.Context()
poller = zmq.Poller()
vEgo = 0.0
controlsState = messaging.sub_sock(service_list['controlsState'].port)
#controlsState = messaging.sub_sock(context, service_list['controlsState'].port, addr=ipaddress, conflate=False, poller=poller)
carState = None #messaging.sub_sock(context, service_list['carState'].port, addr=ipaddress, conflate=False, poller=poller)
liveMap = None #messaging.sub_sock(context, service_list['liveMapData'].port, addr=ipaddress, conflate=False, poller=poller)
liveStreamData = None #messaging.sub_sock(context, 8600, addr=ipaddress, conflate=False, poller=poller)
osmData = None #messaging.sub_sock(context, 8601, addr=ipaddress, conflate=False, poller=poller)
canData = None #messaging.sub_sock(context, 8602, addr=ipaddress, conflate=False, poller=poller)
#pathPlan = messaging.sub_sock(context, service_list['pathPlan'].port, addr=ipaddress, conflate=False, poller=poller)
pathPlan = messaging.sub_sock(service_list['pathPlan'].port)
liveParameters = messaging.sub_sock(service_list['liveParameters'].port)
gpsLocation = messaging.sub_sock(service_list['gpsLocation'].port)
#gpsNMEA = messaging.sub_sock(context, service_list['gpsNMEA'].port, addr=ipaddress, conflate=True)
#_controlsState = None
frame_count = 0
if len(sys.argv) < 2 or sys.argv[1] == 0:
server_address = "tcp://gernstation.synology.me"
elif int(sys.argv[1]) == 1:
server_address = "tcp://192.168.137.1"
elif int(sys.argv[1]) == 2:
server_address = "tcp://192.168.1.3"
elif int(sys.argv[1]) == 3:
server_address = "tcp://192.168.43.221"
elif int(sys.argv[1]) == 4:
server_address = "tcp://kevo.live"
else:
server_address = None
print("using %s" % (server_address))
context = zmq.Context()
if server_address != None:
steerPush = context.socket(zmq.PUSH)
steerPush.connect(server_address + ":8593")
tunePush = context.socket(zmq.PUSH)
tunePush.connect(server_address + ":8595")
tuneSub = context.socket(zmq.SUB)
tuneSub.connect(server_address + ":8596")
poller.register(tuneSub, zmq.POLLIN)
if controlsState != None: poller.register(controlsState, zmq.POLLIN)
if liveParameters != None: poller.register(liveParameters, zmq.POLLIN)
if gpsLocation != None: poller.register(gpsLocation, zmq.POLLIN)
if pathPlan != None: poller.register(pathPlan, zmq.POLLIN)
try:
if os.path.isfile('/data/kegman.json'):
with open('/data/kegman.json', 'r') as f:
config = json.load(f)
user_id = config['userID']
if server_address != None:
tunePush.send_json(config)
tunePush = None
else:
params = Params()
user_id = params.get("DongleId")
except:
params = Params()
user_id = params.get("DongleId")
config['userID'] = user_id
if server_address != None:
tunePush.send_json(config)
tunePush = None
lateral_type = ""
if server_address != None: tuneSub.setsockopt(zmq.SUBSCRIBE, str(user_id))
#influxFormatString = user_id + ",sources=capnp angle_accel=%s,damp_angle_rate=%s,angle_rate=%s,damp_angle=%s,apply_steer=%s,noise_feedback=%s,ff_standard=%s,ff_rate=%s,ff_angle=%s,angle_steers_des=%s,angle_steers=%s,dampened_angle_steers_des=%s,steer_override=%s,v_ego=%s,p2=%s,p=%s,i=%s,f=%s %s\n"
mapFormatString = "location,user="******" latitude=%s,longitude=%s,altitude=%s,speed=%s,bearing=%s,accuracy=%s,speedLimitValid=%s,speedLimit=%s,curvatureValid=%s,curvature=%s,wayId=%s,distToTurn=%s,mapValid=%s,speedAdvisoryValid=%s,speedAdvisory=%s,speedAdvisoryValid=%s,speedAdvisory=%s,speedLimitAheadValid=%s,speedLimitAhead=%s,speedLimitAheadDistance=%s %s\n"
gpsFormatString = "location,user="******" latitude=%s,longitude=%s,altitude=%s,speed=%s,bearing=%s %s\n"
canFormatString = "CANData,user="******",src=%s,pid=%s d1=%si,d2=%si "
liveStreamFormatString = "curvature,user="******" l_curv=%s,p_curv=%s,r_curv=%s,map_curv=%s,map_rcurv=%s,map_rcurvx=%s,v_curv=%s,l_diverge=%s,r_diverge=%s %s\n"
pathFormatString = "pathPlan,user="******" l0=%s,l1=%s,l2=%s,l3=%s,r0=%s,r1=%s,r2=%s,r3=%s,c0=%s,c1=%s,c2=%s,c3=%s,d0=%s,d1=%s,d2=%s,d3=%s,p0=%s,p1=%s,p2=%s,p3=%s,l_prob=%s,r_prob=%s,c_prob=%s,p_prob=%s,lane_width=%s %s\n"
pathDataFormatString = "%0.2f,%0.2f,%0.2f,%0.2f,%0.2f,%d|"
polyDataString = "%.10f,%0.8f,%0.6f,%0.4f,"
pathDataString = ""
liveParamsFormatString = "liveParameters,user="******" yaw_rate=%s,gyro_bias=%s,angle_offset=%s,angle_offset_avg=%s,tire_stiffness=%s,steer_ratio=%s %s\n"
liveParamsString = "%0.2f,%0.2f,%0.2f,%0.2f,%0.2f,%0.2f,%d|"
liveParamsDataString = ""
influxDataString = ""
kegmanDataString = ""
liveStreamDataString = ""
gpsDataString = ""
mapDataString = ""
insertString = ""
canInsertString = ""
lastGPStime = 0
lastMaptime = 0
monoTimeOffset = 0
receiveTime = 0
active = False
while 1:
for socket, event in poller.poll(0):
if socket is osmData:
if vEgo > 0 and active:
_osmData = osmData.recv_multipart()
#print(_osmData)
if socket is tuneSub:
config = json.loads(tuneSub.recv_multipart()[1])
print(config)
if len(json.dumps(config)) > 40:
print(config)
with open('/data/kegman.json', 'w') as f:
json.dump(config, f, indent=2, sort_keys=True)
os.chmod("/data/kegman.json", 0o764)
if socket is liveStreamData:
livestream = liveStreamData.recv_string() + str(
receiveTime) + "|"
if vEgo > 0 and active: liveStreamDataString += livestream
if socket is liveParameters:
_liveParams = messaging.drain_sock(socket)
for _lp in _liveParams:
#print(" Live params!")
lp = _lp.liveParameters
if vEgo > 0 and active:
liveParamsDataString += (
liveParamsString %
(lp.yawRate, lp.gyroBias, lp.angleOffset,
lp.angleOffsetAverage, lp.stiffnessFactor,
lp.steerRatio, receiveTime))
#print(liveParamsDataString)
if socket is gpsLocation:
_gps = messaging.drain_sock(socket)
for _g in _gps:
receiveTime = int(
(monoTimeOffset + _g.logMonoTime) * .0000002) * 5
if (abs(receiveTime - int(time.time() * 1000)) > 10000):
monoTimeOffset = (time.time() *
1000000000) - _g.logMonoTime
receiveTime = int(
(monoTimeOffset + _g.logMonoTime) * 0.0000002) * 5
lg = _g.gpsLocation
#print(lg)
gpsDataString += ("%f,%f,%f,%f,%f,%d|" %
(lg.latitude, lg.longitude, lg.altitude,
lg.speed, lg.bearing, receiveTime))
frame_count += 100
if socket is canData:
canString = canData.recv_string()
#print(canString)
if vEgo > 0 and active:
canInsertString += canFormatString + str(
receiveTime) + "\n~" + canString + "!"
if socket is liveMap:
_liveMap = messaging.drain_sock(socket)
for lmap in _liveMap:
if vEgo > 0 and active:
receiveTime = int(
(monoTimeOffset + lmap.logMonoTime) * .0000002) * 5
if (abs(receiveTime - int(time.time() * 1000)) >
10000):
monoTimeOffset = (time.time() *
1000000000) - lmap.logMonoTime
receiveTime = int(
(monoTimeOffset + lmap.logMonoTime) *
0.0000002) * 5
lm = lmap.liveMapData
lg = lm.lastGps
#print(lm)
mapDataString += (
"%f,%f,%f,%f,%f,%f,%d,%f,%d,%f,%f,%f,%d,%d,%f,%d,%f,%d,%f,%f,%d|"
%
(lg.latitude, lg.longitude, lg.altitude, lg.speed,
lg.bearing, lg.accuracy, lm.speedLimitValid,
lm.speedLimit, lm.curvatureValid, lm.curvature,
lm.wayId, lm.distToTurn, lm.mapValid,
lm.speedAdvisoryValid, lm.speedAdvisory,
lm.speedAdvisoryValid, lm.speedAdvisory,
lm.speedLimitAheadValid, lm.speedLimitAhead,
lm.speedLimitAheadDistance, receiveTime))
if socket is pathPlan:
_pathPlan = messaging.drain_sock(socket)
for _pp in _pathPlan:
pp = _pp.pathPlan
if vEgo > 0 and active and (carState == None
or boolStockRcvd):
boolStockRcvd = False
pathDataString += polyDataString % tuple(
map(float, pp.lPoly))
pathDataString += polyDataString % tuple(
map(float, pp.rPoly))
pathDataString += polyDataString % tuple(
map(float, pp.cPoly))
pathDataString += polyDataString % tuple(
map(float, pp.dPoly))
pathDataString += polyDataString % tuple(
map(float, pp.pPoly))
pathDataString += (
pathDataFormatString %
(pp.lProb, pp.rProb, pp.cProb, pp.pProb,
pp.laneWidth,
int((monoTimeOffset + _pp.logMonoTime) * .0000002)
* 5))
if socket is controlsState:
_controlsState = messaging.drain_sock(socket)
#print("controlsState")
for l100 in _controlsState:
if lateral_type == "":
if l100.controlsState.lateralControlState.which == "pidState":
lateral_type = "pid"
influxFormatString = user_id + ",sources=capnp ff_angle=%s,damp_angle_steers_des=%s,angle_steers_des=%s,angle_steers=%s,damp_angle_steers=%s,angle_bias=%s,steer_override=%s,v_ego=%s,p2=%s,p=%s,i=%s,f=%s,output=%s %s\n"
kegmanFormatString = user_id + ",sources=kegman KpV=%s,KiV=%s,Kf=%s,dampMPC=%s,reactMPC=%s,rate_ff_gain=%s,dampTime=%s,polyFactor=%s,reactPoly=%s,dampPoly=%s %s\n"
else:
lateral_type = "indi"
influxFormatString = user_id + ",sources=capnp angle_steers_des=%s,damp_angle_steers_des=%s,angle_steers=%s,steer_override=%s,v_ego=%s,output=%s,indi_angle=%s,indi_rate=%s,indi_rate_des=%s,indi_accel=%s,indi_accel_des=%s,accel_error=%s,delayed_output=%s,indi_delta=%s %s\n"
kegmanFormatString = user_id + ",sources=kegman time_const=%s,act_effect=%s,inner_gain=%s,outer_gain=%s,reactMPC=%s %s\n"
vEgo = l100.controlsState.vEgo
active = l100.controlsState.active
#active = True
#vEgo = 1.
#print(active)
receiveTime = int(
(monoTimeOffset + l100.logMonoTime) * .0000002) * 5
if (abs(receiveTime - int(time.time() * 1000)) > 10000):
monoTimeOffset = (time.time() *
1000000000) - l100.logMonoTime
receiveTime = int((monoTimeOffset + l100.logMonoTime) *
0.0000002) * 5
if vEgo > 0 and active:
dat = l100.controlsState
#print(dat)
if lateral_type == "pid":
influxDataString += (
"%0.3f,%0.3f,%0.3f,%0.2f,%0.3f,%0.4f,%d,%0.1f,%0.4f,%0.4f,%0.4f,%0.4f,%0.4f,%d|"
%
(dat.lateralControlState.pidState.angleFFRatio,
dat.dampAngleSteersDes, dat.angleSteersDes,
dat.angleSteers, dat.dampAngleSteers,
dat.lateralControlState.pidState.angleBias,
dat.steerOverride, vEgo,
dat.lateralControlState.pidState.p2,
dat.lateralControlState.pidState.p,
dat.lateralControlState.pidState.i,
dat.lateralControlState.pidState.f,
dat.lateralControlState.pidState.output,
receiveTime))
else:
s = dat.lateralControlState.indiState
influxDataString += (
"%0.3f,%0.2f,%0.2f,%d,%0.1f,%0.4f,%0.4f,%0.4f,%0.4f,%0.4f,%0.4f,%0.4f,%0.4f,%0.4f,%d|"
% (dat.angleSteersDes, dat.dampAngleSteersDes,
dat.angleSteers, dat.steerOverride, vEgo,
s.output, s.steerAngle, s.steerRate,
s.rateSetPoint, s.steerAccel,
s.accelSetPoint, s.accelError,
s.delayedOutput, s.delta, receiveTime))
#print(dat.upFine, dat.uiFine)
frame_count += 1
#if lastGPStime + 2.0 <= time.time():
# lastGPStime = time.time()
# _gps = messaging.recv_one_or_none(gpsNMEA)
# print(_gps)
#if lastMaptime + 2.0 <= time.time():
# lastMaptime = time.time()
# _map = messaging.recv_one_or_none(liveMap)
'''liveMapData = (
speedLimitValid = false,
speedLimit = 0,
curvatureValid = false,
curvature = 0,
wayId = 0,
lastGps = (
flags = 0,
latitude = 44.7195573,
longitude = -100.8218663,
altitude = 10542.853000000001,
speed = 0,
bearing = 0,
accuracy = 4294967.5,
timestamp = 1556581592999,
source = ublox,
vNED = [0, 0, 0],
verticalAccuracy = 3750000.2,
bearingAccuracy = 180,
speedAccuracy = 20.001 ),
distToTurn = 0,
mapValid = false,
speedAdvisoryValid = false,
speedAdvisory = 0,
speedLimitAheadValid = false,
speedLimitAhead = 0,
speedLimitAheadDistance = 0 ) )
'''
#else:
# print(time.time())
#print(frame_count)
if frame_count >= 100:
if kegman_valid:
try:
if os.path.isfile('/data/kegman.json'):
with open('/data/kegman.json', 'r') as f:
config = json.load(f)
if lateral_type == "pid":
steerKpV = config['Kp']
steerKiV = config['Ki']
steerKf = config['Kf']
reactMPC = config['reactMPC']
dampMPC = config['dampMPC']
rateFF = config['rateFFGain']
dampTime = config['dampTime']
polyFactor = config['polyFactor']
polyReact = config['polyReact']
polyDamp = config['polyDamp']
kegmanDataString += ("%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s|" % \
(steerKpV, steerKiV, steerKf, dampMPC, reactMPC, rateFF, dampTime, polyFactor, polyReact, polyDamp, receiveTime))
else:
timeConst = config['timeConst']
actEffect = config['actEffect']
innerGain = config['innerGain']
outerGain = config['outerGain']
reactMPC = config['reactMPC']
kegmanDataString += ("%s,%s,%s,%s,%s,%s|" % \
(timeConst, actEffect, innerGain, outerGain, reactMPC, receiveTime))
print(kegmanDataString, kegmanFormatString)
insertString += kegmanFormatString + "~" + kegmanDataString + "!"
except:
kegman_valid = False
if liveStreamDataString != "":
insertString += liveStreamFormatString + "~" + liveStreamDataString + "!"
#print(insertString)
liveStreamDataString = ""
insertString += influxFormatString + "~" + influxDataString + "!"
insertString += pathFormatString + "~" + pathDataString + "!"
insertString += mapFormatString + "~" + mapDataString + "!"
insertString += liveParamsFormatString + "~" + liveParamsDataString + "!"
insertString += gpsFormatString + "~" + gpsDataString + "!"
#insertString += canInsertString
#print(canInsertString)
if server_address != None:
steerPush.send_string(insertString)
print(len(insertString))
elif f_out != None:
f_out.write(insertString + "\r\n")
else:
f_out = open("/sdcard/videos/gernby-" + receiveTime + ".txt",
"w+")
frame_count = 0
influxDataString = ""
gpsDataString = ""
kegmanDataString = ""
mapDataString = ""
liveParamsDataString = ""
pathDataString = ""
insertString = ""
canInsertString = ""
else:
time.sleep(0.01)