sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

openpilot/sunnypilot/system/params_migration.py

@@ -0,0 +1,82 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import json
+
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.selfdrive.car.sync_sunnylink_params import CAR_LIST_JSON_OUT
+
+ONROAD_BRIGHTNESS_MIGRATION_VERSION: str = "1.0"
+ONROAD_BRIGHTNESS_TIMER_MIGRATION_VERSION: str = "1.0"
+
+# index → seconds mapping for OnroadScreenOffTimer (SSoT)
+ONROAD_BRIGHTNESS_TIMER_VALUES = {0: 3, 1: 5, 2: 7, 3: 10, 4: 15, 5: 30, **{i: (i - 5) * 60 for i in range(6, 16)}}
+VALID_TIMER_VALUES = set(ONROAD_BRIGHTNESS_TIMER_VALUES.values())
+
+
+def _migrate_car_platform_bundle(_params):
+  bundle = _params.get("CarPlatformBundle")
+  if bundle is None:
+    return
+
+  old_platform = bundle.get("platform")
+  if not old_platform:
+    return
+
+  from opendbc.car.fingerprints import MIGRATION  # lazy: avoids heavy import at module level
+  if old_platform not in MIGRATION:
+    return
+
+  new_platform = str(MIGRATION[old_platform])
+
+  with open(CAR_LIST_JSON_OUT) as f:
+    car_list = json.load(f)
+
+  candidates = [(k, v) for k, v in car_list.items() if v.get("platform") == new_platform]
+  if candidates:
+    old_model = bundle.get("model")
+    key, data = next(((k, v) for k, v in candidates if v.get("model") == old_model), candidates[0])
+    bundle = {**data, "name": key}
+  else:
+    bundle["platform"] = new_platform
+
+  _params.put("CarPlatformBundle", bundle, block=True)
+  cloudlog.info(f"params_migration: CarPlatformBundle migrated {old_platform!r} -> {new_platform!r}")
+
+
+def run_migration(_params):
+  # migrate OnroadScreenOffBrightness
+  if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
+    try:
+      val = _params.get("OnroadScreenOffBrightness", return_default=True)
+      if val >= 2:  # old: 5%, new: Screen Off
+        new_val = val + 1
+        _params.put("OnroadScreenOffBrightness", new_val, block=True)
+        log_str = f"Successfully migrated OnroadScreenOffBrightness from {val} to {new_val}."
+      else:
+        log_str = "Migration not required for OnroadScreenOffBrightness."
+
+      _params.put("OnroadScreenOffBrightnessMigrated", ONROAD_BRIGHTNESS_MIGRATION_VERSION, block=True)
+      cloudlog.info(log_str + f" Setting OnroadScreenOffBrightnessMigrated to {ONROAD_BRIGHTNESS_MIGRATION_VERSION}")
+    except Exception as e:
+      cloudlog.exception(f"Error migrating OnroadScreenOffBrightness: {e}")
+
+  # migrate OnroadScreenOffTimer
+  if _params.get("OnroadScreenOffTimerMigrated") != ONROAD_BRIGHTNESS_TIMER_MIGRATION_VERSION:
+    try:
+      val = _params.get("OnroadScreenOffTimer", return_default=True)
+      if val not in VALID_TIMER_VALUES:
+        _params.put("OnroadScreenOffTimer", 15, block=True)
+        log_str = f"Successfully migrated OnroadScreenOffTimer from {val} to 15 (default)."
+      else:
+        log_str = "Migration not required for OnroadScreenOffTimer."
+
+      _params.put("OnroadScreenOffTimerMigrated", ONROAD_BRIGHTNESS_TIMER_MIGRATION_VERSION, block=True)
+      cloudlog.info(log_str + f" Setting OnroadScreenOffTimerMigrated to {ONROAD_BRIGHTNESS_TIMER_MIGRATION_VERSION}")
+    except Exception as e:
+      cloudlog.exception(f"Error migrating OnroadScreenOffTimer: {e}")
+
+  _migrate_car_platform_bundle(_params)

openpilot/sunnypilot/system/sensord/.gitignore

@@ -0,0 +1 @@
+sensord

openpilot/sunnypilot/system/sensord/sensors/bmx055_accel.cc

@@ -0,0 +1,85 @@
+#include "sunnypilot/system/sensord/sensors/bmx055_accel.h"
+
+#include <cassert>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Accel::init() {
+  int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
+  if (ret == -1) {
+    goto fail;
+  }
+
+  ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // bmx055 accel has a 1.3ms wakeup time from deep suspend mode
+  util::sleep_for(10);
+
+  // High bandwidth
+  // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE);
+  // if (ret < 0) {
+  //   goto fail;
+  // }
+
+  // Low bandwidth
+  ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  enabled = true;
+
+fail:
+  return ret;
+}
+
+int BMX055_Accel::shutdown()  {
+  if (!enabled) return 0;
+
+  // enter deep suspend mode (lowest power mode)
+  int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 ACCEL in deep suspend mode!");
+  }
+
+  return ret;
+}
+
+bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[6];
+  int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer));
+  assert(len == 6);
+
+  // 12 bit = +-2g
+  float scale = 9.81 * 2.0f / (1 << 11);
+  float x = -read_12_bit(buffer[0], buffer[1]) * scale;
+  float y = -read_12_bit(buffer[2], buffer[3]) * scale;
+  float z = read_12_bit(buffer[4], buffer[5]) * scale;
+
+  auto event = msg.initEvent().initAccelerometer2();
+  event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+  event.setVersion(1);
+  event.setSensor(SENSOR_ACCELEROMETER);
+  event.setType(SENSOR_TYPE_ACCELEROMETER);
+  event.setTimestamp(start_time);
+
+  float xyz[] = {x, y, z};
+  auto svec = event.initAcceleration();
+  svec.setV(xyz);
+  svec.setStatus(true);
+
+  return true;
+}

openpilot/sunnypilot/system/sensord/sensors/bmx055_accel.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define BMX055_ACCEL_I2C_ADDR       0x18
+
+// Registers of the chip
+#define BMX055_ACCEL_I2C_REG_ID     0x00
+#define BMX055_ACCEL_I2C_REG_X_LSB  0x02
+#define BMX055_ACCEL_I2C_REG_TEMP   0x08
+#define BMX055_ACCEL_I2C_REG_BW     0x10
+#define BMX055_ACCEL_I2C_REG_PMU    0x11
+#define BMX055_ACCEL_I2C_REG_HBW    0x13
+#define BMX055_ACCEL_I2C_REG_FIFO   0x3F
+
+// Constants
+#define BMX055_ACCEL_CHIP_ID        0xFA
+
+#define BMX055_ACCEL_HBW_ENABLE       0b10000000
+#define BMX055_ACCEL_HBW_DISABLE      0b00000000
+#define BMX055_ACCEL_DEEP_SUSPEND     0b00100000
+#define BMX055_ACCEL_NORMAL_MODE      0b00000000
+
+#define BMX055_ACCEL_BW_7_81HZ  0b01000
+#define BMX055_ACCEL_BW_15_63HZ 0b01001
+#define BMX055_ACCEL_BW_31_25HZ 0b01010
+#define BMX055_ACCEL_BW_62_5HZ  0b01011
+#define BMX055_ACCEL_BW_125HZ   0b01100
+#define BMX055_ACCEL_BW_250HZ   0b01101
+#define BMX055_ACCEL_BW_500HZ   0b01110
+#define BMX055_ACCEL_BW_1000HZ  0b01111
+
+class BMX055_Accel : public I2CSensor {
+  uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
+public:
+  BMX055_Accel(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};

openpilot/sunnypilot/system/sensord/sensors/bmx055_gyro.cc

@@ -0,0 +1,92 @@
+#include "sunnypilot/system/sensord/sensors/bmx055_gyro.h"
+
+#include <cassert>
+#include <cmath>
+
+#include "common/swaglog.h"
+#include "common/util.h"
+
+#define DEG2RAD(x) ((x) * M_PI / 180.0)
+
+
+BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Gyro::init() {
+  int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID});
+  if (ret == -1) return -1;
+
+  ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+  // bmx055 gyro has a 30ms wakeup time from deep suspend mode
+  util::sleep_for(50);
+
+  // High bandwidth
+  // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE);
+  // if (ret < 0) {
+  //   goto fail;
+  // }
+
+  // Low bandwidth
+  ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // 116 Hz filter
+  ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // +- 125 deg/s range
+  ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  enabled = true;
+
+fail:
+  return ret;
+}
+
+int BMX055_Gyro::shutdown()  {
+  if (!enabled) return 0;
+
+  // enter deep suspend mode (lowest power mode)
+  int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 GYRO in deep suspend mode!");
+  }
+
+  return ret;
+}
+
+bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[6];
+  int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer));
+  assert(len == 6);
+
+  // 16 bit = +- 125 deg/s
+  float scale = 125.0f / (1 << 15);
+  float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
+  float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
+  float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
+
+  auto event = msg.initEvent().initGyroscope2();
+  event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+  event.setVersion(1);
+  event.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
+  event.setTimestamp(start_time);
+
+  float xyz[] = {x, y, z};
+  auto svec = event.initGyroUncalibrated();
+  svec.setV(xyz);
+  svec.setStatus(true);
+
+  return true;
+}

