MMC5603NJ support (#21564)

* read chip id

* set source

* untested driver

* periodic set

* optional

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

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) {
 }

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 {

selfdrive/sensord/sensors/mmc5603nj_magn.cc

@@ -0,0 +1,77 @@
+#include "mmc5603nj_magn.h"
+
+#include <cassert>
+
+#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);
+
+}

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);
+};

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,28 +44,39 @@ 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<Sensor *> sensors;
-  sensors.push_back(&bmx055_accel);
-  sensors.push_back(&bmx055_gyro);
-  sensors.push_back(&bmx055_magn);
-  sensors.push_back(&bmx055_temp);
+  std::vector<std::pair<Sensor *, bool>> 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_init.push_back({&lsm6ds3_accel, true});
+  sensors_init.push_back({&lsm6ds3_gyro, true});
+  sensors_init.push_back({&lsm6ds3_temp, true});
 
-  sensors.push_back(&lsm6ds3_accel);
-  sensors.push_back(&lsm6ds3_gyro);
-  sensors.push_back(&lsm6ds3_temp);
+  sensors_init.push_back({&mmc5603nj_magn, false});
 
-  sensors.push_back(&light);
+  sensors_init.push_back({&light, true});
 
 
-  for (Sensor * sensor : sensors) {
-    int err = sensor->init();
+  // Initialize sensors
+  std::vector<Sensor *> sensors;
+  for (auto &sensor : sensors_init) {
+    int err = sensor.first->init();
     if (err < 0) {
+      // Fail on required sensors
+      if (sensor.second) {
         LOGE("Error initializing sensors");
         return -1;
       }
+    } else {
+      sensors.push_back(sensor.first);
+    }
   }
 
   PubMaster pm({"sensorEvents"});