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

sensord: lower temperature sensor freq to 2Hz (#29563)

Browse Source
pull/29564/head
Adeeb Shihadeh 2 years ago committed by GitHub
parent e726505918
commit 42769345d5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 44 additions and 60 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 37
      system/sensord/sensors_qcom2.cc
  2. 67
      system/sensord/tests/test_sensord.py

37
system/sensord/sensors_qcom2.cc
Unescape View File

@ -7,6 +7,7 @@
#include <poll.h> #include <poll.h>
#include <linux/gpio.h> #include <linux/gpio.h>
#include "cereal/services.h"
#include "cereal/messaging/messaging.h" #include "cereal/messaging/messaging.h"
#include "common/i2c.h" #include "common/i2c.h"
#include "common/ratekeeper.h" #include "common/ratekeeper.h"
@ -27,11 +28,11 @@
ExitHandler do_exit; ExitHandler do_exit;
void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool, int>> sensors) { void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool>> sensors) {
PubMaster pm({"gyroscope", "accelerometer"}); PubMaster pm({"gyroscope", "accelerometer"});
int fd = -1; int fd = -1;
for (auto &[sensor, msg_name, required, polling_freq] : sensors) { for (auto &[sensor, msg_name, required] : sensors) {
if (sensor->has_interrupt_enabled()) { if (sensor->has_interrupt_enabled()) {
fd = sensor->gpio_fd; fd = sensor->gpio_fd;
break; break;
@ -71,7 +72,7 @@ void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool, int>> se
uint64_t offset = nanos_since_epoch() - nanos_since_boot(); uint64_t offset = nanos_since_epoch() - nanos_since_boot();
uint64_t ts = evdata[num_events - 1].timestamp - offset; uint64_t ts = evdata[num_events - 1].timestamp - offset;
for (auto &[sensor, msg_name, required, polling_freq] : sensors) { for (auto &[sensor, msg_name, required] : sensors) {
if (!sensor->has_interrupt_enabled()) { if (!sensor->has_interrupt_enabled()) {
continue; continue;
} }
@ -90,16 +91,16 @@ void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool, int>> se
} }
// poweroff sensors, disable interrupts // poweroff sensors, disable interrupts
for (auto &[sensor, msg_name, required, polling_freq] : sensors) { for (auto &[sensor, msg_name, required] : sensors) {
if (sensor->has_interrupt_enabled()) { if (sensor->has_interrupt_enabled()) {
sensor->shutdown(); sensor->shutdown();
} }
} }
} }
void polling_loop(Sensor *sensor, std::string msg_name, int frequency) { void polling_loop(Sensor *sensor, std::string msg_name) {
PubMaster pm({msg_name.c_str()}); PubMaster pm({msg_name.c_str()});
RateKeeper rk(msg_name, frequency); RateKeeper rk(msg_name, services.at(msg_name).frequency);
while (!do_exit) { while (!do_exit) {
MessageBuilder msg; MessageBuilder msg;
if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) { if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) {
@ -113,22 +114,22 @@ void polling_loop(Sensor *sensor, std::string msg_name, int frequency) {
int sensor_loop(I2CBus *i2c_bus_imu) { int sensor_loop(I2CBus *i2c_bus_imu) {
// Sensor init // Sensor init
std::vector<std::tuple<Sensor *, std::string, bool, int>> sensors_init = { std::vector<std::tuple<Sensor *, std::string, bool>> sensors_init = {
{new BMX055_Accel(i2c_bus_imu), "accelerometer2", false, 100}, {new BMX055_Accel(i2c_bus_imu), "accelerometer2", false},
{new BMX055_Gyro(i2c_bus_imu), "gyroscope2", false, 100}, {new BMX055_Gyro(i2c_bus_imu), "gyroscope2", false},
{new BMX055_Magn(i2c_bus_imu), "magnetometer", false, 100}, {new BMX055_Magn(i2c_bus_imu), "magnetometer", false},
{new BMX055_Temp(i2c_bus_imu), "temperatureSensor2", false, 100}, {new BMX055_Temp(i2c_bus_imu), "temperatureSensor2", false},
{new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer", true, 100}, {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer", true},
{new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope", true, 100}, {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope", true},
{new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor", true, 100}, {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor", true},
{new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer", false, 100}, {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer", false},
}; };
// Initialize sensors // Initialize sensors
std::vector<std::thread> threads; std::vector<std::thread> threads;
for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) { for (auto &[sensor, msg_name, required] : sensors_init) {
int err = sensor->init(); int err = sensor->init();
if (err < 0) { if (err < 0) {
if (required) { if (required) {
@ -139,7 +140,7 @@ int sensor_loop(I2CBus *i2c_bus_imu) {
} }
if (!sensor->has_interrupt_enabled()) { if (!sensor->has_interrupt_enabled()) {
threads.emplace_back(polling_loop, sensor, msg_name, polling_freq); threads.emplace_back(polling_loop, sensor, msg_name);
} }
} }
@ -156,7 +157,7 @@ int sensor_loop(I2CBus *i2c_bus_imu) {
t.join(); t.join();
} }
for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) { for (auto &[sensor, msg_name, required] : sensors_init) {
delete sensor; delete sensor;
} }
return 0; return 0;

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

@ -7,6 +7,7 @@ from collections import namedtuple, defaultdict
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal import log from cereal import log
from cereal.services import service_list
from openpilot.common.gpio import get_irqs_for_action from openpilot.common.gpio import get_irqs_for_action
from openpilot.system.hardware import TICI from openpilot.system.hardware import TICI
from openpilot.selfdrive.manager.process_config import managed_processes from openpilot.selfdrive.manager.process_config import managed_processes
@ -37,29 +38,29 @@ SENSOR_CONFIGURATIONS = (
) )
Sensor = log.SensorEventData.SensorSource Sensor = log.SensorEventData.SensorSource
SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max', 'expected_freq']) SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
ALL_SENSORS = { ALL_SENSORS = {
Sensor.lsm6ds3: { Sensor.lsm6ds3: {
SensorConfig("acceleration", 5, 15, 100), SensorConfig("acceleration", 5, 15),
SensorConfig("gyroUncalibrated", 0, .2, 100), SensorConfig("gyroUncalibrated", 0, .2),
SensorConfig("temperature", 0, 60, 100), SensorConfig("temperature", 0, 60),
}, },
Sensor.lsm6ds3trc: { Sensor.lsm6ds3trc: {
SensorConfig("acceleration", 5, 15, 104), SensorConfig("acceleration", 5, 15),
SensorConfig("gyroUncalibrated", 0, .2, 104), SensorConfig("gyroUncalibrated", 0, .2),
SensorConfig("temperature", 0, 60, 100), SensorConfig("temperature", 0, 60),
}, },
Sensor.bmx055: { Sensor.bmx055: {
SensorConfig("acceleration", 5, 15, 100), SensorConfig("acceleration", 5, 15),
SensorConfig("gyroUncalibrated", 0, .2, 100), SensorConfig("gyroUncalibrated", 0, .2),
SensorConfig("magneticUncalibrated", 0, 300, 100), SensorConfig("magneticUncalibrated", 0, 300),
SensorConfig("temperature", 0, 60, 100), SensorConfig("temperature", 0, 60),
}, },
Sensor.mmc5603nj: { Sensor.mmc5603nj: {
SensorConfig("magneticUncalibrated", 0, 300, 100), SensorConfig("magneticUncalibrated", 0, 300),
} }
} }
@ -85,7 +86,7 @@ def read_sensor_events(duration_sec):
assert sum(map(len, events.values())) != 0, "No sensor events collected!" assert sum(map(len, events.values())) != 0, "No sensor events collected!"
return events return {k: v for k, v in events.items() if len(v) > 0}
class TestSensord(unittest.TestCase): class TestSensord(unittest.TestCase):
@classmethod @classmethod
@ -119,7 +120,6 @@ class TestSensord(unittest.TestCase):
def test_sensors_present(self): def test_sensors_present(self):
# verify correct sensors configuration # verify correct sensors configuration
seen = set() seen = set()
for etype in self.events: for etype in self.events:
for measurement in self.events[etype]: for measurement in self.events[etype]:
@ -159,22 +159,12 @@ class TestSensord(unittest.TestCase):
stddev = np.std(tdiffs) stddev = np.std(tdiffs)
assert stddev < 2.0, f"Standard-dev to big {stddev}" assert stddev < 2.0, f"Standard-dev to big {stddev}"
def test_events_check(self): def test_sensor_frequency(self):
# verify if all sensors produce events for s, msgs in self.events.items():
with self.subTest(sensor=s):
sensor_events = dict() freq = len(msgs) / self.sample_secs
for etype in self.events: ef = service_list[s].frequency
for measurement in self.events[etype]: assert ef*0.85 <= freq <= ef*1.15
m = getattr(measurement, measurement.which())
if m.type in sensor_events:
sensor_events[m.type] += 1
else:
sensor_events[m.type] = 1
for s in sensor_events:
err_msg = f"Sensor {s}: 200 < {sensor_events[s]}"
assert sensor_events[s] > 200, err_msg
def test_logmonottime_timestamp_diff(self): def test_logmonottime_timestamp_diff(self):
# ensure diff between the message logMonotime and sample timestamp is small # ensure diff between the message logMonotime and sample timestamp is small
@ -193,16 +183,14 @@ class TestSensord(unittest.TestCase):
# before the sensor is read # before the sensor is read
tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6) tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6)
high_delay_diffs = set(filter(lambda d: d >= 15., tdiffs)) # some sensors have a read procedure that will introduce an expected diff on the order of 20ms
assert len(high_delay_diffs) < 20, f"Too many measurements published : {high_delay_diffs}" high_delay_diffs = set(filter(lambda d: d >= 25., tdiffs))
assert len(high_delay_diffs) < 20, f"Too many measurements published: {high_delay_diffs}"
avg_diff = round(sum(tdiffs)/len(tdiffs), 4) avg_diff = round(sum(tdiffs)/len(tdiffs), 4)
assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms" assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms"
stddev = np.std(tdiffs) def test_sensor_values(self):
assert stddev < 2, f"Timing diffs have too high stddev: {stddev}"
def test_sensor_values_sanity_check(self):
sensor_values = dict() sensor_values = dict()
for etype in self.events: for etype in self.events:
for measurement in self.events[etype]: for measurement in self.events[etype]:
@ -219,18 +207,13 @@ class TestSensord(unittest.TestCase):
else: else:
sensor_values[key] = [values] sensor_values[key] = [values]
# Sanity check sensor values and counts # Sanity check sensor values
for sensor, stype in sensor_values: for sensor, stype in sensor_values:
for s in ALL_SENSORS[sensor]: for s in ALL_SENSORS[sensor]:
if s.type != stype: if s.type != stype:
continue continue
key = (sensor, s.type) key = (sensor, s.type)
val_cnt = len(sensor_values[key])
min_samples = self.sample_secs * s.expected_freq
err_msg = f"Sensor {sensor} {s.type} got {val_cnt} measurements, expected {min_samples}"
assert min_samples*0.9 < val_cnt < min_samples*1.1, err_msg
mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1)) mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1))
err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}" err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}"
assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg

Write Preview
Loading…
Cancel
Save