openpilot/sunnypilot/system/sensord/sensors/bmx055_gyro.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define BMX055_GYRO_I2C_ADDR        0x68
+
+// Registers of the chip
+#define BMX055_GYRO_I2C_REG_ID         0x00
+#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02
+#define BMX055_GYRO_I2C_REG_RANGE      0x0F
+#define BMX055_GYRO_I2C_REG_BW         0x10
+#define BMX055_GYRO_I2C_REG_LPM1       0x11
+#define BMX055_GYRO_I2C_REG_HBW        0x13
+#define BMX055_GYRO_I2C_REG_FIFO       0x3F
+
+// Constants
+#define BMX055_GYRO_CHIP_ID         0x0F
+
+#define BMX055_GYRO_HBW_ENABLE       0b10000000
+#define BMX055_GYRO_HBW_DISABLE      0b00000000
+#define BMX055_GYRO_DEEP_SUSPEND     0b00100000
+#define BMX055_GYRO_NORMAL_MODE      0b00000000
+
+#define BMX055_GYRO_RANGE_2000      0b000
+#define BMX055_GYRO_RANGE_1000      0b001
+#define BMX055_GYRO_RANGE_500       0b010
+#define BMX055_GYRO_RANGE_250       0b011
+#define BMX055_GYRO_RANGE_125       0b100
+
+#define BMX055_GYRO_BW_116HZ 0b0010
+
+
+class BMX055_Gyro : public I2CSensor {
+  uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;}
+public:
+  BMX055_Gyro(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};

openpilot/sunnypilot/system/sensord/sensors/bmx055_magn.cc

@@ -0,0 +1,258 @@
+#include "sunnypilot/system/sensord/sensors/bmx055_magn.h"
+
+#include <unistd.h>
+
+#include <algorithm>
+#include <cassert>
+#include <cstdio>
+
+#include "common/swaglog.h"
+#include "common/util.h"
+
+static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) {
+  uint16_t process_comp_x0 = data_rhall;
+  int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384;
+  uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000);
+  int16_t retval = ((int16_t)process_comp_x2);
+  int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval));
+  int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128));
+  int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128);
+  int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5;
+  int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000));
+  int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0)));
+  int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096);
+  int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9;
+  retval = ((int16_t)(process_comp_x10 / 8192));
+  retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16;
+
+  return retval;
+}
+
+static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) {
+  uint16_t process_comp_y0 = trim_data.dig_xyz1;
+  int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0;
+  uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000);
+  int16_t retval = ((int16_t)process_comp_y2);
+  int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval);
+  int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128);
+  int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128));
+  int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512);
+  int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0)));
+  int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096);
+  int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8);
+  retval = (int16_t)(process_comp_y9 / 8192);
+  retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16;
+
+  return retval;
+}
+
+static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) {
+  int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1);
+  int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4;
+  int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768);
+  int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2);
+  int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536);
+  int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4));
+
+  /* saturate result to +/- 2 micro-tesla */
+  retval = std::clamp(retval, -32767, 32767);
+
+  /* Conversion of LSB to micro-tesla*/
+  retval = retval / 16;
+
+  return (int16_t)retval;
+}
+
+BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Magn::init() {
+  uint8_t trim_x1y1[2] = {0};
+  uint8_t trim_x2y2[2] = {0};
+  uint8_t trim_xy1xy2[2] = {0};
+  uint8_t trim_z1[2] = {0};
+  uint8_t trim_z2[2] = {0};
+  uint8_t trim_z3[2] = {0};
+  uint8_t trim_z4[2] = {0};
+  uint8_t trim_xyz1[2] = {0};
+
+  // suspend -> sleep
+  int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
+  if (ret < 0) {
+    LOGD("Enabling power failed: %d", ret);
+    goto fail;
+  }
+  util::sleep_for(5); // wait until the chip is powered on
+
+  ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID});
+  if (ret == -1) {
+    goto fail;
+  }
+
+  // Load magnetometer trim
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2);
+  if (ret < 0) goto fail;
+  ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2);
+  if (ret < 0) goto fail;
+
+  // Read trim data
+  trim_data.dig_x1 = trim_x1y1[0];
+  trim_data.dig_y1 = trim_x1y1[1];
+
+  trim_data.dig_x2 = trim_x2y2[0];
+  trim_data.dig_y2 = trim_x2y2[1];
+
+  trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped
+  trim_data.dig_xy2 = trim_xy1xy2[0];
+
+  trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]);
+  trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]);
+  trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]);
+  trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]);
+
+  trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f);
+  assert(trim_data.dig_xyz1 != 0);
+
+  perform_self_test();
+
+  // f_max = 1 / (145us * nXY + 500us * NZ + 980us)
+  // Chose NXY = 7, NZ = 12, which gives 125 Hz,
+  // and has the same ratio as the high accuracy preset
+  ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  enabled = true;
+  return 0;
+
+ fail:
+  return ret;
+}
+
+int BMX055_Magn::shutdown() {
+  if (!enabled) return 0;
+
+  // move to suspend mode
+  int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 MAGN in suspend mode!");
+  }
+
+  return ret;
+}
+
+bool BMX055_Magn::perform_self_test() {
+  uint8_t buffer[8];
+  int16_t x, y;
+  int16_t neg_z, pos_z;
+
+  // Increase z reps for less false positives (~30 Hz ODR)
+  set_register(BMX055_MAGN_I2C_REG_REPXY, 1);
+  set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1);
+
+  // Clean existing measurement
+  read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+
+  uint8_t forced = BMX055_MAGN_FORCED;
+
+  // Negative current
+  set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
+  util::sleep_for(100);
+
+  read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  parse_xyz(buffer, &x, &y, &neg_z);
+
+  // Positive current
+  set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
+  util::sleep_for(100);
+
+  read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  parse_xyz(buffer, &x, &y, &pos_z);
+
+  // Put back in normal mode
+  set_register(BMX055_MAGN_I2C_REG_MAG, 0);
+
+  int16_t diff = pos_z - neg_z;
+  bool passed = (diff > 180) && (diff < 240);
+
+  if (!passed) {
+    LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff);
+  }
+
+  return passed;
+}
+
+bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) {
+  bool ready = buffer[6] & 0x1;
+  if (ready) {
+    int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3;
+    int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3;
+    int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1;
+    uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2;
+    assert(data_r != 0);
+
+    *x = compensate_x(trim_data, mdata_x, data_r);
+    *y = compensate_y(trim_data, mdata_y, data_r);
+    *z = compensate_z(trim_data, mdata_z, data_r);
+  }
+  return ready;
+}
+
+
+bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[8];
+  int16_t _x, _y, x, y, z;
+
+  int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  bool parsed = parse_xyz(buffer, &_x, &_y, &z);
+  if (parsed) {
+
+    auto event = msg.initEvent().initMagnetometer();
+    event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+    event.setVersion(2);
+    event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+    event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
+    event.setTimestamp(start_time);
+
+    // Move magnetometer into same reference frame as accel/gryo
+    x = -_y;
+    y = _x;
+
+    // Axis convention
+    x = -x;
+    y = -y;
+
+    float xyz[] = {(float)x, (float)y, (float)z};
+    auto svec = event.initMagneticUncalibrated();
+    svec.setV(xyz);
+    svec.setStatus(true);
+  }
+
+  // The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
+  // up to 30 Hz. Therefore we put in forced mode, and request measurements
+  // at a 100 Hz. When reading the registers we have to check the ready bit
+  // To verify the measurement was completed this cycle.
+  set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
+
+  return parsed;
+}

