diff --git a/system/sensord/SConscript b/system/sensord/SConscript
new file mode 100644
index 0000000..7974eb0
--- /dev/null
+++ b/system/sensord/SConscript
@@ -0,0 +1,17 @@
+Import('env', 'arch', 'common', 'cereal', 'messaging')
+
+sensors = [
+  'sensors/i2c_sensor.cc',
+  'sensors/bmx055_accel.cc',
+  'sensors/bmx055_gyro.cc',
+  'sensors/bmx055_magn.cc',
+  'sensors/bmx055_temp.cc',
+  'sensors/lsm6ds3_accel.cc',
+  'sensors/lsm6ds3_gyro.cc',
+  'sensors/lsm6ds3_temp.cc',
+  'sensors/mmc5603nj_magn.cc',
+]
+libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj', 'pthread']
+if arch == "larch64":
+  libs.append('i2c')
+env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)
diff --git a/system/sensord/__init__.py b/system/sensord/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/system/sensord/pigeond.py b/system/sensord/pigeond.py
deleted file mode 100755
index 78b3b07..0000000
--- a/system/sensord/pigeond.py
+++ /dev/null
@@ -1,313 +0,0 @@
-#!/usr/bin/env python3
-import sys
-import time
-import signal
-import serial
-import struct
-import requests
-import urllib.parse
-from datetime import datetime
-from typing import List, Optional, Tuple
-
-from cereal import messaging
-from openpilot.common.params import Params
-from openpilot.common.swaglog import cloudlog
-from openpilot.system.hardware import TICI
-from openpilot.common.gpio import gpio_init, gpio_set
-from openpilot.system.hardware.tici.pins import GPIO
-
-UBLOX_TTY = "/dev/ttyHS0"
-
-UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00"
-UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00"
-UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"
-UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"
-UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03"
-UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00"
-
-def set_power(enabled: bool) -> None:
-  gpio_init(GPIO.UBLOX_SAFEBOOT_N, True)
-  gpio_init(GPIO.GNSS_PWR_EN, True)
-  gpio_init(GPIO.UBLOX_RST_N, True)
-
-  gpio_set(GPIO.UBLOX_SAFEBOOT_N, True)
-  gpio_set(GPIO.GNSS_PWR_EN, enabled)
-  gpio_set(GPIO.UBLOX_RST_N, enabled)
-
-def add_ubx_checksum(msg: bytes) -> bytes:
-  A = B = 0
-  for b in msg[2:]:
-    A = (A + b) % 256
-    B = (B + A) % 256
-  return msg + bytes([A, B])
-
-def get_assistnow_messages(token: bytes) -> List[bytes]:
-  # make request
-  # TODO: implement adding the last known location
-  r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({
-    'token': token,
-    'gnss': 'gps,glo',
-    'datatype': 'eph,alm,aux',
-  }, safe=':,'), timeout=5)
-  assert r.status_code == 200, "Got invalid status code"
-  dat = r.content
-
-  # split up messages
-  msgs = []
-  while len(dat) > 0:
-    assert dat[:2] == b"\xB5\x62"
-    msg_len = 6 + (dat[5] << 8 | dat[4]) + 2
-    msgs.append(dat[:msg_len])
-    dat = dat[msg_len:]
-  return msgs
-
-
-class TTYPigeon():
-  def __init__(self):
-    self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0)
-
-  def send(self, dat: bytes) -> None:
-    self.tty.write(dat)
-
-  def receive(self) -> bytes:
-    dat = b''
-    while len(dat) < 0x1000:
-      d = self.tty.read(0x40)
-      dat += d
-      if len(d) == 0:
-        break
-    return dat
-
-  def set_baud(self, baud: int) -> None:
-    self.tty.baudrate = baud
-
-  def wait_for_ack(self, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK, timeout: float = 0.5) -> bool:
-    dat = b''
-    st = time.monotonic()
-    while True:
-      dat += self.receive()
-      if ack in dat:
-        cloudlog.debug("Received ACK from ublox")
-        return True
-      elif nack in dat:
-        cloudlog.error("Received NACK from ublox")
-        return False
-      elif time.monotonic() - st > timeout:
-        cloudlog.error("No response from ublox")
-        raise TimeoutError('No response from ublox')
-      time.sleep(0.001)
-
-  def send_with_ack(self, dat: bytes, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK) -> None:
-    self.send(dat)
-    self.wait_for_ack(ack, nack)
-
-  def wait_for_backup_restore_status(self, timeout: float = 1.) -> int:
-    dat = b''
-    st = time.monotonic()
-    while True:
-      dat += self.receive()
-      position = dat.find(UBLOX_BACKUP_RESTORE_MSG)
-      if position >= 0 and len(dat) >= position + 11:
-        return dat[position + 10]
-      elif time.monotonic() - st > timeout:
-        cloudlog.error("No backup restore response from ublox")
-        raise TimeoutError('No response from ublox')
-      time.sleep(0.001)
-
-  def reset_device(self) -> bool:
-    # deleting the backup does not always work on first try (mostly on second try)
-    for _ in range(5):
-      # device cold start
-      self.send(b"\xb5\x62\x06\x04\x04\x00\xff\xff\x00\x00\x0c\x5d")
-      time.sleep(1) # wait for cold start
-      init_baudrate(self)
-
-      # clear configuration
-      self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\xd7")
-
-      # clear flash memory (almanac backup)
-      self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0")
-
-      # try restoring backup to verify it got deleted
-      self.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
-      # 1: failed to restore, 2: could restore, 3: no backup
-      status = self.wait_for_backup_restore_status()
-      if status == 1 or status == 3:
-        return True
-    return False
-
-def init_baudrate(pigeon: TTYPigeon):
-  # ublox default setting on startup is 9600 baudrate
-  pigeon.set_baud(9600)
-
-  # $PUBX,41,1,0007,0003,460800,0*15\r\n
-  pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A")
-  time.sleep(0.1)
-  pigeon.set_baud(460800)
-
-
-def initialize_pigeon(pigeon: TTYPigeon) -> bool:
-  # try initializing a few times
-  for _ in range(10):
-    try:
-
-      # setup port config
-      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F")
-      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35")
-      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80")
-      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85")
-      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x00\x00\x06\x18")
-      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x01\x08\x22")
-      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x03\x0A\x24")
-
-      # UBX-CFG-RATE (0x06 0x08)
-      pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10")
-
-      # UBX-CFG-NAV5 (0x06 0x24)
-      pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63")
-
-      # UBX-CFG-ODO (0x06 0x1E)
-      pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24")
-
-      # UBX-CFG-NAV5 (0x06 0x24)
-      pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3")
-
-      # UBX-CFG-MSG (set message rate)
-      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x35\x01\x41\xAD")
-      cloudlog.debug("pigeon configured")
-
-      # try restoring almanac backup
-      pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
-      restore_status = pigeon.wait_for_backup_restore_status()
-      if restore_status == 2:
-        cloudlog.warning("almanac backup restored")
-      elif restore_status == 3:
-        cloudlog.warning("no almanac backup found")
-      else:
-        cloudlog.error(f"failed to restore almanac backup, status: {restore_status}")
-
-      # sending time to ublox
-      t_now = datetime.utcnow()
-      if t_now >= datetime(2021, 6, 1):
-        cloudlog.warning("Sending current time to ublox")
-
-        # UBX-MGA-INI-TIME_UTC
-        msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack("<BBBBHBBBBBxIHxxI",
-          0x10,
-          0x00,
-          0x00,
-          0x80,
-          t_now.year,
-          t_now.month,
-          t_now.day,
-          t_now.hour,
-          t_now.minute,
-          t_now.second,
-          0,
-          30,
-          0
-        ))
-        pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
-
-      # try getting AssistNow if we have a token
-      token = Params().get('AssistNowToken')
-      if token is not None:
-        try:
-          for msg in get_assistnow_messages(token):
-            pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
-          cloudlog.warning("AssistNow messages sent")
-        except Exception:
-          cloudlog.warning("failed to get AssistNow messages")
-
-      cloudlog.warning("Pigeon GPS on!")
-      break
-    except TimeoutError:
-      cloudlog.warning("Initialization failed, trying again!")
-  else:
-    cloudlog.warning("Failed to initialize pigeon")
-    return False
-  return True
-
-def deinitialize_and_exit(pigeon: Optional[TTYPigeon]):
-  cloudlog.warning("Storing almanac in ublox flash")
-
-  if pigeon is not None:
-    # controlled GNSS stop
-    pigeon.send(b"\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74")
-
-    # store almanac in flash
-    pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
-    try:
-      if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
-        cloudlog.warning("Done storing almanac")
-      else:
-        cloudlog.error("Error storing almanac")
-    except TimeoutError:
-      pass
-
-  # turn off power and exit cleanly
-  set_power(False)
-  sys.exit(0)
-
-def create_pigeon() -> Tuple[TTYPigeon, messaging.PubMaster]:
-  pigeon = None
-
-  # register exit handler
-  signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon))
-  pm = messaging.PubMaster(['ubloxRaw'])
-
-  # power cycle ublox
-  set_power(False)
-  time.sleep(0.1)
-  set_power(True)
-  time.sleep(0.5)
-
-  pigeon = TTYPigeon()
-  return pigeon, pm
-
-def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0):
-
-  start_time = time.monotonic()
-  def end_condition():
-    return True if duration == 0 else time.monotonic() - start_time < duration
-
-  while end_condition():
-    dat = pigeon.receive()
-    if len(dat) > 0:
-      if dat[0] == 0x00:
-        cloudlog.warning("received invalid data from ublox, re-initing!")
-        init_baudrate(pigeon)
-        initialize_pigeon(pigeon)
-        continue
-
-      # send out to socket
-      msg = messaging.new_message('ubloxRaw', len(dat), valid=True)
-      msg.ubloxRaw = dat[:]
-      pm.send('ubloxRaw', msg)
-    else:
-      # prevent locking up a CPU core if ublox disconnects
-      time.sleep(0.001)
-
-
-def main():
-  assert TICI, "unsupported hardware for pigeond"
-
-  pigeon, pm = create_pigeon()
-  init_baudrate(pigeon)
-  initialize_pigeon(pigeon)
-
-  # start receiving data
-  run_receiving(pigeon, pm)
-
-if __name__ == "__main__":
-  main()
diff --git a/system/sensord/sensord b/system/sensord/sensord
deleted file mode 100755
index c828d91..0000000
Binary files a/system/sensord/sensord and /dev/null differ
diff --git a/system/sensord/sensors/bmx055_accel.cc b/system/sensord/sensors/bmx055_accel.cc
new file mode 100644
index 0000000..bdb0113
--- /dev/null
+++ b/system/sensord/sensors/bmx055_accel.cc
@@ -0,0 +1,78 @@
+#include "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) return -1;
+
+  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;
+  }
+
+fail:
+  return ret;
+}
+
+int BMX055_Accel::shutdown()  {
+  // 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;
+}
diff --git a/system/sensord/sensors/bmx055_accel.h b/system/sensord/sensors/bmx055_accel.h
new file mode 100644
index 0000000..2cc316e
--- /dev/null
+++ b/system/sensord/sensors/bmx055_accel.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include "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();
+};
diff --git a/system/sensord/sensors/bmx055_gyro.cc b/system/sensord/sensors/bmx055_gyro.cc
new file mode 100644
index 0000000..411b2f4
--- /dev/null
+++ b/system/sensord/sensors/bmx055_gyro.cc
@@ -0,0 +1,88 @@
+#include "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;
+  }
+
+fail:
+  return ret;
+}
+
+int BMX055_Gyro::shutdown()  {
+  // 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;
+}
diff --git a/system/sensord/sensors/bmx055_gyro.h b/system/sensord/sensors/bmx055_gyro.h
new file mode 100644
index 0000000..7be3e56
--- /dev/null
+++ b/system/sensord/sensors/bmx055_gyro.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include "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();
+};
diff --git a/system/sensord/sensors/bmx055_magn.cc b/system/sensord/sensors/bmx055_magn.cc
new file mode 100644
index 0000000..3d0d3d2
--- /dev/null
+++ b/system/sensord/sensors/bmx055_magn.cc
@@ -0,0 +1,256 @@
+#include "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) {
+    LOGE("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;
+  }
+
+
+  return 0;
+
+ fail:
+  return ret;
+}
+
+int BMX055_Magn::shutdown() {
+  // 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;
+}
diff --git a/system/sensord/sensors/bmx055_magn.h b/system/sensord/sensors/bmx055_magn.h
new file mode 100644
index 0000000..15c4e73
--- /dev/null
+++ b/system/sensord/sensors/bmx055_magn.h
@@ -0,0 +1,64 @@
+#pragma once
+#include <tuple>
+
+#include "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();
+};
diff --git a/system/sensord/sensors/bmx055_temp.cc b/system/sensord/sensors/bmx055_temp.cc
new file mode 100644
index 0000000..da7b864
--- /dev/null
+++ b/system/sensord/sensors/bmx055_temp.cc
@@ -0,0 +1,31 @@
+#include "system/sensord/sensors/bmx055_temp.h"
+
+#include <cassert>
+
+#include "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;
+}
diff --git a/system/sensord/sensors/bmx055_temp.h b/system/sensord/sensors/bmx055_temp.h
new file mode 100644
index 0000000..a2eabae
--- /dev/null
+++ b/system/sensord/sensors/bmx055_temp.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "system/sensord/sensors/bmx055_accel.h"
+#include "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; }
+};
diff --git a/system/sensord/sensors/constants.h b/system/sensord/sensors/constants.h
new file mode 100644
index 0000000..c216f83
--- /dev/null
+++ b/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
diff --git a/system/sensord/sensors/i2c_sensor.cc b/system/sensord/sensors/i2c_sensor.cc
new file mode 100644
index 0000000..90220f5
--- /dev/null
+++ b/system/sensord/sensors/i2c_sensor.cc
@@ -0,0 +1,50 @@
+#include "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;
+}
diff --git a/system/sensord/sensors/i2c_sensor.h b/system/sensord/sensors/i2c_sensor.h
new file mode 100644
index 0000000..ba100c3
--- /dev/null
+++ b/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 "system/sensord/sensors/constants.h"
+#include "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) {
+      LOGE("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;
+  }
+};
diff --git a/system/sensord/sensors/lsm6ds3_accel.cc b/system/sensord/sensors/lsm6ds3_accel.cc
new file mode 100644
index 0000000..03533e0
--- /dev/null
+++ b/system/sensord/sensors/lsm6ds3_accel.cc
@@ -0,0 +1,250 @@
+#include "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;
+}
diff --git a/system/sensord/sensors/lsm6ds3_accel.h b/system/sensord/sensors/lsm6ds3_accel.h
new file mode 100644
index 0000000..69667cb
--- /dev/null
+++ b/system/sensord/sensors/lsm6ds3_accel.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "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();
+};
diff --git a/system/sensord/sensors/lsm6ds3_gyro.cc b/system/sensord/sensors/lsm6ds3_gyro.cc
new file mode 100644
index 0000000..0459b6a
--- /dev/null
+++ b/system/sensord/sensors/lsm6ds3_gyro.cc
@@ -0,0 +1,233 @@
+#include "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 =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;
+}
diff --git a/system/sensord/sensors/lsm6ds3_gyro.h b/system/sensord/sensors/lsm6ds3_gyro.h
new file mode 100644
index 0000000..adaae62
--- /dev/null
+++ b/system/sensord/sensors/lsm6ds3_gyro.h
@@ -0,0 +1,45 @@
+#pragma once
+
+#include "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();
+};
diff --git a/system/sensord/sensors/lsm6ds3_temp.cc b/system/sensord/sensors/lsm6ds3_temp.cc
new file mode 100644
index 0000000..f481614
--- /dev/null
+++ b/system/sensord/sensors/lsm6ds3_temp.cc
@@ -0,0 +1,37 @@
+#include "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;
+}
diff --git a/system/sensord/sensors/lsm6ds3_temp.h b/system/sensord/sensors/lsm6ds3_temp.h
new file mode 100644
index 0000000..1b5b621
--- /dev/null
+++ b/system/sensord/sensors/lsm6ds3_temp.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "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; }
+};
diff --git a/system/sensord/sensors/mmc5603nj_magn.cc b/system/sensord/sensors/mmc5603nj_magn.cc
new file mode 100644
index 0000000..0e8ba96
--- /dev/null
+++ b/system/sensord/sensors/mmc5603nj_magn.cc
@@ -0,0 +1,108 @@
+#include "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;
+}
diff --git a/system/sensord/sensors/mmc5603nj_magn.h b/system/sensord/sensors/mmc5603nj_magn.h
new file mode 100644
index 0000000..9c0fbd2
--- /dev/null
+++ b/system/sensord/sensors/mmc5603nj_magn.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <vector>
+
+#include "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();
+};
diff --git a/system/sensord/sensors/sensor.h b/system/sensord/sensors/sensor.h
new file mode 100644
index 0000000..1b0e3be
--- /dev/null
+++ b/system/sensord/sensors/sensor.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "cereal/messaging/messaging.h"
+
+class Sensor {
+public:
+  int gpio_fd = -1;
+  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;
+  }
+};
diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc
new file mode 100644
index 0000000..9cbc248
--- /dev/null
+++ b/system/sensord/sensors_qcom2.cc
@@ -0,0 +1,168 @@
+#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 "system/sensord/sensors/bmx055_accel.h"
+#include "system/sensord/sensors/bmx055_gyro.h"
+#include "system/sensord/sensors/bmx055_magn.h"
+#include "system/sensord/sensors/bmx055_temp.h"
+#include "system/sensord/sensors/constants.h"
+#include "system/sensord/sensors/lsm6ds3_accel.h"
+#include "system/sensord/sensors/lsm6ds3_gyro.h"
+#include "system/sensord/sensors/lsm6ds3_temp.h"
+#include "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;
+    }
+  }
+
+  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 = read(fd, evdata, sizeof(evdata));
+    if (err < 0 || err % sizeof(*evdata) != 0) {
+      LOGE("error reading event data %d", err);
+      continue;
+    }
+
+    int num_events = err / sizeof(*evdata);
+    uint64_t offset = nanos_since_epoch() - nanos_since_boot();
+    uint64_t ts = evdata[num_events - 1].timestamp - 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;
+  }
+}
diff --git a/system/sensord/tests/__init__.py b/system/sensord/tests/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/system/sensord/tests/test_sensord.py b/system/sensord/tests/test_sensord.py
new file mode 100644
index 0000000..3075c8a
--- /dev/null
+++ b/system/sensord/tests/test_sensord.py
@@ -0,0 +1,251 @@
+#!/usr/bin/env python3
+import os
+import pytest
+import time
+import unittest
+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.selfdrive.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 = (
+  (BMX | LSM),
+  (MMC | LSM),
+  (BMX | LSM_C),
+  (MMC| 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(unittest.TestCase):
+  @classmethod
+  def setUpClass(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 tearDownClass(cls):
+    managed_processes["sensord"].stop()
+
+  def tearDown(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()))
+
+    self.assertIn(seen, SENSOR_CONFIGURATIONS)
+
+  def test_lsm6ds3_timing(self):
+    # 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 self.subTest(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):
+    for s, msgs in self.events.items():
+      with self.subTest(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!"
+
+
+if __name__ == "__main__":
+  unittest.main()
diff --git a/system/sensord/tests/ttff_test.py b/system/sensord/tests/ttff_test.py
new file mode 100644
index 0000000..e023489
--- /dev/null
+++ b/system/sensord/tests/ttff_test.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+import time
+import atexit
+
+from cereal import messaging
+from openpilot.selfdrive.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")