def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP self.poller = zmq.Poller() self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller) if os.environ.get('GPS_PLANNER_ACTIVE', False): self.gps_planner_plan = messaging.sub_sock( context, service_list['gpsPlannerPlan'].port, conflate=True, poller=self.poller, addr=GPS_PLANNER_ADDR) else: self.gps_planner_plan = None self.plan = messaging.pub_sock(context, service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock( context, service_list['liveLongitudinalMpc'].port) self.last_md_ts = 0 self.last_l20_ts = 0 self.last_model = 0. self.last_l20 = 0. self.model_dead = True self.radar_dead = True self.radar_errors = [] self.PP = PathPlanner() self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.acc_start_time = sec_since_boot() self.v_acc = 0.0 self.v_acc_sol = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.a_acc_sol = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.lead_1 = None self.lead_2 = None self.longitudinalPlanSource = 'cruise' self.fcw = False self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled self.last_gps_planner_plan = None self.gps_planner_active = False
def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP self.poller = zmq.Poller() self.lat_Control = messaging.sub_sock(context, service_list['latControl'].port, conflate=True, poller=self.poller) self.plan = messaging.pub_sock(context, service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock( context, service_list['liveLongitudinalMpc'].port) self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled self.lastlat_Control = None self.params = Params()
def __init__(self, CP, fcw_enabled): self.CP = CP self.plan = messaging.pub_sock(service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock( service_list['liveLongitudinalMpc'].port) self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.v_model = 0.0 self.a_model = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled self.path_x = np.arange(192) self.params = Params()
def main(gctx=None): global gpsLocationExternal, ubloxGnss nav_frame_buffer = {} nav_frame_buffer[0] = {} for i in range(1,33): nav_frame_buffer[0][i] = {} if not CarSettings().get_value("useTeslaGPS"): gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') ubloxGnss = messaging.pub_sock('ubloxGnss') dev = init_reader() while True: try: msg = dev.receive_message() except serial.serialutil.SerialException as e: print(e) dev.close() dev = init_reader() if msg is not None: handle_msg(dev, msg, nav_frame_buffer) else: while True: time.sleep(1.1)
def __init__(self, CP, fcw_enabled): self.CP = CP self.plan = messaging.pub_sock(service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port) self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.v_model = 0.0 self.a_model = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K) self.model_v_kf_ready = False self.params = Params()
def __init__(self, gctx, rate=100): self.rate = rate # *** log *** context = zmq.Context() # pub self.live100 = messaging.pub_sock(context, service_list['live100'].port) self.carstate = messaging.pub_sock(context, service_list['carState'].port) self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port) sendcan = messaging.pub_sock(context, service_list['sendcan'].port) # sub self.thermal = messaging.sub_sock(context, service_list['thermal'].port) self.health = messaging.sub_sock(context, service_list['health'].port) logcan = messaging.sub_sock(context, service_list['can'].port) self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port) self.CC = car.CarControl.new_message() self.CI, self.CP = get_car(logcan, sendcan) self.PL = Planner(self.CP) self.AM = AlertManager() self.LoC = LongControl() self.LaC = LatControl() # write CarParams params = Params() params.put("CarParams", self.CP.to_bytes()) # fake plan self.plan_ts = 0 self.plan = log.Plan.new_message() self.plan.lateralValid = False self.plan.longitudinalValid = False # controls enabled state self.enabled = False self.last_enable_request = 0 # learned angle offset self.angle_offset = 0 # rear view camera state self.rear_view_toggle = False self.rear_view_allowed = bool(params.get("IsRearViewMirror")) self.v_cruise_kph = 255 # 0.0 - 1.0 self.awareness_status = 1.0 self.soft_disable_timer = None self.overtemp = False self.free_space = 1.0 self.cal_status = Calibration.UNCALIBRATED self.cal_perc = 0 self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000)
def steer_thread(): context = zmq.Context() sendcan = messaging.pub_sock(context, service_list['sendcan'].port) logcan = messaging.sub_sock(context, service_list['can'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) CI, CP = get_car(logcan, sendcan, None) print("got car", CP.carName) CC = car.CarControl.new_message() i = 0 rate = 0.001 direction = 1 while True: # send CS = CI.update(CC) actuators = car.CarControl.Actuators.new_message() if i > 0.9 and direction == 1: direction = -1 if i < -0.9 and direction == -1: direction = 1 i += rate * direction axis_3 = clip(i * 1.05, -1., 1.) # -1 to 1 actuators.steer = axis_3 actuators.steerAngle = axis_3 * 43. # deg print("steer", actuators.steer) CC.actuators.steer = actuators.steer CC.actuators.steerAngle = actuators.steerAngle CC.enabled = True CI.apply(CC) # broadcast carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.carState = copy(CS) carstate.send(cs_send.to_bytes()) # broadcast carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.carControl = copy(CC) carcontrol.send(cc_send.to_bytes()) # Limit to 100 frames per second time.sleep(0.01)
def radard_thread(gctx=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) mocked = CP.carName == "mock" cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface can_sock = messaging.sub_sock(service_list['can'].port) sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) RI = RadarInterface(CP) # *** publish radarState and liveTracks radarState = messaging.pub_sock(service_list['radarState'].port) liveTracks = messaging.pub_sock(service_list['liveTracks'].port) rk = Ratekeeper(rate, print_delay_threshold=None) RD = RadarD(mocked) has_radar = not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr = RI.update(can_strings) if rr is None: continue sm.update(0) dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar) dat.radarState.cumLagMs = -rk.remaining * 1000. radarState.send(dat.to_bytes()) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } liveTracks.send(dat.to_bytes()) rk.monitor_time()
def __init__(self,carstate): self.CS = carstate self.uiCustomAlert = messaging.pub_sock('uiCustomAlert') self.uiButtonInfo = messaging.pub_sock('uiButtonInfo') self.uiSetCar = messaging.pub_sock('uiSetCar') self.uiPlaySound = messaging.pub_sock('uiPlaySound') self.uiGyroInfo = messaging.pub_sock('uiGyroInfo') self.uiButtonStatus = messaging.sub_sock('uiButtonStatus', conflate=True) self.prev_cstm_message = "" self.prev_cstm_status = -1
def __init__(self, CP): self.MP = ModelParser() self.last_cloudlog_t = 0 context = zmq.Context() self.plan = messaging.pub_sock(context, service_list['pathPlan'].port) self.livempc = messaging.pub_sock(context, service_list['liveMpc'].port) self.setup_mpc(CP.steerRateCost) self.invalid_counter = 0
def __init__(self, CP): self.LP = LanePlanner(shouldUseAlca=True) self.last_cloudlog_t = 0 self.plan = messaging.pub_sock(service_list['pathPlan'].port) self.livempc = messaging.pub_sock(service_list['liveMpc'].port) self.setup_mpc(CP.steerRateCost) self.solution_invalid_cnt = 0 self.path_offset_i = 0.0
def __init__(self,carstate): self.CS = carstate context = zmq.Context() self.buttons_poller = zmq.Poller() self.uiCustomAlert = messaging.pub_sock(context, service_list['uiCustomAlert'].port) self.uiButtonInfo = messaging.pub_sock(context, service_list['uiButtonInfo'].port) self.uiSetCar = messaging.pub_sock(context, service_list['uiSetCar'].port) self.uiPlaySound = messaging.pub_sock(context, service_list['uiPlaySound'].port) self.uiButtonStatus = messaging.sub_sock(context, service_list['uiButtonStatus'].port, conflate=True, poller=self.buttons_poller) self.prev_cstm_message = "" self.prev_cstm_status = -1
def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0): self.rate = rate self.civic = True self.brake_only = False if not Plant.messaging_initialized: context = zmq.Context() Plant.logcan = messaging.pub_sock(context, service_list['can'].port) Plant.sendcan = messaging.sub_sock(context, service_list['sendcan'].port) Plant.model = messaging.pub_sock(context, service_list['model'].port) Plant.cal = messaging.pub_sock( context, service_list['liveCalibration'].port) Plant.live100 = messaging.sub_sock(context, service_list['live100'].port) Plant.plan = messaging.sub_sock(context, service_list['plan'].port) Plant.messaging_initialized = True 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()
def __init__(self, CP): self.MP = ModelParser() self.l_poly = [0., 0., 0., 0.] self.r_poly = [0., 0., 0., 0.] self.last_cloudlog_t = 0 self.plan = messaging.pub_sock(service_list['pathPlan'].port) self.livempc = messaging.pub_sock(service_list['liveMpc'].port) self.setup_mpc(CP.steerRateCost) self.solution_invalid_cnt = 0
def __init__(self, mocked, RI,use_tesla_radar, delay=0): self.current_time = 0 self.mocked = mocked self.RI = RI self.tracks = defaultdict(dict) self.last_md_ts = 0 self.last_controls_state_ts = 0 self.active = 0 # v_ego self.v_ego = 0. self.v_ego_hist = deque([0], maxlen=delay+1) self.v_ego_t_aligned = 0. self.ready = False self.icCarLR = None self.use_tesla_radar = use_tesla_radar if (RI.TRACK_RIGHT_LANE or RI.TRACK_LEFT_LANE) and self.use_tesla_radar: self.icCarLR = messaging.pub_sock('uiIcCarLR') self.lane_width = 3.0 #only used for left and right lanes self.path_x = np.arange(0.0, 160.0, 0.1) # 160 meters is max self.dPoly = [0.,0.,0.,0.]
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 main(gctx): # setup logentries. we forward log messages to it le_token = "bc65354a-b887-4ef4-8525-15dd51230e8c" le_handler = LogentriesHandler(le_token, use_tls=False) le_level = 20 #logging.INFO ctx = zmq.Context() sock = ctx.socket(zmq.PULL) sock.bind("ipc:///tmp/logmessage") # and we publish them pub_sock = messaging.pub_sock(ctx, service_list['logMessage'].port) while True: dat = ''.join(sock.recv_multipart()) # print "RECV", repr(dat) levelnum = ord(dat[0]) dat = dat[1:] if levelnum >= le_level: # push to logentries le_handler.emit_raw(dat) # then we publish them msg = messaging.new_message() msg.logMessage = dat pub_sock.send(msg.to_bytes())
def speedlimitd_thread(): context = zmq.Context() gps_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True) speedlimit_sock = messaging.pub_sock(context, service_list['speedLimit'].port) while True: gps = messaging.recv_sock(gps_sock) if gps is not None: fix_ok = gps.gpsLocationExternal.flags & 1 if fix_ok: lat = gps.gpsLocationExternal.latitude lon = gps.gpsLocationExternal.longitude try: max_speed = get_max_speed(lat, lon) dat = ui.SpeedLimitData.new_message() if max_speed: dat.speed = max_speed * CV.MPH_TO_MS speedlimit_sock.send(dat.to_bytes()) except Exception as e: print(e)
def main(gctx=None): # setup logentries. we forward log messages to it le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17" le_handler = LogentriesHandler(le_token, use_tls=False, verbose=False) le_level = 20 #logging.INFO ctx = zmq.Context().instance() sock = ctx.socket(zmq.PULL) sock.bind("ipc:///tmp/logmessage") # and we publish them pub_sock = messaging.pub_sock(service_list['logMessage'].port) while True: dat = b''.join(sock.recv_multipart()) dat = dat.decode('utf8') # print "RECV", repr(dat) levelnum = ord(dat[0]) dat = dat[1:] if levelnum >= le_level: # push to logentries # TODO: push to athena instead le_handler.emit_raw(dat) # then we publish them msg = messaging.new_message() msg.logMessage = dat pub_sock.send(msg.to_bytes())
def __init__(self): #ALCA params self.ALCA_error = False self.ALCA_lane_width = 3.6 self.ALCA_direction = 100 #none 0, left 1, right -1,reset 100 self.ALCA_step = 0 self.ALCA_total_steps = 20 * ALCA_duration_seconds #20 Hz, 5 seconds, wifey mode self.ALCA_cancelling = False self.ALCA_enabled = False self.ALCA_OFFSET_C3 = 0. self.ALCA_OFFSET_C2 = 0. self.ALCA_OFFSET_C1 = 0. self.ALCA_over_line = False self.prev_CS_ALCA_error = False self.ALCA_use_visual = True self.ALCA_vego = 0. self.ALCA_vego_prev = 0. self.alcaStatus = messaging.sub_sock('alcaStatus', conflate=True) self.alcaState = messaging.pub_sock('alcaState') self.alcas = None self.hit_prob_low = False self.hit_prob_high = False self.distance_to_line_L = 100. self.prev_distance_to_line_L = 100. self.distance_to_line_R = 100. self.prev_distance_to_line_R = 100.
def calibrationd_thread(gctx=None, addr="127.0.0.1"): context = zmq.Context() live100 = messaging.sub_sock(context, service_list['live100'].port, addr=addr, conflate=True) orbfeatures = messaging.sub_sock(context, service_list['orbFeatures'].port, addr=addr, conflate=True) livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port) calibrator = Calibrator(param_put=True) # buffer with all the messages that still need to be input into the kalman while 1: of = messaging.recv_one(orbfeatures) l100 = messaging.recv_one_or_none(live100) new_vp = calibrator.handle_orb_features(of) if DEBUG and new_vp is not None: print 'got new vp', new_vp if l100 is not None: calibrator.handle_live100(l100) if DEBUG: calibrator.handle_debug() calibrator.send_data(livecalibration)
def 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, CP, CarController): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.alca = messaging.pub_sock(service_list['alcaStatus'].port) # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) mydbc = DBC[CP.carFingerprint]['pt'] if CP.carFingerprint == CAR.MODELS and self.CS.fix1916: mydbc = mydbc + "1916" self.cp = get_can_parser(CP, mydbc) self.epas_cp = None if self.CS.useWithoutHarness: self.epas_cp = get_epas_parser(CP, 0) else: self.epas_cp = get_epas_parser(CP, 2) self.pedal_cp = get_pedal_parser(CP) self.CC = None if CarController is not None: self.CC = CarController(self.cp.dbc_name) self.compute_gb = tesla_compute_gb
def __init__(self, CP): self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR'] self.left_blinker_on = 0 self.right_blinker_on = 0 self.angle_offset = 0. self.init_angle_offset = False self.acc_slow_on = False self.pcm_acc_status = False self.setspeedoffset = 34.0 self.Angles = np.zeros(250) self.Angles_later = np.zeros(250) self.Angle_counter = 0 self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500] self.Angle_Speed = [ 255, 160, 100, 80, 70, 60, 55, 50, 40, 33, 27, 17, 12 ] if BASEDIR == "/data/openpilot" or BASEDIR == "/data/openpilot.arne182": self.traffic_data_sock = messaging.pub_sock( service_list['liveTrafficData'].port) # initialize can parser self.car_fingerprint = CP.carFingerprint # vEgo kalman filter dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], A=[[1.0, dt], [0.0, 1.0]], C=[1.0, 0.0], K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0
def boardd_proxy_loop(rate=200, address="192.168.2.251"): rk = Ratekeeper(rate) context = zmq.Context() can_init() # *** subscribes can logcan = messaging.sub_sock(context, service_list['can'].port, addr=address) # *** publishes to can send sendcan = messaging.pub_sock(context, service_list['sendcan'].port) while 1: # recv @ 100hz can_msgs = can_recv() #for m in can_msgs: # print "R:",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.to_bytes()) # 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:",hex(m[0]), str(m[2]).encode("hex") can_send_many(cl) rk.keep_time()
def __init__(self, mocked, RI): self.current_time = 0 self.mocked = mocked self.RI = RI self.tracks = defaultdict(dict) self.last_md_ts = 0 self.last_controls_state_ts = 0 self.active = 0 # v_ego self.v_ego = 0. self.v_ego_hist_t = deque([0], maxlen=v_len) self.v_ego_hist_v = deque([0], maxlen=v_len) self.v_ego_t_aligned = 0. self.ready = False self.icCarLR = None if (RI.TRACK_RIGHT_LANE or RI.TRACK_LEFT_LANE ) and CarSettings().get_value("useTeslaRadar"): self.icCarLR = messaging.pub_sock(service_list['uiIcCarLR'].port) self.lane_width = 3.0 #only used for left and right lanes self.path_x = np.arange(0.0, 160.0, 0.1) # 160 meters is max self.poller = zmq.Poller() self.pathPlanSocket = messaging.sub_sock(service_list['pathPlan'].port, conflate=True, poller=self.poller) self.dPoly = [0., 0., 0., 0.]
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 manager_thread(): # now loop context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) health_sock = messaging.sub_sock(context, service_list['health'].port) cloudlog.info("manager start") start_managed_process("logmessaged") start_managed_process("logcatd") start_managed_process("uploader") start_managed_process("ui") # *** wait for the board *** wait_for_device() # flash the device if os.getenv("NOPROG") is None: boarddir = os.path.dirname(os.path.abspath(__file__)) + "/../board/" os.system("cd %s && make" % boarddir) start_managed_process("boardd") if os.getenv("STARTALL") is not None: for p in car_started_processes: start_managed_process(p) while 1: # get health of board, log this in "thermal" td = messaging.recv_sock(health_sock, wait=True) print td # replace thermald msg = read_thermal() thermal_sock.send(msg.to_bytes()) print msg # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 # uploader is gated based on the phone temperature if max_temp > 85.0: cloudlog.info("over temp: %r", max_temp) kill_managed_process("uploader") elif max_temp < 70.0: start_managed_process("uploader") # start constellation of processes when the car starts if td.health.started: for p in car_started_processes: start_managed_process(p) else: for p in car_started_processes: kill_managed_process(p) # check the status of all processes, did any of them die? for p in running: cloudlog.info(" running %s %s" % (p, running[p]))
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 run_route(route): can = messaging.pub_sock(service_list['can'].port) CP = CarInterface.get_params(CAR.CIVIC, {}) signals, checks = get_can_signals(CP) parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1") parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1") parser_string = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1) if dict_keys_differ(parser_old.vl, parser_new.vl): return False lr = LogReader(route + ".bz2") route_ok = True t = 0 for msg in lr: if msg.which() == 'can': t += DT msg_bytes = msg.as_builder().to_bytes() can.send(msg_bytes) _, updated_old = parser_old.update(t, True) _, updated_new = parser_new.update(t, True) updated_string = parser_string.update_string(t, msg_bytes) if updated_old != updated_new: route_ok = False print(t, "Diff in seen") if updated_new != updated_string: route_ok = False print(t, "Diff in seen string") if dicts_vals_differ(parser_old.vl, parser_new.vl): print(t, "Diff in dict") route_ok = False if dicts_vals_differ(parser_new.vl, parser_string.vl): print(t, "Diff in dict string") route_ok = False return route_ok