openpilot/sunnypilot/system/sensord/sensors/bmx055_magn.h

@@ -0,0 +1,64 @@
+#pragma once
+#include <tuple>
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define BMX055_MAGN_I2C_ADDR        0x10
+
+// Registers of the chip
+#define BMX055_MAGN_I2C_REG_ID        0x40
+#define BMX055_MAGN_I2C_REG_PWR_0     0x4B
+#define BMX055_MAGN_I2C_REG_MAG       0x4C
+#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42
+#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48
+#define BMX055_MAGN_I2C_REG_REPXY     0x51
+#define BMX055_MAGN_I2C_REG_REPZ      0x52
+
+#define BMX055_MAGN_I2C_REG_DIG_X1       0x5D
+#define BMX055_MAGN_I2C_REG_DIG_Y1       0x5E
+#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB   0x62
+#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB   0x63
+#define BMX055_MAGN_I2C_REG_DIG_X2       0x64
+#define BMX055_MAGN_I2C_REG_DIG_Y2       0x65
+#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB   0x68
+#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB   0x69
+#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB   0x6A
+#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB   0x6B
+#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C
+#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D
+#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB   0x6E
+#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB   0x6F
+#define BMX055_MAGN_I2C_REG_DIG_XY2      0x70
+#define BMX055_MAGN_I2C_REG_DIG_XY1      0x71
+
+// Constants
+#define BMX055_MAGN_CHIP_ID     0x32
+#define BMX055_MAGN_FORCED      (0b01 << 1)
+
+struct trim_data_t {
+    int8_t dig_x1;
+    int8_t dig_y1;
+    int8_t dig_x2;
+    int8_t dig_y2;
+    uint16_t dig_z1;
+    int16_t dig_z2;
+    int16_t dig_z3;
+    int16_t dig_z4;
+    uint8_t dig_xy1;
+    int8_t dig_xy2;
+    uint16_t dig_xyz1;
+};
+
+
+class BMX055_Magn : public I2CSensor{
+  uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;}
+  trim_data_t trim_data = {0};
+  bool perform_self_test();
+  bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z);
+public:
+  BMX055_Magn(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};

openpilot/sunnypilot/system/sensord/sensors/bmx055_temp.cc

@@ -0,0 +1,31 @@
+#include "sunnypilot/system/sensord/sensors/bmx055_temp.h"
+
+#include <cassert>
+
+#include "sunnypilot/system/sensord/sensors/bmx055_accel.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Temp::init() {
+  return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0;
+}
+
+bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[1];
+  int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float temp = 23.0f + int8_t(buffer[0]) / 2.0f;
+
+  auto event = msg.initEvent().initTemperatureSensor();
+  event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+  event.setVersion(1);
+  event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
+  event.setTimestamp(start_time);
+  event.setTemperature(temp);
+
+  return true;
+}

openpilot/sunnypilot/system/sensord/sensors/bmx055_temp.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/bmx055_accel.h"
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+class BMX055_Temp : public I2CSensor {
+  uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
+public:
+  BMX055_Temp(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown() { return 0; }
+};

openpilot/sunnypilot/system/sensord/sensors/constants.h

@@ -0,0 +1,18 @@
+#pragma once
+
+
+#define SENSOR_ACCELEROMETER 1
+#define SENSOR_MAGNETOMETER 2
+#define SENSOR_MAGNETOMETER_UNCALIBRATED 3
+#define SENSOR_GYRO 4
+#define SENSOR_GYRO_UNCALIBRATED 5
+#define SENSOR_LIGHT 7
+
+#define SENSOR_TYPE_ACCELEROMETER 1
+#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2
+#define SENSOR_TYPE_GYROSCOPE 4
+#define SENSOR_TYPE_LIGHT 5
+#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13
+#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14
+#define SENSOR_TYPE_MAGNETIC_FIELD  SENSOR_TYPE_GEOMAGNETIC_FIELD
+#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16

openpilot/sunnypilot/system/sensord/sensors/i2c_sensor.cc

@@ -0,0 +1,50 @@
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+int16_t read_12_bit(uint8_t lsb, uint8_t msb) {
+  uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0);
+  return int16_t(combined) / (1 << 4);
+}
+
+int16_t read_16_bit(uint8_t lsb, uint8_t msb) {
+  uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb);
+  return int16_t(combined);
+}
+
+int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) {
+  uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2);
+  return int32_t(combined) / (1 << 4);
+}
+
+I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {}
+
+I2CSensor::~I2CSensor() {
+  if (gpio_fd != -1) {
+    close(gpio_fd);
+  }
+}
+
+int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) {
+  return bus->read_register(get_device_address(), register_address, buffer, len);
+}
+
+int I2CSensor::set_register(uint register_address, uint8_t data) {
+  return bus->set_register(get_device_address(), register_address, data);
+}
+
+int I2CSensor::init_gpio() {
+  if (shared_gpio || gpio_nr == 0) {
+    return 0;
+  }
+
+  gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr);
+  if (gpio_fd < 0) {
+    return -1;
+  }
+
+  return 0;
+}
+
+bool I2CSensor::has_interrupt_enabled() {
+  return gpio_nr != 0;
+}

openpilot/sunnypilot/system/sensord/sensors/i2c_sensor.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <cstdint>
+#include <unistd.h>
+#include <vector>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/i2c.h"
+#include "common/gpio.h"
+
+#include "common/swaglog.h"
+#include "sunnypilot/system/sensord/sensors/constants.h"
+#include "sunnypilot/system/sensord/sensors/sensor.h"
+
+int16_t read_12_bit(uint8_t lsb, uint8_t msb);
+int16_t read_16_bit(uint8_t lsb, uint8_t msb);
+int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0);
+
+
+class I2CSensor : public Sensor {
+private:
+  I2CBus *bus;
+  int gpio_nr;
+  bool shared_gpio;
+  virtual uint8_t get_device_address() = 0;
+
+public:
+  I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
+  ~I2CSensor();
+  int read_register(uint register_address, uint8_t *buffer, uint8_t len);
+  int set_register(uint register_address, uint8_t data);
+  int init_gpio();
+  bool has_interrupt_enabled();
+  virtual int init() = 0;
+  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
+  virtual int shutdown() = 0;
+
+  int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
+    uint8_t chip_id = 0;
+    int ret = read_register(address, &chip_id, 1);
+    if (ret < 0) {
+      LOGD("Reading chip ID failed: %d", ret);
+      return -1;
+    }
+    for (int i = 0; i < expected_ids.size(); ++i) {
+      if (chip_id == expected_ids[i]) return chip_id;
+    }
+    LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]);
+    return -1;
+  }
+};

openpilot/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc

