mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 00:03:45 +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
system/hardware/tici/hardware.py
+1 -19
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:
system/manager/process_config.py
+1 -1
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),
system/sensord/.gitignore
-1
View File
@@ -1 +0,0 @@
sensord
system/sensord/SConscript
-13
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)
system/sensord/sensord.py
Executable
+139
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()
system/sensord/sensors/__init__.py
View File
system/sensord/sensors/constants.h
-18
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
system/sensord/sensors/i2c_sensor.cc
-50
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;
}
system/sensord/sensors/i2c_sensor.h
-51
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;
}
};
system/sensord/sensors/i2c_sensor.py
+72
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)
system/sensord/sensors/lsm6ds3_accel.cc
-250
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;
}
system/sensord/sensors/lsm6ds3_accel.h
-49
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();
};
system/sensord/sensors/lsm6ds3_accel.py
+157
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()
system/sensord/sensors/lsm6ds3_gyro.cc
-233
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;
}
system/sensord/sensors/lsm6ds3_gyro.h
-45
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();
};
system/sensord/sensors/lsm6ds3_gyro.py
+141
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()
system/sensord/sensors/lsm6ds3_temp.cc
-37
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;
}
system/sensord/sensors/lsm6ds3_temp.h
-26
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; }
};
system/sensord/sensors/lsm6ds3_temp.py
+33
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
system/sensord/sensors/mmc5603nj_magn.cc
-108
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;
}
system/sensord/sensors/mmc5603nj_magn.h
-37
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();
};
system/sensord/sensors/mmc5603nj_magn.py
+76
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),
))
system/sensord/sensors/sensor.h
-24
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;
}
};
system/sensord/sensors_qcom2.cc
-170
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;
}
}