openpilot v0.5.11 release

selfdrive/locationd/Makefile

@@ -0,0 +1,86 @@
+CC = clang
+CXX = clang++
+
+ARCH := $(shell uname -m)
+OS := $(shell uname -o)
+
+BASEDIR = ../..
+PHONELIBS = ../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
+
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
+           -l:libczmq.a -l:libzmq.a \
+           -lgnustl_shared
+
+JSON_FLAGS = -I$(PHONELIBS)/json/src
+
+EXTRA_LIBS = -lpthread
+
+ifeq ($(ARCH),x86_64)
+ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib \
+           -l:libczmq.a -l:libzmq.a
+endif
+
+.PHONY: all
+all: ubloxd
+
+include ../common/cereal.mk
+
+OBJS = ublox_msg.o \
+       ubloxd_main.o \
+       ../common/swaglog.o \
+       ../common/params.o \
+       ../common/util.o \
+       $(PHONELIBS)/json/src/json.o \
+       $(CEREAL_OBJS)
+
+DEPS := $(OBJS:.o=.d) ubloxd.d ubloxd_test.d
+
+ubloxd: ubloxd.o $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(ZMQ_LIBS) \
+            $(EXTRA_LIBS)
+
+ubloxd_test: ubloxd_test.o $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(ZMQ_LIBS) \
+            $(EXTRA_LIBS)
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+          -Iinclude -I.. -I../.. \
+           $(CEREAL_CXXFLAGS) \
+           $(ZMQ_FLAGS) \
+           $(JSON_FLAGS) \
+           -I../ \
+           -I../../ \
+           -c -o '$@' '$<'
+
+%.o: %.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -MMD \
+          -Iinclude -I.. -I../.. \
+          $(CEREAL_CFLAGS) \
+          $(ZMQ_FLAGS) \
+          $(JSON_FLAGS) \
+          -c -o '$@' '$<'
+
+.PHONY: clean
+clean:
+	rm -f ubloxd ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/locationd/calibrationd.py

@@ -39,6 +39,7 @@ class Calibrator(object):
     self.vps = []
     self.cal_status = Calibration.UNCALIBRATED
     self.write_counter = 0
+    self.just_calibrated = False
     self.params = Params()
     calibration_params = self.params.get("CalibrationParams")
     if calibration_params:
@@ -52,10 +53,16 @@ class Calibrator(object):
 
 
   def update_status(self):
+    start_status = self.cal_status
     if len(self.vps) < INPUTS_NEEDED:
       self.cal_status = Calibration.UNCALIBRATED
     else:
       self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
+    end_status = self.cal_status
+
+    self.just_calibrated = False
+    if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
+      self.just_calibrated = True
 
   def handle_cam_odom(self, log):
     trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot
@@ -67,7 +74,7 @@ class Calibrator(object):
       self.vp = np.mean(self.vps, axis=0)
       self.update_status()
       self.write_counter += 1
-      if self.param_put and self.write_counter % WRITE_CYCLES == 0:
+      if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated):
         cal_params = {"vanishing_point": list(self.vp),
                       "valid_points": len(self.vps)}
         self.params.put("CalibrationParams", json.dumps(cal_params))

selfdrive/locationd/kalman/ekf_sym.py

@@ -340,7 +340,7 @@ class EKF_sym(object):
     # rewind
     if t < self.filter_time:
       if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] -1.0:
-        print "observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time)
+        print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
         return None
       rewound = self.rewind(t)
     else:
@@ -457,7 +457,7 @@ class EKF_sym(object):
 
       # TODO If nullspace isn't the dimension we want
       if A.shape[1] + He.shape[1] != A.shape[0]:
-        print 'Warning: null space projection failed, measurement ignored'
+        print('Warning: null space projection failed, measurement ignored')
         return x, P, np.zeros(A.shape[0] - He.shape[1])
 
     # if using eskf

selfdrive/locationd/locationd_local.py

@@ -73,10 +73,10 @@ class Localizer(object):
     self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
                                                                                              log.cameraOdometry.transStd]))
 
-  def handle_car_state(self, log, current_time):
+  def handle_live100(self, log, current_time):
     self.speed_counter += 1
     if self.speed_counter % 5 == 0:
-      self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.carState.vEgo]))
+      self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.live100.vEgo]))
 
   def handle_sensors(self, log, current_time):
     for sensor_reading in log.sensorEvents:
@@ -93,8 +93,8 @@ class Localizer(object):
       return
     if typ == "sensorEvents":
       self.handle_sensors(log, current_time)
-    elif typ == "carState":
-      self.handle_car_state(log, current_time)
+    elif typ == "live100":
+      self.handle_live100(log, current_time)
     elif typ == "cameraOdometry":
       self.handle_cam_odo(log, current_time)
 
@@ -113,7 +113,7 @@ class ParamsLearner(object):
     self.MAX_SR_TH = MAX_SR_TH * self.VM.sR
 
     self.alpha1 = 0.01 * learning_rate
-    self.alpha2 = 0.00025 * learning_rate
+    self.alpha2 = 0.0005 * learning_rate
     self.alpha3 = 0.1 * learning_rate
     self.alpha4 = 1.0 * learning_rate
 
@@ -154,7 +154,7 @@ class ParamsLearner(object):
       # instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa
       s4 = "Instant AO: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao))
       s5 = "Stiffnes: % .3f x" % self.x
-      print s4, s5
+      print("{0} {1}".format(s4, s5))
 
 
     self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
@@ -173,7 +173,7 @@ def locationd_thread(gctx, addr, disabled_logs):
   ctx = zmq.Context()
   poller = zmq.Poller()
 
-  car_state_socket = messaging.sub_sock(ctx, service_list['carState'].port, poller, addr=addr, conflate=True)
+  live100_socket = messaging.sub_sock(ctx, service_list['live100'].port, poller, addr=addr, conflate=True)
   sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True)
   camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True)
 
@@ -219,19 +219,19 @@ def locationd_thread(gctx, addr, disabled_logs):
       log = messaging.recv_one(socket)
       localizer.handle_log(log)
 
-      if socket is car_state_socket:
+      if socket is live100_socket:
         if not localizer.kf.t:
           continue
 
         if i % LEARNING_RATE == 0:
-          # carState is not updating the Kalman Filter, so update KF manually
+          # live100 is not updating the Kalman Filter, so update KF manually
           localizer.kf.predict(1e-9 * log.logMonoTime)
 
           predicted_state = localizer.kf.x
           yaw_rate = -float(predicted_state[5])
 
-          steering_angle = math.radians(log.carState.steeringAngle)
-          params_valid = learner.update(yaw_rate, log.carState.vEgo, steering_angle)
+          steering_angle = math.radians(log.live100.angleSteers)
+          params_valid = learner.update(yaw_rate, log.live100.vEgo, steering_angle)
 
           params = messaging.new_message()
           params.init('liveParameters')
@@ -246,6 +246,7 @@ def locationd_thread(gctx, addr, disabled_logs):
           params = learner.get_values()
           params['carFingerprint'] = CP.carFingerprint
           params_reader.put("LiveParameters", json.dumps(params))
+          params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.live100.angleModelBias}))
 
         i += 1
       elif socket is camera_odometry_socket:
@@ -263,7 +264,7 @@ def main(gctx=None, addr="127.0.0.1"):
   disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
 
   # No speed for now
-  disabled_logs.append('carState')
+  disabled_logs.append('live100')
   if IN_CAR:
     addr = "192.168.5.11"
 