@@ -0,0 +1,250 @@
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_accel.h"
+
+#include <cassert>
+#include <cmath>
+#include <cstring>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  I2CSensor(bus, gpio_nr, shared_gpio) {}
+
+void LSM6DS3_Accel::wait_for_data_ready() {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  do {
+    read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+    drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
+  } while (drdy == 0);
+
+  read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
+}
+
+void LSM6DS3_Accel::read_and_avg_data(float* out_buf) {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  float scaling = 0.061f;
+  if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
+    scaling = 0.122f;
+  }
+
+  for (int i = 0; i < 5; i++) {
+    do {
+      read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+      drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
+    } while (drdy == 0);
+
+    int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
+    assert(len == sizeof(buffer));
+
+    for (int j = 0; j < 3; j++) {
+      out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling;
+    }
+  }
+
+  for (int i = 0; i < 3; i++) {
+    out_buf[i] /= 5.0f;
+  }
+}
+
+int LSM6DS3_Accel::self_test(int test_type) {
+  float val_st_off[3] = {0};
+  float val_st_on[3] = {0};
+  float test_val[3] = {0};
+  uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz
+
+  // prepare sensor for self-test
+
+  // enable block data update and automatic increment
+  int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU);
+  if (ret < 0) {
+    return ret;
+  }
+
+  if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
+    ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ;
+  }
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(100);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_off);
+
+  // enable Self Test positive (or negative)
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(100);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_on);
+
+  // disable sensor
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // disable self test
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // calculate the mg values for self test
+  for (int i = 0; i < 3; i++) {
+    test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
+  }
+
+  // verify test result
+  for (int i = 0; i < 3; i++) {
+    if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) ||
+        (test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) {
+      return -1;
+    }
+  }
+
+  return ret;
+}
+
+int LSM6DS3_Accel::init() {
+  uint8_t value = 0;
+  bool do_self_test = false;
+
+  const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
+  if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
+    do_self_test = true;
+  }
+
+  int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID});
+  if (ret == -1) return -1;
+
+  if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) {
+    source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
+  }
+
+  ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 accel positive self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 accel negative self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  ret = init_gpio();
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // enable continuous update, and automatic increase
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // enable data ready interrupt for accel on INT1
+  // (without resetting existing interrupts)
+  ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value |= LSM6DS3_ACCEL_INT1_DRDY_XL;
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
+
+fail:
+  return ret;
+}
+
+int LSM6DS3_Accel::shutdown() {
+  int ret = 0;
+
+  // disable data ready interrupt for accel on INT1
+  uint8_t value = 0;
+  ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL);
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
+  if (ret < 0) {
+    LOGE("Could not disable lsm6ds3 acceleration interrupt!");
+    goto fail;
+  }
+
+  // enable power-down mode
+  value = 0;
+  ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= 0x0F;
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value);
+  if (ret < 0) {
+    LOGE("Could not power-down lsm6ds3 accelerometer!");
+    goto fail;
+  }
+
+fail:
+  return ret;
+}
+
+bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
+
+  // INT1 shared with gyro, check STATUS_REG who triggered
+  uint8_t status_reg = 0;
+  read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
+  if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) {
+    return false;
+  }
+
+  uint8_t buffer[6];
+  int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float scale = 9.81 * 2.0f / (1 << 15);
+  float x = read_16_bit(buffer[0], buffer[1]) * scale;
+  float y = read_16_bit(buffer[2], buffer[3]) * scale;
+  float z = read_16_bit(buffer[4], buffer[5]) * scale;
+
+  auto event = msg.initEvent().initAccelerometer();
+  event.setSource(source);
+  event.setVersion(1);
+  event.setSensor(SENSOR_ACCELEROMETER);
+  event.setType(SENSOR_TYPE_ACCELEROMETER);
+  event.setTimestamp(ts);
+
+  float xyz[] = {y, -x, z};
+  auto svec = event.initAcceleration();
+  svec.setV(xyz);
+  svec.setStatus(true);
+
+  return true;
+}

openpilot/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define LSM6DS3_ACCEL_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG  0x0B
+#define LSM6DS3_ACCEL_I2C_REG_ID        0x0F
+#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D
+#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL  0x10
+#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C   0x12
+#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C   0x14
+#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL   0x18
+#define LSM6DS3_ACCEL_I2C_REG_STAT_REG  0x1E
+#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28
+
+// Constants
+#define LSM6DS3_ACCEL_CHIP_ID         0x69
+#define LSM6DS3TRC_ACCEL_CHIP_ID      0x6A
+#define LSM6DS3_ACCEL_FS_4G           (0b10 << 2)
+#define LSM6DS3_ACCEL_ODR_52HZ        (0b0011 << 4)
+#define LSM6DS3_ACCEL_ODR_104HZ       (0b0100 << 4)
+#define LSM6DS3_ACCEL_INT1_DRDY_XL    0b1
+#define LSM6DS3_ACCEL_DRDY_XLDA       0b1
+#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7)
+#define LSM6DS3_ACCEL_IF_INC          0b00000100
+#define LSM6DS3_ACCEL_IF_INC_BDU      0b01000100
+#define LSM6DS3_ACCEL_XYZ_DEN         0b11100000
+#define LSM6DS3_ACCEL_POSITIVE_TEST   0b01
+#define LSM6DS3_ACCEL_NEGATIVE_TEST   0b10
+#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f
+#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f
+
+class LSM6DS3_Accel : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;}
+  cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
+
+  // self test functions
+  int self_test(int test_type);
+  void wait_for_data_ready();
+  void read_and_avg_data(float* val_st_off);
+public:
+  LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};

openpilot/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc

@@ -0,0 +1,233 @@
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h"
+
+#include <cassert>
+#include <cmath>
+#include <cstring>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+#define DEG2RAD(x) ((x) * M_PI / 180.0)
+
+LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  I2CSensor(bus, gpio_nr, shared_gpio) {}
+
+void LSM6DS3_Gyro::wait_for_data_ready() {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  do {
+    read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+    drdy &= LSM6DS3_GYRO_DRDY_GDA;
+  } while (drdy == 0);
+
+  read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
+}
+
+void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  for (int i = 0; i < 5; i++) {
+    do {
+      read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+      drdy &= LSM6DS3_GYRO_DRDY_GDA;
+    } while (drdy == 0);
+
+    int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
+    assert(len == sizeof(buffer));
+
+    for (int j = 0; j < 3; j++) {
+      out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f;
+    }
+  }
+
+  // calculate the mg average values
+  for (int i = 0; i < 3; i++) {
+    out_buf[i] /= 5.0f;
+  }
+}
+
+int LSM6DS3_Gyro::self_test(int test_type) {
+  float val_st_off[3] = {0};
+  float val_st_on[3] = {0};
+  float test_val[3] = {0};
+
+  // prepare sensor for self-test
+
+  // full scale: 2000dps, ODR: 208Hz
+  int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(150);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_off);
+
+  // enable Self Test positive (or negative)
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(50);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_on);
+
+  // disable sensor
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // disable self test
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // calculate the mg values for self test
+  for (int i = 0; i < 3; i++) {
+    test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
+  }
+
+  // verify test result
+  for (int i = 0; i < 3; i++) {
+    if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) ||
+        (test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) {
+      return -1;
+    }
+  }
+
+  return ret;
+}
+
+int LSM6DS3_Gyro::init() {
+  uint8_t value = 0;
+  bool do_self_test = false;
+
+  const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
+  if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
+    do_self_test = true;
+  }
+
+  int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID});
+  if (ret == -1) return -1;
+
+  if (ret == LSM6DS3TRC_GYRO_CHIP_ID) {
+    source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
+  }
+
+  ret = init_gpio();
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 gyro positive self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 gyro negative self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  // TODO: set scale. Default is +- 250 deg/s
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // enable data ready interrupt for gyro on INT1
+  // (without resetting existing interrupts)
+  ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value |= LSM6DS3_GYRO_INT1_DRDY_G;
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
+
+fail:
+  return ret;
+}
+
+int LSM6DS3_Gyro::shutdown() {
+  int ret = 0;
+
+  // disable data ready interrupt for gyro on INT1
+  uint8_t value = 0;
+  ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= ~(LSM6DS3_GYRO_INT1_DRDY_G);
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
+  if (ret < 0) {
+    LOGE("Could not disable lsm6ds3 gyroscope interrupt!");
+    goto fail;
+  }
+
+  // enable power-down mode
+  value = 0;
+  ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= 0x0F;
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value);
+  if (ret < 0) {
+    LOGE("Could not power-down lsm6ds3 gyroscope!");
+    goto fail;
+  }
+
+fail:
+  return ret;
+}
+
+bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
+
+  // INT1 shared with accel, check STATUS_REG who triggered
+  uint8_t status_reg = 0;
+  read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
+  if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) {
+    return false;
+  }
+
+  uint8_t buffer[6];
+  int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float scale = 8.75 / 1000.0;
+  float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
+  float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
+  float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
+
+  auto event = msg.initEvent().initGyroscope();
+  event.setSource(source);
+  event.setVersion(2);
+  event.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
+  event.setTimestamp(ts);
+
+  float xyz[] = {y, -x, z};
+  auto svec = event.initGyroUncalibrated();
+  svec.setV(xyz);
+  svec.setStatus(true);
+
+  return true;
+}

