diff --git a/cereal b/cereal index e7c42fd11c..736a4cce2c 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e7c42fd11cc71f16ac762e6aed3409ce4c644b42 +Subproject commit 736a4cce2c51cb73465e88dae6ff4cec5b9c3b43 diff --git a/system/sensord/sensors/lsm6ds3_accel.cc b/system/sensord/sensors/lsm6ds3_accel.cc index 2a09702c96..139b5df67a 100644 --- a/system/sensord/sensors/lsm6ds3_accel.cc +++ b/system/sensord/sensors/lsm6ds3_accel.cc @@ -121,7 +121,7 @@ int LSM6DS3_Accel::init() { uint8_t value = 0; bool do_self_test = false; - const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST"); + 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; } diff --git a/system/sensord/sensors/sensor.h b/system/sensord/sensors/sensor.h index 2557155229..1b0e3be0dc 100644 --- a/system/sensord/sensors/sensor.h +++ b/system/sensord/sensors/sensor.h @@ -5,6 +5,7 @@ class Sensor { public: int gpio_fd = -1; + uint64_t start_ts = 0; uint64_t init_delay = 500e6; // default dealy 500ms virtual ~Sensor() {} virtual int init() = 0; @@ -12,7 +13,10 @@ public: virtual bool has_interrupt_enabled() = 0; virtual int shutdown() = 0; - virtual bool is_data_valid(uint64_t st, uint64_t ct) { - return (ct - st) > init_delay; + virtual bool is_data_valid(uint64_t current_ts) { + if (start_ts == 0) { + start_ts = current_ts; + } + return (current_ts - start_ts) > init_delay; } }; diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc index 9276a029c3..313dc50095 100644 --- a/system/sensord/sensors_qcom2.cc +++ b/system/sensord/sensors_qcom2.cc @@ -22,19 +22,22 @@ #include "system/sensord/sensors/lsm6ds3_gyro.h" #include "system/sensord/sensors/lsm6ds3_temp.h" #include "system/sensord/sensors/mmc5603nj_magn.h" -#include "system/sensord/sensors/sensor.h" #define I2C_BUS_IMU 1 ExitHandler do_exit; -uint64_t init_ts = 0; -void interrupt_loop(std::vector& sensors, - std::map& sensor_service) -{ - PubMaster pm_int({"gyroscope", "accelerometer"}); +void interrupt_loop(std::vector> sensors) { + PubMaster pm({"gyroscope", "accelerometer"}); + + int fd = -1; + for (auto &[sensor, msg_name, required, polling_freq] : sensors) { + if (sensor->has_interrupt_enabled()) { + fd = sensor->gpio_fd; + break; + } + } - int fd = sensors[0]->gpio_fd; struct pollfd fd_list[1] = {0}; fd_list[0].fd = fd; fd_list[0].events = POLLIN | POLLPRI; @@ -68,90 +71,76 @@ void interrupt_loop(std::vector& sensors, uint64_t offset = nanos_since_epoch() - nanos_since_boot(); uint64_t ts = evdata[num_events - 1].timestamp - offset; - for (Sensor *sensor : sensors) { + for (auto &[sensor, msg_name, required, polling_freq] : sensors) { + if (!sensor->has_interrupt_enabled()) { + continue; + } + MessageBuilder msg; if (!sensor->get_event(msg, ts)) { continue; } - if (!sensor->is_data_valid(init_ts, ts)) { + if (!sensor->is_data_valid(ts)) { continue; } - pm_int.send(sensor_service[sensor].c_str(), msg); + pm.send(msg_name.c_str(), msg); } } // poweroff sensors, disable interrupts - for (Sensor *sensor : sensors) { - sensor->shutdown(); + for (auto &[sensor, msg_name, required, polling_freq] : sensors) { + if (sensor->has_interrupt_enabled()) { + sensor->shutdown(); + } } } -int sensor_loop(I2CBus *i2c_bus_imu) { - BMX055_Accel bmx055_accel(i2c_bus_imu); - BMX055_Gyro bmx055_gyro(i2c_bus_imu); - BMX055_Magn bmx055_magn(i2c_bus_imu); - BMX055_Temp bmx055_temp(i2c_bus_imu); - - LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu, GPIO_LSM_INT); - LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu, GPIO_LSM_INT, true); // GPIO shared with accel - LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu); - - MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu); - - std::map sensor_service = { - {&bmx055_accel, "accelerometer2"}, - {&bmx055_gyro, "gyroscope2"}, - {&bmx055_magn, "magnetometer"}, - {&bmx055_temp, "temperatureSensor"}, - - {&lsm6ds3_accel, "accelerometer"}, - {&lsm6ds3_gyro, "gyroscope"}, - {&lsm6ds3_temp, "temperatureSensor"}, +void polling_loop(Sensor *sensor, std::string msg_name, int frequency) { + PubMaster pm({msg_name.c_str()}); + RateKeeper rk(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(); + } - {&mmc5603nj_magn, "magnetometer"}, - }; + sensor->shutdown(); +} +int sensor_loop(I2CBus *i2c_bus_imu) { // Sensor init - std::vector> sensors_init; // Sensor, required - sensors_init.push_back({&bmx055_accel, false}); - sensors_init.push_back({&bmx055_gyro, false}); - sensors_init.push_back({&bmx055_magn, false}); - sensors_init.push_back({&bmx055_temp, false}); - - sensors_init.push_back({&lsm6ds3_accel, true}); - sensors_init.push_back({&lsm6ds3_gyro, true}); - sensors_init.push_back({&lsm6ds3_temp, true}); + std::vector> sensors_init = { + {new BMX055_Accel(i2c_bus_imu), "accelerometer2", false, 100}, + {new BMX055_Gyro(i2c_bus_imu), "gyroscope2", false, 100}, + {new BMX055_Magn(i2c_bus_imu), "magnetometer", false, 100}, + {new BMX055_Temp(i2c_bus_imu), "temperatureSensor2", false, 100}, - sensors_init.push_back({&mmc5603nj_magn, false}); + {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer", true, 100}, + {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope", true, 100}, + {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor", true, 100}, - bool has_magnetometer = false; + {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer", false, 100}, + }; // Initialize sensors - std::vector sensors; - for (auto &[sensor, required] : sensors_init) { + std::vector threads; + for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) { int err = sensor->init(); if (err < 0) { if (required) { LOGE("Error initializing sensors"); return -1; } - } else { - - if (sensor == &bmx055_magn || sensor == &mmc5603nj_magn) { - has_magnetometer = true; - } - - if (!sensor->has_interrupt_enabled()) { - sensors.push_back(sensor); - } + continue; } - } - if (!has_magnetometer) { - LOGE("No magnetometer present"); - return -1; + if (!sensor->has_interrupt_enabled()) { + threads.emplace_back(polling_loop, sensor, msg_name, polling_freq); + } } // increase interrupt quality by pinning interrupt and process to core 1 @@ -159,39 +148,17 @@ int sensor_loop(I2CBus *i2c_bus_imu) { util::set_core_affinity({1}); std::system("sudo su -c 'echo 1 > /proc/irq/336/smp_affinity_list'"); - PubMaster pm_non_int({"gyroscope2", "accelerometer2", "temperatureSensor", "magnetometer"}); - init_ts = nanos_since_boot(); - // thread for reading events via interrupts - std::vector lsm_interrupt_sensors = {&lsm6ds3_accel, &lsm6ds3_gyro}; - std::thread lsm_interrupt_thread(&interrupt_loop, std::ref(lsm_interrupt_sensors), - std::ref(sensor_service)); - - RateKeeper rk("sensord", 100); - - // polling loop for non interrupt handled sensors - while (!do_exit) { - for (Sensor *sensor : sensors) { - MessageBuilder msg; - if (!sensor->get_event(msg)) { - continue; - } + threads.emplace_back(&interrupt_loop, std::ref(sensors_init)); - if (!sensor->is_data_valid(init_ts, nanos_since_boot())) { - continue; - } - - pm_non_int.send(sensor_service[sensor].c_str(), msg); - } - - rk.keepTime(); + // wait for all threads to finish + for (auto &t : threads) { + t.join(); } - for (Sensor *sensor : sensors) { - sensor->shutdown(); + for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) { + delete sensor; } - - lsm_interrupt_thread.join(); return 0; } diff --git a/system/sensord/tests/test_sensord.py b/system/sensord/tests/test_sensord.py index 87ead0bc1b..815dd8b444 100755 --- a/system/sensord/tests/test_sensord.py +++ b/system/sensord/tests/test_sensord.py @@ -71,7 +71,7 @@ def get_irq_count(irq: int): def read_sensor_events(duration_sec): sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2', - 'gyroscope2', 'temperatureSensor'] + 'gyroscope2', 'temperatureSensor', 'temperatureSensor2'] esocks = {} events = defaultdict(list) for stype in sensor_types: