diff --git a/cereal b/cereal index 5aa49864bc..d4cf8728e2 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 5aa49864bce38f520705b6ed0b98e7cf9560ed0a +Subproject commit d4cf8728e2fa2d87d90098efa7ddeaf8f98a03db diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 77e3703c2d..36062da1d1 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase): cloudlog.debug("Using Mock Car Interface") - self.sensor = messaging.sub_sock('sensorEvents') + self.gyro = messaging.sub_sock('gyroscope') self.gps = messaging.sub_sock('gpsLocationExternal') self.speed = 0. @@ -46,11 +46,9 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): # get basic data from phone and gps since CAN isn't connected - sensors = messaging.recv_sock(self.sensor) - if sensors is not None: - for sensor in sensors.sensorEvents: - if sensor.type == 4: # gyro - self.yaw_rate_meas = -sensor.gyro.v[0] + gyro_sensor = messaging.recv_sock(self.gyro) + if gyro_sensor is not None: + self.yaw_rate_meas = -gyro_sensor.gyroscope.gyroUncalibrated.v[0] gps = messaging.recv_sock(self.gps) if gps is not None: diff --git a/selfdrive/debug/sensor_data_to_hist.py b/selfdrive/debug/sensor_data_to_hist.py index 3c30b3c17c..ceed4b0ec3 100755 --- a/selfdrive/debug/sensor_data_to_hist.py +++ b/selfdrive/debug/sensor_data_to_hist.py @@ -7,6 +7,7 @@ get interrupts in a 2kHz rate. import argparse import sys +import numpy as np from collections import defaultdict from tools.lib.logreader import LogReader @@ -23,28 +24,22 @@ def parseEvents(log_reader): lsm_data = defaultdict(list) for m in log_reader: - # only sensorEvents - if m.which() != 'sensorEvents': + if m.which() not in ['accelerometer', 'gyroscope']: continue - for se in m.sensorEvents: - # convert data to dictionary - d = se.to_dict() + d = getattr(m, m.which()).to_dict() - if d["timestamp"] == 0: - continue # empty event? + if d["source"] == SRC_BMX and "acceleration" in d: + bmx_data["accel"].append(d["timestamp"] / 1e9) - if d["source"] == SRC_BMX and "acceleration" in d: - bmx_data["accel"].append(d["timestamp"] / 1e9) + if d["source"] == SRC_BMX and "gyroUncalibrated" in d: + bmx_data["gyro"].append(d["timestamp"] / 1e9) - if d["source"] == SRC_BMX and "gyroUncalibrated" in d: - bmx_data["gyro"].append(d["timestamp"] / 1e9) + if d["source"] == SRC_LSM and "acceleration" in d: + lsm_data["accel"].append(d["timestamp"] / 1e9) - if d["source"] == SRC_LSM and "acceleration" in d: - lsm_data["accel"].append(d["timestamp"] / 1e9) - - if d["source"] == SRC_LSM and "gyroUncalibrated" in d: - lsm_data["gyro"].append(d["timestamp"] / 1e9) + if d["source"] == SRC_LSM and "gyroUncalibrated" in d: + lsm_data["gyro"].append(d["timestamp"] / 1e9) return bmx_data, lsm_data @@ -54,11 +49,7 @@ def cleanData(data): return [], [] data.sort() - prev = data[0] - diffs = [] - for v in data[1:]: - diffs.append(v - prev) - prev = v + diffs = np.diff(data) return data, diffs @@ -118,4 +109,3 @@ if __name__ == "__main__": print("check plot...") plt.show() - diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 2fb3e0081d..087710d846 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -195,51 +195,48 @@ VectorXd Localizer::get_stdev() { return this->kf->get_P().diagonal().array().sqrt(); } -void Localizer::handle_sensors(double current_time, const capnp::List::Reader& log) { +void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) { // TODO does not yet account for double sensor readings in the log - for (int i = 0; i < log.size(); i++) { - const cereal::SensorEventData::Reader& sensor_reading = log[i]; - // Ignore empty readings (e.g. in case the magnetometer had no data ready) - if (sensor_reading.getTimestamp() == 0) { - continue; - } + // Ignore empty readings (e.g. in case the magnetometer had no data ready) + if (log.getTimestamp() == 0) { + return; + } - double sensor_time = 1e-9 * sensor_reading.getTimestamp(); + double sensor_time = 1e-9 * log.getTimestamp(); - // sensor time and log time should be close - if (std::abs(current_time - sensor_time) > 0.1) { - LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); - return; - } + // sensor time and log time should be close + if (std::abs(current_time - sensor_time) > 0.1) { + LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); + return; + } - // TODO: handle messages from two IMUs at the same time - if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::BMX055) { - continue; - } + // TODO: handle messages from two IMUs at the same time + if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) { + return; + } - // Gyro Uncalibrated - if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { - auto v = sensor_reading.getGyroUncalibrated().getV(); - auto meas = Vector3d(-v[2], -v[1], -v[0]); - if (meas.norm() < ROTATION_SANITY_CHECK) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); - } + // Gyro Uncalibrated + if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { + auto v = log.getGyroUncalibrated().getV(); + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ROTATION_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); } + } - // Accelerometer - if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) { - auto v = sensor_reading.getAcceleration().getV(); + // Accelerometer + if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) { + auto v = log.getAcceleration().getV(); - // TODO: reduce false positives and re-enable this check - // check if device fell, estimate 10 for g - // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving - //this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; + // TODO: reduce false positives and re-enable this check + // check if device fell, estimate 10 for g + // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving + // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; - auto meas = Vector3d(-v[2], -v[1], -v[0]); - if (meas.norm() < ACCEL_SANITY_CHECK) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); - } + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ACCEL_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); } } } @@ -441,8 +438,10 @@ void Localizer::handle_msg_bytes(const char *data, const size_t size) { void Localizer::handle_msg(const cereal::Event::Reader& log) { double t = log.getLogMonoTime() * 1e-9; this->time_check(t); - if (log.isSensorEvents()) { - this->handle_sensors(t, log.getSensorEvents()); + if (log.isAccelerometer()) { + this->handle_sensor(t, log.getAccelerometer()); + } else if (log.isGyroscope()) { + this->handle_sensor(t, log.getGyroscope()); } else if (log.isGpsLocation()) { this->handle_gps(t, log.getGpsLocation()); } else if (log.isGpsLocationExternal()) { @@ -498,7 +497,9 @@ int Localizer::locationd_thread() { } else { gps_location_socket = "gpsLocationExternal"; } - const std::initializer_list service_list = {gps_location_socket, "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"}; + const std::initializer_list service_list = {gps_location_socket, + "cameraOdometry", "liveCalibration", "carState", "carParams", + "accelerometer", "gyroscope", "magnetometer"}; PubMaster pm({"liveLocationKalman"}); // TODO: remove carParams once we're always sending at 100Hz @@ -521,11 +522,11 @@ int Localizer::locationd_thread() { } // 100Hz publish for notcars, 20Hz for cars - const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "sensorEvents" : "cameraOdometry"; + const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "accelerometer" : "cameraOdometry"; if (sm.updated(trigger_msg)) { bool inputsOK = sm.allAliveAndValid(); - bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents"); bool gpsOK = this->isGpsOK(); + bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope", "magnetometer"}); MessageBuilder msg_builder; kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 7c0cb6b7fe..b17ab04b23 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -45,7 +45,7 @@ public: void handle_msg_bytes(const char *data, const size_t size); void handle_msg(const cereal::Event::Reader& log); - void handle_sensors(double current_time, const capnp::List::Reader& log); + void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log); void handle_car_state(double current_time, const cereal::CarState::Reader& log); void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); diff --git a/selfdrive/locationd/test/_test_locationd_lib.py b/selfdrive/locationd/test/_test_locationd_lib.py index 8a0ed3ef05..c4a053bbc6 100755 --- a/selfdrive/locationd/test/_test_locationd_lib.py +++ b/selfdrive/locationd/test/_test_locationd_lib.py @@ -51,23 +51,23 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t @unittest.skip("temporarily disabled due to false positives") def test_device_fell(self): - msg = messaging.new_message('sensorEvents', 1) - msg.sensorEvents[0].sensor = 1 - msg.sensorEvents[0].timestamp = msg.logMonoTime - msg.sensorEvents[0].type = 1 - msg.sensorEvents[0].init('acceleration') - msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0] # zero with gravity + msg = messaging.new_message('accelerometer') + msg.accelerometer.sensor = 1 + msg.accelerometer.timestamp = msg.logMonoTime + msg.accelerometer.type = 1 + msg.accelerometer.init('acceleration') + msg.accelerometer.acceleration.v = [10.0, 0.0, 0.0] # zero with gravity self.localizer_handle_msg(msg) ret = self.localizer_get_msg() self.assertTrue(ret.liveLocationKalman.deviceStable) - msg = messaging.new_message('sensorEvents', 1) - msg.sensorEvents[0].sensor = 1 - msg.sensorEvents[0].timestamp = msg.logMonoTime - msg.sensorEvents[0].type = 1 - msg.sensorEvents[0].init('acceleration') - msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0] # more than 40 m/s**2 + msg = messaging.new_message('accelerometer') + msg.accelerometer.sensor = 1 + msg.accelerometer.timestamp = msg.logMonoTime + msg.accelerometer.type = 1 + msg.accelerometer.init('acceleration') + msg.accelerometer.acceleration.v = [50.1, 0.0, 0.0] # more than 40 m/s**2 self.localizer_handle_msg(msg) ret = self.localizer_get_msg() diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index e30331a460..8841b3e67c 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -14,7 +14,8 @@ from selfdrive.manager.process_config import managed_processes class TestLocationdProc(unittest.TestCase): MAX_WAITS = 1000 - LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration'] + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', + 'accelerometer', 'gyroscope', 'magnetometer'] def setUp(self): random.seed(123489234) diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc index c6dcdbd7aa..78b3ac526d 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.cc +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -42,6 +42,7 @@ int BMX055_Accel::init() { if (ret < 0) { goto fail; } + ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ); if (ret < 0) { goto fail; @@ -61,7 +62,7 @@ int BMX055_Accel::shutdown() { return ret; } -bool BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer)); @@ -73,6 +74,7 @@ bool BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { float y = -read_12_bit(buffer[2], buffer[3]) * scale; float z = read_12_bit(buffer[4], buffer[5]) * scale; + auto event = msg.initEvent().initAccelerometer2(); event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(1); event.setSensor(SENSOR_ACCELEROMETER); diff --git a/selfdrive/sensord/sensors/bmx055_accel.h b/selfdrive/sensord/sensors/bmx055_accel.h index 6a0f9f1ada..8ef660a99f 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.h +++ b/selfdrive/sensord/sensors/bmx055_accel.h @@ -36,6 +36,6 @@ class BMX055_Accel : public I2CSensor { public: BMX055_Accel(I2CBus *bus); int init(); - bool get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown(); }; diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc index 4deb15ec6d..9d70b9e431 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.cc +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -72,7 +72,7 @@ int BMX055_Gyro::shutdown() { return ret; } -bool BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer)); @@ -84,6 +84,7 @@ bool BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { 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().initGyroscope2(); event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(1); event.setSensor(SENSOR_GYRO_UNCALIBRATED); @@ -94,5 +95,6 @@ bool BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { auto svec = event.initGyroUncalibrated(); svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/bmx055_gyro.h b/selfdrive/sensord/sensors/bmx055_gyro.h index ac5dacc4a6..80b93f128c 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.h +++ b/selfdrive/sensord/sensors/bmx055_gyro.h @@ -36,6 +36,6 @@ class BMX055_Gyro : public I2CSensor { public: BMX055_Gyro(I2CBus *bus); int init(); - bool get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown(); }; diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index 74e18b7c82..394b1e83d3 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -223,7 +223,7 @@ bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t * } -bool BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[8]; int16_t _x, _y, x, y, z; @@ -233,6 +233,8 @@ bool BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { bool parsed = parse_xyz(buffer, &_x, &_y, &z); if (parsed) { + + auto event = msg.initEvent().initMagnetometer(); event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(2); event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); diff --git a/selfdrive/sensord/sensors/bmx055_magn.h b/selfdrive/sensord/sensors/bmx055_magn.h index 0549e163f6..e4a79bc7e0 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.h +++ b/selfdrive/sensord/sensors/bmx055_magn.h @@ -59,6 +59,6 @@ class BMX055_Magn : public I2CSensor{ public: BMX055_Magn(I2CBus *bus); int init(); - bool get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown(); }; diff --git a/selfdrive/sensord/sensors/bmx055_temp.cc b/selfdrive/sensord/sensors/bmx055_temp.cc index 3cee34ef19..bdb34f1508 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.cc +++ b/selfdrive/sensord/sensors/bmx055_temp.cc @@ -28,7 +28,7 @@ fail: return ret; } -bool BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[1]; int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer)); @@ -36,10 +36,12 @@ bool BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) { float temp = 23.0f + int8_t(buffer[0]) / 2.0f; + auto event = msg.initEvent().initTemperatureSensor(); event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(1); event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); event.setTimestamp(start_time); event.setTemperature(temp); + return true; } diff --git a/selfdrive/sensord/sensors/bmx055_temp.h b/selfdrive/sensord/sensors/bmx055_temp.h index f5d771a29c..0b6802deaa 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.h +++ b/selfdrive/sensord/sensors/bmx055_temp.h @@ -8,6 +8,6 @@ class BMX055_Temp : public I2CSensor { public: BMX055_Temp(I2CBus *bus); int init(); - bool get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/file_sensor.cc b/selfdrive/sensord/sensors/file_sensor.cc index 80e5121564..a74ae1ae1e 100644 --- a/selfdrive/sensord/sensors/file_sensor.cc +++ b/selfdrive/sensord/sensors/file_sensor.cc @@ -2,8 +2,7 @@ #include -FileSensor::FileSensor(std::string filename) : file(filename) { -} +FileSensor::FileSensor(std::string filename) : file(filename) {} int FileSensor::init() { return file.is_open() ? 0 : 1; diff --git a/selfdrive/sensord/sensors/file_sensor.h b/selfdrive/sensord/sensors/file_sensor.h index 5bcaee66a8..39d695167d 100644 --- a/selfdrive/sensord/sensors/file_sensor.h +++ b/selfdrive/sensord/sensors/file_sensor.h @@ -15,5 +15,5 @@ public: ~FileSensor(); int init(); bool has_interrupt_enabled(); - virtual bool get_event(cereal::SensorEventData::Builder &event) = 0; + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; }; diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/selfdrive/sensord/sensors/i2c_sensor.cc index 06a216478b..f563f93d2b 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.cc +++ b/selfdrive/sensord/sensors/i2c_sensor.cc @@ -15,7 +15,8 @@ int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) { 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(I2CBus *bus, int gpio_nr, bool shared_gpio) : + bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {} I2CSensor::~I2CSensor() { if (gpio_fd != -1) { diff --git a/selfdrive/sensord/sensors/i2c_sensor.h b/selfdrive/sensord/sensors/i2c_sensor.h index f6820a2471..0de2a98738 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.h +++ b/selfdrive/sensord/sensors/i2c_sensor.h @@ -31,6 +31,6 @@ public: int init_gpio(); bool has_interrupt_enabled(); virtual int init() = 0; - virtual bool get_event(cereal::SensorEventData::Builder &event) = 0; + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; virtual int shutdown() = 0; }; diff --git a/selfdrive/sensord/sensors/light_sensor.cc b/selfdrive/sensord/sensors/light_sensor.cc index 321ac75c7e..58c602ea39 100644 --- a/selfdrive/sensord/sensors/light_sensor.cc +++ b/selfdrive/sensord/sensors/light_sensor.cc @@ -5,7 +5,9 @@ #include "common/timing.h" #include "selfdrive/sensord/sensors/constants.h" -bool LightSensor::get_event(cereal::SensorEventData::Builder &event) { +LightSensor::LightSensor(std::string filename) : FileSensor(filename) {} + +bool LightSensor::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); file.clear(); file.seekg(0); @@ -13,6 +15,7 @@ bool LightSensor::get_event(cereal::SensorEventData::Builder &event) { int value; file >> value; + auto event = msg.initEvent().initLightSensor(); event.setSource(cereal::SensorEventData::SensorSource::RPR0521); event.setVersion(1); event.setSensor(SENSOR_LIGHT); diff --git a/selfdrive/sensord/sensors/light_sensor.h b/selfdrive/sensord/sensors/light_sensor.h index 63bda74755..7ed1c1f70c 100644 --- a/selfdrive/sensord/sensors/light_sensor.h +++ b/selfdrive/sensord/sensors/light_sensor.h @@ -3,7 +3,7 @@ class LightSensor : public FileSensor { public: - LightSensor(std::string filename) : FileSensor(filename){}; - bool get_event(cereal::SensorEventData::Builder &event); + LightSensor(std::string filename); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index 513125fd59..27cd4d0c70 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -5,7 +5,8 @@ #include "common/swaglog.h" #include "common/timing.h" -LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : I2CSensor(bus, gpio_nr, shared_gpio) {} +LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : + I2CSensor(bus, gpio_nr, shared_gpio) {} int LSM6DS3_Accel::init() { int ret = 0; @@ -93,15 +94,13 @@ fail: return ret; } -bool LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { +bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) { - if (has_interrupt_enabled()) { - // 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; - } + // 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]; @@ -113,10 +112,12 @@ bool LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { 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(); diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.h b/selfdrive/sensord/sensors/lsm6ds3_accel.h index 6ed94a8f12..84084fc916 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.h +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.h @@ -28,6 +28,6 @@ class LSM6DS3_Accel : public I2CSensor { public: LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); int init(); - bool get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown(); }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index fd0436a5f0..014a72bb73 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -8,7 +8,8 @@ #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) {} +LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) : + I2CSensor(bus, gpio_nr, shared_gpio) {} int LSM6DS3_Gyro::init() { int ret = 0; @@ -96,15 +97,13 @@ fail: return ret; } -bool LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { +bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { - if (has_interrupt_enabled()) { - // 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; - } + // 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]; @@ -116,10 +115,12 @@ bool LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { 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(); diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.h b/selfdrive/sensord/sensors/lsm6ds3_gyro.h index c2ed5ab76e..6c61ffcef2 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.h +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.h @@ -28,6 +28,6 @@ class LSM6DS3_Gyro : public I2CSensor { public: LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); int init(); - bool get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown(); }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc index 082ffee08f..7e1c4d4c81 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -31,8 +31,7 @@ fail: return ret; } -bool LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { - +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)); @@ -41,6 +40,7 @@ bool LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { 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); diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.h b/selfdrive/sensord/sensors/lsm6ds3_temp.h index 9d95236901..1d6bcc228a 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.h +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.h @@ -21,6 +21,6 @@ class LSM6DS3_Temp : public I2CSensor { public: LSM6DS3_Temp(I2CBus *bus); int init(); - bool get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.cc b/selfdrive/sensord/sensors/mmc5603nj_magn.cc index 8af4956edf..7a9b7a298b 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.cc +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.cc @@ -79,8 +79,7 @@ fail: return ret; } -bool MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { - +bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[9]; int len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer)); @@ -91,6 +90,7 @@ bool MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale; float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale; + auto event = msg.initEvent().initMagnetometer(); event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ); event.setVersion(1); event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.h b/selfdrive/sensord/sensors/mmc5603nj_magn.h index 2c06cab96f..a364c7c37a 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.h +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.h @@ -25,6 +25,6 @@ class MMC5603NJ_Magn : public I2CSensor { public: MMC5603NJ_Magn(I2CBus *bus); int init(); - bool get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); int shutdown(); }; diff --git a/selfdrive/sensord/sensors/sensor.h b/selfdrive/sensord/sensors/sensor.h index 0bdc560275..603aa3586e 100644 --- a/selfdrive/sensord/sensors/sensor.h +++ b/selfdrive/sensord/sensors/sensor.h @@ -1,13 +1,18 @@ #pragma once -#include "cereal/gen/cpp/log.capnp.h" +#include "cereal/messaging/messaging.h" class Sensor { public: int gpio_fd = -1; + uint64_t init_delay = 500e6; // default dealy 500ms virtual ~Sensor() {}; virtual int init() = 0; - virtual bool get_event(cereal::SensorEventData::Builder &event) = 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 st, uint64_t ct) { + return (ct - st) > init_delay; + } }; diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index 5e741a89a5..2279cf2532 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -27,18 +28,18 @@ ExitHandler do_exit; std::mutex pm_mutex; - -// filter first values (0.5sec) as those may contain inaccuracies uint64_t init_ts = 0; -constexpr uint64_t init_delay = 500*1e6; -void interrupt_loop(int fd, std::vector& sensors, PubMaster& pm) { +void interrupt_loop(std::vector& sensors, + std::map& sensor_service) +{ + PubMaster pm_int({"gyroscope", "accelerometer"}); + + int fd = sensors[0]->gpio_fd; struct pollfd fd_list[1] = {0}; fd_list[0].fd = fd; fd_list[0].events = POLLIN | POLLPRI; - uint64_t offset = nanos_since_epoch() - nanos_since_boot(); - while (!do_exit) { int err = poll(fd_list, 1, 100); if (err == -1) { @@ -65,39 +66,21 @@ void interrupt_loop(int fd, std::vector& sensors, PubMaster& pm) { } int num_events = err / sizeof(*evdata); + uint64_t offset = nanos_since_epoch() - nanos_since_boot(); uint64_t ts = evdata[num_events - 1].timestamp - offset; - MessageBuilder msg; - auto orphanage = msg.getOrphanage(); - std::vector> collected_events; - collected_events.reserve(sensors.size()); - for (Sensor *sensor : sensors) { - auto orphan = orphanage.newOrphan(); - auto event = orphan.get(); - if (!sensor->get_event(event)) { + MessageBuilder msg; + if (!sensor->get_event(msg, ts)) { continue; } - event.setTimestamp(ts); - collected_events.push_back(kj::mv(orphan)); - } - - if (collected_events.size() == 0) { - continue; - } - - auto events = msg.initEvent().initSensorEvents(collected_events.size()); - for (int i = 0; i < collected_events.size(); ++i) { - events.adoptWithCaveats(i, kj::mv(collected_events[i])); - } + if (!sensor->is_data_valid(init_ts, ts)) { + continue; + } - if (ts - init_ts < init_delay) { - continue; + pm_int.send(sensor_service[sensor].c_str(), msg); } - - std::lock_guard lock(pm_mutex); - pm.send("sensorEvents", msg); } // poweroff sensors, disable interrupts @@ -106,16 +89,7 @@ void interrupt_loop(int fd, std::vector& sensors, PubMaster& pm) { } } -int sensor_loop() { - I2CBus *i2c_bus_imu; - - try { - i2c_bus_imu = new I2CBus(I2C_BUS_IMU); - } catch (std::exception &e) { - LOGE("I2CBus init failed"); - return -1; - } - +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); @@ -129,6 +103,20 @@ int sensor_loop() { LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw"); + 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"}, + + {&mmc5603nj_magn, "magnetometer"}, + {&light, "lightSensor"} + }; + // Sensor init std::vector> sensors_init; // Sensor, required sensors_init.push_back({&bmx055_accel, false}); @@ -148,29 +136,27 @@ int sensor_loop() { // Initialize sensors std::vector sensors; - for (auto &sensor : sensors_init) { - int err = sensor.first->init(); + for (auto &[sensor, required] : sensors_init) { + int err = sensor->init(); if (err < 0) { - // Fail on required sensors - if (sensor.second) { + if (required) { LOGE("Error initializing sensors"); - delete i2c_bus_imu; return -1; } } else { - if (sensor.first == &bmx055_magn || sensor.first == &mmc5603nj_magn) { + + if (sensor == &bmx055_magn || sensor == &mmc5603nj_magn) { has_magnetometer = true; } - if (!sensor.first->has_interrupt_enabled()) { - sensors.push_back(sensor.first); + if (!sensor->has_interrupt_enabled()) { + sensors.push_back(sensor); } } } if (!has_magnetometer) { LOGE("No magnetometer present"); - delete i2c_bus_imu; return -1; } @@ -179,33 +165,30 @@ int sensor_loop() { util::set_core_affinity({1}); std::system("sudo su -c 'echo 1 > /proc/irq/336/smp_affinity_list'"); - PubMaster pm({"sensorEvents"}); + PubMaster pm_non_int({"gyroscope2", "accelerometer2", "temperatureSensor", + "lightSensor", "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, lsm6ds3_accel.gpio_fd, std::ref(lsm_interrupt_sensors), std::ref(pm)); + std::thread lsm_interrupt_thread(&interrupt_loop, std::ref(lsm_interrupt_sensors), + std::ref(sensor_service)); // polling loop for non interrupt handled sensors while (!do_exit) { std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); - const int num_events = sensors.size(); - MessageBuilder msg; - auto sensor_events = msg.initEvent().initSensorEvents(num_events); - - for (int i = 0; i < num_events; i++) { - auto event = sensor_events[i]; - sensors[i]->get_event(event); - } + for (Sensor *sensor : sensors) { + MessageBuilder msg; + if (!sensor->get_event(msg)) { + continue; + } - if (nanos_since_boot() - init_ts < init_delay) { - continue; - } + if (!sensor->is_data_valid(init_ts, nanos_since_boot())) { + continue; + } - { - std::lock_guard lock(pm_mutex); - pm.send("sensorEvents", msg); + pm_non_int.send(sensor_service[sensor].c_str(), msg); } std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); @@ -217,10 +200,15 @@ int sensor_loop() { } lsm_interrupt_thread.join(); - delete i2c_bus_imu; return 0; } int main(int argc, char *argv[]) { - return sensor_loop(); + try { + auto i2c_bus_imu = std::make_unique(I2C_BUS_IMU); + return sensor_loop(i2c_bus_imu.get()); + } catch (std::exception &e) { + LOGE("I2CBus init failed"); + return -1; + } } diff --git a/selfdrive/sensord/tests/test_sensord.py b/selfdrive/sensord/tests/test_sensord.py old mode 100755 new mode 100644 index 60fe8b06d4..1af76c8384 --- a/selfdrive/sensord/tests/test_sensord.py +++ b/selfdrive/sensord/tests/test_sensord.py @@ -3,7 +3,7 @@ import os import time import unittest import numpy as np -from collections import namedtuple +from collections import namedtuple, defaultdict import cereal.messaging as messaging from cereal import log @@ -78,21 +78,8 @@ ALL_SENSORS = { } } -SENSOR_BUS = 1 -I2C_ADDR_LSM = 0x6A LSM_INT_GPIO = 84 -def read_sensor_events(duration_sec): - sensor_events = messaging.sub_sock("sensorEvents", timeout=0.1) - start_time_sec = time.monotonic() - events = [] - while time.monotonic() - start_time_sec < duration_sec: - events += messaging.drain_sock(sensor_events) - time.sleep(0.01) - - assert len(events) != 0, "No sensor events collected" - return events - def get_proc_interrupts(int_pin): with open("/proc/interrupts") as f: lines = f.read().split("\n") @@ -100,9 +87,27 @@ def get_proc_interrupts(int_pin): for line in lines: if f" {int_pin} " in line: return ''.join(list(filter(lambda e: e != '', line.split(' ')))[1:6]) - return "" +def read_sensor_events(duration_sec): + + sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2', + 'gyroscope2', 'lightSensor', 'temperatureSensor'] + esocks = {} + events = defaultdict(list) + for stype in sensor_types: + esocks[stype] = messaging.sub_sock(stype, timeout=0.1) + + start_time_sec = time.monotonic() + while time.monotonic() - start_time_sec < duration_sec: + for esock in esocks: + events[esock] += messaging.drain_sock(esocks[esock]) + time.sleep(0.1) + + for etype in events: + assert len(events[etype]) != 0, f"No {etype} events collected" + + return events class TestSensord(unittest.TestCase): @classmethod @@ -132,12 +137,10 @@ class TestSensord(unittest.TestCase): # verify correct sensors configuration seen = set() - for event in self.events: - for measurement in event.sensorEvents: - # filter unset events (bmx magn) - if measurement.version == 0: - continue - seen.add((str(measurement.source), measurement.which())) + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + seen.add((str(m.source), m.which())) self.assertIn(seen, SENSOR_CONFIGURATIONS) @@ -148,10 +151,14 @@ class TestSensord(unittest.TestCase): 1: [], # accel 5: [], # gyro } - for event in self.events: - for measurement in event.sensorEvents: - if str(measurement.source).startswith("lsm6ds3") and measurement.sensor in sensor_t: - sensor_t[measurement.sensor].append(measurement.timestamp) + + for measurement in self.events['accelerometer']: + m = getattr(measurement, measurement.which()) + sensor_t[m.sensor].append(m.timestamp) + + for measurement in self.events['gyroscope']: + m = getattr(measurement, measurement.which()) + sensor_t[m.sensor].append(m.timestamp) for s, vals in sensor_t.items(): with self.subTest(sensor=s): @@ -172,17 +179,14 @@ class TestSensord(unittest.TestCase): # verify if all sensors produce events sensor_events = dict() - for event in self.events: - for measurement in event.sensorEvents: + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) - # filter unset events (bmx magn) - if measurement.version == 0: - continue - - if measurement.type in sensor_events: - sensor_events[measurement.type] += 1 + if m.type in sensor_events: + sensor_events[m.type] += 1 else: - sensor_events[measurement.type] = 1 + sensor_events[m.type] = 1 for s in sensor_events: err_msg = f"Sensor {s}: 200 < {sensor_events[s]}" @@ -192,22 +196,19 @@ class TestSensord(unittest.TestCase): # ensure diff between the message logMonotime and sample timestamp is small tdiffs = list() - for event in self.events: - for measurement in event.sensorEvents: - - # filter unset events (bmx magn) - if measurement.version == 0: - continue + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) # check if gyro and accel timestamps are before logMonoTime - if str(measurement.source).startswith("lsm6ds3"): - if measurement.which() != 'temperature': - err_msg = f"Timestamp after logMonoTime: {measurement.timestamp} > {event.logMonoTime}" - assert measurement.timestamp < event.logMonoTime, err_msg + if str(m.source).startswith("lsm6ds3"): + if m.which() != 'temperature': + err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}" + assert m.timestamp < measurement.logMonoTime, err_msg # negative values might occur, as non interrupt packages created # before the sensor is read - tdiffs.append(abs(event.logMonoTime - measurement.timestamp)) + tdiffs.append(abs(measurement.logMonoTime - m.timestamp)) high_delay_diffs = set(filter(lambda d: d >= 10*10**6, tdiffs)) assert len(high_delay_diffs) < 15, f"Too many high delay packages: {high_delay_diffs}" @@ -219,16 +220,14 @@ class TestSensord(unittest.TestCase): assert stddev < 2*10**6, f"Timing diffs have to high stddev: {stddev}" def test_sensor_values_sanity_check(self): - sensor_values = dict() - for event in self.events: - for m in event.sensorEvents: - - # filter unset events (bmx magn) - if m.version == 0: - continue + sensor_values = dict() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) key = (m.source.raw, m.which()) values = getattr(m, m.which()) + if hasattr(values, 'v'): values = values.v values = np.atleast_1d(values) @@ -265,7 +264,8 @@ class TestSensord(unittest.TestCase): time.sleep(1) state_two = get_proc_interrupts(LSM_INT_GPIO) - assert state_one != state_two, f"no interrupts received after sensord start!\n{state_one} {state_two}" + error_msg = f"no interrupts received after sensord start!\n{state_one} {state_two}" + assert state_one != state_two, error_msg managed_processes["sensord"].stop() time.sleep(1) @@ -276,6 +276,5 @@ class TestSensord(unittest.TestCase): state_two = get_proc_interrupts(LSM_INT_GPIO) assert state_one == state_two, "Interrupts received after sensord stop!" - if __name__ == "__main__": unittest.main() diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 038b0cf468..c8389bb71c 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -328,7 +328,8 @@ CONFIGS = [ proc_name="locationd", pub_sub={ "cameraOdometry": ["liveLocationKalman"], - "sensorEvents": [], "gpsLocationExternal": [], "liveCalibration": [], "carState": [], + "accelerometer": [], "gyroscope": [], "magnetometer": [], + "gpsLocationExternal": [], "liveCalibration": [], "carState": [], }, ignore=["logMonoTime", "valid"], init_callback=get_car_params, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 98fbb74ae4..e308d2dff4 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -9098c1cf6993598071c3e27448356eef86660d02 +761eada809a0eaa67989e6e435042633f965d1fe \ No newline at end of file diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 356016c642..54c84978c2 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -90,30 +90,10 @@ def replay_device_state(s, msgs): rk.keep_time() -def replay_sensor_events(s, msgs): - pm = messaging.PubMaster([s, ]) - rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) - smsgs = [m for m in msgs if m.which() == s] - while True: - for m in smsgs: - new_m = m.as_builder() - new_m.logMonoTime = int(sec_since_boot() * 1e9) - - for evt in new_m.sensorEvents: - evt.timestamp = new_m.logMonoTime - - pm.send(s, new_m) - rk.keep_time() - - def replay_sensor_event(s, msgs): - smsgs = [m for m in msgs if m.which() == s] - #if len(smsgs) == 0: - # return - pm = messaging.PubMaster([s, ]) rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) - + smsgs = [m for m in msgs if m.which() == s] while True: for m in smsgs: m = m.as_builder() @@ -213,12 +193,12 @@ def migrate_carparams(lr): def migrate_sensorEvents(lr): all_msgs = [] for msg in lr: - if msg.which() != 'sensorEvents': + if msg.which() != 'sensorEventsDEPRECATED': all_msgs.append(msg) continue # migrate to split sensor events - for evt in msg.sensorEvents: + for evt in msg.sensorEventsDEPRECATED: # build new message for each sensor type sensor_service = '' if evt.which() == 'acceleration': @@ -244,9 +224,6 @@ def migrate_sensorEvents(lr): all_msgs.append(m.as_reader()) - # append also legacy sensorEvents, to have both (remove later) - all_msgs.append(msg) - return all_msgs @@ -270,7 +247,6 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): vs, cam_procs = replay_cameras(lr, frs, disable_tqdm=disable_tqdm) fake_daemons = { 'sensord': [ - multiprocessing.Process(target=replay_sensor_events, args=('sensorEvents', lr)), multiprocessing.Process(target=replay_sensor_event, args=('accelerometer', lr)), multiprocessing.Process(target=replay_sensor_event, args=('gyroscope', lr)), multiprocessing.Process(target=replay_sensor_event, args=('magnetometer', lr)), diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 0e4f47963b..cf836f3db7 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -29,7 +29,7 @@ REPEAT_COUNTER = 5 PRINT_DECIMATION = 100 STEER_RATIO = 15. -pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'sensorEvents', 'can', "gpsLocationExternal"]) +pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'accelerometer', 'gyroscope', 'can', "gpsLocationExternal"]) sm = messaging.SubMaster(['carControl', 'controlsState']) def parse_args(add_args=None): @@ -123,17 +123,20 @@ class Camerad: def imu_callback(imu, vehicle_state): vehicle_state.bearing_deg = math.degrees(imu.compass) - dat = messaging.new_message('sensorEvents', 2) - dat.sensorEvents[0].sensor = 4 - dat.sensorEvents[0].type = 0x10 - dat.sensorEvents[0].init('acceleration') - dat.sensorEvents[0].acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z] + dat = messaging.new_message('accelerometer') + dat.accelerometer.sensor = 4 + dat.accelerometer.type = 0x1 + dat.accelerometer.init('acceleration') + dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z] + pm.send('accelerometer', dat) + # copied these numbers from locationd - dat.sensorEvents[1].sensor = 5 - dat.sensorEvents[1].type = 0x10 - dat.sensorEvents[1].init('gyroUncalibrated') - dat.sensorEvents[1].gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z] - pm.send('sensorEvents', dat) + dat = messaging.new_message('gyroscope') + dat.gyroscope.sensor = 5 + dat.gyroscope.type = 0x10 + dat.gyroscope.init('gyroUncalibrated') + dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z] + pm.send('gyroscope', dat) def panda_state_function(vs: VehicleState, exit_event: threading.Event):