openpilot/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define LSM6DS3_GYRO_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG  0x0B
+#define LSM6DS3_GYRO_I2C_REG_ID        0x0F
+#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D
+#define LSM6DS3_GYRO_I2C_REG_CTRL2_G   0x11
+#define LSM6DS3_GYRO_I2C_REG_CTRL5_C   0x14
+#define LSM6DS3_GYRO_I2C_REG_STAT_REG  0x1E
+#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G  0x22
+#define LSM6DS3_GYRO_POSITIVE_TEST   (0b01 << 2)
+#define LSM6DS3_GYRO_NEGATIVE_TEST   (0b11 << 2)
+
+// Constants
+#define LSM6DS3_GYRO_CHIP_ID         0x69
+#define LSM6DS3TRC_GYRO_CHIP_ID      0x6A
+#define LSM6DS3_GYRO_FS_2000dps      (0b11 << 2)
+#define LSM6DS3_GYRO_ODR_104HZ       (0b0100 << 4)
+#define LSM6DS3_GYRO_ODR_208HZ       (0b0101 << 4)
+#define LSM6DS3_GYRO_INT1_DRDY_G     0b10
+#define LSM6DS3_GYRO_DRDY_GDA        0b10
+#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7)
+#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f
+#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f
+
+
+class LSM6DS3_Gyro : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;}
+  cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
+
+  // self test functions
+  int self_test(int test_type);
+  void wait_for_data_ready();
+  void read_and_avg_data(float* val_st_off);
+public:
+  LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};

openpilot/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc

@@ -0,0 +1,37 @@
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_temp.h"
+
+#include <cassert>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
+
+int LSM6DS3_Temp::init() {
+  int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID});
+  if (ret == -1) return -1;
+
+  if (ret == LSM6DS3TRC_TEMP_CHIP_ID) {
+    source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
+  }
+  return 0;
+}
+
+bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[2];
+  int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f;
+  float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale;
+
+  auto event = msg.initEvent().initTemperatureSensor();
+  event.setSource(source);
+  event.setVersion(1);
+  event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
+  event.setTimestamp(start_time);
+  event.setTemperature(temp);
+
+  return true;
+}

openpilot/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define LSM6DS3_TEMP_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_TEMP_I2C_REG_ID           0x0F
+#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L   0x20
+
+// Constants
+#define LSM6DS3_TEMP_CHIP_ID        0x69
+#define LSM6DS3TRC_TEMP_CHIP_ID     0x6A
+
+
+class LSM6DS3_Temp : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;}
+  cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
+
+public:
+  LSM6DS3_Temp(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown() { return 0; }
+};

openpilot/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc

@@ -0,0 +1,108 @@
+#include "sunnypilot/system/sensord/sensors/mmc5603nj_magn.h"
+
+#include <algorithm>
+#include <cassert>
+#include <vector>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
+
+int MMC5603NJ_Magn::init() {
+  int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
+  if (ret == -1) return -1;
+
+  // Set ODR to 0
+  ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // Set BW to 0b01 for 1-150 Hz operation
+  ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01);
+  if (ret < 0) {
+    goto fail;
+  }
+
+fail:
+  return ret;
+}
+
+int MMC5603NJ_Magn::shutdown() {
+  int ret = 0;
+
+  // disable auto reset of measurements
+  uint8_t value = 0;
+  ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
+  ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // set ODR to 0 to leave continuous mode
+  ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
+  if (ret < 0) {
+    goto fail;
+  }
+  return ret;
+
+fail:
+  LOGE("Could not disable mmc5603nj auto set reset");
+  return ret;
+}
+
+void MMC5603NJ_Magn::start_measurement() {
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01);
+  util::sleep_for(5);
+}
+
+std::vector<float> MMC5603NJ_Magn::read_measurement() {
+  int len;
+  uint8_t buffer[9];
+  len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+  float scale = 1.0 / 16384.0;
+  float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0;
+  float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0;
+  float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0;
+  std::vector<float> xyz = {x, y, z};
+  return xyz;
+}
+
+bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  // SET - RESET cycle
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET);
+  util::sleep_for(5);
+  MMC5603NJ_Magn::start_measurement();
+  std::vector<float> xyz = MMC5603NJ_Magn::read_measurement();
+
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET);
+  util::sleep_for(5);
+  MMC5603NJ_Magn::start_measurement();
+  std::vector<float> reset_xyz = MMC5603NJ_Magn::read_measurement();
+
+  auto event = msg.initEvent().initMagnetometer();
+  event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
+  event.setVersion(1);
+  event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+  event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
+  event.setTimestamp(start_time);
+
+  float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]};
+  bool valid = true;
+  if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) {
+    valid = false;
+  }
+  auto svec = event.initMagneticUncalibrated();
+  svec.setV(vals);
+  svec.setStatus(valid);
+  return true;
+}

openpilot/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <vector>
+
+#include "sunnypilot/system/sensord/sensors/i2c_sensor.h"
+
+// Address of the chip on the bus
+#define MMC5603NJ_I2C_ADDR       0x30
+
+// Registers of the chip
+#define MMC5603NJ_I2C_REG_XOUT0       0x00
+#define MMC5603NJ_I2C_REG_ODR         0x1A
+#define MMC5603NJ_I2C_REG_INTERNAL_0  0x1B
+#define MMC5603NJ_I2C_REG_INTERNAL_1  0x1C
+#define MMC5603NJ_I2C_REG_INTERNAL_2  0x1D
+#define MMC5603NJ_I2C_REG_ID          0x39
+
+// Constants
+#define MMC5603NJ_CHIP_ID        0x10
+#define MMC5603NJ_CMM_FREQ_EN    (1 << 7)
+#define MMC5603NJ_AUTO_SR_EN     (1 << 5)
+#define MMC5603NJ_CMM_EN         (1 << 4)
+#define MMC5603NJ_EN_PRD_SET     (1 << 3)
+#define MMC5603NJ_SET            (1 << 3)
+#define MMC5603NJ_RESET          (1 << 4)
+
+class MMC5603NJ_Magn : public I2CSensor {
+private:
+  uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
+  void start_measurement();
+  std::vector<float> read_measurement();
+public:
+  MMC5603NJ_Magn(I2CBus *bus);
+  int init();
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
+  int shutdown();
+};

openpilot/sunnypilot/system/sensord/sensors/sensor.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "cereal/messaging/messaging.h"
+
+class Sensor {
+public:
+  int gpio_fd = -1;
+  bool enabled = false;
+  uint64_t start_ts = 0;
+  uint64_t init_delay = 500e6; // default dealy 500ms
+
+  virtual ~Sensor() {}
+  virtual int init() = 0;
+  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
+  virtual bool has_interrupt_enabled() = 0;
+  virtual int shutdown() = 0;
+
+  virtual bool is_data_valid(uint64_t current_ts) {
+    if (start_ts == 0) {
+      start_ts = current_ts;
+    }
+    return (current_ts - start_ts) > init_delay;
+  }
+};

openpilot/sunnypilot/system/sensord/sensors_qcom2.cc