selfdrive/locationd/test/ci_test.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python2
+import subprocess
+import os
+import sys
+import argparse
+import tempfile
+
+from ubloxd_py_test import parser_test
+from ubloxd_regression_test import compare_results
+
+
+def mkdirs_exists_ok(path):
+  try:
+    os.makedirs(path)
+  except OSError:
+    if not os.path.isdir(path):
+      raise
+
+
+def main(args):
+  cur_dir = os.path.dirname(os.path.realpath(__file__))
+  ubloxd_dir = os.path.join(cur_dir, '../')
+
+  cc_output_dir = os.path.join(args.output_dir, 'cc')
+  mkdirs_exists_ok(cc_output_dir)
+
+  py_output_dir = os.path.join(args.output_dir, 'py')
+  mkdirs_exists_ok(py_output_dir)
+
+  archive_file = os.path.join(cur_dir, args.stream_gz_file)
+
+  try:
+    print('Extracting stream file')
+    subprocess.check_call(['tar', 'zxf', archive_file], cwd=tempfile.gettempdir())
+    stream_file_path = os.path.join(tempfile.gettempdir(), 'ubloxRaw.stream')
+
+    if not os.path.isfile(stream_file_path):
+      print('Extract file failed')
+      sys.exit(-3)
+
+    print('Compiling test app...')
+    subprocess.check_call(["make", "ubloxd_test"], cwd=ubloxd_dir)
+
+    print('Run regression test - CC parser...')
+    if args.valgrind:
+      subprocess.check_call(["valgrind", "--leak-check=full", os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir])
+    else:
+      subprocess.check_call([os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir])
+
+    print('Running regression test - py parser...')
+    parser_test(stream_file_path, py_output_dir)
+
+    print('Running regression test - compare result...')
+    r = compare_results(cc_output_dir, py_output_dir)
+
+    print('All done!')
+
+    subprocess.check_call(["rm", stream_file_path])
+    subprocess.check_call(["rm", '-rf', cc_output_dir])
+    subprocess.check_call(["rm", '-rf', py_output_dir])
+    sys.exit(r)
+
+  except subprocess.CalledProcessError as e:
+    print('CI test failed with {}'.format(e.returncode))
+    sys.exit(e.returncode)
+
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser(description="Ubloxd CI test",
+                                   formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+  parser.add_argument("stream_gz_file", nargs='?', default='ubloxRaw.tar.gz',
+                      help="UbloxRaw data stream zip file")
+
+  parser.add_argument("output_dir", nargs='?', default='out',
+                      help="Output events temp directory")
+
+  parser.add_argument("--valgrind", default=False, action='store_true',
+                      help="Run in valgrind")
+
+  args = parser.parse_args()
+  main(args)

selfdrive/locationd/test/ubloxd_easy.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+import os
+import ublox
+from common import realtime
+from ubloxd import gen_raw, gen_solution
+import zmq
+import selfdrive.messaging as messaging
+from selfdrive.services import service_list
+
+
+unlogger = os.getenv("UNLOGGER") is not None   # debug prints
+
+def main(gctx=None):
+  context = zmq.Context()
+  poller = zmq.Poller()
+
+  context = zmq.Context()
+  gpsLocationExternal = messaging.pub_sock(context, service_list['gpsLocationExternal'].port)
+  ubloxGnss = messaging.pub_sock(context, service_list['ubloxGnss'].port)
+
+  # ubloxRaw = messaging.sub_sock(context, 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
+        time = log.logMonoTime
+        msg = ublox.UBloxMessage()
+        msg.add(buff)
+        if msg.valid():
+          if msg.name() == 'NAV_PVT':
+            sol = gen_solution(msg)
+            if unlogger:
+              sol.logMonoTime = time
+            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 = time
+            else:
+              raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+            ubloxGnss.send(raw.to_bytes())
+        else:
+          print "INVALID MESSAGE"
+
+
+if __name__ == "__main__":
+  main()

selfdrive/locationd/test/ubloxd_py_test.py

@@ -0,0 +1,77 @@
+import sys
+import os
+
+from ublox import UBloxMessage
+from ubloxd import gen_solution, gen_raw, gen_nav_data
+from common import realtime
+
+
+def mkdirs_exists_ok(path):
+  try:
+    os.makedirs(path)
+  except OSError:
+    if not os.path.isdir(path):
+      raise
+
+
+def parser_test(fn, prefix):
+  nav_frame_buffer = {}
+  nav_frame_buffer[0] = {}
+  for i in xrange(1, 33):
+    nav_frame_buffer[0][i] = {}
+
+  if not os.path.exists(prefix):
+    print('Prefix invalid')
+    sys.exit(-1)
+
+  with open(fn, 'rb') as f:
+    i = 0
+    saved_i = 0
+    msg = UBloxMessage()
+    while True:
+      n = msg.needed_bytes()
+      b = f.read(n)
+      if not b:
+        break
+      msg.add(b)
+      if msg.valid():
+        i += 1
+        if msg.name() == 'NAV_PVT':
+          sol = gen_solution(msg)
+          sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+          with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+            f1.write(sol.to_bytes())
+            saved_i += 1
+        elif msg.name() == 'RXM_RAW':
+          raw = gen_raw(msg)
+          raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+          with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+            f1.write(raw.to_bytes())
+            saved_i += 1
+        elif msg.name() == 'RXM_SFRBX':
+          nav = gen_nav_data(msg, nav_frame_buffer)
+          if nav is not None:
+            nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+            with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+              f1.write(nav.to_bytes())
+              saved_i += 1
+
+        msg = UBloxMessage()
+        msg.debug_level = 0
+    print('Parsed {} msgs'.format(i))
+    print('Generated {} cereal events'.format(saved_i))
+
+
+if __name__ == "__main__":
+  if len(sys.argv) < 3:
+    print('Format: ubloxd_py_test.py file_path prefix')
+    sys.exit(0)
+
+  fn = sys.argv[1]
+  if not os.path.isfile(fn):
+    print('File path invalid')
+    sys.exit(0)
+
+  prefix = sys.argv[2]
+  mkdirs_exists_ok(prefix)
+  parser_test(fn, prefix)

selfdrive/locationd/test/ubloxd_regression_test.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python
+import os
+import sys
+import argparse
+
+from cereal import log
+from common.basedir import BASEDIR
+os.environ['BASEDIR'] = BASEDIR
+
+
+def get_arg_parser():
+  parser = argparse.ArgumentParser(
+      description="Compare two result files",
+      formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+  parser.add_argument("dir1", nargs='?', default='/data/ubloxdc',
+                      help="Directory path 1 from which events are loaded")
+
+  parser.add_argument("dir2", nargs='?', default='/data/ubloxdpy',
+                      help="Directory path 2 from which msgs are loaded")
+
+  return parser
+
+
+def read_file(fn):
+  with open(fn, 'rb') as f:
+    return f.read()
+
+
+def compare_results(dir1, dir2):
+  onlyfiles1 = [f for f in os.listdir(dir1) if os.path.isfile(os.path.join(dir1, f))]
+  onlyfiles1.sort()
+
+  onlyfiles2 = [f for f in os.listdir(dir2) if os.path.isfile(os.path.join(dir2, f))]
+  onlyfiles2.sort()
+
+  if len(onlyfiles1) != len(onlyfiles2):
+    print('len mismatch: {} != {}'.format(len(onlyfiles1), len(onlyfiles2)))
+    return -1
+  events1 = [log.Event.from_bytes(read_file(os.path.join(dir1, f))) for f in onlyfiles1]
+  events2 = [log.Event.from_bytes(read_file(os.path.join(dir2, f))) for f in onlyfiles2]
+
+  for i in range(len(events1)):
+    if events1[i].which() != events2[i].which():
+      print('event {} type mismatch: {} != {}'.format(i, events1[i].which(), events2[i].which()))
+      return -2
+    if events1[i].which() == 'gpsLocationExternal':
+      old_gps = events1[i].gpsLocationExternal
+      gps = events2[i].gpsLocationExternal
+      # print(gps, old_gps)
+      attrs = ['flags', 'latitude', 'longitude', 'altitude', 'speed', 'bearing',
+               'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracy', 'speedAccuracy']
+      for attr in attrs:
+        o = getattr(old_gps, attr)
+        n = getattr(gps, attr)
+        if attr == 'vNED':
+          if len(o) != len(n):
+            print('Gps vNED len mismatch', o, n)
+            return -3
+          else:
+            for i in range(len(o)):
+              if abs(o[i] - n[i]) > 1e-3:
+                print('Gps vNED mismatch', o, n)
+                return
+        elif o != n:
+          print('Gps mismatch', attr, o, n)
+          return -4
+    elif events1[i].which() == 'ubloxGnss':
+      old_gnss = events1[i].ubloxGnss
+      gnss = events2[i].ubloxGnss
+      if old_gnss.which() == 'measurementReport' and gnss.which() == 'measurementReport':
+        attrs = ['gpsWeek', 'leapSeconds', 'measurements', 'numMeas', 'rcvTow', 'receiverStatus', 'schema']
+        for attr in attrs:
+          o = getattr(old_gnss.measurementReport, attr)
+          n = getattr(gnss.measurementReport, attr)
+          if str(o) != str(n):
+            print('measurementReport {} mismatched'.format(attr))
+            return -5
+        if not (str(old_gnss.measurementReport) == str(gnss.measurementReport)):
+          print('Gnss measurementReport mismatched!')
+          print('gnss measurementReport old', old_gnss.measurementReport.measurements)
+          print('gnss measurementReport new', gnss.measurementReport.measurements)
+          return -6
+      elif old_gnss.which() == 'ephemeris' and gnss.which() == 'ephemeris':
+        if not (str(old_gnss.ephemeris) == str(gnss.ephemeris)):
+          print('Gnss ephemeris mismatched!')
+          print('gnss ephemeris old', old_gnss.ephemeris)
+          print('gnss ephemeris new', gnss.ephemeris)
+          return -7
+  print('All {} events matched!'.format(len(events1)))
+  return 0
+
+
+if __name__ == "__main__":
+  args = get_arg_parser().parse_args(sys.argv[1:])
+  compare_results(args.dir1, args.dir2)

selfdrive/locationd/ublox_msg.cc

@@ -0,0 +1,375 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <assert.h>
+#include <math.h>
+#include <ctime>
+#include <chrono>
+#include <map>
+#include <vector>
+#include <algorithm>
+
+#include <zmq.h>
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/params.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+#include "ublox_msg.h"
+
+#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4])
+#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1))
+
+namespace ublox {
+
+inline int twos_complement(uint32_t v, uint32_t nb) {
+  int sign = v >> (nb - 1);
+  int value = v;
+  if(sign != 0)
+    value = value - (1 << nb);
+  return value;
+}
+
+inline int GET_FIELD_S(uint32_t w, uint32_t nb, uint32_t pos) {
+  int v = GET_FIELD_U(w, nb, pos);
+  return twos_complement(v, nb);
+}
+
+class EphemerisData {
+  public:
+    EphemerisData(uint8_t svId, subframes_map subframes) {
+      this->svId = svId;
+      int week_no = GET_FIELD_U(subframes[1][2+0], 10, 20);
+      int t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6);
+      int iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U(
+        subframes[1][2+5], 8, 22);
+
+      int t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6);
+      int a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22);
+      int a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6);
+      int a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8);
+
+      int c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6);
+      int delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14);
+      int m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U(
+        subframes[2][2+2], 24, 6);
+      int c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14);
+      int e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6);
+      int c_us = GET_FIELD_S(subframes[2][2+5], 16, 14);
+      uint32_t a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U(
+        subframes[2][2+6], 24, 6);
+      int t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14);
+
+      int c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14);
+      int omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U(
+        subframes[3][2+1], 24, 6);
+      int c_is = GET_FIELD_S(subframes[3][2+2], 16, 14);
+      int i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U(
+        subframes[3][2+3], 24, 6);
+      int c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14);
+      int w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6);
+      int omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6);
+      int idot = GET_FIELD_S(subframes[3][2+7], 14, 8);
+
+      this->_rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6);
+      this->_rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6);
+      this->_rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6);
+      this->_rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14);
+      this->aodo = GET_FIELD_U(subframes[2][2+7], 5, 8);
+
+      double gpsPi = 3.1415926535898;
+
+      // now form variables in radians, meters and seconds etc
+      this->Tgd = t_gd * pow(2, -31);
+      this->A = pow(a_powhalf * pow(2, -19), 2.0);
+      this->cic = c_ic * pow(2, -29);
+      this->cis = c_is * pow(2, -29);
+      this->crc = c_rc * pow(2, -5);
+      this->crs = c_rs * pow(2, -5);
+      this->cuc = c_uc * pow(2, -29);
+      this->cus = c_us * pow(2, -29);
+      this->deltaN = delta_n * pow(2, -43) * gpsPi;
+      this->ecc = e * pow(2, -33);
+      this->i0 = i_0 * pow(2, -31) * gpsPi;
+      this->idot = idot * pow(2, -43) * gpsPi;
+      this->M0 = m_0 * pow(2, -31) * gpsPi;
+      this->omega = w * pow(2, -31) * gpsPi;
+      this->omega_dot = omega_dot * pow(2, -43) * gpsPi;
+      this->omega0 = omega_0 * pow(2, -31) * gpsPi;
+      this->toe = t_oe * pow(2, 4);
+
+      this->toc = t_oc * pow(2, 4);
+      this->gpsWeek = week_no;
+      this->af0 = a_f0 * pow(2, -31);
+      this->af1 = a_f1 * pow(2, -43);
+      this->af2 = a_f2 * pow(2, -55);
+
+      uint32_t iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22);
+      uint32_t iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22);
+      this->valid = (iode1 == iode2) && (iode1 == (iodc & 0xff));
+      this->iode = iode1;
+
+      if (GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 &&
+        GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 &&
+        GET_FIELD_U(subframes[5][2+0], 2, 28) == 1) {
+        double a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30);
+        double a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27);
+        double a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24);
+        double a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24);
+        double b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11);
+        double b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14);
+        double b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16);
+        double b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16);
+        this->ionoAlpha[0] = a0;this->ionoAlpha[1] = a1;this->ionoAlpha[2] = a2;this->ionoAlpha[3] = a3;
+        this->ionoBeta[0] = b0;this->ionoBeta[1] = b1;this->ionoBeta[2] = b2;this->ionoBeta[3] = b3;
+        this->ionoCoeffsValid = true;
+      } else {
+        this->ionoCoeffsValid = false;
+      }
+    }
+    uint16_t svId;
+    double Tgd, A, cic, cis, crc, crs, cuc, cus, deltaN, ecc, i0, idot, M0, omega, omega_dot, omega0, toe, toc;
+    uint32_t gpsWeek, iode, _rsvd1, _rsvd2, _rsvd3, _rsvd4, aodo;
+    double af0, af1, af2;
+    bool valid;
+    double ionoAlpha[4], ionoBeta[4];
+    bool ionoCoeffsValid;
+};
+
+UbloxMsgParser::UbloxMsgParser() :bytes_in_parse_buf(0) {
+  nav_frame_buffer[0U] = std::map<uint8_t, subframes_map>();
+  for(int i = 1;i < 33;i++)
+    nav_frame_buffer[0U][i] = subframes_map();
+}
+
+inline int UbloxMsgParser::needed_bytes() {
+  // Msg header incomplete?
+  if(bytes_in_parse_buf < UBLOX_HEADER_SIZE)
+    return UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf;
+  uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE;
+  // too much data
+  if(needed < (uint16_t)bytes_in_parse_buf)
+    return -1;
+  return needed - (uint16_t)bytes_in_parse_buf;
+}
+
+inline bool UbloxMsgParser::valid_cheksum() {
+  uint8_t ck_a = 0, ck_b = 0;
+  for(int i = 2; i < bytes_in_parse_buf - UBLOX_CHECKSUM_SIZE;i++) {
+    ck_a = (ck_a + msg_parse_buf[i]) & 0xFF;
+    ck_b = (ck_b + ck_a) & 0xFF;
+  }
+  if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
+    LOGD("Checksum a mismtach: %02X, %02X", ck_a, msg_parse_buf[6]);
+    return false;
+  }
+  if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
+    LOGD("Checksum b mismtach: %02X, %02X", ck_b, msg_parse_buf[7]);
+    return false;
+  }
+  return true;
+}
+
+inline bool UbloxMsgParser::valid() {
+  return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE &&
+    needed_bytes() == 0 &&
+    valid_cheksum();
+}
+
+inline bool UbloxMsgParser::valid_so_far() {
+  if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != PREAMBLE1) {
+    //LOGD("PREAMBLE1 invalid, %02X.", msg_parse_buf[0]);
+    return false;
+  }
+  if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != PREAMBLE2) {
+    //LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]);
+    return false;
+  }
+  if(needed_bytes() == 0 && !valid())
+    return false;
+  return true;
+}
+
+kj::Array<capnp::word> UbloxMsgParser::gen_solution() {
+  nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
+  capnp::MallocMessageBuilder msg_builder;
+  cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  auto gpsLoc = event.initGpsLocationExternal();
+  gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX);
+  gpsLoc.setFlags(msg->flags);
+  gpsLoc.setLatitude(msg->lat * 1e-07);
+  gpsLoc.setLongitude(msg->lon * 1e-07);
+  gpsLoc.setAltitude(msg->height * 1e-03);
+  gpsLoc.setSpeed(msg->gSpeed * 1e-03);
+  gpsLoc.setBearing(msg->headMot * 1e-5);
+  gpsLoc.setAccuracy(msg->hAcc * 1e-03);
+  std::tm timeinfo = std::tm();
+  timeinfo.tm_year = msg->year - 1900;
+  timeinfo.tm_mon = msg->month - 1;
+  timeinfo.tm_mday = msg->day;
+  timeinfo.tm_hour = msg->hour;
+  timeinfo.tm_min = msg->min;
+  timeinfo.tm_sec = msg->sec;
+  std::time_t utc_tt = timegm(&timeinfo);
+  gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06);
+  float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f };
+  kj::ArrayPtr<const float> ap(&f[0], sizeof(f) / sizeof(f[0]));
+  gpsLoc.setVNED(ap);
+  gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03);
+  gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03);
+  gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05);
+  return capnp::messageToFlatArray(msg_builder);
+}
+
+inline bool bit_to_bool(uint8_t val, int shifts) {
+  return (val & (1 << shifts)) ? true : false;
+}
+
+kj::Array<capnp::word> UbloxMsgParser::gen_raw() {
+  rxm_raw_msg *msg = (rxm_raw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
+  if(bytes_in_parse_buf != (
+    UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg) + msg->numMeas * sizeof(rxm_raw_msg_extra) + UBLOX_CHECKSUM_SIZE
+    )) {
+    LOGD("Invalid measurement size %u, %u, %u, %u", msg->numMeas, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg));
+    return kj::Array<capnp::word>();
+  }
+  rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)];
+  capnp::MallocMessageBuilder msg_builder;
+  cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  auto gnss = event.initUbloxGnss();
+  auto mr = gnss.initMeasurementReport();
+  mr.setRcvTow(msg->rcvTow);
+  mr.setGpsWeek(msg->week);
+  mr.setLeapSeconds(msg->leapS);
+  mr.setGpsWeek(msg->week);
+  auto mb = mr.initMeasurements(msg->numMeas);
+  for(int8_t i = 0; i < msg->numMeas; i++) {
+    mb[i].setSvId(measurements[i].svId);
+    mb[i].setSigId(measurements[i].sigId);
+    mb[i].setPseudorange(measurements[i].prMes);
+    mb[i].setCarrierCycles(measurements[i].cpMes);
+    mb[i].setDoppler(measurements[i].doMes);
+    mb[i].setGnssId(measurements[i].gnssId);
+    mb[i].setGlonassFrequencyIndex(measurements[i].freqId);
+    mb[i].setLocktime(measurements[i].locktime);
+    mb[i].setCno(measurements[i].cno);
+    mb[i].setPseudorangeStdev(0.01*(pow(2, (measurements[i].prStdev & 15)))); // weird scaling, might be wrong
+    mb[i].setCarrierPhaseStdev(0.004*(measurements[i].cpStdev & 15));
+    mb[i].setDopplerStdev(0.002*(pow(2, (measurements[i].doStdev & 15)))); // weird scaling, might be wrong
+    auto ts = mb[i].initTrackingStatus();
+    ts.setPseudorangeValid(bit_to_bool(measurements[i].trkStat, 0));
+    ts.setCarrierPhaseValid(bit_to_bool(measurements[i].trkStat, 1));
+    ts.setHalfCycleValid(bit_to_bool(measurements[i].trkStat, 2));
+    ts.setHalfCycleSubtracted(bit_to_bool(measurements[i].trkStat, 3));
+  }
+
+  mr.setNumMeas(msg->numMeas);
+  auto rs = mr.initReceiverStatus();
+  rs.setLeapSecValid(bit_to_bool(msg->recStat, 0));
+  rs.setClkReset(bit_to_bool(msg->recStat, 2));
+  return capnp::messageToFlatArray(msg_builder);
+}
+
+kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() {
+  rxm_sfrbx_msg *msg = (rxm_sfrbx_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
+  if(bytes_in_parse_buf != (
+    UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg) + msg->numWords * sizeof(rxm_sfrbx_msg_extra) + UBLOX_CHECKSUM_SIZE
+    )) {
+    LOGD("Invalid sfrbx words size %u, %u, %u, %u", msg->numWords, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg));
+    return kj::Array<capnp::word>();
+  }
+  rxm_sfrbx_msg_extra *measurements = (rxm_sfrbx_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg)];
+  if(msg->gnssId  == 0) {
+    uint8_t subframeId =  GET_FIELD_U(measurements[1].dwrd, 3, 8);
+    std::vector<uint32_t> words;
+    for(int i = 0; i < msg->numWords;i++)
+      words.push_back(measurements[i].dwrd);
+
+    if(subframeId == 1) {
+      nav_frame_buffer[msg->gnssId][msg->svid] = subframes_map();
+      nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words;
+    } else if(nav_frame_buffer[msg->gnssId][msg->svid].find(subframeId-1) != nav_frame_buffer[msg->gnssId][msg->svid].end())
+      nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words;
+    if(nav_frame_buffer[msg->gnssId][msg->svid].size() == 5) {
+      EphemerisData ephem_data(msg->svid, nav_frame_buffer[msg->gnssId][msg->svid]);
+      capnp::MallocMessageBuilder msg_builder;
+      cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
+      event.setLogMonoTime(nanos_since_boot());
+      auto gnss = event.initUbloxGnss();
+      auto eph = gnss.initEphemeris();
+      eph.setSvId(ephem_data.svId);
+      eph.setToc(ephem_data.toc);
+      eph.setGpsWeek(ephem_data.gpsWeek);
+      eph.setAf0(ephem_data.af0);
+      eph.setAf1(ephem_data.af1);
+      eph.setAf2(ephem_data.af2);
+      eph.setIode(ephem_data.iode);
+      eph.setCrs(ephem_data.crs);
+      eph.setDeltaN(ephem_data.deltaN);
+      eph.setM0(ephem_data.M0);
+      eph.setCuc(ephem_data.cuc);
+      eph.setEcc(ephem_data.ecc);
+      eph.setCus(ephem_data.cus);
+      eph.setA(ephem_data.A);
+      eph.setToe(ephem_data.toe);
+      eph.setCic(ephem_data.cic);
+      eph.setOmega0(ephem_data.omega0);
+      eph.setCis(ephem_data.cis);
+      eph.setI0(ephem_data.i0);
+      eph.setCrc(ephem_data.crc);
+      eph.setOmega(ephem_data.omega);
+      eph.setOmegaDot(ephem_data.omega_dot);
+      eph.setIDot(ephem_data.idot);
+      eph.setTgd(ephem_data.Tgd);
+      eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid);
+      if(ephem_data.ionoCoeffsValid) {
+        kj::ArrayPtr<const double> apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0]));
+        eph.setIonoAlpha(apa);
+        kj::ArrayPtr<const double> apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0]));
+        eph.setIonoBeta(apb);
+      } else {
+        eph.setIonoAlpha(kj::ArrayPtr<const double>());
+        eph.setIonoBeta(kj::ArrayPtr<const double>());
+      }
+      return capnp::messageToFlatArray(msg_builder);
+    }
+  }
+  return kj::Array<capnp::word>();
+}
+
+bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
+  int needed = needed_bytes();
+  if(needed > 0) {
+    bytes_consumed = min((size_t)needed, incoming_data_len );
+    // Add data to buffer
+    memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
+    bytes_in_parse_buf += bytes_consumed;
+  } else {
+    bytes_consumed = incoming_data_len;
+  }
+  // Validate msg format, detect invalid header and invalid checksum.
+  while(!valid_so_far() && bytes_in_parse_buf != 0) {
+    //LOGD("Drop corrupt data, remained in buf: %u", bytes_in_parse_buf);
+    // Corrupted msg, drop a byte.
+    bytes_in_parse_buf -= 1;
+    if(bytes_in_parse_buf > 0)
+      memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf);
+  }
+  // There is redundant data at the end of buffer, reset the buffer.
+  if(needed_bytes() == -1)
+    bytes_in_parse_buf = 0;
+  return valid();
+}
+
+}

