From caabc8cb0b9dfa87650234b6a04cadb05a3e7285 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Tue, 13 Jul 2021 15:38:06 +0200 Subject: [PATCH] MMC5603NJ support (#21564) * read chip id * set source * untested driver * periodic set * optional old-commit-hash: 8eba02e119a9d63bcc99ce945b48ed8316cb68c4 --- selfdrive/sensord/SConscript | 1 + selfdrive/sensord/sensors/i2c_sensor.cc | 5 ++ selfdrive/sensord/sensors/i2c_sensor.h | 1 + selfdrive/sensord/sensors/mmc5603nj_magn.cc | 77 +++++++++++++++++++++ selfdrive/sensord/sensors/mmc5603nj_magn.h | 29 ++++++++ selfdrive/sensord/sensors_qcom2.cc | 38 ++++++---- 6 files changed, 138 insertions(+), 13 deletions(-) create mode 100644 selfdrive/sensord/sensors/mmc5603nj_magn.cc create mode 100644 selfdrive/sensord/sensors/mmc5603nj_magn.h diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index 0817a4d30c..186e302bc3 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -14,6 +14,7 @@ else: 'sensors/lsm6ds3_accel.cc', 'sensors/lsm6ds3_gyro.cc', 'sensors/lsm6ds3_temp.cc', + 'sensors/mmc5603nj_magn.cc', ] libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj'] if arch == "larch64": diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/selfdrive/sensord/sensors/i2c_sensor.cc index 88e17a3c09..40dfa4a736 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.cc +++ b/selfdrive/sensord/sensors/i2c_sensor.cc @@ -10,6 +10,11 @@ int16_t read_16_bit(uint8_t lsb, uint8_t msb) { 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) : bus(bus) { } diff --git a/selfdrive/sensord/sensors/i2c_sensor.h b/selfdrive/sensord/sensors/i2c_sensor.h index 98e2e2f858..83c66eac87 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.h +++ b/selfdrive/sensord/sensors/i2c_sensor.h @@ -9,6 +9,7 @@ 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 { diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.cc b/selfdrive/sensord/sensors/mmc5603nj_magn.cc new file mode 100644 index 0000000000..829a7f7d25 --- /dev/null +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.cc @@ -0,0 +1,77 @@ +#include "mmc5603nj_magn.h" + +#include + +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" + +MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {} + +int MMC5603NJ_Magn::init() { + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(MMC5603NJ_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != MMC5603NJ_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], MMC5603NJ_CHIP_ID); + ret = -1; + goto fail; + } + + // Set 100 Hz + ret = set_register(MMC5603NJ_I2C_REG_ODR, 100); + 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; + } + + // Set compute measurement rate + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN); + if (ret < 0) { + goto fail; + } + + // Enable continous mode, set every 100 measurements + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_2, MMC5603NJ_CMM_EN | MMC5603NJ_EN_PRD_SET | 0b11); + if (ret < 0) { + goto fail; + } + +fail: + return ret; +} + +void MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { + + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[9]; + int 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; + float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale; + float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale; + + 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 xyz[] = {x, y, z}; + auto svec = event.initMagneticUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.h b/selfdrive/sensord/sensors/mmc5603nj_magn.h new file mode 100644 index 0000000000..58840bbf27 --- /dev/null +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.h @@ -0,0 +1,29 @@ +#pragma once + +#include "selfdrive/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) + +class MMC5603NJ_Magn : public I2CSensor { + uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;} +public: + MMC5603NJ_Magn(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index d143c209d5..36526df0fb 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -18,6 +18,7 @@ #include "selfdrive/sensord/sensors/lsm6ds3_accel.h" #include "selfdrive/sensord/sensors/lsm6ds3_gyro.h" #include "selfdrive/sensord/sensors/lsm6ds3_temp.h" +#include "selfdrive/sensord/sensors/mmc5603nj_magn.h" #include "selfdrive/sensord/sensors/sensor.h" #define I2C_BUS_IMU 1 @@ -43,27 +44,38 @@ int sensor_loop() { LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu); LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu); + MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu); + LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw"); // Sensor init - std::vector sensors; - sensors.push_back(&bmx055_accel); - sensors.push_back(&bmx055_gyro); - sensors.push_back(&bmx055_magn); - sensors.push_back(&bmx055_temp); + std::vector> sensors_init; // Sensor, required + sensors_init.push_back({&bmx055_accel, true}); + sensors_init.push_back({&bmx055_gyro, true}); + sensors_init.push_back({&bmx055_magn, true}); + sensors_init.push_back({&bmx055_temp, true}); - sensors.push_back(&lsm6ds3_accel); - sensors.push_back(&lsm6ds3_gyro); - sensors.push_back(&lsm6ds3_temp); + sensors_init.push_back({&lsm6ds3_accel, true}); + sensors_init.push_back({&lsm6ds3_gyro, true}); + sensors_init.push_back({&lsm6ds3_temp, true}); - sensors.push_back(&light); + sensors_init.push_back({&mmc5603nj_magn, false}); + sensors_init.push_back({&light, true}); - for (Sensor * sensor : sensors) { - int err = sensor->init(); + + // Initialize sensors + std::vector sensors; + for (auto &sensor : sensors_init) { + int err = sensor.first->init(); if (err < 0) { - LOGE("Error initializing sensors"); - return -1; + // Fail on required sensors + if (sensor.second) { + LOGE("Error initializing sensors"); + return -1; + } + } else { + sensors.push_back(sensor.first); } }