@@ -0,0 +1,179 @@
+#include <sys/resource.h>
+
+#include <chrono>
+#include <thread>
+#include <vector>
+#include <map>
+#include <poll.h>
+#include <linux/gpio.h>
+
+#include "cereal/services.h"
+#include "cereal/messaging/messaging.h"
+#include "common/i2c.h"
+#include "common/ratekeeper.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+#include "sunnypilot/system/sensord/sensors/bmx055_accel.h"
+#include "sunnypilot/system/sensord/sensors/bmx055_gyro.h"
+#include "sunnypilot/system/sensord/sensors/bmx055_magn.h"
+#include "sunnypilot/system/sensord/sensors/bmx055_temp.h"
+#include "sunnypilot/system/sensord/sensors/constants.h"
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_accel.h"
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h"
+#include "sunnypilot/system/sensord/sensors/lsm6ds3_temp.h"
+#include "sunnypilot/system/sensord/sensors/mmc5603nj_magn.h"
+
+#define I2C_BUS_IMU 1
+
+ExitHandler do_exit;
+
+void interrupt_loop(std::vector<std::tuple<Sensor *, std::string>> sensors) {
+  PubMaster pm({"gyroscope", "accelerometer"});
+
+  int fd = -1;
+  for (auto &[sensor, msg_name] : sensors) {
+    if (sensor->has_interrupt_enabled()) {
+      fd = sensor->gpio_fd;
+      break;
+    }
+  }
+
+  uint64_t offset = nanos_since_epoch() - nanos_since_boot();
+  struct pollfd fd_list[1] = {0};
+  fd_list[0].fd = fd;
+  fd_list[0].events = POLLIN | POLLPRI;
+
+  while (!do_exit) {
+    int err = poll(fd_list, 1, 100);
+    if (err == -1) {
+      if (errno == EINTR) {
+        continue;
+      }
+      return;
+    } else if (err == 0) {
+      LOGE("poll timed out");
+      continue;
+    }
+
+    if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) {
+      LOGE("no poll events set");
+      continue;
+    }
+
+    // Read all events
+    struct gpioevent_data evdata[16];
+    err = HANDLE_EINTR(read(fd, evdata, sizeof(evdata)));
+    if (err < 0 || err % sizeof(*evdata) != 0) {
+      LOGE("error reading event data %d", err);
+      continue;
+    }
+
+    uint64_t cur_offset = nanos_since_epoch() - nanos_since_boot();
+    uint64_t diff = cur_offset > offset ? cur_offset - offset : offset - cur_offset;
+    if (diff > 10*1e6) { // 10ms
+      LOGW("time jumped: %lu %lu", cur_offset, offset);
+      offset = cur_offset;
+
+      // we don't have a valid timestamp since the
+      // time jumped, so throw out this measurement.
+      continue;
+    }
+
+    int num_events = err / sizeof(*evdata);
+    uint64_t ts = evdata[num_events - 1].timestamp - cur_offset;
+
+    for (auto &[sensor, msg_name] : sensors) {
+      if (!sensor->has_interrupt_enabled()) {
+        continue;
+      }
+
+      MessageBuilder msg;
+      if (!sensor->get_event(msg, ts)) {
+        continue;
+      }
+
+      if (!sensor->is_data_valid(ts)) {
+        continue;
+      }
+
+      pm.send(msg_name.c_str(), msg);
+    }
+  }
+}
+
+void polling_loop(Sensor *sensor, std::string msg_name) {
+  PubMaster pm({msg_name.c_str()});
+  RateKeeper rk(msg_name, services.at(msg_name).frequency);
+  while (!do_exit) {
+    MessageBuilder msg;
+    if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) {
+      pm.send(msg_name.c_str(), msg);
+    }
+    rk.keepTime();
+  }
+}
+
+int sensor_loop(I2CBus *i2c_bus_imu) {
+  // Sensor init
+  std::vector<std::tuple<Sensor *, std::string>> sensors_init = {
+    {new BMX055_Accel(i2c_bus_imu), "accelerometer2"},
+    {new BMX055_Gyro(i2c_bus_imu), "gyroscope2"},
+    {new BMX055_Magn(i2c_bus_imu), "magnetometer"},
+    {new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"},
+
+    {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"},
+    {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"},
+    {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"},
+
+    {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer"},
+  };
+
+  // Initialize sensors
+  std::vector<std::thread> threads;
+  for (auto &[sensor, msg_name] : sensors_init) {
+    int err = sensor->init();
+    if (err < 0) {
+      continue;
+    }
+
+    if (!sensor->has_interrupt_enabled()) {
+      threads.emplace_back(polling_loop, sensor, msg_name);
+    }
+  }
+
+  // increase interrupt quality by pinning interrupt and process to core 1
+  setpriority(PRIO_PROCESS, 0, -18);
+  util::set_core_affinity({1});
+
+  // TODO: get the IRQ number from gpiochip
+  std::string irq_path = "/proc/irq/336/smp_affinity_list";
+  if (!util::file_exists(irq_path)) {
+    irq_path = "/proc/irq/335/smp_affinity_list";
+  }
+  std::system(util::string_format("sudo su -c 'echo 1 > %s'", irq_path.c_str()).c_str());
+
+  // thread for reading events via interrupts
+  threads.emplace_back(&interrupt_loop, std::ref(sensors_init));
+
+  // wait for all threads to finish
+  for (auto &t : threads) {
+    t.join();
+  }
+
+  for (auto &[sensor, msg_name] : sensors_init) {
+    sensor->shutdown();
+    delete sensor;
+  }
+  return 0;
+}
+
+int main(int argc, char *argv[]) {
+  try {
+    auto i2c_bus_imu = std::make_unique<I2CBus>(I2C_BUS_IMU);
+    return sensor_loop(i2c_bus_imu.get());
+  } catch (std::exception &e) {
+    LOGE("I2CBus init failed");
+    return -1;
+  }
+}

openpilot/sunnypilot/system/sensord/tests/test_sensord.py

@@ -0,0 +1,252 @@
+import os
+import subprocess
+import pytest
+import time
+import numpy as np
+from collections import namedtuple, defaultdict
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import log
+from openpilot.cereal.services import SERVICE_LIST
+from openpilot.common.gpio import get_irqs_for_action
+from openpilot.common.timeout import Timeout
+from openpilot.common.hardware import HARDWARE
+from openpilot.system.manager.process_config import managed_processes
+
+BMX = {
+  ('bmx055', 'acceleration'),
+  ('bmx055', 'gyroUncalibrated'),
+  ('bmx055', 'magneticUncalibrated'),
+  ('bmx055', 'temperature'),
+}
+
+LSM = {
+  ('lsm6ds3', 'acceleration'),
+  ('lsm6ds3', 'gyroUncalibrated'),
+  ('lsm6ds3', 'temperature'),
+}
+LSM_C = {(x[0]+'trc', x[1]) for x in LSM}
+
+MMC = {
+  ('mmc5603nj', 'magneticUncalibrated'),
+}
+
+SENSOR_CONFIGURATIONS: list[set] = [
+  BMX | LSM,
+  MMC | LSM,
+  BMX | LSM_C,
+  MMC| LSM_C,
+]
+if HARDWARE.get_device_type() == "mici":
+  SENSOR_CONFIGURATIONS = [
+    LSM,
+    LSM_C,
+  ]
+
+Sensor = log.SensorEventData.SensorSource
+SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
+ALL_SENSORS = {
+  Sensor.lsm6ds3: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.lsm6ds3trc: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.bmx055: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("magneticUncalibrated", 0, 300),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.mmc5603nj: {
+    SensorConfig("magneticUncalibrated", 0, 300),
+  }
+}
+
+
+def get_irq_count(irq: int):
+  with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f:
+    per_cpu = map(int, f.read().split(","))
+    return sum(per_cpu)
+
+def read_sensor_events(duration_sec):
+  sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2',
+                  'gyroscope2', 'temperatureSensor', 'temperatureSensor2']
+  socks = {}
+  poller = messaging.Poller()
+  events = defaultdict(list)
+  for stype in sensor_types:
+    socks[stype] = messaging.sub_sock(stype, poller=poller, timeout=100)
+
+  # wait for sensors to come up
+  with Timeout(int(os.environ.get("SENSOR_WAIT", "5")), "sensors didn't come up"):
+    while len(poller.poll(250)) == 0:
+      pass
+  time.sleep(1)
+  for s in socks.values():
+    messaging.drain_sock_raw(s)
+
+  st = time.monotonic()
+  while time.monotonic() - st < duration_sec:
+    for s in socks:
+      events[s] += messaging.drain_sock(socks[s])
+    time.sleep(0.1)
+
+  assert sum(map(len, events.values())) != 0, "No sensor events collected!"
+
+  return {k: v for k, v in events.items() if len(v) > 0}
+
+@pytest.mark.tici
+class TestSensord:
+  @classmethod
+  def setup_class(cls):
+    # enable LSM self test
+    os.environ["LSM_SELF_TEST"] = "1"
+
+    # read initial sensor values every test case can use
+    subprocess.run(["pkill", "-f", r"\\./sensord"], check=False)
+    try:
+      managed_processes["sensord"].start()
+      cls.sample_secs = int(os.getenv("SAMPLE_SECS", "10"))
+      cls.events = read_sensor_events(cls.sample_secs)
+
+      # determine sensord's irq
+      cls.sensord_irq = get_irqs_for_action("sensord")[0]
+    finally:
+      # teardown won't run if this doesn't succeed
+      managed_processes["sensord"].stop()
+
+  @classmethod
+  def teardown_class(cls):
+    managed_processes["sensord"].stop()
+
+  def teardown_method(self):
+    managed_processes["sensord"].stop()
+
+  def test_sensors_present(self):
+    # verify correct sensors configuration
+    seen = set()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+        seen.add((str(m.source), m.which()))
+
+    assert seen in SENSOR_CONFIGURATIONS
+
+  def test_lsm6ds3_timing(self, subtests):
+    # verify measurements are sampled and published at 104Hz
+
+    sensor_t = {
+      1: [], # accel
+      5: [], # gyro
+    }
+
+    for measurement in self.events['accelerometer']:
+      m = getattr(measurement, measurement.which())
+      sensor_t[m.sensor].append(m.timestamp)
+
+    for measurement in self.events['gyroscope']:
+      m = getattr(measurement, measurement.which())
+      sensor_t[m.sensor].append(m.timestamp)
+
+    for s, vals in sensor_t.items():
+      with subtests.test(sensor=s):
+        assert len(vals) > 0
+        tdiffs = np.diff(vals) / 1e6 # millis
+
+        high_delay_diffs = list(filter(lambda d: d >= 20., tdiffs))
+        assert len(high_delay_diffs) < 15, f"Too many large diffs: {high_delay_diffs}"
+
+        avg_diff = sum(tdiffs)/len(tdiffs)
+        avg_freq = 1. / (avg_diff * 1e-3)
+        assert 92. < avg_freq < 114., f"avg freq {avg_freq}Hz wrong, expected 104Hz"
+
+        stddev = np.std(tdiffs)
+        assert stddev < 2.0, f"Standard-dev to big {stddev}"
+
+  def test_sensor_frequency(self, subtests):
+    for s, msgs in self.events.items():
+      with subtests.test(sensor=s):
+        freq = len(msgs) / self.sample_secs
+        ef = SERVICE_LIST[s].frequency
+        assert ef*0.85 <= freq <= ef*1.15
+
+  def test_logmonottime_timestamp_diff(self):
+    # ensure diff between the message logMonotime and sample timestamp is small
+
+    tdiffs = list()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+
+        # check if gyro and accel timestamps are before logMonoTime
+        if str(m.source).startswith("lsm6ds3") and m.which() != 'temperature':
+          err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}"
+          assert m.timestamp < measurement.logMonoTime, err_msg
+
+        # negative values might occur, as non interrupt packages created
+        # before the sensor is read
+        tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6)
+
+    # some sensors have a read procedure that will introduce an expected diff on the order of 20ms
+    high_delay_diffs = set(filter(lambda d: d >= 25., tdiffs))
+    assert len(high_delay_diffs) < 20, f"Too many measurements published: {high_delay_diffs}"
+
+    avg_diff = round(sum(tdiffs)/len(tdiffs), 4)
+    assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms"
+
+  def test_sensor_values(self):
+    sensor_values = dict()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+        key = (m.source.raw, m.which())
+        values = getattr(m, m.which())
+
+        if hasattr(values, 'v'):
+          values = values.v
+        values = np.atleast_1d(values)
+
+        if key in sensor_values:
+          sensor_values[key].append(values)
+        else:
+          sensor_values[key] = [values]
+
+    # Sanity check sensor values
+    for sensor, stype in sensor_values:
+      for s in ALL_SENSORS[sensor]:
+        if s.type != stype:
+          continue
+
+        key = (sensor, s.type)
+        mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1))
+        err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}"
+        assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg
+
+  def test_sensor_verify_no_interrupts_after_stop(self):
+    managed_processes["sensord"].start()
+    time.sleep(3)
+
+    # read /proc/interrupts to verify interrupts are received
+    state_one = get_irq_count(self.sensord_irq)
+    time.sleep(1)
+    state_two = get_irq_count(self.sensord_irq)
+
+    error_msg = f"no interrupts received after sensord start!\n{state_one} {state_two}"
+    assert state_one != state_two, error_msg
+
+    managed_processes["sensord"].stop()
+    time.sleep(1)
+
+    # read /proc/interrupts to verify no more interrupts are received
+    state_one = get_irq_count(self.sensord_irq)
+    time.sleep(1)
+    state_two = get_irq_count(self.sensord_irq)
+    assert state_one == state_two, "Interrupts received after sensord stop!"

