mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 14:13:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensord: rewrite in Python (#35353)

Browse Source 
* 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 <device@comma.ai>
This commit is contained in:
Adeeb Shihadeh
2025-05-30 13:31:07 -07:00
committed by GitHub
parent 45f90b1a55
commit 2f80854644
33 changed files with 681 additions and 1369 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
SConstruct
View File

@@ -348,7 +348,6 @@ SConscript([
])
if arch != "Darwin":
SConscript([
'system/sensord/SConscript',
'system/logcatd/SConscript',
])

4
common/SConscript
View File

@@ -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 = [

84
common/gpio.cc
View File

@@ -1,84 +0,0 @@
#include "common/gpio.h"
#include <string>
#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 <fcntl.h>
#include <unistd.h>
#include <cstring>
#include <linux/gpio.h>
#include <sys/ioctl.h>
#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

33
common/gpio.h
View File

@@ -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);

35
common/gpio.py
View File

@@ -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)

92
common/i2c.cc
View File

@@ -1,92 +0,0 @@
#include "common/i2c.h"
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <cassert>
#include <cstdio>
#include <stdexcept>
#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 <linux/i2c-dev.h>
#include <i2c/smbus.h>
}
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

19
common/i2c.h
View File

@@ -1,19 +0,0 @@
#pragma once
#include <cstdint>
#include <mutex>
#include <sys/types.h>
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);
};

22
common/util.py
View File

@@ -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

8
selfdrive/test/test_onroad.py
View File

@@ -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,

20
system/hardware/tici/hardware.py
View File

@@ -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:

2
system/manager/process_config.py
View File

@@ -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),

1
system/sensord/.gitignore vendored
View File

@@ -1 +0,0 @@
sensord

13
system/sensord/SConscript
View File

@@ -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)

139
system/sensord/sensord.py Executable file
View File

@@ -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()

0
system/sensord/sensors/__init__.py Normal file
View File

18
system/sensord/sensors/constants.h
View File

@@ -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

50
system/sensord/sensors/i2c_sensor.cc
View File

@@ -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;
}

51
system/sensord/sensors/i2c_sensor.h
View File

@@ -1,51 +0,0 @@
#pragma once
#include <cstdint>
#include <unistd.h>
#include <vector>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/i2c.h"
#include "common/gpio.h"
#include "common/swaglog.h"
#include "system/sensord/sensors/constants.h"
#include "system/sensord/sensors/sensor.h"
int16_t read_12_bit(uint8_t lsb, uint8_t msb);
int16_t read_16_bit(uint8_t lsb, uint8_t msb);
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0);
class I2CSensor : public Sensor {
private:
I2CBus *bus;
int gpio_nr;
bool shared_gpio;
virtual uint8_t get_device_address() = 0;
public:
I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
~I2CSensor();
int read_register(uint register_address, uint8_t *buffer, uint8_t len);
int set_register(uint register_address, uint8_t data);
int init_gpio();
bool has_interrupt_enabled();
virtual int init() = 0;
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
virtual int shutdown() = 0;
int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
uint8_t chip_id = 0;
int ret = read_register(address, &chip_id, 1);
if (ret < 0) {
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;
}
};

72
system/sensord/sensors/i2c_sensor.py Normal file
View File

@@ -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)

250
system/sensord/sensors/lsm6ds3_accel.cc
View File

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

49
system/sensord/sensors/lsm6ds3_accel.h
View File

@@ -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();
};

157
system/sensord/sensors/lsm6ds3_accel.py Normal file
View File

@@ -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()

233
system/sensord/sensors/lsm6ds3_gyro.cc
View File

@@ -1,233 +0,0 @@
#include "system/sensord/sensors/lsm6ds3_gyro.h"
#include <cassert>
#include <cmath>
#include <cstring>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#define DEG2RAD(x) ((x) * M_PI / 180.0)
LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) :
I2CSensor(bus, gpio_nr, shared_gpio) {}
void LSM6DS3_Gyro::wait_for_data_ready() {
uint8_t drdy = 0;
uint8_t buffer[6];
do {
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_GYRO_DRDY_GDA;
} while (drdy == 0);
read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
}
void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) {
uint8_t drdy = 0;
uint8_t buffer[6];
for (int i = 0; i < 5; i++) {
do {
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_GYRO_DRDY_GDA;
} while (drdy == 0);
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
for (int j = 0; j < 3; j++) {
out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f;
}
}
// calculate the mg average values
for (int i = 0; i < 3; i++) {
out_buf[i] /= 5.0f;
}
}
int LSM6DS3_Gyro::self_test(int test_type) {
float val_st_off[3] = {0};
float val_st_on[3] = {0};
float test_val[3] = {0};
// prepare sensor for self-test
// full scale: 2000dps, ODR: 208Hz
int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(150);
wait_for_data_ready();
read_and_avg_data(val_st_off);
// enable Self Test positive (or negative)
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(50);
wait_for_data_ready();
read_and_avg_data(val_st_on);
// disable sensor
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0);
if (ret < 0) {
return ret;
}
// disable self test
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0);
if (ret < 0) {
return ret;
}
// calculate the mg values for self test
for (int i = 0; i < 3; i++) {
test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
}
// verify test result
for (int i = 0; i < 3; i++) {
if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) ||
(test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) {
return -1;
}
}
return ret;
}
int LSM6DS3_Gyro::init() {
uint8_t value = 0;
bool do_self_test = false;
const char* env_lsm_selftest = 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;
}

45
system/sensord/sensors/lsm6ds3_gyro.h
View File

@@ -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();
};

141
system/sensord/sensors/lsm6ds3_gyro.py Normal file
View File

@@ -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()

37
system/sensord/sensors/lsm6ds3_temp.cc
View File

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

26
system/sensord/sensors/lsm6ds3_temp.h
View File

@@ -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; }
};

33
system/sensord/sensors/lsm6ds3_temp.py Normal file
View File

@@ -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

108
system/sensord/sensors/mmc5603nj_magn.cc
View File

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

37
system/sensord/sensors/mmc5603nj_magn.h
View File

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

76
system/sensord/sensors/mmc5603nj_magn.py Normal file
View File

@@ -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),
))

24
system/sensord/sensors/sensor.h
View File

@@ -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;
}
};

170
system/sensord/sensors_qcom2.cc
View File

@@ -1,170 +0,0 @@
#include <sys/resource.h>
#include <chrono>
#include <thread>
#include <vector>
#include <map>
#include <poll.h>
#include <linux/gpio.h>
#include "cereal/services.h"
#include "cereal/messaging/messaging.h"
#include "common/i2c.h"
#include "common/ratekeeper.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "system/sensord/sensors/constants.h"
#include "system/sensord/sensors/lsm6ds3_accel.h"
#include "system/sensord/sensors/lsm6ds3_gyro.h"
#include "system/sensord/sensors/lsm6ds3_temp.h"
#include "system/sensord/sensors/mmc5603nj_magn.h"
#define I2C_BUS_IMU 1
ExitHandler do_exit;
void interrupt_loop(std::vector<std::tuple<Sensor *, std::string>> sensors) {
PubMaster pm({"gyroscope", "accelerometer"});
int fd = -1;
for (auto &[sensor, msg_name] : sensors) {
if (sensor->has_interrupt_enabled()) {
fd = sensor->gpio_fd;
break;
}
}
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 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;
}
}