openpilot v0.9.6 release

date: 2024-01-12T10:13:37 master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
2024-01-12 22:39:28 -07:00
commit 08e9fb1edc
1881 changed files with 653708 additions and 0 deletions
--- a/system/sensord/.gitignore
+++ b/system/sensord/.gitignore
@@ -0,0 +1 @@
+sensord
--- a/system/sensord/SConscript
+++ 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)
--- a/system/sensord/pigeond.py
+++ b/system/sensord/pigeond.py
@@ -0,0 +1,313 @@
+#!/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()
--- a/system/sensord/sensors/bmx055_accel.cc
+++ 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;
+}
--- a/system/sensord/sensors/bmx055_accel.h
+++ 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();
+};
--- a/system/sensord/sensors/bmx055_gyro.cc
+++ 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;
+}
--- a/system/sensord/sensors/bmx055_gyro.h
+++ 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();
+};
--- a/system/sensord/sensors/bmx055_magn.cc
+++ 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;
+}
--- a/system/sensord/sensors/bmx055_magn.h
+++ 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();
+};
--- a/system/sensord/sensors/bmx055_temp.cc
+++ 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;
+}
--- a/system/sensord/sensors/bmx055_temp.h
+++ 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; }
+};
--- a/system/sensord/sensors/constants.h
+++ 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
--- a/system/sensord/sensors/i2c_sensor.cc
+++ 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;
+}
--- a/system/sensord/sensors/i2c_sensor.h
+++ 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;
+  }
+};
--- a/system/sensord/sensors/lsm6ds3_accel.cc
+++ 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;
+}
--- a/system/sensord/sensors/lsm6ds3_accel.h
+++ 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();
+};
--- a/system/sensord/sensors/lsm6ds3_gyro.cc
+++ 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;
+}
--- a/system/sensord/sensors/lsm6ds3_gyro.h
+++ 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();
+};
--- a/system/sensord/sensors/lsm6ds3_temp.cc
+++ 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;
+}
--- a/system/sensord/sensors/lsm6ds3_temp.h
+++ 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; }
+};
--- a/system/sensord/sensors/mmc5603nj_magn.cc
+++ 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;
+}
--- a/system/sensord/sensors/mmc5603nj_magn.h
+++ 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();
+};
--- a/system/sensord/sensors/sensor.h
+++ 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;
+  }
+};
--- a/system/sensord/sensors_qcom2.cc
+++ b/system/sensord/sensors_qcom2.cc
@@ -0,0 +1,162 @@
+#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});
+  std::system("sudo su -c 'echo 1 > /proc/irq/336/smp_affinity_list'");
+
+  // 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;
+  }
+}