openpilot/sunnypilot/system/sensord/tests/ttff_test.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+import time
+import atexit
+
+from openpilot.cereal import messaging
+from openpilot.system.manager.process_config import managed_processes
+
+TIMEOUT = 10*60
+
+def kill():
+  for proc in ['ubloxd', 'pigeond']:
+    managed_processes[proc].stop(retry=True, block=True)
+
+if __name__ == "__main__":
+  # start ubloxd
+  managed_processes['ubloxd'].start()
+  atexit.register(kill)
+
+  sm = messaging.SubMaster(['ubloxGnss'])
+
+  times = []
+  for i in range(20):
+    # start pigeond
+    st = time.monotonic()
+    managed_processes['pigeond'].start()
+
+    # wait for a >4 satellite fix
+    while True:
+      sm.update(0)
+      msg = sm['ubloxGnss']
+      if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]:
+        report = msg.measurementReport
+        if report.numMeas > 4:
+          times.append(time.monotonic() - st)
+          print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m")
+          break
+
+      if time.monotonic() - st > TIMEOUT:
+        raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m")
+
+      time.sleep(0.1)
+
+    # stop pigeond
+    managed_processes['pigeond'].stop(retry=True, block=True)
+    time.sleep(20)
+
+  print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m")

openpilot/sunnypilot/system/statsd.py

