IQ.Pilot Prebuilt Release @ 27f668a

iqpilot/system/sensord/sensors/mmc5603nj_magn.py

@@ -0,0 +1,76 @@
+import time
+
+from iqpilot.cereal import log
+from iqpilot.system.sensord.sensors.i2c_sensor import Sensor
+
+# https://www.mouser.com/datasheet/2/821/Memsic_09102019_Datasheet_Rev.B-1635324.pdf
+
+# Register addresses
+REG_ODR = 0x1A
+REG_INTERNAL_0 = 0x1B
+REG_INTERNAL_1 = 0x1C
+
+# Control register settings
+CMM_FREQ_EN = (1 << 7)
+AUTO_SR_EN  = (1 << 5)
+SET         = (1 << 3)
+RESET       = (1 << 4)
+
+class MMC5603NJ_Magn(Sensor):
+  @property
+  def device_address(self) -> int:
+    return 0x30
+
+  def init(self):
+    self.verify_chip_id(0x39, [0x10, ])
+    self.writes((
+      (REG_ODR, 0),
+
+      # Set BW to 0b01 for 1-150 Hz operation
+      (REG_INTERNAL_1, 0b01),
+    ))
+
+  def _read_data(self, cycle) -> list[float]:
+    # start measurement
+    self.write(REG_INTERNAL_0, cycle)
+    self.wait()
+
+    # read out XYZ
+    scale = 1.0 / 16384.0
+    b = self.read(0x00, 9)
+    return [
+      (self.parse_20bit(b[6], b[1], b[0]) * scale) - 32.0,
+      (self.parse_20bit(b[7], b[3], b[2]) * scale) - 32.0,
+      (self.parse_20bit(b[8], b[5], b[4]) * scale) - 32.0,
+    ]
+
+  def get_event(self, ts: int | None = None) -> log.SensorEventData:
+    ts = time.monotonic_ns()
+
+    # SET - RESET cycle
+    xyz = self._read_data(SET)
+    reset_xyz = self._read_data(RESET)
+    vals = [*xyz, *reset_xyz]
+
+    event = log.SensorEventData.new_message()
+    event.timestamp = ts
+    event.version = 1
+    event.sensor = 3 # SENSOR_MAGNETOMETER_UNCALIBRATED
+    event.type = 14  # SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED
+    event.source = log.SensorEventData.SensorSource.mmc5603nj
+
+    m = event.init('magneticUncalibrated')
+    m.v = vals
+    m.status = int(all(int(v) != -32 for v in vals))
+
+    return event
+
+  def shutdown(self) -> None:
+    v = self.read(REG_INTERNAL_0, 1)[0]
+    self.writes((
+      # disable auto-reset of measurements
+      (REG_INTERNAL_0, (v & (~(CMM_FREQ_EN | AUTO_SR_EN)))),
+
+      # disable continuous mode
+      (REG_ODR, 0),
+    ))