sensord: fix LSM magnetometer thermal variation (#29265)

* do set reset for every measurement cycle

* use one sensorvec

* convert from continuous to manual mode

* separate the set/reset and TM_M

* make magnetometer readings manual, set appropriate sleep

* add more sleep and check validity of values

* add magnetometer to a separate thread

* refactor

* bugfix

* each sensor in a new thread, handle generically

* bugfix

* rm global

* update cpu

* update test

* update test

* revert cereal change

* split out temp sensors

* little more

* fix

* fix

* bump cereal

* linter fixes

---------

Co-authored-by: nuwandavek <vivekaithal44@gmail.com>
Co-authored-by: Comma Device <device@comma.ai>

system/sensord/sensors/mmc5603nj_magn.cc

@@ -1,9 +1,12 @@
 #include "mmc5603nj_magn.h"
 
+#include <algorithm>
 #include <cassert>
+#include <vector>
 
 #include "common/swaglog.h"
 #include "common/timing.h"
+#include "common/util.h"
 
 MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
 
@@ -11,8 +14,8 @@ int MMC5603NJ_Magn::init() {
   int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
   if (ret == -1) return -1;
 
-  // Set 100 Hz
-  ret = set_register(MMC5603NJ_I2C_REG_ODR, 100);
+  // Set ODR to 0
+  ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
   if (ret < 0) {
     goto fail;
   }
@@ -23,18 +26,6 @@ int MMC5603NJ_Magn::init() {
     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 continuous 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;
 }
@@ -67,16 +58,36 @@ fail:
   return ret;
 }
 
+void MMC5603NJ_Magn::start_measurement() {
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01);
+  util::sleep_for(5);
+}
+
+std::vector<float> MMC5603NJ_Magn::read_measurement() {
+  int len;
+  uint8_t buffer[9];
+  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) - 32.0;
+  float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0;
+  float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0;
+  std::vector<float> xyz = {x, y, z};
+  return xyz;
+}
+
 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));
-  assert(len == sizeof(buffer));
+  // SET - RESET cycle
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET);
+  util::sleep_for(5);
+  MMC5603NJ_Magn::start_measurement();
+  std::vector<float> xyz = MMC5603NJ_Magn::read_measurement();
 
-  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;
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET);
+  util::sleep_for(5);
+  MMC5603NJ_Magn::start_measurement();
+  std::vector<float> reset_xyz = MMC5603NJ_Magn::read_measurement();
 
   auto event = msg.initEvent().initMagnetometer();
   event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
@@ -85,10 +96,13 @@ bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
   event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
   event.setTimestamp(start_time);
 
-  float xyz[] = {x, y, z};
+  float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]};
+  bool valid = true;
+  if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) {
+    valid = false;
+  }
   auto svec = event.initMagneticUncalibrated();
-  svec.setV(xyz);
-  svec.setStatus(true);
-
+  svec.setV(vals);
+  svec.setStatus(valid);
   return true;
 }

system/sensord/sensors/mmc5603nj_magn.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <vector>
+
 #include "system/sensord/sensors/i2c_sensor.h"
 
 // Address of the chip on the bus
@@ -19,9 +21,14 @@
 #define MMC5603NJ_AUTO_SR_EN     (1 << 5)
 #define MMC5603NJ_CMM_EN         (1 << 4)
 #define MMC5603NJ_EN_PRD_SET     (1 << 3)
+#define MMC5603NJ_SET            (1 << 3)
+#define MMC5603NJ_RESET          (1 << 4)
 
 class MMC5603NJ_Magn : public I2CSensor {
+private:
   uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
+  void start_measurement();
+  std::vector<float> read_measurement();
 public:
   MMC5603NJ_Magn(I2CBus *bus);
   int init();