From 2f808546449ea1aa072a2ab7f0bf6f876fe40de8 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 30 May 2025 13:31:07 -0700 Subject: [PATCH] sensord: rewrite in Python (#35353) * py sensord * fix up mmc * temp * port over accel * lil more * kinda works * rm that * gpiochip * mostly there * lil more * lil more * irq timestamps * fix ts * fix double deg2rad * test passes * fix up mypy * rm one more * exception * lint: * read in all events * bump that * get under budget: * accel self test * gyro self-test * keep these readable * give it more cores * debug * valid * rewrite that --------- Co-authored-by: Comma Device --- SConstruct | 1 - common/SConscript | 4 - common/gpio.cc | 84 -------- common/gpio.h | 33 --- common/gpio.py | 35 ++++ common/i2c.cc | 92 --------- common/i2c.h | 19 -- common/util.py | 22 ++ selfdrive/test/test_onroad.py | 8 +- system/hardware/tici/hardware.py | 20 +- system/manager/process_config.py | 2 +- system/sensord/.gitignore | 1 - system/sensord/SConscript | 13 -- system/sensord/sensord.py | 139 +++++++++++++ system/sensord/sensors/__init__.py | 0 system/sensord/sensors/constants.h | 18 -- system/sensord/sensors/i2c_sensor.cc | 50 ----- system/sensord/sensors/i2c_sensor.h | 51 ----- system/sensord/sensors/i2c_sensor.py | 72 +++++++ system/sensord/sensors/lsm6ds3_accel.cc | 250 ----------------------- system/sensord/sensors/lsm6ds3_accel.h | 49 ----- system/sensord/sensors/lsm6ds3_accel.py | 157 ++++++++++++++ system/sensord/sensors/lsm6ds3_gyro.cc | 233 --------------------- system/sensord/sensors/lsm6ds3_gyro.h | 45 ---- system/sensord/sensors/lsm6ds3_gyro.py | 141 +++++++++++++ system/sensord/sensors/lsm6ds3_temp.cc | 37 ---- system/sensord/sensors/lsm6ds3_temp.h | 26 --- system/sensord/sensors/lsm6ds3_temp.py | 33 +++ system/sensord/sensors/mmc5603nj_magn.cc | 108 ---------- system/sensord/sensors/mmc5603nj_magn.h | 37 ---- system/sensord/sensors/mmc5603nj_magn.py | 76 +++++++ system/sensord/sensors/sensor.h | 24 --- system/sensord/sensors_qcom2.cc | 170 --------------- 33 files changed, 681 insertions(+), 1369 deletions(-) delete mode 100644 common/gpio.cc delete mode 100644 common/gpio.h delete mode 100644 common/i2c.cc delete mode 100644 common/i2c.h delete mode 100644 system/sensord/.gitignore delete mode 100644 system/sensord/SConscript create mode 100755 system/sensord/sensord.py create mode 100644 system/sensord/sensors/__init__.py delete mode 100644 system/sensord/sensors/constants.h delete mode 100644 system/sensord/sensors/i2c_sensor.cc delete mode 100644 system/sensord/sensors/i2c_sensor.h create mode 100644 system/sensord/sensors/i2c_sensor.py delete mode 100644 system/sensord/sensors/lsm6ds3_accel.cc delete mode 100644 system/sensord/sensors/lsm6ds3_accel.h create mode 100644 system/sensord/sensors/lsm6ds3_accel.py delete mode 100644 system/sensord/sensors/lsm6ds3_gyro.cc delete mode 100644 system/sensord/sensors/lsm6ds3_gyro.h create mode 100644 system/sensord/sensors/lsm6ds3_gyro.py delete mode 100644 system/sensord/sensors/lsm6ds3_temp.cc delete mode 100644 system/sensord/sensors/lsm6ds3_temp.h create mode 100644 system/sensord/sensors/lsm6ds3_temp.py delete mode 100644 system/sensord/sensors/mmc5603nj_magn.cc delete mode 100644 system/sensord/sensors/mmc5603nj_magn.h create mode 100644 system/sensord/sensors/mmc5603nj_magn.py delete mode 100644 system/sensord/sensors/sensor.h delete mode 100644 system/sensord/sensors_qcom2.cc diff --git a/SConstruct b/SConstruct index d5597a70fe..c534eedbe8 100644 --- a/SConstruct +++ b/SConstruct @@ -348,7 +348,6 @@ SConscript([ ]) if arch != "Darwin": SConscript([ - 'system/sensord/SConscript', 'system/logcatd/SConscript', ]) diff --git a/common/SConscript b/common/SConscript index 829db6eeec..3cdb6fc5a2 100644 --- a/common/SConscript +++ b/common/SConscript @@ -4,14 +4,10 @@ common_libs = [ 'params.cc', 'swaglog.cc', 'util.cc', - 'i2c.cc', 'watchdog.cc', 'ratekeeper.cc' ] -if arch != "Darwin": - common_libs.append('gpio.cc') - _common = env.Library('common', common_libs, LIBS="json11") files = [ diff --git a/common/gpio.cc b/common/gpio.cc deleted file mode 100644 index dd7ba34b6d..0000000000 --- a/common/gpio.cc +++ /dev/null @@ -1,84 +0,0 @@ -#include "common/gpio.h" - -#include - -#ifdef __APPLE__ -int gpio_init(int pin_nr, bool output) { - return 0; -} - -int gpio_set(int pin_nr, bool high) { - return 0; -} - -int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) { - return 0; -} - -#else - -#include -#include - -#include -#include -#include - -#include "common/util.h" -#include "common/swaglog.h" - -int gpio_init(int pin_nr, bool output) { - char pin_dir_path[50]; - int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path), - "/sys/class/gpio/gpio%d/direction", pin_nr); - if (pin_dir_path_len <= 0) { - return -1; - } - const char *value = output ? "out" : "in"; - return util::write_file(pin_dir_path, (void*)value, strlen(value)); -} - -int gpio_set(int pin_nr, bool high) { - char pin_val_path[50]; - int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path), - "/sys/class/gpio/gpio%d/value", pin_nr); - if (pin_val_path_len <= 0) { - return -1; - } - return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1); -} - -int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) { - - // Assumed that all interrupt pins are unexported and rights are given to - // read from gpiochip0. - std::string gpiochip_path = "/dev/gpiochip" + std::to_string(gpiochiop_id); - int fd = open(gpiochip_path.c_str(), O_RDONLY); - if (fd < 0) { - LOGE("Error opening gpiochip0 fd"); - return -1; - } - - // Setup event - struct gpioevent_request rq; - rq.lineoffset = pin_nr; - rq.handleflags = GPIOHANDLE_REQUEST_INPUT; - - /* Requesting both edges as the data ready pulse from the lsm6ds sensor is - very short(75us) and is mostly detected as falling edge instead of rising. - So if it is detected as rising the following falling edge is skipped. */ - rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES; - - strncpy(rq.consumer_label, consumer_label, std::size(rq.consumer_label) - 1); - int ret = util::safe_ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq); - if (ret == -1) { - LOGE("Unable to get line event from ioctl : %s", strerror(errno)); - close(fd); - return -1; - } - - close(fd); - return rq.fd; -} - -#endif diff --git a/common/gpio.h b/common/gpio.h deleted file mode 100644 index 89cdedd66c..0000000000 --- a/common/gpio.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -// Pin definitions -#ifdef QCOM2 - #define GPIO_HUB_RST_N 30 - #define GPIO_UBLOX_RST_N 32 - #define GPIO_UBLOX_SAFEBOOT_N 33 - #define GPIO_GNSS_PWR_EN 34 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */ - #define GPIO_STM_RST_N 124 - #define GPIO_STM_BOOT0 134 - #define GPIO_BMX_ACCEL_INT 21 - #define GPIO_BMX_GYRO_INT 23 - #define GPIO_BMX_MAGN_INT 87 - #define GPIO_LSM_INT 84 - #define GPIOCHIP_INT 0 -#else - #define GPIO_HUB_RST_N 0 - #define GPIO_UBLOX_RST_N 0 - #define GPIO_UBLOX_SAFEBOOT_N 0 - #define GPIO_GNSS_PWR_EN 0 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */ - #define GPIO_STM_RST_N 0 - #define GPIO_STM_BOOT0 0 - #define GPIO_BMX_ACCEL_INT 0 - #define GPIO_BMX_GYRO_INT 0 - #define GPIO_BMX_MAGN_INT 0 - #define GPIO_LSM_INT 0 - #define GPIOCHIP_INT 0 -#endif - -int gpio_init(int pin_nr, bool output); -int gpio_set(int pin_nr, bool high); - -int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr); diff --git a/common/gpio.py b/common/gpio.py index 66b2adf438..8f025a2daf 100644 --- a/common/gpio.py +++ b/common/gpio.py @@ -1,4 +1,6 @@ import os +import fcntl +import ctypes from functools import cache def gpio_init(pin: int, output: bool) -> None: @@ -52,3 +54,36 @@ def get_irqs_for_action(action: str) -> list[str]: if irq.isdigit() and action in get_irq_action(irq): ret.append(irq) return ret + +# *** gpiochip *** + +class gpioevent_data(ctypes.Structure): + _fields_ = [ + ("timestamp", ctypes.c_uint64), + ("id", ctypes.c_uint32), + ] + +class gpioevent_request(ctypes.Structure): + _fields_ = [ + ("lineoffset", ctypes.c_uint32), + ("handleflags", ctypes.c_uint32), + ("eventflags", ctypes.c_uint32), + ("label", ctypes.c_char * 32), + ("fd", ctypes.c_int) + ] + +def gpiochip_get_ro_value_fd(label: str, gpiochip_id: int, pin: int) -> int: + GPIOEVENT_REQUEST_BOTH_EDGES = 0x3 + GPIOHANDLE_REQUEST_INPUT = 0x1 + GPIO_GET_LINEEVENT_IOCTL = 0xc030b404 + + rq = gpioevent_request() + rq.lineoffset = pin + rq.handleflags = GPIOHANDLE_REQUEST_INPUT + rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES + rq.label = label.encode('utf-8')[:31] + b'\0' + + fd = os.open(f"/dev/gpiochip{gpiochip_id}", os.O_RDONLY) + fcntl.ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, rq) + os.close(fd) + return int(rq.fd) diff --git a/common/i2c.cc b/common/i2c.cc deleted file mode 100644 index 3d6c79efc5..0000000000 --- a/common/i2c.cc +++ /dev/null @@ -1,92 +0,0 @@ -#include "common/i2c.h" - -#include -#include -#include - -#include -#include -#include - -#include "common/swaglog.h" -#include "common/util.h" - -#define UNUSED(x) (void)(x) - -#ifdef QCOM2 -// TODO: decide if we want to install libi2c-dev everywhere -extern "C" { - #include - #include -} - -I2CBus::I2CBus(uint8_t bus_id) { - char bus_name[20]; - snprintf(bus_name, 20, "/dev/i2c-%d", bus_id); - - i2c_fd = HANDLE_EINTR(open(bus_name, O_RDWR)); - if (i2c_fd < 0) { - throw std::runtime_error("Failed to open I2C bus"); - } -} - -I2CBus::~I2CBus() { - if (i2c_fd >= 0) { - close(i2c_fd); - } -} - -int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) { - std::lock_guard lk(m); - - int ret = 0; - - ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address)); - if (ret < 0) { goto fail; } - - ret = i2c_smbus_read_i2c_block_data(i2c_fd, register_address, len, buffer); - if ((ret < 0) || (ret != len)) { goto fail; } - -fail: - return ret; -} - -int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) { - std::lock_guard lk(m); - - int ret = 0; - - ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address)); - if (ret < 0) { goto fail; } - - ret = i2c_smbus_write_byte_data(i2c_fd, register_address, data); - if (ret < 0) { goto fail; } - -fail: - return ret; -} - -#else - -I2CBus::I2CBus(uint8_t bus_id) { - UNUSED(bus_id); - i2c_fd = -1; -} - -I2CBus::~I2CBus() {} - -int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) { - UNUSED(device_address); - UNUSED(register_address); - UNUSED(buffer); - UNUSED(len); - return -1; -} - -int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) { - UNUSED(device_address); - UNUSED(register_address); - UNUSED(data); - return -1; -} -#endif diff --git a/common/i2c.h b/common/i2c.h deleted file mode 100644 index ca0d4635b8..0000000000 --- a/common/i2c.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include -#include - -#include - -class I2CBus { - private: - int i2c_fd; - std::mutex m; - - public: - I2CBus(uint8_t bus_id); - ~I2CBus(); - - int read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len); - int set_register(uint8_t device_address, uint register_address, uint8_t data); -}; diff --git a/common/util.py b/common/util.py index 33885a9024..e6ddb46e7b 100644 --- a/common/util.py +++ b/common/util.py @@ -1,3 +1,25 @@ +import os +import subprocess + +def sudo_write(val: str, path: str) -> None: + try: + with open(path, 'w') as f: + f.write(str(val)) + except PermissionError: + os.system(f"sudo chmod a+w {path}") + try: + with open(path, 'w') as f: + f.write(str(val)) + except PermissionError: + # fallback for debugfs files + os.system(f"sudo su -c 'echo {val} > {path}'") + +def sudo_read(path: str) -> str: + try: + return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip() + except Exception: + return "" + class MovingAverage: def __init__(self, window_size: int): self.window_size: int = window_size diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index d0c725cacb..f98e3bd88c 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -40,14 +40,14 @@ PROCS = { "selfdrive.selfdrived.selfdrived": 16.0, "selfdrive.car.card": 26.0, "./loggerd": 14.0, - "./encoderd": 17.0, + "./encoderd": 13.0, "./camerad": 10.0, - "selfdrive.controls.plannerd": 9.0, + "selfdrive.controls.plannerd": 8.0, "./ui": 18.0, - "./sensord": 7.0, + "system.sensord.sensord": 13.0, "selfdrive.controls.radard": 2.0, "selfdrive.modeld.modeld": 22.0, - "selfdrive.modeld.dmonitoringmodeld": 21.0, + "selfdrive.modeld.dmonitoringmodeld": 18.0, "system.hardware.hardwared": 4.0, "selfdrive.locationd.calibrationd": 2.0, "selfdrive.locationd.torqued": 5.0, diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index f5991ec7fa..f46c94c4f0 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -8,6 +8,7 @@ from functools import cached_property, lru_cache from pathlib import Path from cereal import log +from openpilot.common.util import sudo_read, sudo_write from openpilot.common.gpio import gpio_set, gpio_init, get_irqs_for_action from openpilot.system.hardware.base import HardwareBase, LPABase, ThermalConfig, ThermalZone from openpilot.system.hardware.tici import iwlist @@ -61,25 +62,6 @@ MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5 MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14 -def sudo_write(val, path): - try: - with open(path, 'w') as f: - f.write(str(val)) - except PermissionError: - os.system(f"sudo chmod a+w {path}") - try: - with open(path, 'w') as f: - f.write(str(val)) - except PermissionError: - # fallback for debugfs files - os.system(f"sudo su -c 'echo {val} > {path}'") - -def sudo_read(path: str) -> str: - try: - return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip() - except Exception: - return "" - def affine_irq(val, action): irqs = get_irqs_for_action(action) if len(irqs) == 0: diff --git a/system/manager/process_config.py b/system/manager/process_config.py index e25d556037..738c20a85b 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -78,7 +78,7 @@ procs = [ PythonProcess("modeld", "selfdrive.modeld.modeld", only_onroad), PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)), - NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), + PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), diff --git a/system/sensord/.gitignore b/system/sensord/.gitignore deleted file mode 100644 index e17675e254..0000000000 --- a/system/sensord/.gitignore +++ /dev/null @@ -1 +0,0 @@ -sensord diff --git a/system/sensord/SConscript b/system/sensord/SConscript deleted file mode 100644 index 1222f0bfcd..0000000000 --- a/system/sensord/SConscript +++ /dev/null @@ -1,13 +0,0 @@ -Import('env', 'arch', 'common', 'messaging') - -sensors = [ - 'sensors/i2c_sensor.cc', - 'sensors/lsm6ds3_accel.cc', - 'sensors/lsm6ds3_gyro.cc', - 'sensors/lsm6ds3_temp.cc', - 'sensors/mmc5603nj_magn.cc', -] -libs = [common, messaging, 'pthread'] -if arch == "larch64": - libs.append('i2c') -env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/system/sensord/sensord.py b/system/sensord/sensord.py new file mode 100755 index 0000000000..ed9d0ed415 --- /dev/null +++ b/system/sensord/sensord.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +import os +import time +import ctypes +import select +import threading + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.util import sudo_write +from openpilot.common.realtime import config_realtime_process, Ratekeeper +from openpilot.common.swaglog import cloudlog +from openpilot.common.gpio import gpiochip_get_ro_value_fd, gpioevent_data + +from openpilot.system.sensord.sensors.i2c_sensor import Sensor +from openpilot.system.sensord.sensors.lsm6ds3_accel import LSM6DS3_Accel +from openpilot.system.sensord.sensors.lsm6ds3_gyro import LSM6DS3_Gyro +from openpilot.system.sensord.sensors.lsm6ds3_temp import LSM6DS3_Temp +from openpilot.system.sensord.sensors.mmc5603nj_magn import MMC5603NJ_Magn + +I2C_BUS_IMU = 1 + +def interrupt_loop(sensors: list[tuple[Sensor, str, bool]], event) -> None: + pm = messaging.PubMaster([service for sensor, service, interrupt in sensors if interrupt]) + + # Requesting both edges as the data ready pulse from the lsm6ds sensor is + # very short (75us) and is mostly detected as falling edge instead of rising. + # So if it is detected as rising the following falling edge is skipped. + fd = gpiochip_get_ro_value_fd("sensord", 0, 84) + + # Configure IRQ affinity + irq_path = "/proc/irq/336/smp_affinity_list" + if not os.path.exists(irq_path): + irq_path = "/proc/irq/335/smp_affinity_list" + if os.path.exists(irq_path): + sudo_write('1\n', irq_path) + + offset = time.time_ns() - time.monotonic_ns() + + poller = select.poll() + poller.register(fd, select.POLLIN | select.POLLPRI) + while not event.is_set(): + events = poller.poll(100) + if not events: + cloudlog.error("poll timed out") + continue + if not (events[0][1] & (select.POLLIN | select.POLLPRI)): + cloudlog.error("no poll events set") + continue + + dat = os.read(fd, ctypes.sizeof(gpioevent_data)*16) + evd = gpioevent_data.from_buffer_copy(dat) + + cur_offset = time.time_ns() - time.monotonic_ns() + if abs(cur_offset - offset) > 10 * 1e6: # ms + cloudlog.warning(f"time jumped: {cur_offset} {offset}") + offset = cur_offset + continue + + ts = evd.timestamp - cur_offset + for sensor, service, interrupt in sensors: + if interrupt: + try: + evt = sensor.get_event(ts) + if not sensor.is_data_valid(): + continue + msg = messaging.new_message(service, valid=True) + setattr(msg, service, evt) + pm.send(service, msg) + except Sensor.DataNotReady: + pass + except Exception: + cloudlog.exception(f"Error processing {service}") + + +def polling_loop(sensor: Sensor, service: str, event: threading.Event) -> None: + pm = messaging.PubMaster([service]) + rk = Ratekeeper(SERVICE_LIST[service].frequency, print_delay_threshold=None) + while not event.is_set(): + try: + evt = sensor.get_event() + if not sensor.is_data_valid(): + continue + msg = messaging.new_message(service, valid=True) + setattr(msg, service, evt) + pm.send(service, msg) + except Exception: + cloudlog.exception(f"Error in {service} polling loop") + rk.keep_time() + +def main() -> None: + config_realtime_process([1, ], 1) + + sensors_cfg = [ + (LSM6DS3_Accel(I2C_BUS_IMU), "accelerometer", True), + (LSM6DS3_Gyro(I2C_BUS_IMU), "gyroscope", True), + (LSM6DS3_Temp(I2C_BUS_IMU), "temperatureSensor", False), + (MMC5603NJ_Magn(I2C_BUS_IMU), "magnetometer", False), + ] + + # Initialize sensors + exit_event = threading.Event() + threads = [ + threading.Thread(target=interrupt_loop, args=(sensors_cfg, exit_event), daemon=True) + ] + for sensor, service, interrupt in sensors_cfg: + try: + sensor.init() + if not interrupt: + # Start polling thread for sensors without interrupts + threads.append(threading.Thread( + target=polling_loop, + args=(sensor, service, exit_event), + daemon=True + )) + except Exception: + cloudlog.exception(f"Error initializing {service} sensor") + + try: + for t in threads: + t.start() + while any(t.is_alive() for t in threads): + time.sleep(1) + except KeyboardInterrupt: + pass + finally: + exit_event.set() + for t in threads: + if t.is_alive(): + t.join() + + for sensor, _, _ in sensors_cfg: + try: + sensor.shutdown() + except Exception: + cloudlog.exception("Error shutting down sensor") + +if __name__ == "__main__": + main() diff --git a/system/sensord/sensors/__init__.py b/system/sensord/sensors/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/sensord/sensors/constants.h b/system/sensord/sensors/constants.h deleted file mode 100644 index c216f838a5..0000000000 --- a/system/sensord/sensors/constants.h +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - - -#define SENSOR_ACCELEROMETER 1 -#define SENSOR_MAGNETOMETER 2 -#define SENSOR_MAGNETOMETER_UNCALIBRATED 3 -#define SENSOR_GYRO 4 -#define SENSOR_GYRO_UNCALIBRATED 5 -#define SENSOR_LIGHT 7 - -#define SENSOR_TYPE_ACCELEROMETER 1 -#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2 -#define SENSOR_TYPE_GYROSCOPE 4 -#define SENSOR_TYPE_LIGHT 5 -#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13 -#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14 -#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD -#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16 diff --git a/system/sensord/sensors/i2c_sensor.cc b/system/sensord/sensors/i2c_sensor.cc deleted file mode 100644 index 90220f551d..0000000000 --- a/system/sensord/sensors/i2c_sensor.cc +++ /dev/null @@ -1,50 +0,0 @@ -#include "system/sensord/sensors/i2c_sensor.h" - -int16_t read_12_bit(uint8_t lsb, uint8_t msb) { - uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0); - return int16_t(combined) / (1 << 4); -} - -int16_t read_16_bit(uint8_t lsb, uint8_t msb) { - uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb); - return int16_t(combined); -} - -int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) { - uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2); - return int32_t(combined) / (1 << 4); -} - -I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) : - bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {} - -I2CSensor::~I2CSensor() { - if (gpio_fd != -1) { - close(gpio_fd); - } -} - -int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) { - return bus->read_register(get_device_address(), register_address, buffer, len); -} - -int I2CSensor::set_register(uint register_address, uint8_t data) { - return bus->set_register(get_device_address(), register_address, data); -} - -int I2CSensor::init_gpio() { - if (shared_gpio || gpio_nr == 0) { - return 0; - } - - gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr); - if (gpio_fd < 0) { - return -1; - } - - return 0; -} - -bool I2CSensor::has_interrupt_enabled() { - return gpio_nr != 0; -} diff --git a/system/sensord/sensors/i2c_sensor.h b/system/sensord/sensors/i2c_sensor.h deleted file mode 100644 index e6d328ce72..0000000000 --- a/system/sensord/sensors/i2c_sensor.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include -#include -#include -#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 &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; - } -}; diff --git a/system/sensord/sensors/i2c_sensor.py b/system/sensord/sensors/i2c_sensor.py new file mode 100644 index 0000000000..0e15a6622b --- /dev/null +++ b/system/sensord/sensors/i2c_sensor.py @@ -0,0 +1,72 @@ +import time +import smbus2 +import ctypes +from collections.abc import Iterable + +from cereal import log + +class Sensor: + class SensorException(Exception): + pass + + class DataNotReady(SensorException): + pass + + def __init__(self, bus: int) -> None: + self.bus = smbus2.SMBus(bus) + self.source = log.SensorEventData.SensorSource.velodyne # unknown + self.start_ts = 0. + + def __del__(self): + self.bus.close() + + def read(self, addr: int, length: int) -> bytes: + return bytes(self.bus.read_i2c_block_data(self.device_address, addr, length)) + + def write(self, addr: int, data: int) -> None: + self.bus.write_byte_data(self.device_address, addr, data) + + def writes(self, writes: Iterable[tuple[int, int]]) -> None: + for addr, data in writes: + self.write(addr, data) + + def verify_chip_id(self, address: int, expected_ids: list[int]) -> int: + chip_id = self.read(address, 1)[0] + assert chip_id in expected_ids + return chip_id + + # Abstract methods that must be implemented by subclasses + @property + def device_address(self) -> int: + raise NotImplementedError + + def init(self) -> None: + raise NotImplementedError + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + raise NotImplementedError + + def shutdown(self) -> None: + raise NotImplementedError + + def is_data_valid(self) -> bool: + if self.start_ts == 0: + self.start_ts = time.monotonic() + + # unclear whether we need this... + return (time.monotonic() - self.start_ts) > 0.5 + + # *** helpers *** + @staticmethod + def wait(): + # a standard small sleep + time.sleep(0.005) + + @staticmethod + def parse_16bit(lsb: int, msb: int) -> int: + return ctypes.c_int16((msb << 8) | lsb).value + + @staticmethod + def parse_20bit(b2: int, b1: int, b0: int) -> int: + combined = ctypes.c_uint32((b0 << 16) | (b1 << 8) | b2).value + return ctypes.c_int32(combined).value // (1 << 4) diff --git a/system/sensord/sensors/lsm6ds3_accel.cc b/system/sensord/sensors/lsm6ds3_accel.cc deleted file mode 100644 index 03533e0657..0000000000 --- a/system/sensord/sensors/lsm6ds3_accel.cc +++ /dev/null @@ -1,250 +0,0 @@ -#include "system/sensord/sensors/lsm6ds3_accel.h" - -#include -#include -#include - -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" - -LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : - I2CSensor(bus, gpio_nr, shared_gpio) {} - -void LSM6DS3_Accel::wait_for_data_ready() { - uint8_t drdy = 0; - uint8_t buffer[6]; - - do { - read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); - drdy &= LSM6DS3_ACCEL_DRDY_XLDA; - } while (drdy == 0); - - read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); -} - -void LSM6DS3_Accel::read_and_avg_data(float* out_buf) { - uint8_t drdy = 0; - uint8_t buffer[6]; - - float scaling = 0.061f; - if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { - scaling = 0.122f; - } - - for (int i = 0; i < 5; i++) { - do { - read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); - drdy &= LSM6DS3_ACCEL_DRDY_XLDA; - } while (drdy == 0); - - int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - for (int j = 0; j < 3; j++) { - out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling; - } - } - - for (int i = 0; i < 3; i++) { - out_buf[i] /= 5.0f; - } -} - -int LSM6DS3_Accel::self_test(int test_type) { - float val_st_off[3] = {0}; - float val_st_on[3] = {0}; - float test_val[3] = {0}; - uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz - - // prepare sensor for self-test - - // enable block data update and automatic increment - int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU); - if (ret < 0) { - return ret; - } - - if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { - ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ; - } - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO); - if (ret < 0) { - return ret; - } - - // wait for stable output, and discard first values - util::sleep_for(100); - wait_for_data_ready(); - read_and_avg_data(val_st_off); - - // enable Self Test positive (or negative) - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type); - if (ret < 0) { - return ret; - } - - // wait for stable output, and discard first values - util::sleep_for(100); - wait_for_data_ready(); - read_and_avg_data(val_st_on); - - // disable sensor - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0); - if (ret < 0) { - return ret; - } - - // disable self test - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0); - if (ret < 0) { - return ret; - } - - // calculate the mg values for self test - for (int i = 0; i < 3; i++) { - test_val[i] = fabs(val_st_on[i] - val_st_off[i]); - } - - // verify test result - for (int i = 0; i < 3; i++) { - if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) || - (test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) { - return -1; - } - } - - return ret; -} - -int LSM6DS3_Accel::init() { - uint8_t value = 0; - bool do_self_test = false; - - const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST"); - if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { - do_self_test = true; - } - - int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID}); - if (ret == -1) return -1; - - if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) { - source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; - } - - ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST); - if (ret < 0) { - LOGE("LSM6DS3 accel positive self-test failed!"); - if (do_self_test) goto fail; - } - - ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST); - if (ret < 0) { - LOGE("LSM6DS3 accel negative self-test failed!"); - if (do_self_test) goto fail; - } - - ret = init_gpio(); - if (ret < 0) { - goto fail; - } - - // enable continuous update, and automatic increase - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC); - if (ret < 0) { - goto fail; - } - - // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ); - if (ret < 0) { - goto fail; - } - - ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE); - if (ret < 0) { - goto fail; - } - - // enable data ready interrupt for accel on INT1 - // (without resetting existing interrupts) - ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); - if (ret < 0) { - goto fail; - } - - value |= LSM6DS3_ACCEL_INT1_DRDY_XL; - ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); - -fail: - return ret; -} - -int LSM6DS3_Accel::shutdown() { - int ret = 0; - - // disable data ready interrupt for accel on INT1 - uint8_t value = 0; - ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); - if (ret < 0) { - goto fail; - } - - value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL); - ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); - if (ret < 0) { - LOGE("Could not disable lsm6ds3 acceleration interrupt!"); - goto fail; - } - - // enable power-down mode - value = 0; - ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1); - if (ret < 0) { - goto fail; - } - - value &= 0x0F; - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value); - if (ret < 0) { - LOGE("Could not power-down lsm6ds3 accelerometer!"); - goto fail; - } - -fail: - return ret; -} - -bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) { - - // INT1 shared with gyro, check STATUS_REG who triggered - uint8_t status_reg = 0; - read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); - if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) { - return false; - } - - uint8_t buffer[6]; - int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - float scale = 9.81 * 2.0f / (1 << 15); - float x = read_16_bit(buffer[0], buffer[1]) * scale; - float y = read_16_bit(buffer[2], buffer[3]) * scale; - float z = read_16_bit(buffer[4], buffer[5]) * scale; - - auto event = msg.initEvent().initAccelerometer(); - event.setSource(source); - event.setVersion(1); - event.setSensor(SENSOR_ACCELEROMETER); - event.setType(SENSOR_TYPE_ACCELEROMETER); - event.setTimestamp(ts); - - float xyz[] = {y, -x, z}; - auto svec = event.initAcceleration(); - svec.setV(xyz); - svec.setStatus(true); - - return true; -} diff --git a/system/sensord/sensors/lsm6ds3_accel.h b/system/sensord/sensors/lsm6ds3_accel.h deleted file mode 100644 index 69667cb759..0000000000 --- a/system/sensord/sensors/lsm6ds3_accel.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define LSM6DS3_ACCEL_I2C_ADDR 0x6A - -// Registers of the chip -#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B -#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F -#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D -#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10 -#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C 0x12 -#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C 0x14 -#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL 0x18 -#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E -#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28 - -// Constants -#define LSM6DS3_ACCEL_CHIP_ID 0x69 -#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A -#define LSM6DS3_ACCEL_FS_4G (0b10 << 2) -#define LSM6DS3_ACCEL_ODR_52HZ (0b0011 << 4) -#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) -#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1 -#define LSM6DS3_ACCEL_DRDY_XLDA 0b1 -#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7) -#define LSM6DS3_ACCEL_IF_INC 0b00000100 -#define LSM6DS3_ACCEL_IF_INC_BDU 0b01000100 -#define LSM6DS3_ACCEL_XYZ_DEN 0b11100000 -#define LSM6DS3_ACCEL_POSITIVE_TEST 0b01 -#define LSM6DS3_ACCEL_NEGATIVE_TEST 0b10 -#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f -#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f - -class LSM6DS3_Accel : public I2CSensor { - uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} - cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; - - // self test functions - int self_test(int test_type); - void wait_for_data_ready(); - void read_and_avg_data(float* val_st_off); -public: - LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/lsm6ds3_accel.py b/system/sensord/sensors/lsm6ds3_accel.py new file mode 100644 index 0000000000..2d788fcbe2 --- /dev/null +++ b/system/sensord/sensors/lsm6ds3_accel.py @@ -0,0 +1,157 @@ +import os +import time + +from cereal import log +from openpilot.system.sensord.sensors.i2c_sensor import Sensor + +class LSM6DS3_Accel(Sensor): + LSM6DS3_ACCEL_I2C_REG_DRDY_CFG = 0x0B + LSM6DS3_ACCEL_I2C_REG_INT1_CTRL = 0x0D + LSM6DS3_ACCEL_I2C_REG_CTRL1_XL = 0x10 + LSM6DS3_ACCEL_I2C_REG_CTRL3_C = 0x12 + LSM6DS3_ACCEL_I2C_REG_CTRL5_C = 0x14 + LSM6DS3_ACCEL_I2C_REG_STAT_REG = 0x1E + LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL = 0x28 + + LSM6DS3_ACCEL_ODR_104HZ = (0b0100 << 4) + LSM6DS3_ACCEL_INT1_DRDY_XL = 0b1 + LSM6DS3_ACCEL_DRDY_XLDA = 0b1 + LSM6DS3_ACCEL_DRDY_PULSE_MODE = (1 << 7) + LSM6DS3_ACCEL_IF_INC = 0b00000100 + + LSM6DS3_ACCEL_ODR_52HZ = (0b0011 << 4) + LSM6DS3_ACCEL_FS_4G = (0b10 << 2) + LSM6DS3_ACCEL_IF_INC_BDU = 0b01000100 + LSM6DS3_ACCEL_POSITIVE_TEST = 0b01 + LSM6DS3_ACCEL_NEGATIVE_TEST = 0b10 + LSM6DS3_ACCEL_MIN_ST_LIMIT_mg = 90.0 + LSM6DS3_ACCEL_MAX_ST_LIMIT_mg = 1700.0 + + @property + def device_address(self) -> int: + return 0x6A + + def init(self): + chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) + if chip_id == 0x6A: + self.source = log.SensorEventData.SensorSource.lsm6ds3trc + else: + self.source = log.SensorEventData.SensorSource.lsm6ds3 + + # self-test + if os.getenv("LSM_SELF_TEST") == "1": + self.self_test(self.LSM6DS3_ACCEL_POSITIVE_TEST) + self.self_test(self.LSM6DS3_ACCEL_NEGATIVE_TEST) + + # actual init + int1 = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0] + int1 |= self.LSM6DS3_ACCEL_INT1_DRDY_XL + self.writes(( + # Enable continuous update and automatic address increment + (self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC), + # Set ODR to 104 Hz, FS to ±2g (default) + (self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, self.LSM6DS3_ACCEL_ODR_104HZ), + # Configure data ready signal to pulse mode + (self.LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, self.LSM6DS3_ACCEL_DRDY_PULSE_MODE), + # Enable data ready interrupt on INT1 without resetting existing interrupts + (self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, int1), + )) + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + assert ts is not None # must come from the IRQ event + + # Check if data is ready since IRQ is shared with gyro + status_reg = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0] + if (status_reg & self.LSM6DS3_ACCEL_DRDY_XLDA) == 0: + raise self.DataNotReady + + scale = 9.81 * 2.0 / (1 << 15) + b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6) + x = self.parse_16bit(b[0], b[1]) * scale + y = self.parse_16bit(b[2], b[3]) * scale + z = self.parse_16bit(b[4], b[5]) * scale + + event = log.SensorEventData.new_message() + event.timestamp = ts + event.version = 1 + event.sensor = 1 # SENSOR_ACCELEROMETER + event.type = 1 # SENSOR_TYPE_ACCELEROMETER + event.source = self.source + a = event.init('acceleration') + a.v = [y, -x, z] + a.status = 1 + return event + + def shutdown(self) -> None: + # Disable data ready interrupt on INT1 + value = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0] + value &= ~self.LSM6DS3_ACCEL_INT1_DRDY_XL + self.write(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value) + + # Power down by clearing ODR bits + value = self.read(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 1)[0] + value &= 0x0F + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value) + + # *** self-test stuff *** + def _wait_for_data_ready(self): + while True: + drdy = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0] + if drdy & self.LSM6DS3_ACCEL_DRDY_XLDA: + break + + def _read_and_avg_data(self, scaling: float) -> list[float]: + out_buf = [0.0, 0.0, 0.0] + for _ in range(5): + self._wait_for_data_ready() + b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6) + for j in range(3): + val = self.parse_16bit(b[j*2], b[j*2+1]) * scaling + out_buf[j] += val + return [x / 5.0 for x in out_buf] + + def self_test(self, test_type: int) -> None: + # Prepare sensor for self-test + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC_BDU) + + # Configure ODR and full scale based on sensor type + if self.source == log.SensorEventData.SensorSource.lsm6ds3trc: + odr_fs = self.LSM6DS3_ACCEL_FS_4G | self.LSM6DS3_ACCEL_ODR_52HZ + scaling = 0.122 # mg/LSB for ±4g + else: + odr_fs = self.LSM6DS3_ACCEL_ODR_52HZ + scaling = 0.061 # mg/LSB for ±2g + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, odr_fs) + + # Wait for stable output + time.sleep(0.1) + self._wait_for_data_ready() + val_st_off = self._read_and_avg_data(scaling) + + # Enable self-test + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type) + + # Wait for stable output + time.sleep(0.1) + self._wait_for_data_ready() + val_st_on = self._read_and_avg_data(scaling) + + # Disable sensor and self-test + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0) + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0) + + # Calculate differences and check limits + test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)] + for val in test_val: + if val < self.LSM6DS3_ACCEL_MIN_ST_LIMIT_mg or val > self.LSM6DS3_ACCEL_MAX_ST_LIMIT_mg: + raise self.SensorException(f"Accelerometer self-test failed for test type {test_type}") + +if __name__ == "__main__": + import numpy as np + s = LSM6DS3_Accel(1) + s.init() + time.sleep(0.2) + e = s.get_event(0) + print(e) + print(np.linalg.norm(e.acceleration.v)) + s.shutdown() diff --git a/system/sensord/sensors/lsm6ds3_gyro.cc b/system/sensord/sensors/lsm6ds3_gyro.cc deleted file mode 100644 index bb560edeab..0000000000 --- a/system/sensord/sensors/lsm6ds3_gyro.cc +++ /dev/null @@ -1,233 +0,0 @@ -#include "system/sensord/sensors/lsm6ds3_gyro.h" - -#include -#include -#include - -#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; -} diff --git a/system/sensord/sensors/lsm6ds3_gyro.h b/system/sensord/sensors/lsm6ds3_gyro.h deleted file mode 100644 index adaae62dd2..0000000000 --- a/system/sensord/sensors/lsm6ds3_gyro.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define LSM6DS3_GYRO_I2C_ADDR 0x6A - -// Registers of the chip -#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B -#define LSM6DS3_GYRO_I2C_REG_ID 0x0F -#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D -#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11 -#define LSM6DS3_GYRO_I2C_REG_CTRL5_C 0x14 -#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E -#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22 -#define LSM6DS3_GYRO_POSITIVE_TEST (0b01 << 2) -#define LSM6DS3_GYRO_NEGATIVE_TEST (0b11 << 2) - -// Constants -#define LSM6DS3_GYRO_CHIP_ID 0x69 -#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A -#define LSM6DS3_GYRO_FS_2000dps (0b11 << 2) -#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) -#define LSM6DS3_GYRO_ODR_208HZ (0b0101 << 4) -#define LSM6DS3_GYRO_INT1_DRDY_G 0b10 -#define LSM6DS3_GYRO_DRDY_GDA 0b10 -#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7) -#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f -#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f - - -class LSM6DS3_Gyro : public I2CSensor { - uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} - cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; - - // self test functions - int self_test(int test_type); - void wait_for_data_ready(); - void read_and_avg_data(float* val_st_off); -public: - LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/lsm6ds3_gyro.py b/system/sensord/sensors/lsm6ds3_gyro.py new file mode 100644 index 0000000000..68fd267df2 --- /dev/null +++ b/system/sensord/sensors/lsm6ds3_gyro.py @@ -0,0 +1,141 @@ +import os +import math +import time + +from cereal import log +from openpilot.system.sensord.sensors.i2c_sensor import Sensor + +class LSM6DS3_Gyro(Sensor): + LSM6DS3_GYRO_I2C_REG_DRDY_CFG = 0x0B + LSM6DS3_GYRO_I2C_REG_INT1_CTRL = 0x0D + LSM6DS3_GYRO_I2C_REG_CTRL2_G = 0x11 + LSM6DS3_GYRO_I2C_REG_CTRL5_C = 0x14 + LSM6DS3_GYRO_I2C_REG_STAT_REG = 0x1E + LSM6DS3_GYRO_I2C_REG_OUTX_L_G = 0x22 + + LSM6DS3_GYRO_ODR_104HZ = (0b0100 << 4) + LSM6DS3_GYRO_INT1_DRDY_G = 0b10 + LSM6DS3_GYRO_DRDY_GDA = 0b10 + LSM6DS3_GYRO_DRDY_PULSE_MODE = (1 << 7) + + LSM6DS3_GYRO_ODR_208HZ = (0b0101 << 4) + LSM6DS3_GYRO_FS_2000dps = (0b11 << 2) + LSM6DS3_GYRO_POSITIVE_TEST = (0b01 << 2) + LSM6DS3_GYRO_NEGATIVE_TEST = (0b11 << 2) + LSM6DS3_GYRO_MIN_ST_LIMIT_mdps = 150000.0 + LSM6DS3_GYRO_MAX_ST_LIMIT_mdps = 700000.0 + + @property + def device_address(self) -> int: + return 0x6A + + def init(self): + chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) + if chip_id == 0x6A: + self.source = log.SensorEventData.SensorSource.lsm6ds3trc + else: + self.source = log.SensorEventData.SensorSource.lsm6ds3 + + # self-test + if "LSM_SELF_TEST" in os.environ: + self.self_test(self.LSM6DS3_GYRO_POSITIVE_TEST) + self.self_test(self.LSM6DS3_GYRO_NEGATIVE_TEST) + + # actual init + self.writes(( + # TODO: set scale. Default is +- 250 deg/s + (self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_104HZ), + # Configure data ready signal to pulse mode + (self.LSM6DS3_GYRO_I2C_REG_DRDY_CFG, self.LSM6DS3_GYRO_DRDY_PULSE_MODE), + )) + value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0] + value |= self.LSM6DS3_GYRO_INT1_DRDY_G + self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value) + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + assert ts is not None # must come from the IRQ event + + # Check if gyroscope data is ready, since it's shared with accelerometer + status_reg = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0] + if not (status_reg & self.LSM6DS3_GYRO_DRDY_GDA): + raise self.DataNotReady + + b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6) + x = self.parse_16bit(b[0], b[1]) + y = self.parse_16bit(b[2], b[3]) + z = self.parse_16bit(b[4], b[5]) + scale = (8.75 / 1000.0) * (math.pi / 180.0) + xyz = [y * scale, -x * scale, z * scale] + + event = log.SensorEventData.new_message() + event.timestamp = ts + event.version = 2 + event.sensor = 5 # SENSOR_GYRO_UNCALIBRATED + event.type = 16 # SENSOR_TYPE_GYROSCOPE_UNCALIBRATED + event.source = self.source + g = event.init('gyroUncalibrated') + g.v = xyz + g.status = 1 + return event + + def shutdown(self) -> None: + # Disable data ready interrupt on INT1 + value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0] + value &= ~self.LSM6DS3_GYRO_INT1_DRDY_G + self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value) + + # Power down by clearing ODR bits + value = self.read(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 1)[0] + value &= 0x0F + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, value) + + # *** self-test stuff *** + def _wait_for_data_ready(self): + while True: + drdy = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0] + if drdy & self.LSM6DS3_GYRO_DRDY_GDA: + break + + def _read_and_avg_data(self) -> list[float]: + out_buf = [0.0, 0.0, 0.0] + for _ in range(5): + self._wait_for_data_ready() + b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6) + for j in range(3): + val = self.parse_16bit(b[j*2], b[j*2+1]) * 70.0 # mdps/LSB for 2000 dps + out_buf[j] += val + return [x / 5.0 for x in out_buf] + + def self_test(self, test_type: int): + # Set ODR to 208Hz, FS to 2000dps + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_208HZ | self.LSM6DS3_GYRO_FS_2000dps) + + # Wait for stable output + time.sleep(0.15) + self._wait_for_data_ready() + val_st_off = self._read_and_avg_data() + + # Enable self-test + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type) + + # Wait for stable output + time.sleep(0.05) + self._wait_for_data_ready() + val_st_on = self._read_and_avg_data() + + # Disable sensor and self-test + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0) + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0) + + # Calculate differences and check limits + test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)] + for val in test_val: + if val < self.LSM6DS3_GYRO_MIN_ST_LIMIT_mdps or val > self.LSM6DS3_GYRO_MAX_ST_LIMIT_mdps: + raise Exception(f"Gyroscope self-test failed for test type {test_type}") + +if __name__ == "__main__": + s = LSM6DS3_Gyro(1) + s.init() + time.sleep(0.1) + print(s.get_event(0)) + s.shutdown() diff --git a/system/sensord/sensors/lsm6ds3_temp.cc b/system/sensord/sensors/lsm6ds3_temp.cc deleted file mode 100644 index f481614154..0000000000 --- a/system/sensord/sensors/lsm6ds3_temp.cc +++ /dev/null @@ -1,37 +0,0 @@ -#include "system/sensord/sensors/lsm6ds3_temp.h" - -#include - -#include "common/swaglog.h" -#include "common/timing.h" - -LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {} - -int LSM6DS3_Temp::init() { - int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID}); - if (ret == -1) return -1; - - if (ret == LSM6DS3TRC_TEMP_CHIP_ID) { - source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; - } - return 0; -} - -bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) { - uint64_t start_time = nanos_since_boot(); - uint8_t buffer[2]; - int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f; - float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale; - - auto event = msg.initEvent().initTemperatureSensor(); - event.setSource(source); - event.setVersion(1); - event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); - event.setTimestamp(start_time); - event.setTemperature(temp); - - return true; -} diff --git a/system/sensord/sensors/lsm6ds3_temp.h b/system/sensord/sensors/lsm6ds3_temp.h deleted file mode 100644 index 1b5b621814..0000000000 --- a/system/sensord/sensors/lsm6ds3_temp.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define LSM6DS3_TEMP_I2C_ADDR 0x6A - -// Registers of the chip -#define LSM6DS3_TEMP_I2C_REG_ID 0x0F -#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20 - -// Constants -#define LSM6DS3_TEMP_CHIP_ID 0x69 -#define LSM6DS3TRC_TEMP_CHIP_ID 0x6A - - -class LSM6DS3_Temp : public I2CSensor { - uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;} - cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; - -public: - LSM6DS3_Temp(I2CBus *bus); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown() { return 0; } -}; diff --git a/system/sensord/sensors/lsm6ds3_temp.py b/system/sensord/sensors/lsm6ds3_temp.py new file mode 100644 index 0000000000..7d7b1c2ce1 --- /dev/null +++ b/system/sensord/sensors/lsm6ds3_temp.py @@ -0,0 +1,33 @@ +import time + +from cereal import log +from openpilot.system.sensord.sensors.i2c_sensor import Sensor + +# https://content.arduino.cc/assets/st_imu_lsm6ds3_datasheet.pdf +class LSM6DS3_Temp(Sensor): + @property + def device_address(self) -> int: + return 0x6A # Default I2C address for LSM6DS3 + + def _read_temperature(self) -> float: + scale = 16.0 if log.SensorEventData.SensorSource.lsm6ds3 else 256.0 + data = self.read(0x20, 2) + return 25 + (self.parse_16bit(data[0], data[1]) / scale) + + def init(self): + chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) + if chip_id == 0x6A: + self.source = log.SensorEventData.SensorSource.lsm6ds3trc + else: + self.source = log.SensorEventData.SensorSource.lsm6ds3 + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + event = log.SensorEventData.new_message() + event.version = 1 + event.timestamp = int(time.monotonic() * 1e9) + event.source = self.source + event.temperature = self._read_temperature() + return event + + def shutdown(self) -> None: + pass diff --git a/system/sensord/sensors/mmc5603nj_magn.cc b/system/sensord/sensors/mmc5603nj_magn.cc deleted file mode 100644 index 0e8ba967e3..0000000000 --- a/system/sensord/sensors/mmc5603nj_magn.cc +++ /dev/null @@ -1,108 +0,0 @@ -#include "system/sensord/sensors/mmc5603nj_magn.h" - -#include -#include -#include - -#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 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 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 xyz = MMC5603NJ_Magn::read_measurement(); - - set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET); - util::sleep_for(5); - MMC5603NJ_Magn::start_measurement(); - std::vector reset_xyz = MMC5603NJ_Magn::read_measurement(); - - auto event = msg.initEvent().initMagnetometer(); - event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ); - event.setVersion(1); - event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); - event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); - event.setTimestamp(start_time); - - float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]}; - bool valid = true; - if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) { - valid = false; - } - auto svec = event.initMagneticUncalibrated(); - svec.setV(vals); - svec.setStatus(valid); - return true; -} diff --git a/system/sensord/sensors/mmc5603nj_magn.h b/system/sensord/sensors/mmc5603nj_magn.h deleted file mode 100644 index 9c0fbd2521..0000000000 --- a/system/sensord/sensors/mmc5603nj_magn.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include - -#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 read_measurement(); -public: - MMC5603NJ_Magn(I2CBus *bus); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/mmc5603nj_magn.py b/system/sensord/sensors/mmc5603nj_magn.py new file mode 100644 index 0000000000..255e99eb3e --- /dev/null +++ b/system/sensord/sensors/mmc5603nj_magn.py @@ -0,0 +1,76 @@ +import time + +from cereal import log +from openpilot.system.sensord.sensors.i2c_sensor import Sensor + +# https://www.mouser.com/datasheet/2/821/Memsic_09102019_Datasheet_Rev.B-1635324.pdf + +# Register addresses +REG_ODR = 0x1A +REG_INTERNAL_0 = 0x1B +REG_INTERNAL_1 = 0x1C + +# Control register settings +CMM_FREQ_EN = (1 << 7) +AUTO_SR_EN = (1 << 5) +SET = (1 << 3) +RESET = (1 << 4) + +class MMC5603NJ_Magn(Sensor): + @property + def device_address(self) -> int: + return 0x30 + + def init(self): + self.verify_chip_id(0x39, [0x10, ]) + self.writes(( + (REG_ODR, 0), + + # Set BW to 0b01 for 1-150 Hz operation + (REG_INTERNAL_1, 0b01), + )) + + def _read_data(self, cycle) -> list[float]: + # start measurement + self.write(REG_INTERNAL_0, cycle) + self.wait() + + # read out XYZ + scale = 1.0 / 16384.0 + b = self.read(0x00, 9) + return [ + (self.parse_20bit(b[6], b[1], b[0]) * scale) - 32.0, + (self.parse_20bit(b[7], b[3], b[2]) * scale) - 32.0, + (self.parse_20bit(b[8], b[5], b[4]) * scale) - 32.0, + ] + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + ts = time.monotonic_ns() + + # SET - RESET cycle + xyz = self._read_data(SET) + reset_xyz = self._read_data(RESET) + vals = [*xyz, *reset_xyz] + + event = log.SensorEventData.new_message() + event.timestamp = ts + event.version = 1 + event.sensor = 3 # SENSOR_MAGNETOMETER_UNCALIBRATED + event.type = 14 # SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED + event.source = log.SensorEventData.SensorSource.mmc5603nj + + m = event.init('magneticUncalibrated') + m.v = vals + m.status = int(all(int(v) != -32 for v in vals)) + + return event + + def shutdown(self) -> None: + v = self.read(REG_INTERNAL_0, 1)[0] + self.writes(( + # disable auto-reset of measurements + (REG_INTERNAL_0, (v & (~(CMM_FREQ_EN | AUTO_SR_EN)))), + + # disable continuous mode + (REG_ODR, 0), + )) diff --git a/system/sensord/sensors/sensor.h b/system/sensord/sensors/sensor.h deleted file mode 100644 index ccf998d161..0000000000 --- a/system/sensord/sensors/sensor.h +++ /dev/null @@ -1,24 +0,0 @@ -#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; - } -}; diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc deleted file mode 100644 index 8ff9331b39..0000000000 --- a/system/sensord/sensors_qcom2.cc +++ /dev/null @@ -1,170 +0,0 @@ -#include - -#include -#include -#include -#include -#include -#include - -#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/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> 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> sensors_init = { - {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 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(I2C_BUS_IMU); - return sensor_loop(i2c_bus_imu.get()); - } catch (std::exception &e) { - LOGE("I2CBus init failed"); - return -1; - } -}