sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

sensord: setup per-sensor threads (#29558)

Browse Source 
* sensord: setup per-sensor threads

* revert cereal

* more cleanup

* bump cereal

* update test

---------

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 6a66b75556
beeps
Adeeb Shihadeh 2 years ago committed by GitHub
parent 6b98b83ad4
commit 5c213d7a38
5 changed files with 65 additions and 94 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cereal
  2. 2
      system/sensord/sensors/lsm6ds3_accel.cc
  3. 8
      system/sensord/sensors/sensor.h
  4. 139
      system/sensord/sensors_qcom2.cc
  5. 2
      system/sensord/tests/test_sensord.py

2
cereal

@ -1 +1 @@
Subproject commit e7c42fd11cc71f16ac762e6aed3409ce4c644b42
Subproject commit 736a4cce2c51cb73465e88dae6ff4cec5b9c3b43

2
system/sensord/sensors/lsm6ds3_accel.cc
Unescape View File

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

8
system/sensord/sensors/sensor.h
Unescape View File

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

139
system/sensord/sensors_qcom2.cc
Unescape View File

@ -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<Sensor *>& sensors,
std::map<Sensor*, std::string>& sensor_service)
{
PubMaster pm_int({"gyroscope", "accelerometer"});
void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool, int>> 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,130 +71,94 @@ void interrupt_loop(std::vector<Sensor *>& 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) {
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*, std::string> 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<std::pair<Sensor *, bool>> 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<std::tuple<Sensor *, std::string, bool, int>> 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<Sensor *> sensors;
for (auto &[sensor, required] : sensors_init) {
std::vector<std::thread> 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;
continue;
}
if (!sensor->has_interrupt_enabled()) {
sensors.push_back(sensor);
}
threads.emplace_back(polling_loop, sensor, msg_name, polling_freq);
}
}
if (!has_magnetometer) {
LOGE("No magnetometer present");
return -1;
}
// increase interrupt quality by pinning interrupt and process to core 1
setpriority(PRIO_PROCESS, 0, -18);
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<Sensor *> 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;
// wait for all threads to finish
for (auto &t : threads) {
t.join();
}
pm_non_int.send(sensor_service[sensor].c_str(), msg);
for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) {
delete sensor;
}
rk.keepTime();
}
for (Sensor *sensor : sensors) {
sensor->shutdown();
}
lsm_interrupt_thread.join();
return 0;
}

2
system/sensord/tests/test_sensord.py
Unescape View File

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

Write Preview
Loading…
Cancel
Save