Locationd: reimplement liveLocationKalman and use position_geodetic in liveMapDataSP (#1275)

* init * desc * llk welcome back * more * new param to write * update mapd * no migration * no refactor for now * exec * rename * bearing * fix test * lint
2026-02-18 22:23:56 +08:00 · 2025-09-22 23:39:55 -04:00
parent ea6178e53e
commit ecee67dd64
49 changed files with 3755 additions and 23 deletions
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -2684,7 +2684,7 @@ struct Event {
    lateralPlanDEPRECATED @64 :LateralPlan;
    navModelDEPRECATED @104 :NavModelData;
    uiPlanDEPRECATED @106 :UiPlan;
-    liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
+    liveLocationKalman @72 :LiveLocationKalman;
    liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED);
    onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED);
  }
--- a/cereal/services.py
+++ b/cereal/services.py
@@ -89,6 +89,7 @@ _services: dict[str, tuple] = {
  "carStateSP": (True, 100., 10),
  "liveMapDataSP": (True, 1., 1),
  "modelDataV2SP": (True, 20.),
+  "liveLocationKalman": (True, 20.),

  # debug
  "uiDebug": (True, 0., 1),
--- a/common/params_keys.h
+++ b/common/params_keys.h
@@ -152,6 +152,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}},
    {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
    {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
+    {"LastGPSPositionLLK", {PERSISTENT, STRING}},
    {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
    {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
    {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},
--- a/selfdrive/test/process_replay/migration.py
+++ b/selfdrive/test/process_replay/migration.py
@@ -28,7 +28,8 @@ MigrationFunc = Callable[[list[MessageWithIndex]], MigrationOps]
 # 3. product is the message type created by the migration function, and the function will be skipped if product type already exists in lr
 # 4. it must return a list of operations to be applied to the logreader (replace, add, delete)
 # 5. all migration functions must be independent of each other
-def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False):
+def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False,
+                live_location_kalman: bool = True):
  migrations = [
    migrate_sensorEvents,
    migrate_carParams,
@@ -37,7 +38,6 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo
    migrate_carOutput,
    migrate_controlsState,
    migrate_carState,
-    migrate_liveLocationKalman,
    migrate_liveTracks,
    migrate_driverAssistance,
    migrate_drivingModelData,
@@ -51,6 +51,8 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo
    migrations.extend([migrate_pandaStates, migrate_peripheralState])
  if camera_states:
    migrations.append(migrate_cameraStates)
+  if live_location_kalman:
+    migrations.append(migrate_liveLocationKalman)

  return migrate(lr, migrations)

--- a/selfdrive/test/process_replay/process_replay.py
+++ b/selfdrive/test/process_replay/process_replay.py
@@ -496,7 +496,7 @@ CONFIGS = [
    pubs=[
      "cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState"
    ],
-    subs=["livePose"],
+    subs=["liveLocationKalman", "livePose"],
    ignore=["logMonoTime"],
    should_recv_callback=MessageBasedRcvCallback("cameraOdometry"),
    tolerance=NUMPY_TOLERANCE,
--- a/sunnypilot/SConscript
+++ b/sunnypilot/SConscript
@@ -1,2 +1,3 @@
 SConscript(['modeld/SConscript'])
-SConscript(['modeld_v2/SConscript'])
+SConscript(['modeld_v2/SConscript'])
+SConscript(['selfdrive/locationd/SConscript'])
--- a/sunnypilot/mapd/live_map_data/base_map_data.py
+++ b/sunnypilot/mapd/live_map_data/base_map_data.py
@@ -6,8 +6,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 from abc import abstractmethod, ABC

-from cereal import messaging
-from openpilot.common.gps import get_gps_location_service
+import cereal.messaging as messaging
 from openpilot.common.params import Params
 from openpilot.sunnypilot.navd.helpers import coordinate_from_param

@@ -16,14 +15,12 @@ class BaseMapData(ABC):
  def __init__(self):
    self.params = Params()

-    self.gps_location_service = get_gps_location_service(self.params)
-    gps_packets = [self.gps_location_service]
-    self.sm = messaging.SubMaster(['livePose'] + gps_packets, ignore_alive=gps_packets, ignore_avg_freq=gps_packets,
-                                  ignore_valid=gps_packets, poll='livePose')
+    self.sm = messaging.SubMaster(['liveLocationKalman'])
    self.pm = messaging.PubMaster(['liveMapDataSP'])

+    self.localizer_valid = False
    self.last_bearing = None
-    self.last_position = coordinate_from_param("LastGPSPosition", self.params)
+    self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)

  @abstractmethod
  def update_location(self) -> None:
@@ -46,7 +43,7 @@ class BaseMapData(ABC):
    next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()

    mapd_sp_send = messaging.new_message('liveMapDataSP')
-    mapd_sp_send.valid = self.sm.all_checks(['livePose'])
+    mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
    live_map_data = mapd_sp_send.liveMapDataSP

    live_map_data.speedLimitValid = bool(speed_limit > 0)
--- a/sunnypilot/mapd/live_map_data/osm_map_data.py
+++ b/sunnypilot/mapd/live_map_data/osm_map_data.py
@@ -8,8 +8,8 @@ import json
 import math
 import platform

+from cereal import log
 from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
 from openpilot.sunnypilot.navd.helpers import Coordinate

@@ -20,15 +20,12 @@ class OsmMapData(BaseMapData):
    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params

  def update_location(self) -> None:
-    gps = self.sm[self.gps_location_service]
-    gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_MDL < 2.0
+    location = self.sm['liveLocationKalman']
+    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid

-    if gps_ok:
-      self.last_bearing = gps.bearingDeg * 180/math.pi
-      self.last_position = Coordinate(gps.latitude, gps.longitude)
-
-    if not gps_ok and self.sm['livePose'].inputsOK:
-      return
+    if self.localizer_valid:
+      self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
+      self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])

    if self.last_position is None:
      return
@@ -36,9 +33,11 @@ class OsmMapData(BaseMapData):
    params = {
      "latitude": self.last_position.latitude,
      "longitude": self.last_position.longitude,
-      "bearing": self.last_bearing,
    }

+    if self.last_bearing is not None:
+      params['bearing'] = self.last_bearing
+
    self.mem_params.put("LastGPSPosition", json.dumps(params))

  def get_current_speed_limit(self) -> float:
--- a/sunnypilot/selfdrive/locationd/.gitignore
+++ b/sunnypilot/selfdrive/locationd/.gitignore
@@ -0,0 +1,3 @@
+params_learner
+paramsd
+locationd
--- a/sunnypilot/selfdrive/locationd/SConscript
+++ b/sunnypilot/selfdrive/locationd/SConscript
@@ -0,0 +1,37 @@
+Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
+
+loc_libs = [messaging, common, 'pthread', 'dl']
+
+# build ekf models
+rednose_gen_dir = 'models/generated'
+rednose_gen_deps = [
+  "models/constants.py",
+]
+live_ekf = env.RednoseCompileFilter(
+  target='live',
+  filter_gen_script='models/live_kf.py',
+  output_dir=rednose_gen_dir,
+  extra_gen_artifacts=['live_kf_constants.h'],
+  gen_script_deps=rednose_gen_deps,
+)
+car_ekf = env.RednoseCompileFilter(
+  target='car',
+  filter_gen_script='models/car_kf.py',
+  output_dir=rednose_gen_dir,
+  extra_gen_artifacts=[],
+  gen_script_deps=rednose_gen_deps,
+)
+
+# locationd build
+locationd_sources = ["locationd.cc", "models/live_kf.cc"]
+
+lenv = env.Clone()
+# ekf filter libraries need to be linked, even if no symbols are used
+if arch != "Darwin":
+  lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
+
+lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
+lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
+locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
+lenv.Depends(locationd, rednose)
+lenv.Depends(locationd, live_ekf)
--- a/sunnypilot/selfdrive/locationd/init.py
+++ b/sunnypilot/selfdrive/locationd/init.py
--- a/sunnypilot/selfdrive/locationd/locationd.cc
+++ b/sunnypilot/selfdrive/locationd/locationd.cc
@@ -0,0 +1,750 @@
+#include "sunnypilot/selfdrive/locationd/locationd.h"
+
+#include <sys/time.h>
+#include <sys/resource.h>
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+using namespace EKFS;
+using namespace Eigen;
+
+ExitHandler do_exit;
+const double ACCEL_SANITY_CHECK = 100.0;  // m/s^2
+const double ROTATION_SANITY_CHECK = 10.0;  // rad/s
+const double TRANS_SANITY_CHECK = 200.0;  // m/s
+const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
+const double ALTITUDE_SANITY_CHECK = 10000; // m
+const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
+const double VALID_TIME_SINCE_RESET = 1.0; // s
+const double VALID_POS_STD = 50.0; // m
+const double MAX_RESET_TRACKER = 5.0;
+const double SANE_GPS_UNCERTAINTY = 1500.0; // m
+const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
+const double RESET_TRACKER_DECAY = 0.99995;
+const double DECAY = 0.9993; // ~10 secs to resume after a bad input
+const double MAX_FILTER_REWIND_TIME = 0.8; // s
+const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
+
+// TODO: GPS sensor time offsets are empirically calculated
+// They should be replaced with synced time from a real clock
+const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
+const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
+const float  GPS_POS_STD_THRESHOLD = 50.0;
+const float  GPS_VEL_STD_THRESHOLD = 5.0;
+const float  GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
+const float  GPS_POS_STD_RESET_THRESHOLD = 2.0;
+const float  GPS_VEL_STD_RESET_THRESHOLD = 0.5;
+const float  GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
+const int    GPS_ORIENTATION_ERROR_RESET_CNT = 3;
+
+const bool   DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
+
+static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
+  VectorXd res(floatlist.size());
+  for (int i = 0; i < floatlist.size(); i++) {
+    res[i] = floatlist[i];
+  }
+  return res;
+}
+
+static Vector4d quat2vector(const Quaterniond& quat) {
+  return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
+}
+
+static Quaterniond vector2quat(const VectorXd& vec) {
+  return Quaterniond(vec(0), vec(1), vec(2), vec(3));
+}
+
+static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
+  meas.setValue(kj::arrayPtr(val.data(), val.size()));
+  meas.setStd(kj::arrayPtr(std.data(), std.size()));
+  meas.setValid(valid);
+}
+
+
+static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
+  // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
+  return ((rot_matrix *  cov_in) * rot_matrix.transpose());
+}
+
+static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
+  // Stds cannot be rotated like values, only covariances can be rotated
+  return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
+}
+
+Localizer::Localizer(LocalizerGnssSource gnss_source) {
+  this->kf = std::make_unique<LiveKalman>();
+  this->reset_kalman();
+
+  this->calib = Vector3d(0.0, 0.0, 0.0);
+  this->device_from_calib = MatrixXdr::Identity(3, 3);
+  this->calib_from_device = MatrixXdr::Identity(3, 3);
+
+  for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
+    this->posenet_stds.push_back(10.0);
+  }
+
+  VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
+  this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
+  this->configure_gnss_source(gnss_source);
+}
+
+void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
+  VectorXd predicted_state = this->kf->get_x();
+  MatrixXdr predicted_cov = this->kf->get_P();
+  VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
+
+  VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
+  ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
+  VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
+  VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
+  VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
+  VectorXd fix_pos_geo_vec = this->get_position_geodetic();
+  VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
+  VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
+  MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
+  MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
+  VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
+
+  VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
+  MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
+  VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
+  VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
+
+  MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
+  VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
+
+  VectorXd vel_device = device_from_ecef * vel_ecef;
+  VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
+  MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
+  condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
+    predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
+  condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
+    predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
+  condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
+    predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
+  condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
+    predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
+  VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
+  H_input << device_from_ecef_eul, vel_ecef;
+  MatrixXdr HH = this->kf->H(H_input);
+  MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
+  VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
+
+  VectorXd vel_calib = this->calib_from_device * vel_device;
+  VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
+
+  VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
+  VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
+  VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
+  VectorXd nextfix_ecef = fix_ecef + vel_ecef;
+  VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
+
+  VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
+  VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
+
+  VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
+  VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
+
+  Vector3d nans = Vector3d(NAN, NAN, NAN);
+
+  // TODO fill in NED and Calibrated stds
+  // write measurements to msg
+  init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
+  init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
+  init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
+  init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
+  init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
+  init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
+  init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
+  init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
+  init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
+  init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
+  init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
+  init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
+  init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
+  init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
+  if (DEBUG) {
+    init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
+  }
+
+  double old_mean = 0.0, new_mean = 0.0;
+  int i = 0;
+  for (double x : this->posenet_stds) {
+    if (i < POSENET_STD_HIST_HALF) {
+      old_mean += x;
+    } else {
+      new_mean += x;
+    }
+    i++;
+  }
+  old_mean /= POSENET_STD_HIST_HALF;
+  new_mean /= POSENET_STD_HIST_HALF;
+  // experimentally found these values, no false positives in 20k minutes of driving
+  bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
+
+  fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
+  fix.setDeviceStable(!this->device_fell);
+  fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
+  fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
+  this->device_fell = false;
+
+  //fix.setGpsWeek(this->time.week);
+  //fix.setGpsTimeOfWeek(this->time.tow);
+  fix.setUnixTimestampMillis(this->unix_timestamp_millis);
+
+  double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
+  fix.setTimeSinceReset(time_since_reset);
+  if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
+    fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
+  } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
+    fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
+  } else {
+    fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
+  }
+}
+
+VectorXd Localizer::get_position_geodetic() {
+  VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
+  ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
+  Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
+  return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
+}
+
+VectorXd Localizer::get_state() {
+  return this->kf->get_x();
+}
+
+VectorXd Localizer::get_stdev() {
+  return this->kf->get_P().diagonal().array().sqrt();
+}
+
+bool Localizer::are_inputs_ok() {
+  return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
+}
+
+void Localizer::observation_timings_invalid_reset(){
+  this->observation_timings_invalid = false;
+}
+
+void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
+  // TODO does not yet account for double sensor readings in the log
+
+  // Ignore empty readings (e.g. in case the magnetometer had no data ready)
+  if (log.getTimestamp() == 0) {
+    return;
+  }
+
+  double sensor_time = 1e-9 * log.getTimestamp();
+
+  // sensor time and log time should be close
+  if (std::abs(current_time - sensor_time) > 0.1) {
+    LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
+    this->observation_timings_invalid = true;
+    return;
+  } else if (!this->is_timestamp_valid(sensor_time)) {
+    this->observation_timings_invalid = true;
+    return;
+  }
+
+  // TODO: handle messages from two IMUs at the same time
+  if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
+    return;
+  }
+
+  // Gyro Uncalibrated
+  if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
+    auto v = log.getGyroUncalibrated().getV();
+    auto meas = Vector3d(-v[2], -v[1], -v[0]);
+
+    VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
+    float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
+    float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
+    bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
+
+    if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
+      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
+      this->observation_values_invalid["gyroscope"] *= DECAY;
+    } else {
+      this->observation_values_invalid["gyroscope"] += 1.0;
+    }
+  }
+
+  // Accelerometer
+  if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
+    auto v = log.getAcceleration().getV();
+
+    // TODO: reduce false positives and re-enable this check
+    // check if device fell, estimate 10 for g
+    // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
+    // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
+
+    auto meas = Vector3d(-v[2], -v[1], -v[0]);
+    if (meas.norm() < ACCEL_SANITY_CHECK) {
+      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
+      this->observation_values_invalid["accelerometer"] *= DECAY;
+    } else {
+      this->observation_values_invalid["accelerometer"] += 1.0;
+    }
+  }
+}
+
+void Localizer::input_fake_gps_observations(double current_time) {
+  // This is done to make sure that the error estimate of the position does not blow up
+  // when the filter is in no-gps mode
+  // Steps : first predict -> observe current obs with reasonable STD
+  this->kf->predict(current_time);
+
+  VectorXd current_x = this->kf->get_x();
+  VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
+  VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
+  const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
+  const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
+
+  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
+void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
+  bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
+  bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
+  bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
+  bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
+
+  if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
+    //this->gps_valid = false;
+    this->determine_gps_mode(current_time);
+    return;
+  }
+
+  double sensor_time = current_time - sensor_time_offset;
+
+  // Process message
+  //this->gps_valid = true;
+  this->gps_mode = true;
+  Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
+  this->converter = std::make_unique<LocalCoord>(geodetic);
+
+  VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
+  VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
+  float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
+  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
+  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
+
+  this->unix_timestamp_millis = log.getUnixTimestampMillis();
+  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
+
+  VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
+  VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
+  VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
+  VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
+  for (int i = 0; i < orientation_error.size(); i++) {
+    orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
+    if (orientation_error(i) < 0.0) {
+      orientation_error(i) += 2.0 * M_PI;
+    }
+    orientation_error(i) -= M_PI;
+  }
+  VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
+
+  if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
+    LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+    this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
+  } else if (gps_est_error > 100.0) {
+    LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+  }
+
+  this->last_gps_msg = sensor_time;
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
+void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
+
+  if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
+    this->determine_gps_mode(current_time);
+    return;
+  }
+
+  double sensor_time = log.getMeasTime() * 1e-9;
+  sensor_time -= this->gps_time_offset;
+
+  auto ecef_pos_v = log.getPositionECEF().getValue();
+  VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
+
+  // indexed at 0 cause all std values are the same MAE
+  auto ecef_pos_std = log.getPositionECEF().getStd()[0];
+  MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
+
+  auto ecef_vel_v = log.getVelocityECEF().getValue();
+  VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
+
+  // indexed at 0 cause all std values are the same MAE
+  auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
+  MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
+
+  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
+
+  VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
+  VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
+
+  LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
+  ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
+  VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
+  double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
+
+  VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
+  VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
+  for (int i = 0; i < orientation_error.size(); i++) {
+    orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
+    if (orientation_error(i) < 0.0) {
+      orientation_error(i) += 2.0 * M_PI;
+    }
+    orientation_error(i) -= M_PI;
+  }
+  VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
+
+  if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
+    this->determine_gps_mode(current_time);
+    return;
+  }
+
+  // prevent jumping gnss measurements (covered lots, standstill...)
+  bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
+  orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
+  orientation_reset &= !this->standstill;
+  if (orientation_reset) {
+    this->orientation_reset_count++;
+  } else {
+    this->orientation_reset_count = 0;
+  }
+
+  if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
+    // always reset on first gps message and if the location is off but the accuracy is high
+    LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+  } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
+    LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+    this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
+    this->orientation_reset_count = 0;
+  }
+
+  this->gps_mode = true;
+  this->last_gps_msg = sensor_time;
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
+void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
+  this->car_speed = std::abs(log.getVEgo());
+  this->standstill = log.getStandstill();
+  if (this->standstill) {
+    this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
+    this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
+  }
+}
+
+void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
+  VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
+  VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
+
+  if (!this->is_timestamp_valid(current_time)) {
+    this->observation_timings_invalid = true;
+    return;
+  }
+
+  if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
+    this->observation_values_invalid["cameraOdometry"] += 1.0;
+    return;
+  }
+
+  VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
+  VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
+
+  if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
+    this->observation_values_invalid["cameraOdometry"] += 1.0;
+    return;
+  }
+
+  if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
+    this->observation_values_invalid["cameraOdometry"] += 1.0;
+    return;
+  }
+
+  this->posenet_stds.pop_front();
+  this->posenet_stds.push_back(trans_calib_std[0]);
+
+  // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
+  trans_calib_std *= 10.0;
+  rot_calib_std *= 10.0;
+  MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
+  MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
+  this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
+    { rot_device }, { rot_device_cov });
+  this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
+    { trans_device }, { trans_device_cov });
+  this->observation_values_invalid["cameraOdometry"] *= DECAY;
+  this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
+}
+
+void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
+  if (!this->is_timestamp_valid(current_time)) {
+    this->observation_timings_invalid = true;
+    return;
+  }
+
+  if (log.getRpyCalib().size() > 0) {
+    auto live_calib = floatlist2vector(log.getRpyCalib());
+    if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
+      this->observation_values_invalid["liveCalibration"] += 1.0;
+      return;
+    }
+
+    this->calib = live_calib;
+    this->device_from_calib = euler2rot(this->calib);
+    this->calib_from_device = this->device_from_calib.transpose();
+    this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED;
+    this->observation_values_invalid["liveCalibration"] *= DECAY;
+  }
+}
+
+void Localizer::reset_kalman(double current_time) {
+  const VectorXd &init_x = this->kf->get_initial_x();
+  const MatrixXdr &init_P = this->kf->get_initial_P();
+  this->reset_kalman(current_time, init_x, init_P);
+}
+
+void Localizer::finite_check(double current_time) {
+  bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
+  if (!all_finite) {
+    LOGE("Non-finite values detected, kalman reset");
+    this->reset_kalman(current_time);
+  }
+}
+
+void Localizer::time_check(double current_time) {
+  if (std::isnan(this->last_reset_time)) {
+    this->last_reset_time = current_time;
+  }
+  if (std::isnan(this->first_valid_log_time)) {
+    this->first_valid_log_time = current_time;
+  }
+  double filter_time = this->kf->get_filter_time();
+  bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
+  if (big_time_gap) {
+    LOGE("Time gap of over 10s detected, kalman reset");
+    this->reset_kalman(current_time);
+  }
+}
+
+void Localizer::update_reset_tracker() {
+  // reset tracker is tuned to trigger when over 1reset/10s over 2min period
+  if (this->is_gps_ok()) {
+    this->reset_tracker *= RESET_TRACKER_DECAY;
+  } else {
+    this->reset_tracker = 0.0;
+  }
+}
+
+void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
+  // too nonlinear to init on completely wrong
+  VectorXd current_x = this->kf->get_x();
+  MatrixXdr current_P = this->kf->get_P();
+  MatrixXdr init_P = this->kf->get_initial_P();
+  const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
+  int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
+
+  current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
+  current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
+  current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
+
+  init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
+  init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
+  init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
+  init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
+    STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
+
+  this->reset_kalman(current_time, current_x, init_P);
+}
+
+void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
+  this->kf->init_state(init_x, init_P, current_time);
+  this->last_reset_time = current_time;
+  this->reset_tracker += 1.0;
+}
+
+void Localizer::handle_msg_bytes(const char *data, const size_t size) {
+  AlignedBuffer aligned_buf;
+
+  capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
+  cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+  this->handle_msg(event);
+}
+
+void Localizer::handle_msg(const cereal::Event::Reader& log) {
+  double t = log.getLogMonoTime() * 1e-9;
+  this->time_check(t);
+  if (log.isAccelerometer()) {
+    this->handle_sensor(t, log.getAccelerometer());
+  } else if (log.isGyroscope()) {
+    this->handle_sensor(t, log.getGyroscope());
+  } else if (log.isGpsLocation()) {
+    this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
+  } else if (log.isGpsLocationExternal()) {
+    this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
+  //} else if (log.isGnssMeasurements()) {
+  //  this->handle_gnss(t, log.getGnssMeasurements());
+  } else if (log.isCarState()) {
+    this->handle_car_state(t, log.getCarState());
+  } else if (log.isCameraOdometry()) {
+    this->handle_cam_odo(t, log.getCameraOdometry());
+  } else if (log.isLiveCalibration()) {
+    this->handle_live_calib(t, log.getLiveCalibration());
+  }
+  this->finite_check();
+  this->update_reset_tracker();
+}
+
+kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
+                                                       bool sensorsOK, bool gpsOK, bool msgValid) {
+  cereal::Event::Builder evt = msg_builder.initEvent();
+  evt.setValid(msgValid);
+  cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
+  this->build_live_location(liveLoc);
+  liveLoc.setSensorsOK(sensorsOK);
+  liveLoc.setGpsOK(gpsOK);
+  liveLoc.setInputsOK(inputsOK);
+  return msg_builder.toBytes();
+}
+
+bool Localizer::is_gps_ok() {
+  return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
+}
+
+bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
+  for (auto &kv : critical_services){
+    if (kv.second >= INPUT_INVALID_THRESHOLD){
+      return false;
+    }
+  }
+  return true;
+}
+
+bool Localizer::is_timestamp_valid(double current_time) {
+  double filter_time = this->kf->get_filter_time();
+  if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
+    LOGE("Observation timestamp is older than the max rewind threshold of the filter");
+    return false;
+  }
+  return true;
+}
+
+void Localizer::determine_gps_mode(double current_time) {
+  // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
+  // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
+  // 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
+  VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
+  if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
+    if (this->gps_mode){
+      this->gps_mode = false;
+      this->reset_kalman(current_time);
+    } else {
+      this->input_fake_gps_observations(current_time);
+    }
+  }
+}
+
+void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
+  this->gnss_source = source;
+  if (source == LocalizerGnssSource::UBLOX) {
+    this->gps_std_factor = 10.0;
+    this->gps_variance_factor = 1.0;
+    this->gps_vertical_variance_factor = 1.0;
+    this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
+  } else {
+    this->gps_std_factor = 2.0;
+    this->gps_variance_factor = 0.0;
+    this->gps_vertical_variance_factor = 3.0;
+    this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
+  }
+}
+
+int Localizer::locationd_thread() {
+  Params params;
+  LocalizerGnssSource source;
+  const char* gps_location_socket;
+  if (params.getBool("UbloxAvailable")) {
+    source = LocalizerGnssSource::UBLOX;
+    gps_location_socket = "gpsLocationExternal";
+  } else {
+    source = LocalizerGnssSource::QCOM;
+    gps_location_socket = "gpsLocation";
+  }
+
+  this->configure_gnss_source(source);
+  const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
+                                                          "carState", "accelerometer", "gyroscope"};
+
+  SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
+  PubMaster pm({"liveLocationKalman"});
+
+  uint64_t cnt = 0;
+  bool filterInitialized = false;
+  const std::vector<std::string> critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"};
+  for (std::string service : critical_input_services) {
+    this->observation_values_invalid.insert({service, 0.0});
+  }
+
+  while (!do_exit) {
+    sm.update();
+    if (filterInitialized){
+      this->observation_timings_invalid_reset();
+      for (const char* service : service_list) {
+        if (sm.updated(service) && sm.valid(service)){
+          const cereal::Event::Reader log = sm[service];
+          this->handle_msg(log);
+        }
+      }
+    } else {
+      filterInitialized = sm.allAliveAndValid();
+    }
+
+    const char* trigger_msg = "cameraOdometry";
+    if (sm.updated(trigger_msg)) {
+      bool inputsOK = sm.allValid() && this->are_inputs_ok();
+      bool gpsOK = this->is_gps_ok();
+      bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
+
+      // Log time to first fix
+      if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
+        this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
+      }
+
+      MessageBuilder msg_builder;
+      kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
+      pm.send("liveLocationKalman", bytes.begin(), bytes.size());
+
+      if (cnt % 1200 == 0 && gpsOK) {  // once a minute
+        VectorXd posGeo = this->get_position_geodetic();
+        std::string lastGPSPosJSON = util::string_format(
+          "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
+        params.putNonBlocking("LastGPSPositionLLK", lastGPSPosJSON);
+      }
+      cnt++;
+    }
+  }
+  return 0;
+}
+
+int main() {
+  util::set_realtime_priority(5);
+
+  Localizer localizer;
+  return localizer.locationd_thread();
+}
--- a/sunnypilot/selfdrive/locationd/locationd.h
+++ b/sunnypilot/selfdrive/locationd/locationd.h
@@ -0,0 +1,100 @@
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include <deque>
+#include <fstream>
+#include <memory>
+#include <map>
+#include <string>
+
+#include "cereal/messaging/messaging.h"
+#include "common/transformations/coordinates.hpp"
+#include "common/transformations/orientation.hpp"
+#include "common/params.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+#include "sunnypilot/system/sensord/sensors/constants.h"
+#define VISION_DECIMATION 2
+#define SENSOR_DECIMATION 10
+#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
+
+#define POSENET_STD_HIST_HALF 20
+
+enum LocalizerGnssSource {
+  UBLOX, QCOM
+};
+
+class Localizer {
+public:
+  Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
+
+  int locationd_thread();
+
+  void reset_kalman(double current_time = NAN);
+  void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
+  void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
+  void finite_check(double current_time = NAN);
+  void time_check(double current_time = NAN);
+  void update_reset_tracker();
+  bool is_gps_ok();
+  bool critical_services_valid(const std::map<std::string, double> &critical_services);
+  bool is_timestamp_valid(double current_time);
+  void determine_gps_mode(double current_time);
+  bool are_inputs_ok();
+  void observation_timings_invalid_reset();
+
+  kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
+    bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
+  void build_live_location(cereal::LiveLocationKalman::Builder& fix);
+
+  Eigen::VectorXd get_position_geodetic();
+  Eigen::VectorXd get_state();
+  Eigen::VectorXd get_stdev();
+
+  void handle_msg_bytes(const char *data, const size_t size);
+  void handle_msg(const cereal::Event::Reader& log);
+  void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
+  void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
+  void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
+  void handle_car_state(double current_time, const cereal::CarState::Reader& log);
+  void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
+  void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
+
+  void input_fake_gps_observations(double current_time);
+
+private:
+  std::unique_ptr<LiveKalman> kf;
+
+  Eigen::VectorXd calib;
+  MatrixXdr device_from_calib;
+  MatrixXdr calib_from_device;
+  bool calibrated = false;
+
+  double car_speed = 0.0;
+  double last_reset_time = NAN;
+  std::deque<double> posenet_stds;
+
+  std::unique_ptr<LocalCoord> converter;
+
+  int64_t unix_timestamp_millis = 0;
+  double reset_tracker = 0.0;
+  bool device_fell = false;
+  bool gps_mode = false;
+  double first_valid_log_time = NAN;
+  double ttff = NAN;
+  double last_gps_msg = 0;
+  LocalizerGnssSource gnss_source;
+  bool observation_timings_invalid = false;
+  std::map<std::string, double> observation_values_invalid;
+  bool standstill = true;
+  int32_t orientation_reset_count = 0;
+  float gps_std_factor;
+  float gps_variance_factor;
+  float gps_vertical_variance_factor;
+  double gps_time_offset;
+  Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
+
+  void configure_gnss_source(const LocalizerGnssSource &source);
+};
--- a/sunnypilot/selfdrive/locationd/models/.gitignore
+++ b/sunnypilot/selfdrive/locationd/models/.gitignore
@@ -0,0 +1 @@
+generated/
--- a/sunnypilot/selfdrive/locationd/models/init.py
+++ b/sunnypilot/selfdrive/locationd/models/init.py
--- a/sunnypilot/selfdrive/locationd/models/car_kf.py
+++ b/sunnypilot/selfdrive/locationd/models/car_kf.py
@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+import math
+import sys
+from typing import Any
+
+import numpy as np
+
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
+from openpilot.common.swaglog import cloudlog
+
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__ == '__main__':  # Generating sympy
+  import sympy as sp
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+
+
+i = 0
+
+def _slice(n):
+  global i
+  s = slice(i, i + n)
+  i += n
+
+  return s
+
+
+class States:
+  # Vehicle model params
+  STIFFNESS = _slice(1)  # [-]
+  STEER_RATIO = _slice(1)  # [-]
+  ANGLE_OFFSET = _slice(1)  # [rad]
+  ANGLE_OFFSET_FAST = _slice(1)  # [rad]
+
+  VELOCITY = _slice(2)  # (x, y) [m/s]
+  YAW_RATE = _slice(1)  # [rad/s]
+  STEER_ANGLE = _slice(1)  # [rad]
+  ROAD_ROLL = _slice(1)  # [rad]
+
+
+class CarKalman(KalmanFilter):
+  name = 'car'
+
+  initial_x = np.array([
+    1.0,
+    15.0,
+    0.0,
+    0.0,
+
+    10.0, 0.0,
+    0.0,
+    0.0,
+    0.0
+  ])
+
+  # process noise
+  Q = np.diag([
+    (.05 / 100)**2,
+    .01**2,
+    math.radians(0.02)**2,
+    math.radians(0.25)**2,
+
+    .1**2, .01**2,
+    math.radians(0.1)**2,
+    math.radians(0.1)**2,
+    math.radians(1)**2,
+  ])
+  P_initial = Q.copy()
+
+  obs_noise: dict[int, Any] = {
+    ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
+    ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
+    ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
+    ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
+    ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
+    ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
+  }
+
+  global_vars = [
+    'mass',
+    'rotational_inertia',
+    'center_to_front',
+    'center_to_rear',
+    'stiffness_front',
+    'stiffness_rear',
+  ]
+
+  @staticmethod
+  def generate_code(generated_dir):
+    dim_state = CarKalman.initial_x.shape[0]
+    name = CarKalman.name
+
+    # Linearized single-track lateral dynamics, equations 7.211-7.213
+    # Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
+    # Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
+
+    # globals
+    global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
+    m, j, aF, aR, cF_orig, cR_orig = global_vars
+
+    # make functions and jacobians with sympy
+    # state variables
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    # Vehicle model constants
+    sf = state[States.STIFFNESS, :][0, 0]
+
+    cF, cR = sf * cF_orig, sf * cR_orig
+    angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
+    angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
+    theta = state[States.ROAD_ROLL, :][0, 0]
+    sa = state[States.STEER_ANGLE, :][0, 0]
+
+    sR = state[States.STEER_RATIO, :][0, 0]
+    u, v = state[States.VELOCITY, :]
+    r = state[States.YAW_RATE, :][0, 0]
+
+    A = sp.Matrix(np.zeros((2, 2)))
+    A[0, 0] = -(cF + cR) / (m * u)
+    A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
+    A[1, 0] = -(cF * aF - cR * aR) / (j * u)
+    A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
+
+    B = sp.Matrix(np.zeros((2, 1)))
+    B[0, 0] = cF / m / sR
+    B[1, 0] = (cF * aF) / j / sR
+
+    C = sp.Matrix(np.zeros((2, 1)))
+    C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
+    C[1, 0] = 0
+
+    x = sp.Matrix([v, r])  # lateral velocity, yaw rate
+    x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
+    state_dot[States.YAW_RATE.start, 0] = x_dot[1]
+
+    # Basic descretization, 1st order integrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    #
+    # Observation functions
+    #
+    obs_eqs = [
+      [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
+      [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
+      [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
+      [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
+      [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
+      [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
+      [sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
+      [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
+    ]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
+
+  def __init__(self, generated_dir):
+    dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
+    self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
+                              dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
+
+  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+    self.filter.set_global("mass", mass)
+    self.filter.set_global("rotational_inertia", rotational_inertia)
+    self.filter.set_global("center_to_front", center_to_front)
+    self.filter.set_global("center_to_rear", center_to_rear)
+    self.filter.set_global("stiffness_front", stiffness_front)
+    self.filter.set_global("stiffness_rear", stiffness_rear)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  CarKalman.generate_code(generated_dir)
--- a/sunnypilot/selfdrive/locationd/models/constants.py
+++ b/sunnypilot/selfdrive/locationd/models/constants.py
@@ -0,0 +1,92 @@
+import os
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+class ObservationKind:
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  GPS_NED = 2
+  ODOMETRIC_SPEED = 3
+  PHONE_GYRO = 4
+  GPS_VEL = 5
+  PSEUDORANGE_GPS = 6
+  PSEUDORANGE_RATE_GPS = 7
+  SPEED = 8
+  NO_ROT = 9
+  PHONE_ACCEL = 10
+  ORB_POINT = 11
+  ECEF_POS = 12
+  CAMERA_ODO_TRANSLATION = 13
+  CAMERA_ODO_ROTATION = 14
+  ORB_FEATURES = 15
+  MSCKF_TEST = 16
+  FEATURE_TRACK_TEST = 17
+  LANE_PT = 18
+  IMU_FRAME = 19
+  PSEUDORANGE_GLONASS = 20
+  PSEUDORANGE_RATE_GLONASS = 21
+  PSEUDORANGE = 22
+  PSEUDORANGE_RATE = 23
+  ECEF_VEL = 35
+  ECEF_ORIENTATION_FROM_GPS = 32
+  NO_ACCEL = 33
+  ORB_FEATURES_WIDE = 34
+
+  ROAD_FRAME_XY_SPEED = 24  # (x, y) [m/s]
+  ROAD_FRAME_YAW_RATE = 25  # [rad/s]
+  STEER_ANGLE = 26  # [rad]
+  ANGLE_OFFSET_FAST = 27  # [rad]
+  STIFFNESS = 28  # [-]
+  STEER_RATIO = 29  # [-]
+  ROAD_FRAME_X_SPEED = 30  # (x) [m/s]
+  ROAD_ROLL = 31  # [rad]
+
+  names = [
+    'Unknown',
+    'No observation',
+    'GPS NED',
+    'Odometric speed',
+    'Phone gyro',
+    'GPS velocity',
+    'GPS pseudorange',
+    'GPS pseudorange rate',
+    'Speed',
+    'No rotation',
+    'Phone acceleration',
+    'ORB point',
+    'ECEF pos',
+    'camera odometric translation',
+    'camera odometric rotation',
+    'ORB features',
+    'MSCKF test',
+    'Feature track test',
+    'Lane ecef point',
+    'imu frame eulers',
+    'GLONASS pseudorange',
+    'GLONASS pseudorange rate',
+    'pseudorange',
+    'pseudorange rate',
+
+    'Road Frame x,y speed',
+    'Road Frame yaw rate',
+    'Steer Angle',
+    'Fast Angle Offset',
+    'Stiffness',
+    'Steer Ratio',
+    'Road Frame x speed',
+    'Road Roll',
+    'ECEF orientation from GPS',
+    'NO accel',
+    'ORB features wide camera',
+    'ECEF_VEL',
+  ]
+
+  @classmethod
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
+           ObservationKind.PSEUDORANGE_RATE_GPS,
+           ObservationKind.PSEUDORANGE_GLONASS,
+           ObservationKind.PSEUDORANGE_RATE_GLONASS]
--- a/sunnypilot/selfdrive/locationd/models/live_kf.cc
+++ b/sunnypilot/selfdrive/locationd/models/live_kf.cc
@@ -0,0 +1,122 @@
+#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
+
+using namespace EKFS;
+using namespace Eigen;
+
+Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
+  return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
+}
+
+Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
+  return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
+}
+
+std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
+  std::vector<Eigen::Map<Eigen::VectorXd>> res;
+  for (const Eigen::VectorXd &vec : vec_vec) {
+    res.push_back(get_mapvec(vec));
+  }
+  return res;
+}
+
+std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
+  std::vector<Eigen::Map<MatrixXdr>> res;
+  for (const MatrixXdr &mat : mat_vec) {
+    res.push_back(get_mapmat(mat));
+  }
+  return res;
+}
+
+LiveKalman::LiveKalman() {
+  this->dim_state = live_initial_x.rows();
+  this->dim_state_err = live_initial_P_diag.rows();
+
+  this->initial_x = live_initial_x;
+  this->initial_P = live_initial_P_diag.asDiagonal();
+  this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
+  this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
+  this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
+  this->Q = live_Q_diag.asDiagonal();
+  for (auto& pair : live_obs_noise_diag) {
+    this->obs_noise[pair.first] = pair.second.asDiagonal();
+  }
+
+  // init filter
+  this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
+    get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
+    std::vector<int>{3}, std::vector<std::string>(), 0.8);
+}
+
+void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
+  MatrixXdr covs = covs_diag.asDiagonal();
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+void LiveKalman::init_state(const VectorXd &state, double filter_time) {
+  MatrixXdr covs = this->filter->covs();
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+VectorXd LiveKalman::get_x() {
+  return this->filter->state();
+}
+
+MatrixXdr LiveKalman::get_P() {
+  return this->filter->covs();
+}
+
+double LiveKalman::get_filter_time() {
+  return this->filter->get_filter_time();
+}
+
+std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
+  std::vector<MatrixXdr> R;
+  for (int i = 0; i < n; i++) {
+    R.push_back(this->obs_noise[kind]);
+  }
+  return R;
+}
+
+std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
+  std::optional<Estimate> r;
+  if (R.size() == 0) {
+    R = this->get_R(kind, meas.size());
+  }
+  r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
+  return r;
+}
+
+void LiveKalman::predict(double t) {
+  this->filter->predict(t);
+}
+
+const Eigen::VectorXd &LiveKalman::get_initial_x() {
+  return this->initial_x;
+}
+
+const MatrixXdr &LiveKalman::get_initial_P() {
+  return this->initial_P;
+}
+
+const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
+  return this->fake_gps_pos_cov;
+}
+
+const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
+  return this->fake_gps_vel_cov;
+}
+
+const MatrixXdr &LiveKalman::get_reset_orientation_P() {
+  return this->reset_orientation_P;
+}
+
+MatrixXdr LiveKalman::H(const VectorXd &in) {
+  assert(in.size() == 6);
+  Matrix<double, 3, 6, Eigen::RowMajor> res;
+  this->filter->get_extra_routine("H")((double*)in.data(), res.data());
+  return res;
+}
--- a/sunnypilot/selfdrive/locationd/models/live_kf.h
+++ b/sunnypilot/selfdrive/locationd/models/live_kf.h
@@ -0,0 +1,66 @@
+#pragma once
+
+#include <string>
+#include <cmath>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+
+#include "generated/live_kf_constants.h"
+#include "rednose/helpers/ekf_sym.h"
+
+#define EARTH_GM 3.986005e14  // m^3/s^2 (gravitational constant * mass of earth)
+
+using namespace EKFS;
+
+Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
+Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
+std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
+std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
+
+class LiveKalman {
+public:
+  LiveKalman();
+
+  void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
+  void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
+  void init_state(const Eigen::VectorXd &state, double filter_time);
+
+  Eigen::VectorXd get_x();
+  MatrixXdr get_P();
+  double get_filter_time();
+  std::vector<MatrixXdr> get_R(int kind, int n);
+
+  std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
+  std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
+  std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
+  std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
+  void predict(double t);
+
+  const Eigen::VectorXd &get_initial_x();
+  const MatrixXdr &get_initial_P();
+  const MatrixXdr &get_fake_gps_pos_cov();
+  const MatrixXdr &get_fake_gps_vel_cov();
+  const MatrixXdr &get_reset_orientation_P();
+
+  MatrixXdr H(const Eigen::VectorXd &in);
+
+private:
+  std::string name = "live";
+
+  std::shared_ptr<EKFSym> filter;
+
+  int dim_state;
+  int dim_state_err;
+
+  Eigen::VectorXd initial_x;
+  MatrixXdr initial_P;
+  MatrixXdr fake_gps_pos_cov;
+  MatrixXdr fake_gps_vel_cov;
+  MatrixXdr reset_orientation_P;
+  MatrixXdr Q;  // process noise
+  std::unordered_map<int, MatrixXdr> obs_noise;
+};
--- a/sunnypilot/selfdrive/locationd/models/live_kf.py
+++ b/sunnypilot/selfdrive/locationd/models/live_kf.py
@@ -0,0 +1,242 @@
+#!/usr/bin/env python3
+
+import sys
+import os
+import numpy as np
+
+from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
+
+import sympy as sp
+import inspect
+from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
+from rednose.helpers.ekf_sym import gen_code
+
+EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
+
+
+def numpy2eigenstring(arr):
+  assert(len(arr.shape) == 1)
+  arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
+  return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
+
+
+class States:
+  ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
+  ECEF_ORIENTATION = slice(3, 7)  # quat for pose of phone in ecef
+  ECEF_VELOCITY = slice(7, 10)  # ecef velocity in m/s
+  ANGULAR_VELOCITY = slice(10, 13)  # roll, pitch and yaw rates in device frame in radians/s
+  GYRO_BIAS = slice(13, 16)  # roll, pitch and yaw biases
+  ACCELERATION = slice(16, 19)  # Acceleration in device frame in m/s**2
+  ACC_BIAS = slice(19, 22)  # Acceletometer bias in m/s**2
+
+  # Error-state has different slices because it is an ESKF
+  ECEF_POS_ERR = slice(0, 3)
+  ECEF_ORIENTATION_ERR = slice(3, 6)  # euler angles for orientation error
+  ECEF_VELOCITY_ERR = slice(6, 9)
+  ANGULAR_VELOCITY_ERR = slice(9, 12)
+  GYRO_BIAS_ERR = slice(12, 15)
+  ACCELERATION_ERR = slice(15, 18)
+  ACC_BIAS_ERR = slice(18, 21)
+
+
+class LiveKalman:
+  name = 'live'
+
+  initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
+                        0.42254641, -0.31238054, -0.83602975, -0.15788347,  # NED [0,0,0] -> ECEF Quat
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0])
+
+  # state covariance
+  initial_P_diag = np.array([10**2, 10**2, 10**2,
+                             0.01**2, 0.01**2, 0.01**2,
+                             10**2, 10**2, 10**2,
+                             1**2, 1**2, 1**2,
+                             1**2, 1**2, 1**2,
+                             100**2, 100**2, 100**2,
+                             0.01**2, 0.01**2, 0.01**2])
+
+  # state covariance when resetting midway in a segment
+  reset_orientation_diag = np.array([1**2, 1**2, 1**2])
+
+  # fake observation covariance, to ensure the uncertainty estimate of the filter is under control
+  fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
+  fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
+
+  # process noise
+  Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
+                     0.001**2, 0.001**2, 0.001**2,
+                     0.01**2, 0.01**2, 0.01**2,
+                     0.1**2, 0.1**2, 0.1**2,
+                     (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+                     3**2, 3**2, 3**2,
+                     0.005**2, 0.005**2, 0.005**2])
+
+  obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
+                    ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
+                    ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
+                    ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
+                    ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
+                    ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
+                    ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
+                    ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = LiveKalman.name
+    dim_state = LiveKalman.initial_x.shape[0]
+    dim_state_err = LiveKalman.initial_P_diag.shape[0]
+
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+    x, y, z = state[States.ECEF_POS, :]
+    q = state[States.ECEF_ORIENTATION, :]
+    v = state[States.ECEF_VELOCITY, :]
+    vx, vy, vz = v
+    omega = state[States.ANGULAR_VELOCITY, :]
+    vroll, vpitch, vyaw = omega
+    roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
+    acceleration = state[States.ACCELERATION, :]
+    acc_bias = state[States.ACC_BIAS, :]
+
+    dt = sp.Symbol('dt')
+
+    # calibration and attitude rotation matrices
+    quat_rot = quat_rotate(*q)
+
+    # Got the quat predict equations from here
+    # A New Quaternion-Based Kalman Filter for
+    # Real-Time Attitude Estimation Using the Two-Step
+    # Geometrically-Intuitive Correction Algorithm
+    A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
+                         [vroll, 0, vyaw, -vpitch],
+                         [vpitch, -vyaw, 0, vroll],
+                         [vyaw, vpitch, -vroll, 0]])
+    q_dot = A * q
+
+    # Time derivative of the state as a function of state
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.ECEF_POS, :] = v
+    state_dot[States.ECEF_ORIENTATION, :] = q_dot
+    state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
+
+    # Basic descretization, 1st order intergrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
+    state_err = sp.Matrix(state_err_sym)
+    quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
+    v_err = state_err[States.ECEF_VELOCITY_ERR, :]
+    omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
+    acceleration_err = state_err[States.ACCELERATION_ERR, :]
+
+    # Time derivative of the state error as a function of state error and state
+    quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
+    q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
+    state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
+    state_err_dot[States.ECEF_POS_ERR, :] = v_err
+    state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
+    state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
+    f_err_sym = state_err + dt * state_err_dot
+
+    # Observation matrix modifier
+    H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
+    H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
+    H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
+    H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
+
+    # these error functions are defined so that say there
+    # is a nominal x and true x:
+    # true x = err_function(nominal x, delta x)
+    # delta x = inv_err_function(nominal x, true x)
+    nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
+    true_x = sp.MatrixSymbol('true_x', dim_state, 1)
+    delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
+
+    err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
+    delta_quat = sp.Matrix(np.ones(4))
+    delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
+    err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
+    err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
+    err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
+
+    inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
+    inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
+    delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
+    inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
+    inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
+
+    eskf_params = [[err_function_sym, nom_x, delta_x],
+                   [inv_err_function_sym, nom_x, true_x],
+                   H_mod_sym, f_err_sym, state_err_sym]
+    #
+    # Observation functions
+    #
+    h_gyro_sym = sp.Matrix([
+      vroll + roll_bias,
+      vpitch + pitch_bias,
+      vyaw + yaw_bias])
+
+    pos = sp.Matrix([x, y, z])
+    gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
+    h_acc_sym = (gravity + acceleration + acc_bias)
+    h_acc_stationary_sym = acceleration
+    h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
+    h_pos_sym = sp.Matrix([x, y, z])
+    h_vel_sym = sp.Matrix([vx, vy, vz])
+    h_orientation_sym = q
+    h_relative_motion = sp.Matrix(quat_rot.T * v)
+
+    obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+               [h_phone_rot_sym, ObservationKind.NO_ROT, None],
+               [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
+               [h_pos_sym, ObservationKind.ECEF_POS, None],
+               [h_vel_sym, ObservationKind.ECEF_VEL, None],
+               [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
+               [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
+               [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
+               [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
+
+    # this returns a sympy routine for the jacobian of the observation function of the local vel
+    in_vec = sp.MatrixSymbol('in_vec', 6, 1)  # roll, pitch, yaw, vx, vy, vz
+    h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
+    extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
+
+    # write constants to extra header file for use in cpp
+    live_kf_header = "#pragma once\n\n"
+    live_kf_header += "#include <unordered_map>\n"
+    live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
+    for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
+      assert(slc.step is None)  # unsupported
+      live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
+      live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
+      live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
+    live_kf_header += "\n"
+
+    for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
+      live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
+    live_kf_header += "\n"
+
+    live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
+    live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
+    for kind, noise in LiveKalman.obs_noise_diag.items():
+      live_kf_header += f"  {{ {kind}, {numpy2eigenstring(noise)} }},\n"
+    live_kf_header += "};\n\n"
+
+    open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  LiveKalman.generate_code(generated_dir)
--- a/sunnypilot/selfdrive/locationd/tests/.gitignore
+++ b/sunnypilot/selfdrive/locationd/tests/.gitignore
@@ -0,0 +1 @@
+out/
--- a/sunnypilot/selfdrive/locationd/tests/init.py
+++ b/sunnypilot/selfdrive/locationd/tests/init.py
--- a/sunnypilot/selfdrive/locationd/tests/test_locationd.py
+++ b/sunnypilot/selfdrive/locationd/tests/test_locationd.py
@@ -0,0 +1,89 @@
+import pytest
+import json
+import random
+import time
+import capnp
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.common.params import Params
+from openpilot.common.transformations.coordinates import ecef2geodetic
+
+from openpilot.system.manager.process_config import managed_processes
+
+
+class TestLocationdProc:
+  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
+              'accelerometer', 'gyroscope', 'magnetometer']
+
+  def setup_method(self):
+    self.pm = messaging.PubMaster(self.LLD_MSGS)
+
+    self.params = Params()
+    self.params.put_bool("UbloxAvailable", True)
+    managed_processes['locationd_llk'].prepare()
+    managed_processes['locationd_llk'].start()
+
+  def teardown_method(self):
+    managed_processes['locationd_llk'].stop()
+
+  def get_msg(self, name, t):
+    try:
+      msg = messaging.new_message(name)
+    except capnp.lib.capnp.KjException:
+      msg = messaging.new_message(name, 0)
+
+    if name == "gpsLocationExternal":
+      msg.gpsLocationExternal.flags = 1
+      msg.gpsLocationExternal.hasFix = True
+      msg.gpsLocationExternal.verticalAccuracy = 1.0
+      msg.gpsLocationExternal.speedAccuracy = 1.0
+      msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
+      msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
+      msg.gpsLocationExternal.latitude = float(self.lat)
+      msg.gpsLocationExternal.longitude = float(self.lon)
+      msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
+      msg.gpsLocationExternal.altitude = float(self.alt)
+    #if name == "gnssMeasurements":
+    #  msg.gnssMeasurements.measTime = t
+    #  msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
+    #  msg.gnssMeasurements.positionECEF.std = [0,0,0]
+    #  msg.gnssMeasurements.positionECEF.valid = True
+    #  msg.gnssMeasurements.velocityECEF.value = []
+    #  msg.gnssMeasurements.velocityECEF.std = [0,0,0]
+    #  msg.gnssMeasurements.velocityECEF.valid = True
+    elif name == 'cameraOdometry':
+      msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
+    msg.logMonoTime = t
+    msg.valid = True
+    return msg
+
+  def test_params_gps(self):
+    random.seed(123489234)
+    self.params.remove('LastGPSPositionLLK')
+
+    self.x = -2710700 + (random.random() * 1e5)
+    self.y = -4280600 + (random.random() * 1e5)
+    self.z = 3850300 + (random.random() * 1e5)
+    self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
+
+    # get fake messages at the correct frequency, listed in services.py
+    msgs = []
+    for sec in range(65):
+      for name in self.LLD_MSGS:
+        for j in range(int(SERVICE_LIST[name].frequency)):
+          msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
+
+    for msg in sorted(msgs, key=lambda x: x.logMonoTime):
+      self.pm.send(msg.which(), msg)
+      if msg.which() == "cameraOdometry":
+        self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005)
+    time.sleep(1)  # wait for async params write
+
+    lastGPS = json.loads(self.params.get('LastGPSPositionLLK'))
+    assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001)
+    assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001)
+    assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001)
--- a/sunnypilot/system/sensord/.gitignore
+++ b/sunnypilot/system/sensord/.gitignore
@@ -0,0 +1 @@
+sensord
--- a/sunnypilot/system/sensord/SConscript
+++ b/sunnypilot/system/sensord/SConscript
@@ -0,0 +1,13 @@
+Import('env', 'arch', 'common', 'messaging')
+
+sensors = [
+  'sensors/i2c_sensor.cc',
+  'sensors/lsm6ds3_accel.cc',
+  'sensors/lsm6ds3_gyro.cc',
+  'sensors/lsm6ds3_temp.cc',
+  'sensors/mmc5603nj_magn.cc',
+]
+libs = [common, messaging, 'pthread']
+if arch == "larch64":
+  libs.append('i2c')
+env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)
--- a/sunnypilot/system/sensord/sensors/bmx055_accel.cc
+++ b/sunnypilot/system/sensord/sensors/bmx055_accel.cc
@@ -0,0 +1,85 @@
+#include "sunnypilot/system/sensord/sensors/bmx055_accel.h"
+
+#include <cassert>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Accel::init() {
+  int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
+  if (ret == -1) {
+    goto fail;
+  }
+
+  ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // bmx055 accel has a 1.3ms wakeup time from deep suspend mode
+  util::sleep_for(10);
+
+  // High bandwidth
+  // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE);
+  // if (ret < 0) {
+  //   goto fail;
+  // }
+
+  // Low bandwidth
+  ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  enabled = true;
+
+fail:
+  return ret;
+}
+
+int BMX055_Accel::shutdown()  {
+  if (!enabled) return 0;
+
+  // enter deep suspend mode (lowest power mode)
+  int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 ACCEL in deep suspend mode!");
+  }
+
+  return ret;
+}
+
+bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[6];
+  int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer));
+  assert(len == 6);
+
+  // 12 bit = +-2g
+  float scale = 9.81 * 2.0f / (1 << 11);
+  float x = -read_12_bit(buffer[0], buffer[1]) * scale;
+  float y = -read_12_bit(buffer[2], buffer[3]) * scale;
+  float z = read_12_bit(buffer[4], buffer[5]) * scale;
+
+  auto event = msg.initEvent().initAccelerometer2();
+  event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+  event.setVersion(1);
+  event.setSensor(SENSOR_ACCELEROMETER);
+  event.setType(SENSOR_TYPE_ACCELEROMETER);
+  event.setTimestamp(start_time);
+
+  float xyz[] = {x, y, z};
+  auto svec = event.initAcceleration();
+  svec.setV(xyz);
+  svec.setStatus(true);
+
+  return true;
+}
--- a/sunnypilot/system/sensord/sensors/bmx055_accel.h
+++ b/sunnypilot/system/sensord/sensors/bmx055_accel.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define BMX055_ACCEL_I2C_ADDR       0x18
+
+// Registers of the chip
+#define BMX055_ACCEL_I2C_REG_ID     0x00
+#define BMX055_ACCEL_I2C_REG_X_LSB  0x02
+#define BMX055_ACCEL_I2C_REG_TEMP   0x08
+#define BMX055_ACCEL_I2C_REG_BW     0x10
+#define BMX055_ACCEL_I2C_REG_PMU    0x11
+#define BMX055_ACCEL_I2C_REG_HBW    0x13
+#define BMX055_ACCEL_I2C_REG_FIFO   0x3F
+
+// Constants
+#define BMX055_ACCEL_CHIP_ID        0xFA
+
+#define BMX055_ACCEL_HBW_ENABLE       0b10000000
+#define BMX055_ACCEL_HBW_DISABLE      0b00000000
+#define BMX055_ACCEL_DEEP_SUSPEND     0b00100000
+#define BMX055_ACCEL_NORMAL_MODE      0b00000000
+
+#define BMX055_ACCEL_BW_7_81HZ  0b01000
+#define BMX055_ACCEL_BW_15_63HZ 0b01001
+#define BMX055_ACCEL_BW_31_25HZ 0b01010
+#define BMX055_ACCEL_BW_62_5HZ  0b01011
+#define BMX055_ACCEL_BW_125HZ   0b01100
+#define BMX055_ACCEL_BW_250HZ   0b01101
+#define BMX055_ACCEL_BW_500HZ   0b01110
+#define BMX055_ACCEL_BW_1000HZ  0b01111
+
+class BMX055_Accel : public I2CSensor {
+  uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
+public:
+  BMX055_Accel(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};
--- a/sunnypilot/system/sensord/sensors/bmx055_gyro.cc
+++ b/sunnypilot/system/sensord/sensors/bmx055_gyro.cc
@@ -0,0 +1,92 @@
+#include "sunnypilot/system/sensord/sensors/bmx055_gyro.h"
+
+#include <cassert>
+#include <cmath>
+
+#include "common/swaglog.h"
+#include "common/util.h"
+
+#define DEG2RAD(x) ((x) * M_PI / 180.0)
+
+
+BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Gyro::init() {
+  int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID});
+  if (ret == -1) return -1;
+
+  ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+  // bmx055 gyro has a 30ms wakeup time from deep suspend mode
+  util::sleep_for(50);
+
+  // High bandwidth
+  // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE);
+  // if (ret < 0) {
+  //   goto fail;
+  // }
+
+  // Low bandwidth
+  ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // 116 Hz filter
+  ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // +- 125 deg/s range
+  ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  enabled = true;
+
+fail:
+  return ret;
+}
+
+int BMX055_Gyro::shutdown()  {
+  if (!enabled) return 0;
+
+  // enter deep suspend mode (lowest power mode)
+  int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 GYRO in deep suspend mode!");
+  }
+
+  return ret;
+}
+
+bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[6];
+  int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer));
+  assert(len == 6);
+
+  // 16 bit = +- 125 deg/s
+  float scale = 125.0f / (1 << 15);
+  float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
+  float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
+  float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
+
+  auto event = msg.initEvent().initGyroscope2();
+  event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+  event.setVersion(1);
+  event.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
+  event.setTimestamp(start_time);
+
+  float xyz[] = {x, y, z};
+  auto svec = event.initGyroUncalibrated();
+  svec.setV(xyz);
+  svec.setStatus(true);
+
+  return true;
+}
--- a/sunnypilot/system/sensord/sensors/bmx055_gyro.h
+++ b/sunnypilot/system/sensord/sensors/bmx055_gyro.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define BMX055_GYRO_I2C_ADDR        0x68
+
+// Registers of the chip
+#define BMX055_GYRO_I2C_REG_ID         0x00
+#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02
+#define BMX055_GYRO_I2C_REG_RANGE      0x0F
+#define BMX055_GYRO_I2C_REG_BW         0x10
+#define BMX055_GYRO_I2C_REG_LPM1       0x11
+#define BMX055_GYRO_I2C_REG_HBW        0x13
+#define BMX055_GYRO_I2C_REG_FIFO       0x3F
+
+// Constants
+#define BMX055_GYRO_CHIP_ID         0x0F
+
+#define BMX055_GYRO_HBW_ENABLE       0b10000000
+#define BMX055_GYRO_HBW_DISABLE      0b00000000
+#define BMX055_GYRO_DEEP_SUSPEND     0b00100000
+#define BMX055_GYRO_NORMAL_MODE      0b00000000
+
+#define BMX055_GYRO_RANGE_2000      0b000
+#define BMX055_GYRO_RANGE_1000      0b001
+#define BMX055_GYRO_RANGE_500       0b010
+#define BMX055_GYRO_RANGE_250       0b011
+#define BMX055_GYRO_RANGE_125       0b100
+
+#define BMX055_GYRO_BW_116HZ 0b0010
+
+
+class BMX055_Gyro : public I2CSensor {
+  uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;}
+public:
+  BMX055_Gyro(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};
--- a/sunnypilot/system/sensord/sensors/bmx055_magn.cc
+++ b/sunnypilot/system/sensord/sensors/bmx055_magn.cc
@@ -0,0 +1,258 @@
+#include "sunnypilot/system/sensord/sensors/bmx055_magn.h"
+
+#include <unistd.h>
+
+#include <algorithm>
+#include <cassert>
+#include <cstdio>
+
+#include "common/swaglog.h"
+#include "common/util.h"
+
+static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) {
+  uint16_t process_comp_x0 = data_rhall;
+  int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384;
+  uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000);
+  int16_t retval = ((int16_t)process_comp_x2);
+  int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval));
+  int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128));
+  int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128);
+  int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5;
+  int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000));
+  int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0)));
+  int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096);
+  int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9;
+  retval = ((int16_t)(process_comp_x10 / 8192));
+  retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16;
+
+  return retval;
+}
+
+static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) {
+  uint16_t process_comp_y0 = trim_data.dig_xyz1;
+  int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0;
+  uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000);
+  int16_t retval = ((int16_t)process_comp_y2);
+  int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval);
+  int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128);
+  int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128));
+  int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512);
+  int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0)));
+  int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096);
+  int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8);
+  retval = (int16_t)(process_comp_y9 / 8192);
+  retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16;
+
+  return retval;
+}
+
+static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) {
+  int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1);
+  int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4;
+  int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768);
+  int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2);
+  int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536);
+  int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4));
+
+  /* saturate result to +/- 2 micro-tesla */
+  retval = std::clamp(retval, -32767, 32767);
+
+  /* Conversion of LSB to micro-tesla*/
+  retval = retval / 16;
+
+  return (int16_t)retval;
+}
+
+BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Magn::init() {
+  uint8_t trim_x1y1[2] = {0};
+  uint8_t trim_x2y2[2] = {0};
+  uint8_t trim_xy1xy2[2] = {0};
+  uint8_t trim_z1[2] = {0};
+  uint8_t trim_z2[2] = {0};
+  uint8_t trim_z3[2] = {0};
+  uint8_t trim_z4[2] = {0};
+  uint8_t trim_xyz1[2] = {0};
+
+  // suspend -> sleep
+  int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
+  if (ret < 0) {
+    LOGD("Enabling power failed: %d", ret);
+    goto fail;
+  }
+  util::sleep_for(5); // wait until the chip is powered on
+
+  ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID});
+  if (ret == -1) {
+    goto fail;
+  }
+
+  // Load magnetometer trim
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2);
+  if (ret < 0) goto fail;
+
+  // Read trim data
+  trim_data.dig_x1 = trim_x1y1[0];
+  trim_data.dig_y1 = trim_x1y1[1];
+
+  trim_data.dig_x2 = trim_x2y2[0];
+  trim_data.dig_y2 = trim_x2y2[1];
+
+  trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped
+  trim_data.dig_xy2 = trim_xy1xy2[0];
+
+  trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]);
+  trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]);
+  trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]);
+  trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]);
+
+  trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f);
+  assert(trim_data.dig_xyz1 != 0);
+
+  perform_self_test();
+
+  // f_max = 1 / (145us * nXY + 500us * NZ + 980us)
+  // Chose NXY = 7, NZ = 12, which gives 125 Hz,
+  // and has the same ratio as the high accuracy preset
+  ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  enabled = true;
+  return 0;
+
+ fail:
+  return ret;
+}
+
+int BMX055_Magn::shutdown() {
+  if (!enabled) return 0;
+
+  // move to suspend mode
+  int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 MAGN in suspend mode!");
+  }
+
+  return ret;
+}
+
+bool BMX055_Magn::perform_self_test() {
+  uint8_t buffer[8];
+  int16_t x, y;
+  int16_t neg_z, pos_z;
+
+  // Increase z reps for less false positives (~30 Hz ODR)
+  set_register(BMX055_MAGN_I2C_REG_REPXY, 1);
+  set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1);
+
+  // Clean existing measurement
+  read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+
+  uint8_t forced = BMX055_MAGN_FORCED;
+
+  // Negative current
+  set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
+  util::sleep_for(100);
+
+  read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  parse_xyz(buffer, &x, &y, &neg_z);
+
+  // Positive current
+  set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
+  util::sleep_for(100);
+
+  read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  parse_xyz(buffer, &x, &y, &pos_z);
+
+  // Put back in normal mode
+  set_register(BMX055_MAGN_I2C_REG_MAG, 0);
+
+  int16_t diff = pos_z - neg_z;
+  bool passed = (diff > 180) && (diff < 240);
+
+  if (!passed) {
+    LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff);
+  }
+
+  return passed;
+}
+
+bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) {
+  bool ready = buffer[6] & 0x1;
+  if (ready) {
+    int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3;
+    int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3;
+    int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1;
+    uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2;
+    assert(data_r != 0);
+
+    *x = compensate_x(trim_data, mdata_x, data_r);
+    *y = compensate_y(trim_data, mdata_y, data_r);
+    *z = compensate_z(trim_data, mdata_z, data_r);
+  }
+  return ready;
+}
+
+
+bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[8];
+  int16_t _x, _y, x, y, z;
+
+  int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  bool parsed = parse_xyz(buffer, &_x, &_y, &z);
+  if (parsed) {
+
+    auto event = msg.initEvent().initMagnetometer();
+    event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+    event.setVersion(2);
+    event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+    event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
+    event.setTimestamp(start_time);
+
+    // Move magnetometer into same reference frame as accel/gryo
+    x = -_y;
+    y = _x;
+
+    // Axis convention
+    x = -x;
+    y = -y;
+
+    float xyz[] = {(float)x, (float)y, (float)z};
+    auto svec = event.initMagneticUncalibrated();
+    svec.setV(xyz);
+    svec.setStatus(true);
+  }
+
+  // The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
+  // up to 30 Hz. Therefore we put in forced mode, and request measurements
+  // at a 100 Hz. When reading the registers we have to check the ready bit
+  // To verify the measurement was completed this cycle.
+  set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
+
+  return parsed;
+}
--- a/sunnypilot/system/sensord/sensors/bmx055_magn.h
+++ b/sunnypilot/system/sensord/sensors/bmx055_magn.h
@@ -0,0 +1,64 @@
+#pragma once
+#include <tuple>
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define BMX055_MAGN_I2C_ADDR        0x10
+
+// Registers of the chip
+#define BMX055_MAGN_I2C_REG_ID        0x40
+#define BMX055_MAGN_I2C_REG_PWR_0     0x4B
+#define BMX055_MAGN_I2C_REG_MAG       0x4C
+#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42
+#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48
+#define BMX055_MAGN_I2C_REG_REPXY     0x51
+#define BMX055_MAGN_I2C_REG_REPZ      0x52
+
+#define BMX055_MAGN_I2C_REG_DIG_X1       0x5D
+#define BMX055_MAGN_I2C_REG_DIG_Y1       0x5E
+#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB   0x62
+#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB   0x63
+#define BMX055_MAGN_I2C_REG_DIG_X2       0x64
+#define BMX055_MAGN_I2C_REG_DIG_Y2       0x65
+#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB   0x68
+#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB   0x69
+#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB   0x6A
+#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB   0x6B
+#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C
+#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D
+#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB   0x6E
+#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB   0x6F
+#define BMX055_MAGN_I2C_REG_DIG_XY2      0x70
+#define BMX055_MAGN_I2C_REG_DIG_XY1      0x71
+
+// Constants
+#define BMX055_MAGN_CHIP_ID     0x32
+#define BMX055_MAGN_FORCED      (0b01 << 1)
+
+struct trim_data_t {
+    int8_t dig_x1;
+    int8_t dig_y1;
+    int8_t dig_x2;
+    int8_t dig_y2;
+    uint16_t dig_z1;
+    int16_t dig_z2;
+    int16_t dig_z3;
+    int16_t dig_z4;
+    uint8_t dig_xy1;
+    int8_t dig_xy2;
+    uint16_t dig_xyz1;
+};
+
+
+class BMX055_Magn : public I2CSensor{
+  uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;}
+  trim_data_t trim_data = {0};
+  bool perform_self_test();
+  bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z);
+public:
+  BMX055_Magn(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};
--- a/sunnypilot/system/sensord/sensors/bmx055_temp.cc
+++ b/sunnypilot/system/sensord/sensors/bmx055_temp.cc
@@ -0,0 +1,31 @@
+#include "sunnypilot/system/sensord/sensors/bmx055_temp.h"
+
+#include <cassert>
+
+#include "sunnypilot/system/sensord/sensors/bmx055_accel.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Temp::init() {
+  return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0;
+}
+
+bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[1];
+  int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float temp = 23.0f + int8_t(buffer[0]) / 2.0f;
+
+  auto event = msg.initEvent().initTemperatureSensor();
+  event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+  event.setVersion(1);
+  event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
+  event.setTimestamp(start_time);
+  event.setTemperature(temp);
+
+  return true;
+}
--- a/sunnypilot/system/sensord/sensors/bmx055_temp.h
+++ b/sunnypilot/system/sensord/sensors/bmx055_temp.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/bmx055_accel.h"
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+class BMX055_Temp : public I2CSensor {
+  uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
+public:
+  BMX055_Temp(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown() { return 0; }
+};
--- a/sunnypilot/system/sensord/sensors/constants.h
+++ b/sunnypilot/system/sensord/sensors/constants.h
@@ -0,0 +1,18 @@
+#pragma once
+
+
+#define SENSOR_ACCELEROMETER 1
+#define SENSOR_MAGNETOMETER 2
+#define SENSOR_MAGNETOMETER_UNCALIBRATED 3
+#define SENSOR_GYRO 4
+#define SENSOR_GYRO_UNCALIBRATED 5
+#define SENSOR_LIGHT 7
+
+#define SENSOR_TYPE_ACCELEROMETER 1
+#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2
+#define SENSOR_TYPE_GYROSCOPE 4
+#define SENSOR_TYPE_LIGHT 5
+#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13
+#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14
+#define SENSOR_TYPE_MAGNETIC_FIELD  SENSOR_TYPE_GEOMAGNETIC_FIELD
+#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16
--- a/sunnypilot/system/sensord/sensors/i2c_sensor.cc
+++ b/sunnypilot/system/sensord/sensors/i2c_sensor.cc
@@ -0,0 +1,50 @@
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+int16_t read_12_bit(uint8_t lsb, uint8_t msb) {
+  uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0);
+  return int16_t(combined) / (1 << 4);
+}
+
+int16_t read_16_bit(uint8_t lsb, uint8_t msb) {
+  uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb);
+  return int16_t(combined);
+}
+
+int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) {
+  uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2);
+  return int32_t(combined) / (1 << 4);
+}
+
+I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {}
+
+I2CSensor::~I2CSensor() {
+  if (gpio_fd != -1) {
+    close(gpio_fd);
+  }
+}
+
+int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) {
+  return bus->read_register(get_device_address(), register_address, buffer, len);
+}
+
+int I2CSensor::set_register(uint register_address, uint8_t data) {
+  return bus->set_register(get_device_address(), register_address, data);
+}
+
+int I2CSensor::init_gpio() {
+  if (shared_gpio || gpio_nr == 0) {
+    return 0;
+  }
+
+  gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr);
+  if (gpio_fd < 0) {
+    return -1;
+  }
+
+  return 0;
+}
+
+bool I2CSensor::has_interrupt_enabled() {
+  return gpio_nr != 0;
+}
--- a/sunnypilot/system/sensord/sensors/i2c_sensor.h
+++ b/sunnypilot/system/sensord/sensors/i2c_sensor.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <cstdint>
+#include <unistd.h>
+#include <vector>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/i2c.h"
+#include "common/gpio.h"
+
+#include "common/swaglog.h"
+#include "sunnypilot/system/sensord/sensors/constants.h"
+#include "sunnypilot/system/sensord/sensors/sensor.h"
+
+int16_t read_12_bit(uint8_t lsb, uint8_t msb);
+int16_t read_16_bit(uint8_t lsb, uint8_t msb);
+int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0);
+
+
+class I2CSensor : public Sensor {
+private:
+  I2CBus *bus;
+  int gpio_nr;
+  bool shared_gpio;
+  virtual uint8_t get_device_address() = 0;
+
+public:
+  I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
+  ~I2CSensor();
+  int read_register(uint register_address, uint8_t *buffer, uint8_t len);
+  int set_register(uint register_address, uint8_t data);
+  int init_gpio();
+  bool has_interrupt_enabled();
+  virtual int init() = 0;
+  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
+  virtual int shutdown() = 0;
+
+  int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
+    uint8_t chip_id = 0;
+    int ret = read_register(address, &chip_id, 1);
+    if (ret < 0) {
+      LOGD("Reading chip ID failed: %d", ret);
+      return -1;
+    }
+    for (int i = 0; i < expected_ids.size(); ++i) {
+      if (chip_id == expected_ids[i]) return chip_id;
+    }
+    LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]);
+    return -1;
+  }
+};
--- a/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc
+++ b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc
@@ -0,0 +1,250 @@
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_accel.h"
+
+#include <cassert>
+#include <cmath>
+#include <cstring>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  I2CSensor(bus, gpio_nr, shared_gpio) {}
+
+void LSM6DS3_Accel::wait_for_data_ready() {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  do {
+    read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+    drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
+  } while (drdy == 0);
+
+  read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
+}
+
+void LSM6DS3_Accel::read_and_avg_data(float* out_buf) {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  float scaling = 0.061f;
+  if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
+    scaling = 0.122f;
+  }
+
+  for (int i = 0; i < 5; i++) {
+    do {
+      read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+      drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
+    } while (drdy == 0);
+
+    int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
+    assert(len == sizeof(buffer));
+
+    for (int j = 0; j < 3; j++) {
+      out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling;
+    }
+  }
+
+  for (int i = 0; i < 3; i++) {
+    out_buf[i] /= 5.0f;
+  }
+}
+
+int LSM6DS3_Accel::self_test(int test_type) {
+  float val_st_off[3] = {0};
+  float val_st_on[3] = {0};
+  float test_val[3] = {0};
+  uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz
+
+  // prepare sensor for self-test
+
+  // enable block data update and automatic increment
+  int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU);
+  if (ret < 0) {
+    return ret;
+  }
+
+  if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
+    ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ;
+  }
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(100);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_off);
+
+  // enable Self Test positive (or negative)
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(100);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_on);
+
+  // disable sensor
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // disable self test
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // calculate the mg values for self test
+  for (int i = 0; i < 3; i++) {
+    test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
+  }
+
+  // verify test result
+  for (int i = 0; i < 3; i++) {
+    if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) ||
+        (test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) {
+      return -1;
+    }
+  }
+
+  return ret;
+}
+
+int LSM6DS3_Accel::init() {
+  uint8_t value = 0;
+  bool do_self_test = false;
+
+  const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
+  if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
+    do_self_test = true;
+  }
+
+  int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID});
+  if (ret == -1) return -1;
+
+  if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) {
+    source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
+  }
+
+  ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 accel positive self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 accel negative self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  ret = init_gpio();
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // enable continuous update, and automatic increase
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // enable data ready interrupt for accel on INT1
+  // (without resetting existing interrupts)
+  ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value |= LSM6DS3_ACCEL_INT1_DRDY_XL;
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
+
+fail:
+  return ret;
+}
+
+int LSM6DS3_Accel::shutdown() {
+  int ret = 0;
+
+  // disable data ready interrupt for accel on INT1
+  uint8_t value = 0;
+  ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL);
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
+  if (ret < 0) {
+    LOGE("Could not disable lsm6ds3 acceleration interrupt!");
+    goto fail;
+  }
+
+  // enable power-down mode
+  value = 0;
+  ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= 0x0F;
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value);
+  if (ret < 0) {
+    LOGE("Could not power-down lsm6ds3 accelerometer!");
+    goto fail;
+  }
+
+fail:
+  return ret;
+}
+
+bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
+
+  // INT1 shared with gyro, check STATUS_REG who triggered
+  uint8_t status_reg = 0;
+  read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
+  if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) {
+    return false;
+  }
+
+  uint8_t buffer[6];
+  int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float scale = 9.81 * 2.0f / (1 << 15);
+  float x = read_16_bit(buffer[0], buffer[1]) * scale;
+  float y = read_16_bit(buffer[2], buffer[3]) * scale;
+  float z = read_16_bit(buffer[4], buffer[5]) * scale;
+
+  auto event = msg.initEvent().initAccelerometer();
+  event.setSource(source);
+  event.setVersion(1);
+  event.setSensor(SENSOR_ACCELEROMETER);
+  event.setType(SENSOR_TYPE_ACCELEROMETER);
+  event.setTimestamp(ts);
+
+  float xyz[] = {y, -x, z};
+  auto svec = event.initAcceleration();
+  svec.setV(xyz);
+  svec.setStatus(true);
+
+  return true;
+}
--- a/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h
+++ b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define LSM6DS3_ACCEL_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG  0x0B
+#define LSM6DS3_ACCEL_I2C_REG_ID        0x0F
+#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D
+#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL  0x10
+#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C   0x12
+#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C   0x14
+#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL   0x18
+#define LSM6DS3_ACCEL_I2C_REG_STAT_REG  0x1E
+#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28
+
+// Constants
+#define LSM6DS3_ACCEL_CHIP_ID         0x69
+#define LSM6DS3TRC_ACCEL_CHIP_ID      0x6A
+#define LSM6DS3_ACCEL_FS_4G           (0b10 << 2)
+#define LSM6DS3_ACCEL_ODR_52HZ        (0b0011 << 4)
+#define LSM6DS3_ACCEL_ODR_104HZ       (0b0100 << 4)
+#define LSM6DS3_ACCEL_INT1_DRDY_XL    0b1
+#define LSM6DS3_ACCEL_DRDY_XLDA       0b1
+#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7)
+#define LSM6DS3_ACCEL_IF_INC          0b00000100
+#define LSM6DS3_ACCEL_IF_INC_BDU      0b01000100
+#define LSM6DS3_ACCEL_XYZ_DEN         0b11100000
+#define LSM6DS3_ACCEL_POSITIVE_TEST   0b01
+#define LSM6DS3_ACCEL_NEGATIVE_TEST   0b10
+#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f
+#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f
+
+class LSM6DS3_Accel : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;}
+  cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
+
+  // self test functions
+  int self_test(int test_type);
+  void wait_for_data_ready();
+  void read_and_avg_data(float* val_st_off);
+public:
+  LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};
--- a/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc
+++ b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc
@@ -0,0 +1,233 @@
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h"
+
+#include <cassert>
+#include <cmath>
+#include <cstring>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+#define DEG2RAD(x) ((x) * M_PI / 180.0)
+
+LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  I2CSensor(bus, gpio_nr, shared_gpio) {}
+
+void LSM6DS3_Gyro::wait_for_data_ready() {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  do {
+    read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+    drdy &= LSM6DS3_GYRO_DRDY_GDA;
+  } while (drdy == 0);
+
+  read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
+}
+
+void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  for (int i = 0; i < 5; i++) {
+    do {
+      read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+      drdy &= LSM6DS3_GYRO_DRDY_GDA;
+    } while (drdy == 0);
+
+    int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
+    assert(len == sizeof(buffer));
+
+    for (int j = 0; j < 3; j++) {
+      out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f;
+    }
+  }
+
+  // calculate the mg average values
+  for (int i = 0; i < 3; i++) {
+    out_buf[i] /= 5.0f;
+  }
+}
+
+int LSM6DS3_Gyro::self_test(int test_type) {
+  float val_st_off[3] = {0};
+  float val_st_on[3] = {0};
+  float test_val[3] = {0};
+
+  // prepare sensor for self-test
+
+  // full scale: 2000dps, ODR: 208Hz
+  int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(150);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_off);
+
+  // enable Self Test positive (or negative)
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(50);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_on);
+
+  // disable sensor
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // disable self test
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // calculate the mg values for self test
+  for (int i = 0; i < 3; i++) {
+    test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
+  }
+
+  // verify test result
+  for (int i = 0; i < 3; i++) {
+    if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) ||
+        (test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) {
+      return -1;
+    }
+  }
+
+  return ret;
+}
+
+int LSM6DS3_Gyro::init() {
+  uint8_t value = 0;
+  bool do_self_test = false;
+
+  const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
+  if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
+    do_self_test = true;
+  }
+
+  int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID});
+  if (ret == -1) return -1;
+
+  if (ret == LSM6DS3TRC_GYRO_CHIP_ID) {
+    source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
+  }
+
+  ret = init_gpio();
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 gyro positive self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 gyro negative self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  // TODO: set scale. Default is +- 250 deg/s
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // enable data ready interrupt for gyro on INT1
+  // (without resetting existing interrupts)
+  ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value |= LSM6DS3_GYRO_INT1_DRDY_G;
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
+
+fail:
+  return ret;
+}
+
+int LSM6DS3_Gyro::shutdown() {
+  int ret = 0;
+
+  // disable data ready interrupt for gyro on INT1
+  uint8_t value = 0;
+  ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= ~(LSM6DS3_GYRO_INT1_DRDY_G);
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
+  if (ret < 0) {
+    LOGE("Could not disable lsm6ds3 gyroscope interrupt!");
+    goto fail;
+  }
+
+  // enable power-down mode
+  value = 0;
+  ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= 0x0F;
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value);
+  if (ret < 0) {
+    LOGE("Could not power-down lsm6ds3 gyroscope!");
+    goto fail;
+  }
+
+fail:
+  return ret;
+}
+
+bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
+
+  // INT1 shared with accel, check STATUS_REG who triggered
+  uint8_t status_reg = 0;
+  read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
+  if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) {
+    return false;
+  }
+
+  uint8_t buffer[6];
+  int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float scale = 8.75 / 1000.0;
+  float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
+  float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
+  float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
+
+  auto event = msg.initEvent().initGyroscope();
+  event.setSource(source);
+  event.setVersion(2);
+  event.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
+  event.setTimestamp(ts);
+
+  float xyz[] = {y, -x, z};
+  auto svec = event.initGyroUncalibrated();
+  svec.setV(xyz);
+  svec.setStatus(true);
+
+  return true;
+}
--- a/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h
+++ b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h
@@ -0,0 +1,45 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define LSM6DS3_GYRO_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG  0x0B
+#define LSM6DS3_GYRO_I2C_REG_ID        0x0F
+#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D
+#define LSM6DS3_GYRO_I2C_REG_CTRL2_G   0x11
+#define LSM6DS3_GYRO_I2C_REG_CTRL5_C   0x14
+#define LSM6DS3_GYRO_I2C_REG_STAT_REG  0x1E
+#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G  0x22
+#define LSM6DS3_GYRO_POSITIVE_TEST   (0b01 << 2)
+#define LSM6DS3_GYRO_NEGATIVE_TEST   (0b11 << 2)
+
+// Constants
+#define LSM6DS3_GYRO_CHIP_ID         0x69
+#define LSM6DS3TRC_GYRO_CHIP_ID      0x6A
+#define LSM6DS3_GYRO_FS_2000dps      (0b11 << 2)
+#define LSM6DS3_GYRO_ODR_104HZ       (0b0100 << 4)
+#define LSM6DS3_GYRO_ODR_208HZ       (0b0101 << 4)
+#define LSM6DS3_GYRO_INT1_DRDY_G     0b10
+#define LSM6DS3_GYRO_DRDY_GDA        0b10
+#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7)
+#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f
+#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f
+
+
+class LSM6DS3_Gyro : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;}
+  cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
+
+  // self test functions
+  int self_test(int test_type);
+  void wait_for_data_ready();
+  void read_and_avg_data(float* val_st_off);
+public:
+  LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};
--- a/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc
+++ b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc
@@ -0,0 +1,37 @@
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_temp.h"
+
+#include <cassert>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
+
+int LSM6DS3_Temp::init() {
+  int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID});
+  if (ret == -1) return -1;
+
+  if (ret == LSM6DS3TRC_TEMP_CHIP_ID) {
+    source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
+  }
+  return 0;
+}
+
+bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[2];
+  int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f;
+  float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale;
+
+  auto event = msg.initEvent().initTemperatureSensor();
+  event.setSource(source);
+  event.setVersion(1);
+  event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
+  event.setTimestamp(start_time);
+  event.setTemperature(temp);
+
+  return true;
+}
--- a/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h
+++ b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define LSM6DS3_TEMP_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_TEMP_I2C_REG_ID           0x0F
+#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L   0x20
+
+// Constants
+#define LSM6DS3_TEMP_CHIP_ID        0x69
+#define LSM6DS3TRC_TEMP_CHIP_ID     0x6A
+
+
+class LSM6DS3_Temp : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;}
+  cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
+
+public:
+  LSM6DS3_Temp(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown() { return 0; }
+};
--- a/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc
+++ b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc
@@ -0,0 +1,108 @@
+#include "sunnypilot/system/sensord/sensors/mmc5603nj_magn.h"
+
+#include <algorithm>
+#include <cassert>
+#include <vector>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
+
+int MMC5603NJ_Magn::init() {
+  int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
+  if (ret == -1) return -1;
+
+  // Set ODR to 0
+  ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // Set BW to 0b01 for 1-150 Hz operation
+  ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01);
+  if (ret < 0) {
+    goto fail;
+  }
+
+fail:
+  return ret;
+}
+
+int MMC5603NJ_Magn::shutdown() {
+  int ret = 0;
+
+  // disable auto reset of measurements
+  uint8_t value = 0;
+  ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
+  ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // set ODR to 0 to leave continuous mode
+  ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
+  if (ret < 0) {
+    goto fail;
+  }
+  return ret;
+
+fail:
+  LOGE("Could not disable mmc5603nj auto set reset");
+  return ret;
+}
+
+void MMC5603NJ_Magn::start_measurement() {
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01);
+  util::sleep_for(5);
+}
+
+std::vector<float> MMC5603NJ_Magn::read_measurement() {
+  int len;
+  uint8_t buffer[9];
+  len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+  float scale = 1.0 / 16384.0;
+  float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0;
+  float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0;
+  float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0;
+  std::vector<float> xyz = {x, y, z};
+  return xyz;
+}
+
+bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  // SET - RESET cycle
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET);
+  util::sleep_for(5);
+  MMC5603NJ_Magn::start_measurement();
+  std::vector<float> xyz = MMC5603NJ_Magn::read_measurement();
+
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET);
+  util::sleep_for(5);
+  MMC5603NJ_Magn::start_measurement();
+  std::vector<float> reset_xyz = MMC5603NJ_Magn::read_measurement();
+
+  auto event = msg.initEvent().initMagnetometer();
+  event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
+  event.setVersion(1);
+  event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+  event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
+  event.setTimestamp(start_time);
+
+  float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]};
+  bool valid = true;
+  if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) {
+    valid = false;
+  }
+  auto svec = event.initMagneticUncalibrated();
+  svec.setV(vals);
+  svec.setStatus(valid);
+  return true;
+}
--- a/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h
+++ b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <vector>
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define MMC5603NJ_I2C_ADDR       0x30
+
+// Registers of the chip
+#define MMC5603NJ_I2C_REG_XOUT0       0x00
+#define MMC5603NJ_I2C_REG_ODR         0x1A
+#define MMC5603NJ_I2C_REG_INTERNAL_0  0x1B
+#define MMC5603NJ_I2C_REG_INTERNAL_1  0x1C
+#define MMC5603NJ_I2C_REG_INTERNAL_2  0x1D
+#define MMC5603NJ_I2C_REG_ID          0x39
+
+// Constants
+#define MMC5603NJ_CHIP_ID        0x10
+#define MMC5603NJ_CMM_FREQ_EN    (1 << 7)
+#define MMC5603NJ_AUTO_SR_EN     (1 << 5)
+#define MMC5603NJ_CMM_EN         (1 << 4)
+#define MMC5603NJ_EN_PRD_SET     (1 << 3)
+#define MMC5603NJ_SET            (1 << 3)
+#define MMC5603NJ_RESET          (1 << 4)
+
+class MMC5603NJ_Magn : public I2CSensor {
+private:
+  uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
+  void start_measurement();
+  std::vector<float> read_measurement();
+public:
+  MMC5603NJ_Magn(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};
--- a/sunnypilot/system/sensord/sensors/sensor.h
+++ b/sunnypilot/system/sensord/sensors/sensor.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include "cereal/messaging/messaging.h"
+
+class Sensor {
+public:
+  int gpio_fd = -1;
+  bool enabled = false;
+  uint64_t start_ts = 0;
+  uint64_t init_delay = 500e6; // default dealy 500ms
+
+  virtual ~Sensor() {}
+  virtual int init() = 0;
+  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
+  virtual bool has_interrupt_enabled() = 0;
+  virtual int shutdown() = 0;
+
+  virtual bool is_data_valid(uint64_t current_ts) {
+    if (start_ts == 0) {
+      start_ts = current_ts;
+    }
+    return (current_ts - start_ts) > init_delay;
+  }
+};
--- a/sunnypilot/system/sensord/sensors_qcom2.cc
+++ b/sunnypilot/system/sensord/sensors_qcom2.cc
@@ -0,0 +1,179 @@
+#include <sys/resource.h>
+
+#include <chrono>
+#include <thread>
+#include <vector>
+#include <map>
+#include <poll.h>
+#include <linux/gpio.h>
+
+#include "cereal/services.h"
+#include "cereal/messaging/messaging.h"
+#include "common/i2c.h"
+#include "common/ratekeeper.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+#include "sunnypilot/system/sensord/sensors/bmx055_accel.h"
+#include "sunnypilot/system/sensord/sensors/bmx055_gyro.h"
+#include "sunnypilot/system/sensord/sensors/bmx055_magn.h"
+#include "sunnypilot/system/sensord/sensors/bmx055_temp.h"
+#include "sunnypilot/system/sensord/sensors/constants.h"
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_accel.h"
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h"
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_temp.h"
+#include "sunnypilot/system/sensord/sensors/mmc5603nj_magn.h"
+
+#define I2C_BUS_IMU 1
+
+ExitHandler do_exit;
+
+void interrupt_loop(std::vector<std::tuple<Sensor *, std::string>> sensors) {
+  PubMaster pm({"gyroscope", "accelerometer"});
+
+  int fd = -1;
+  for (auto &[sensor, msg_name] : sensors) {
+    if (sensor->has_interrupt_enabled()) {
+      fd = sensor->gpio_fd;
+      break;
+    }
+  }
+
+  uint64_t offset = nanos_since_epoch() - nanos_since_boot();
+  struct pollfd fd_list[1] = {0};
+  fd_list[0].fd = fd;
+  fd_list[0].events = POLLIN | POLLPRI;
+
+  while (!do_exit) {
+    int err = poll(fd_list, 1, 100);
+    if (err == -1) {
+      if (errno == EINTR) {
+        continue;
+      }
+      return;
+    } else if (err == 0) {
+      LOGE("poll timed out");
+      continue;
+    }
+
+    if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) {
+      LOGE("no poll events set");
+      continue;
+    }
+
+    // Read all events
+    struct gpioevent_data evdata[16];
+    err = HANDLE_EINTR(read(fd, evdata, sizeof(evdata)));
+    if (err < 0 || err % sizeof(*evdata) != 0) {
+      LOGE("error reading event data %d", err);
+      continue;
+    }
+
+    uint64_t cur_offset = nanos_since_epoch() - nanos_since_boot();
+    uint64_t diff = cur_offset > offset ? cur_offset - offset : offset - cur_offset;
+    if (diff > 10*1e6) { // 10ms
+      LOGW("time jumped: %lu %lu", cur_offset, offset);
+      offset = cur_offset;
+
+      // we don't have a valid timestamp since the
+      // time jumped, so throw out this measurement.
+      continue;
+    }
+
+    int num_events = err / sizeof(*evdata);
+    uint64_t ts = evdata[num_events - 1].timestamp - cur_offset;
+
+    for (auto &[sensor, msg_name] : sensors) {
+      if (!sensor->has_interrupt_enabled()) {
+        continue;
+      }
+
+      MessageBuilder msg;
+      if (!sensor->get_event(msg, ts)) {
+        continue;
+      }
+
+      if (!sensor->is_data_valid(ts)) {
+        continue;
+      }
+
+      pm.send(msg_name.c_str(), msg);
+    }
+  }
+}
+
+void polling_loop(Sensor *sensor, std::string msg_name) {
+  PubMaster pm({msg_name.c_str()});
+  RateKeeper rk(msg_name, services.at(msg_name).frequency);
+  while (!do_exit) {
+    MessageBuilder msg;
+    if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) {
+      pm.send(msg_name.c_str(), msg);
+    }
+    rk.keepTime();
+  }
+}
+
+int sensor_loop(I2CBus *i2c_bus_imu) {
+  // Sensor init
+  std::vector<std::tuple<Sensor *, std::string>> sensors_init = {
+    {new BMX055_Accel(i2c_bus_imu), "accelerometer2"},
+    {new BMX055_Gyro(i2c_bus_imu), "gyroscope2"},
+    {new BMX055_Magn(i2c_bus_imu), "magnetometer"},
+    {new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"},
+
+    {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"},
+    {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"},
+    {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"},
+
+    {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer"},
+  };
+
+  // Initialize sensors
+  std::vector<std::thread> threads;
+  for (auto &[sensor, msg_name] : sensors_init) {
+    int err = sensor->init();
+    if (err < 0) {
+      continue;
+    }
+
+    if (!sensor->has_interrupt_enabled()) {
+      threads.emplace_back(polling_loop, sensor, msg_name);
+    }
+  }
+
+  // increase interrupt quality by pinning interrupt and process to core 1
+  setpriority(PRIO_PROCESS, 0, -18);
+  util::set_core_affinity({1});
+
+  // TODO: get the IRQ number from gpiochip
+  std::string irq_path = "/proc/irq/336/smp_affinity_list";
+  if (!util::file_exists(irq_path)) {
+    irq_path = "/proc/irq/335/smp_affinity_list";
+  }
+  std::system(util::string_format("sudo su -c 'echo 1 > %s'", irq_path.c_str()).c_str());
+
+  // thread for reading events via interrupts
+  threads.emplace_back(&interrupt_loop, std::ref(sensors_init));
+
+  // wait for all threads to finish
+  for (auto &t : threads) {
+    t.join();
+  }
+
+  for (auto &[sensor, msg_name] : sensors_init) {
+    sensor->shutdown();
+    delete sensor;
+  }
+  return 0;
+}
+
+int main(int argc, char *argv[]) {
+  try {
+    auto i2c_bus_imu = std::make_unique<I2CBus>(I2C_BUS_IMU);
+    return sensor_loop(i2c_bus_imu.get());
+  } catch (std::exception &e) {
+    LOGE("I2CBus init failed");
+    return -1;
+  }
+}
--- a/sunnypilot/system/sensord/tests/test_sensord.py
+++ b/sunnypilot/system/sensord/tests/test_sensord.py
@@ -0,0 +1,251 @@
+import os
+import pytest
+import time
+import numpy as np
+from collections import namedtuple, defaultdict
+
+import cereal.messaging as messaging
+from cereal import log
+from cereal.services import SERVICE_LIST
+from openpilot.common.gpio import get_irqs_for_action
+from openpilot.common.timeout import Timeout
+from openpilot.system.hardware import HARDWARE
+from openpilot.system.manager.process_config import managed_processes
+
+BMX = {
+  ('bmx055', 'acceleration'),
+  ('bmx055', 'gyroUncalibrated'),
+  ('bmx055', 'magneticUncalibrated'),
+  ('bmx055', 'temperature'),
+}
+
+LSM = {
+  ('lsm6ds3', 'acceleration'),
+  ('lsm6ds3', 'gyroUncalibrated'),
+  ('lsm6ds3', 'temperature'),
+}
+LSM_C = {(x[0]+'trc', x[1]) for x in LSM}
+
+MMC = {
+  ('mmc5603nj', 'magneticUncalibrated'),
+}
+
+SENSOR_CONFIGURATIONS: list[set] = [
+  BMX | LSM,
+  MMC | LSM,
+  BMX | LSM_C,
+  MMC| LSM_C,
+]
+if HARDWARE.get_device_type() == "mici":
+  SENSOR_CONFIGURATIONS = [
+    LSM,
+    LSM_C,
+  ]
+
+Sensor = log.SensorEventData.SensorSource
+SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
+ALL_SENSORS = {
+  Sensor.lsm6ds3: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.lsm6ds3trc: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.bmx055: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("magneticUncalibrated", 0, 300),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.mmc5603nj: {
+    SensorConfig("magneticUncalibrated", 0, 300),
+  }
+}
+
+
+def get_irq_count(irq: int):
+  with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f:
+    per_cpu = map(int, f.read().split(","))
+    return sum(per_cpu)
+
+def read_sensor_events(duration_sec):
+  sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2',
+                  'gyroscope2', 'temperatureSensor', 'temperatureSensor2']
+  socks = {}
+  poller = messaging.Poller()
+  events = defaultdict(list)
+  for stype in sensor_types:
+    socks[stype] = messaging.sub_sock(stype, poller=poller, timeout=100)
+
+  # wait for sensors to come up
+  with Timeout(int(os.environ.get("SENSOR_WAIT", "5")), "sensors didn't come up"):
+    while len(poller.poll(250)) == 0:
+      pass
+  time.sleep(1)
+  for s in socks.values():
+    messaging.drain_sock_raw(s)
+
+  st = time.monotonic()
+  while time.monotonic() - st < duration_sec:
+    for s in socks:
+      events[s] += messaging.drain_sock(socks[s])
+    time.sleep(0.1)
+
+  assert sum(map(len, events.values())) != 0, "No sensor events collected!"
+
+  return {k: v for k, v in events.items() if len(v) > 0}
+
+@pytest.mark.tici
+class TestSensord:
+  @classmethod
+  def setup_class(cls):
+    # enable LSM self test
+    os.environ["LSM_SELF_TEST"] = "1"
+
+    # read initial sensor values every test case can use
+    os.system("pkill -f \\\\./sensord")
+    try:
+      managed_processes["sensord"].start()
+      cls.sample_secs = int(os.getenv("SAMPLE_SECS", "10"))
+      cls.events = read_sensor_events(cls.sample_secs)
+
+      # determine sensord's irq
+      cls.sensord_irq = get_irqs_for_action("sensord")[0]
+    finally:
+      # teardown won't run if this doesn't succeed
+      managed_processes["sensord"].stop()
+
+  @classmethod
+  def teardown_class(cls):
+    managed_processes["sensord"].stop()
+
+  def teardown_method(self):
+    managed_processes["sensord"].stop()
+
+  def test_sensors_present(self):
+    # verify correct sensors configuration
+    seen = set()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+        seen.add((str(m.source), m.which()))
+
+    assert seen in SENSOR_CONFIGURATIONS
+
+  def test_lsm6ds3_timing(self, subtests):
+    # verify measurements are sampled and published at 104Hz
+
+    sensor_t = {
+      1: [], # accel
+      5: [], # gyro
+    }
+
+    for measurement in self.events['accelerometer']:
+      m = getattr(measurement, measurement.which())
+      sensor_t[m.sensor].append(m.timestamp)
+
+    for measurement in self.events['gyroscope']:
+      m = getattr(measurement, measurement.which())
+      sensor_t[m.sensor].append(m.timestamp)
+
+    for s, vals in sensor_t.items():
+      with subtests.test(sensor=s):
+        assert len(vals) > 0
+        tdiffs = np.diff(vals) / 1e6 # millis
+
+        high_delay_diffs = list(filter(lambda d: d >= 20., tdiffs))
+        assert len(high_delay_diffs) < 15, f"Too many large diffs: {high_delay_diffs}"
+
+        avg_diff = sum(tdiffs)/len(tdiffs)
+        avg_freq = 1. / (avg_diff * 1e-3)
+        assert 92. < avg_freq < 114., f"avg freq {avg_freq}Hz wrong, expected 104Hz"
+
+        stddev = np.std(tdiffs)
+        assert stddev < 2.0, f"Standard-dev to big {stddev}"
+
+  def test_sensor_frequency(self, subtests):
+    for s, msgs in self.events.items():
+      with subtests.test(sensor=s):
+        freq = len(msgs) / self.sample_secs
+        ef = SERVICE_LIST[s].frequency
+        assert ef*0.85 <= freq <= ef*1.15
+
+  def test_logmonottime_timestamp_diff(self):
+    # ensure diff between the message logMonotime and sample timestamp is small
+
+    tdiffs = list()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+
+        # check if gyro and accel timestamps are before logMonoTime
+        if str(m.source).startswith("lsm6ds3") and m.which() != 'temperature':
+          err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}"
+          assert m.timestamp < measurement.logMonoTime, err_msg
+
+        # negative values might occur, as non interrupt packages created
+        # before the sensor is read
+        tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6)
+
+    # some sensors have a read procedure that will introduce an expected diff on the order of 20ms
+    high_delay_diffs = set(filter(lambda d: d >= 25., tdiffs))
+    assert len(high_delay_diffs) < 20, f"Too many measurements published: {high_delay_diffs}"
+
+    avg_diff = round(sum(tdiffs)/len(tdiffs), 4)
+    assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms"
+
+  def test_sensor_values(self):
+    sensor_values = dict()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+        key = (m.source.raw, m.which())
+        values = getattr(m, m.which())
+
+        if hasattr(values, 'v'):
+          values = values.v
+        values = np.atleast_1d(values)
+
+        if key in sensor_values:
+          sensor_values[key].append(values)
+        else:
+          sensor_values[key] = [values]
+
+    # Sanity check sensor values
+    for sensor, stype in sensor_values:
+      for s in ALL_SENSORS[sensor]:
+        if s.type != stype:
+          continue
+
+        key = (sensor, s.type)
+        mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1))
+        err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}"
+        assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg
+
+  def test_sensor_verify_no_interrupts_after_stop(self):
+    managed_processes["sensord"].start()
+    time.sleep(3)
+
+    # read /proc/interrupts to verify interrupts are received
+    state_one = get_irq_count(self.sensord_irq)
+    time.sleep(1)
+    state_two = get_irq_count(self.sensord_irq)
+
+    error_msg = f"no interrupts received after sensord start!\n{state_one} {state_two}"
+    assert state_one != state_two, error_msg
+
+    managed_processes["sensord"].stop()
+    time.sleep(1)
+
+    # read /proc/interrupts to verify no more interrupts are received
+    state_one = get_irq_count(self.sensord_irq)
+    time.sleep(1)
+    state_two = get_irq_count(self.sensord_irq)
+    assert state_one == state_two, "Interrupts received after sensord stop!"
--- a/sunnypilot/system/sensord/tests/ttff_test.py
+++ b/sunnypilot/system/sensord/tests/ttff_test.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+import time
+import atexit
+
+from cereal import messaging
+from openpilot.system.manager.process_config import managed_processes
+
+TIMEOUT = 10*60
+
+def kill():
+  for proc in ['ubloxd', 'pigeond']:
+    managed_processes[proc].stop(retry=True, block=True)
+
+if __name__ == "__main__":
+  # start ubloxd
+  managed_processes['ubloxd'].start()
+  atexit.register(kill)
+
+  sm = messaging.SubMaster(['ubloxGnss'])
+
+  times = []
+  for i in range(20):
+    # start pigeond
+    st = time.monotonic()
+    managed_processes['pigeond'].start()
+
+    # wait for a >4 satellite fix
+    while True:
+      sm.update(0)
+      msg = sm['ubloxGnss']
+      if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]:
+        report = msg.measurementReport
+        if report.numMeas > 4:
+          times.append(time.monotonic() - st)
+          print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m")
+          break
+
+      if time.monotonic() - st > TIMEOUT:
+        raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m")
+
+      time.sleep(0.1)
+
+    # stop pigeond
+    managed_processes['pigeond'].stop(retry=True, block=True)
+    time.sleep(20)
+
+  print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m")
--- a/system/manager/process_config.py
+++ b/system/manager/process_config.py
@@ -173,6 +173,9 @@ procs += [
  # mapd
  NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
  PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run),
+
+  # locationd
+  NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
 ]

 if os.path.exists("./github_runner.sh"):