sensord: rewrite in Python (#35353)
* py sensord * fix up mmc * temp * port over accel * lil more * kinda works * rm that * gpiochip * mostly there * lil more * lil more * irq timestamps * fix ts * fix double deg2rad * test passes * fix up mypy * rm one more * exception * lint: * read in all events * bump that * get under budget: * accel self test * gyro self-test * keep these readable * give it more cores * debug * valid * rewrite that --------- Co-authored-by: Comma Device <device@comma.ai>pull/35394/head
parent
45f90b1a55
commit
2f80854644
33 changed files with 681 additions and 1369 deletions
@ -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 |
@ -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); |
@ -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 |
@ -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); |
||||
}; |
@ -1 +0,0 @@ |
||||
sensord |
@ -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) |
@ -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() |
@ -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 |
@ -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; |
||||
} |
@ -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; |
||||
} |
||||
}; |
@ -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) |
@ -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; |
||||
} |
@ -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(); |
||||
}; |
@ -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() |
@ -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; |
||||
} |
@ -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(); |
||||
}; |
@ -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() |
@ -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; |
||||
} |
@ -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; } |
||||
}; |
@ -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 |
@ -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; |
||||
} |
@ -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(); |
||||
}; |
@ -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), |
||||
)) |
@ -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; |
||||
} |
||||
}; |
@ -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; |
||||
} |
||||
} |
Loading…
Reference in new issue