selfdrive/locationd/ublox_msg.h

@@ -0,0 +1,149 @@
+#pragma once
+
+#include <stdint.h>
+
+#define min(x, y) ((x) <= (y) ? (x) : (y))
+
+// NAV_PVT
+typedef struct __attribute__((packed)) {
+  uint32_t iTOW;
+  uint16_t year;
+  int8_t month;
+  int8_t day;
+  int8_t hour;
+  int8_t min;
+  int8_t sec;
+  int8_t valid;
+  uint32_t tAcc;
+  int32_t nano;
+  int8_t fixType;
+  int8_t flags;
+  int8_t flags2;
+  int8_t numSV;
+  int32_t lon;
+  int32_t lat;
+  int32_t height;
+  int32_t hMSL;
+  uint32_t hAcc;
+  uint32_t vAcc;
+  int32_t velN;
+  int32_t velE;
+  int32_t velD;
+  int32_t gSpeed;
+  int32_t headMot;
+  uint32_t sAcc;
+  uint32_t headAcc;
+  uint16_t pDOP;
+  int8_t reserverd1[6];
+  int32_t headVeh;
+  int16_t magDec;
+  uint16_t magAcc;
+} nav_pvt_msg;
+
+// RXM_RAW
+typedef struct __attribute__((packed)) {
+  double rcvTow;
+  uint16_t week;
+  int8_t leapS;
+  int8_t numMeas;
+  int8_t recStat;
+  int8_t reserved1[3];
+} rxm_raw_msg;
+
+// Extra data count is in numMeas
+typedef struct __attribute__((packed)) {
+  double prMes;
+  double cpMes;
+  float doMes;
+  int8_t gnssId;
+  int8_t svId;
+  int8_t sigId;
+  int8_t freqId;
+  uint16_t locktime;
+  int8_t cno;
+  int8_t prStdev;
+  int8_t cpStdev;
+  int8_t doStdev;
+  int8_t trkStat;
+  int8_t reserved3;
+} rxm_raw_msg_extra;
+// RXM_SFRBX
+typedef struct __attribute__((packed)) {
+  int8_t gnssId;
+  int8_t svid;
+  int8_t reserved1;
+  int8_t freqId;
+  int8_t numWords;
+  int8_t reserved2;
+  int8_t version;
+  int8_t reserved3;
+} rxm_sfrbx_msg;
+
+// Extra data count is in numWords
+typedef struct __attribute__((packed)) {
+  uint32_t dwrd;
+} rxm_sfrbx_msg_extra;
+
+namespace ublox {
+  // protocol constants
+  const uint8_t PREAMBLE1 = 0xb5;
+  const uint8_t PREAMBLE2 = 0x62;
+
+  // message classes
+  const uint8_t CLASS_NAV = 0x01;
+  const uint8_t CLASS_RXM = 0x02;
+
+  // NAV messages
+  const uint8_t MSG_NAV_PVT = 0x7;
+
+  // RXM messages
+  const uint8_t MSG_RXM_RAW = 0x15;
+  const uint8_t MSG_RXM_SFRBX = 0x13;
+
+  const int UBLOX_HEADER_SIZE = 6;
+  const int UBLOX_CHECKSUM_SIZE = 2;
+  const int UBLOX_MAX_MSG_SIZE = 65536;
+
+  typedef std::map<uint8_t, std::vector<uint32_t>> subframes_map;
+
+  class UbloxMsgParser {
+    public:
+
+      UbloxMsgParser();
+      kj::Array<capnp::word> gen_solution();
+      kj::Array<capnp::word> gen_raw();
+
+      kj::Array<capnp::word> gen_nav_data();
+      bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
+      inline void reset() {bytes_in_parse_buf = 0;}
+      inline uint8_t msg_class() {
+        return msg_parse_buf[2];
+      }
+
+      inline uint8_t msg_id() {
+        return msg_parse_buf[3];
+      }
+      inline int needed_bytes();
+
+      void hexdump(uint8_t *d, int l) {
+        for (int i = 0; i < l; i++) {
+          if (i%0x10 == 0 && i != 0) printf("\n");
+          printf("%02X ", d[i]);
+        }
+        printf("\n");
+      }
+    private:
+      inline bool valid_cheksum();
+      inline bool valid();
+      inline bool valid_so_far();
+
+      uint8_t msg_parse_buf[UBLOX_HEADER_SIZE + UBLOX_MAX_MSG_SIZE];
+      int bytes_in_parse_buf;
+      std::map<uint8_t, std::map<uint8_t, subframes_map>> nav_frame_buffer;
+  };
+
+}
+
+typedef int (*poll_ubloxraw_msg_func)(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg);
+typedef int (*send_gps_event_func)(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags);
+int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func);

