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)