@@ -0,0 +1,234 @@
+#!/usr/bin/env python3
+import base64
+import json
+import os
+from decimal import Decimal
+
+import zmq
+import time
+import uuid
+from pathlib import Path
+from collections import defaultdict
+from datetime import datetime, UTC, date
+from typing import NoReturn
+
+from openpilot.common.params import Params
+from openpilot.cereal.messaging import SubMaster
+from openpilot.common.hardware.hw import Paths
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.hardware import HARDWARE
+from openpilot.common.utils import atomic_write
+from openpilot.common.version import get_build_metadata
+from openpilot.system.loggerd.config import STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S
+
+
+class METRIC_TYPE:
+  GAUGE = 'g'
+  SAMPLE = 'sa'
+  RAW = 'r'
+
+
+class StatLog:
+  def __init__(self):
+    self.pid = None
+    self.zctx = None
+    self.sock = None
+    self.stats_socket = STATS_SOCKET
+
+  def connect(self) -> None:
+    self.zctx = zmq.Context.instance() or zmq.Context()
+    self.sock = self.zctx.socket(zmq.PUSH)
+    self.sock.setsockopt(zmq.LINGER, 10)
+    self.sock.connect(self.stats_socket)
+    self.pid = os.getpid()
+
+  def __del__(self):
+    if self.sock is not None:
+      self.sock.close()
+    if self.zctx is not None:
+      self.zctx.term()
+
+  def _send(self, metric: str) -> None:
+    if os.getpid() != self.pid:
+      self.connect()
+
+    try:
+      self.sock.send_string(metric, zmq.NOBLOCK)
+    except zmq.error.Again:
+      # drop :/
+      pass
+
+  def gauge(self, name: str, value: float) -> None:
+    self._send(f"{name}:{value}|{METRIC_TYPE.GAUGE}")
+
+  # Samples will be recorded in a buffer and at aggregation time,
+  # statistical properties will be logged (mean, count, percentiles, ...)
+  def sample(self, name: str, value: float):
+    self._send(f"{name}:{value}|{METRIC_TYPE.SAMPLE}")
+
+
+class StatLogSP(StatLog):
+  def __init__(self, intercept=True):
+    """
+    Initializes the class instance with an optional parameter to determine
+    if statistical logging should be configured or not.
+
+    :param intercept: A boolean flag that indicates whether to initialize
+        the `comma_statlog`. If True, the `comma_statlog` attribute is
+        instantiated as a `StatLog` object. Defaults to True.
+    """
+    super().__init__()
+    self.comma_statlog = StatLog() if intercept else None
+    self.stats_socket = f"{STATS_SOCKET}_sp"
+
+  def connect(self) -> None:
+    super().connect()
+    if self.comma_statlog:
+      self.comma_statlog.connect()
+
+  def __del__(self):
+    super().__del__()
+    if self.comma_statlog:
+      self.comma_statlog.__del__()
+
+  def _send(self, metric: str) -> None:
+    super()._send(metric)
+    if self.comma_statlog:
+      self.comma_statlog._send(metric)
+
+  @staticmethod
+  def default_converter(obj):
+    if isinstance(obj, (datetime, date)):
+      return obj.isoformat()
+    if isinstance(obj, set):
+      return list(obj)
+    if isinstance(obj, Decimal):
+      return float(obj)
+    return str(obj)  # fallback for unknown types
+
+  def raw(self, name: str, value: dict) -> None:
+    encoded_dict = base64.b64encode(json.dumps(value, default=self.default_converter).encode("utf-8")).decode("utf-8")
+    self._send(f"{name}:{encoded_dict}|{METRIC_TYPE.RAW}")
+
+
+def main() -> NoReturn:
+  dongle_id = Params().get("DongleId")
+  def get_influxdb_line(measurement: str, value: float | dict[str, float],  timestamp: datetime, tags: dict) -> str:
+    res = f"{measurement}"
+    for k, v in tags.items():
+      res += f",{k}={str(v)}"
+    res += " "
+
+    if isinstance(value, float):
+      value = {'value': value}
+
+    for k, v in value.items():
+      res += f"{k}={v},"
+
+    res += f"dongle_id=\"{dongle_id}\" {int(timestamp.timestamp() * 1e9)}\n"
+    return res
+
+  # open statistics socket
+  ctx = zmq.Context.instance()
+  sock = ctx.socket(zmq.PULL)
+  sock.bind(STATS_SOCKET)
+
+  STATS_DIR = Paths.stats_root()
+
+  # initialize stats directory
+  Path(STATS_DIR).mkdir(parents=True, exist_ok=True)
+
+  build_metadata = get_build_metadata()
+
+  # initialize tags
+  tags = {
+    'started': False,
+    'version': build_metadata.openpilot.version,
+    'branch': build_metadata.channel,
+    'dirty': build_metadata.openpilot.is_dirty,
+    'origin': build_metadata.openpilot.git_normalized_origin,
+    'deviceType': HARDWARE.get_device_type(),
+  }
+
+  # subscribe to deviceState for started state
+  sm = SubMaster(['deviceState'])
+
+  idx = 0
+  boot_uid = str(uuid.uuid4())[:8]
+  last_flush_time = time.monotonic()
+  gauges = {}
+  samples: dict[str, list[float]] = defaultdict(list)
+  try:
+    while True:
+      started_prev = sm['deviceState'].started
+      sm.update()
+
+      # Update metrics
+      while True:
+        try:
+          metric = sock.recv_string(zmq.NOBLOCK)
+          try:
+            metric_type = metric.split('|')[1]
+            metric_name = metric.split(':')[0]
+            metric_value = float(metric.split('|')[0].split(':')[1])
+
+            if metric_type == METRIC_TYPE.GAUGE:
+              gauges[metric_name] = metric_value
+            elif metric_type == METRIC_TYPE.SAMPLE:
+              samples[metric_name].append(metric_value)
+            else:
+              cloudlog.event("unknown metric type", metric_type=metric_type)
+          except Exception:
+            cloudlog.event("malformed metric", metric=metric)
+        except zmq.error.Again:
+          break
+
+      # flush when started state changes or after FLUSH_TIME_S
+      if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev):
+        result = ""
+        current_time = datetime.now(UTC)
+        tags['started'] = sm['deviceState'].started
+
+        for key, value in gauges.items():
+          result += get_influxdb_line(f"gauge.{key}", value, current_time, tags)
+
+        for key, values in samples.items():
+          values.sort()
+          sample_count = len(values)
+          sample_sum = sum(values)
+
+          stats = {
+            'count': sample_count,
+            'min': values[0],
+            'max': values[-1],
+            'mean': sample_sum / sample_count,
+          }
+          for percentile in [0.05, 0.5, 0.95]:
+            value = values[int(round(percentile * (sample_count - 1)))]
+            stats[f"p{int(percentile * 100)}"] = value
+
+          result += get_influxdb_line(f"sample.{key}", stats, current_time, tags)
+
+        # clear intermediate data
+        gauges.clear()
+        samples.clear()
+        last_flush_time = time.monotonic()
+
+        # check that we aren't filling up the drive
+        if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT:
+          if len(result) > 0:
+            stats_path = os.path.join(STATS_DIR, f"{boot_uid}_{idx}")
+            with atomic_write(stats_path) as f:
+              f.write(result)
+            idx += 1
+        else:
+          cloudlog.error("stats dir full")
+  finally:
+    sock.close()
+    ctx.term()
+
+
+if __name__ == "__main__":
+  main()
+else:
+  statlog = StatLogSP(intercept=True)

openpilot/sunnypilot/system/updated/tests/test_sp_branch_migrations.py

@@ -0,0 +1,73 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pytest
+
+from openpilot.common.params import Params
+from openpilot.system.updated.updated import Updater
+
+
+@pytest.mark.parametrize(("device_type", "branch", "expected"), [
+  ("tici", "staging-c3-new", "staging-tici"),
+  ("tici", "dev-c3-new", "staging-tici"),
+  ("tici", "master", "master-tici"),
+  ("tici", "master-dev-c3-new", "master-tici"),
+  ("tizi", "staging-c3-new", "staging"),
+  ("tizi", "dev-c3-new", "dev"),
+  ("tizi", "master-dev-c3-new", "master-dev"),
+  ("tizi", "release3", "release-tizi"),
+  ("tizi", "release3-staging", "release-tizi-staging"),
+  ("mici", "release3", "release-mici"),
+  ("mici", "release3-staging", "release-mici-staging"),
+])
+def test_sp_branch_migrations_from_current_branch(mocker, device_type, branch, expected):
+  params = Params()
+  params.remove("UpdaterTargetBranch")
+
+  mocker.patch("openpilot.system.updated.updated.HARDWARE.get_device_type", return_value=device_type)
+  mocker.patch.object(Updater, "get_branch", return_value=branch)
+
+  assert Updater().target_branch == expected
+
+
+@pytest.mark.parametrize(("device_type", "branch", "expected"), [
+  ("tici", "staging-c3-new", "staging-tici"),
+  ("tici", "dev-c3-new", "staging-tici"),
+  ("tici", "master", "master-tici"),
+  ("tici", "master-dev-c3-new", "master-tici"),
+  ("tizi", "staging-c3-new", "staging"),
+  ("tizi", "dev-c3-new", "dev"),
+  ("tizi", "master-dev-c3-new", "master-dev"),
+  ("tizi", "release3", "release-tizi"),
+  ("tizi", "release3-staging", "release-tizi-staging"),
+  ("mici", "release3", "release-mici"),
+  ("mici", "release3-staging", "release-mici-staging"),
+])
+def test_sp_branch_migrations_from_param(mocker, device_type, branch, expected):
+  params = Params()
+  params.put("UpdaterTargetBranch", branch, block=True)
+
+  mocker.patch("openpilot.system.updated.updated.HARDWARE.get_device_type", return_value=device_type)
+
+  try:
+    assert Updater().target_branch == expected
+  finally:
+    params.remove("UpdaterTargetBranch")
+
+
+@pytest.mark.parametrize(("device_type", "branch"), [
+  ("tici", "unknown"),
+  ("tizi", "unknown"),
+  ("mici", "unknown"),
+])
+def test_sp_branch_migrations_passthrough(mocker, device_type, branch):
+  params = Params()
+  params.remove("UpdaterTargetBranch")
+
+  mocker.patch("openpilot.system.updated.updated.HARDWARE.get_device_type", return_value=device_type)
+  mocker.patch.object(Updater, "get_branch", return_value=branch)
+
+  assert Updater().target_branch == branch