selfdrive/locationd/ubloxd.cc

@@ -0,0 +1,45 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <assert.h>
+#include <math.h>
+#include <ctime>
+#include <chrono>
+#include <map>
+#include <vector>
+
+#include <zmq.h>
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/params.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+#include "ublox_msg.h"
+
+const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds
+
+int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) {
+  int err;
+  zmq_pollitem_t item = {.socket = subscriber, .events = ZMQ_POLLIN};
+  err = zmq_poll (&item, 1, ZMQ_POLL_TIMEOUT);
+  if(err <= 0)
+    return err;
+  return zmq_msg_recv(msg, subscriber, 0);
+}
+
+int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) {
+  return zmq_send(s, buf, len, flags);
+}
+
+int main() {
+  return ubloxd_main(poll_ubloxraw_msg, send_gps_event);
+}

selfdrive/locationd/ubloxd_main.cc

@@ -0,0 +1,113 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <assert.h>
+#include <math.h>
+#include <ctime>
+#include <chrono>
+#include <map>
+#include <vector>
+
+#include <zmq.h>
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/params.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+#include "ublox_msg.h"
+
+volatile int do_exit = 0; // Flag for process exit on signal
+
+void set_do_exit(int sig) {
+  do_exit = 1;
+}
+
+using namespace ublox;
+
+int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) {
+  LOGW("starting ubloxd");
+  signal(SIGINT, (sighandler_t) set_do_exit);
+  signal(SIGTERM, (sighandler_t) set_do_exit);
+
+  UbloxMsgParser parser;
+  void *context = zmq_ctx_new();
+  void *gpsLocationExternal = zmq_socket(context, ZMQ_PUB);
+  zmq_bind(gpsLocationExternal, "tcp://*:8032");
+  void *ubloxGnss = zmq_socket(context, ZMQ_PUB);
+  zmq_bind(ubloxGnss, "tcp://*:8033");
+  // ubloxRaw = 8042
+  void *subscriber = zmq_socket(context, ZMQ_SUB);
+  zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
+  zmq_connect(subscriber, "tcp://127.0.0.1:8042");
+  while (!do_exit) {
+    zmq_msg_t msg;
+    zmq_msg_init(&msg);
+    int err = poll_func(gpsLocationExternal, ubloxGnss, subscriber, &msg);
+    if(err < 0) {
+      LOGE_100("zmq_poll error %s in %s", strerror(errno ), __FUNCTION__);
+      break;
+    } else if(err == 0) {
+      continue;
+    }
+    // format for board, make copy due to alignment issues, will be freed on out of scope
+    auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
+    memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
+    capnp::FlatArrayMessageReader cmsg(amsg);
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+    const uint8_t *data = event.getUbloxRaw().begin();
+    size_t len = event.getUbloxRaw().size();
+    size_t bytes_consumed = 0;
+    while(bytes_consumed < len && !do_exit) {
+      size_t bytes_consumed_this_time = 0U;
+      if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
+        // New message available
+        if(parser.msg_class() == CLASS_NAV) {
+          if(parser.msg_id() == MSG_NAV_PVT) {
+            LOGD("MSG_NAV_PVT");
+            auto words = parser.gen_solution();
+            if(words.size() > 0) {
+              auto bytes = words.asBytes();
+              send_func(parser.msg_class(), parser.msg_id(), gpsLocationExternal, bytes.begin(), bytes.size(), 0);
+            }
+          } else
+            LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
+        } else if(parser.msg_class() == CLASS_RXM) {
+          if(parser.msg_id() == MSG_RXM_RAW) {
+            LOGD("MSG_RXM_RAW");
+            auto words = parser.gen_raw();
+            if(words.size() > 0) {
+              auto bytes = words.asBytes();
+              send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0);
+            }
+          } else if(parser.msg_id() == MSG_RXM_SFRBX) {
+            LOGD("MSG_RXM_SFRBX");
+            auto words = parser.gen_nav_data();
+            if(words.size() > 0) {
+              auto bytes = words.asBytes();
+              send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0);
+            }
+          } else
+            LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
+        } else
+          LOGW("Unknown msg class: 0x%02X", parser.msg_class());
+        parser.reset();
+      }
+      bytes_consumed += bytes_consumed_this_time;
+    }
+    zmq_msg_close(&msg);
+  }
+  zmq_close(subscriber);
+  zmq_close(gpsLocationExternal);
+  zmq_close(ubloxGnss);
+  zmq_ctx_destroy(context);
+  return 0;
+}

selfdrive/locationd/ubloxd_test.cc

@@ -0,0 +1,103 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <assert.h>
+#include <math.h>
+#include <ctime>
+#include <chrono>
+#include <map>
+#include <vector>
+#include <iostream>
+
+#include <zmq.h>
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/params.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+#include "ublox_msg.h"
+
+using namespace ublox;
+
+void write_file(std::string fpath, uint8_t *data, int len) {
+  FILE* f = fopen(fpath.c_str(), "wb");
+  if (!f) {
+    std::cout << "Open " << fpath << " failed" << std::endl;
+    return;
+  }
+  fwrite(data, len, 1, f);
+  fclose(f);
+}
+
+static size_t len = 0U;
+static size_t consumed = 0U;
+static uint8_t *data = NULL;
+static int save_idx = 0;
+static std::string prefix;
+static void *gps_sock, *ublox_gnss_sock;
+
+int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) {
+  gps_sock = gpsLocationExternal;
+  ublox_gnss_sock = ubloxGnss;
+  size_t consuming  = min(len - consumed, 128);
+  if(consumed < len) {
+    // create message
+    capnp::MallocMessageBuilder msg_builder;
+    cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
+    auto ublox_raw = event.initUbloxRaw(consuming);
+    memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming);
+    auto words = capnp::messageToFlatArray(msg_builder);
+    auto bytes = words.asBytes();
+    zmq_msg_init_size (msg, bytes.size());
+    memcpy (zmq_msg_data(msg), (void *)bytes.begin(), bytes.size());
+    consumed += consuming;
+    return 1;
+  } else
+    return -1;
+}
+
+int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) {
+  if(msg_cls == CLASS_NAV && msg_id == MSG_NAV_PVT)
+    assert(s == gps_sock);
+  else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_RAW)
+    assert(s == ublox_gnss_sock);
+  else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_SFRBX)
+    assert(s == ublox_gnss_sock);
+  else
+    assert(0);
+  write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, len);
+  save_idx ++;
+  return len;
+}
+
+int main(int argc, char** argv) {
+  if(argc < 3) {
+    printf("Format: ubloxd_test stream_file_path save_prefix\n");
+    return 0;
+  }
+  // Parse 11360 msgs, generate 9452 events
+  data = (uint8_t *)read_file(argv[1], &len);
+  if(data == NULL) {
+    LOGE("Read file %s failed\n", argv[1]);
+    return -1;
+  }
+  prefix = argv[2];
+  ubloxd_main(poll_ubloxraw_msg, send_gps_event);
+  free(data);
+  printf("Generated %d cereal events\n", save_idx);
+  if(save_idx != 9452) {
+    printf("Event count error: %d\n", save_idx);
+    return -1;
+  }
+  return 0;
+}