+/* Read the sensor values and convert to a sample struct */
+static void
+ao_mmc5983_sample(struct ao_mmc5983_sample *s)
+{
+ ao_mmc5983_raw(&raw);
+
+ /* Bias by 32768 to convert from uint16_t to int16_t */
+ s->x = (int32_t) (((uint16_t) raw.x0 << 8) | raw.x1) - 32768;
+ s->y = (int32_t) (((uint16_t) raw.y0 << 8) | raw.y1) - 32768;
+ s->z = (int32_t) (((uint16_t) raw.z0 << 8) | raw.z1) - 32768;;
+}
+
+/* Synchronously sample the sensors */
+static void
+ao_mmc5983_sync_sample(struct ao_mmc5983_sample *v)
+{
+ ao_mmc5983_reg_write(MMC5983_CONTROL_0,
+ (1 << MMC5983_CONTROL_0_TM_M));
+ ao_mmc5983_wait();
+ ao_mmc5983_sample(v);
+}
+
+static struct ao_mmc5983_sample set, reset;
+
+/* Calibrate the device by finding the zero point */
+static void
+ao_mmc5983_cal(void)
+{
+ /* Compute offset */
+
+ ao_delay(AO_MS_TO_TICKS(100));
+
+ /* Measure in 'SET' mode */
+ ao_mmc5983_set();
+ ao_delay(AO_MS_TO_TICKS(100));
+ ao_mmc5983_sync_sample(&set);
+
+ ao_delay(AO_MS_TO_TICKS(100));
+
+ /* Measure in 'RESET' mode */
+ ao_mmc5983_reset();
+ ao_delay(AO_MS_TO_TICKS(100));
+ ao_mmc5983_sync_sample(&reset);
+
+ /* The zero point is the average of SET and RESET values */
+ ao_mmc5983_offset.x = ((int32_t) set.x + (int32_t) reset.x) / 2;
+ ao_mmc5983_offset.y = ((int32_t) set.y + (int32_t) reset.y) / 2;
+ ao_mmc5983_offset.z = ((int32_t) set.z + (int32_t) reset.z) / 2;
+}
+
+/* Configure the device to automatically sample at 200Hz */
+static void
+ao_mmc5983_run(void)
+{
+ /* Set bandwidth to 200Hz */
+ ao_mmc5983_reg_write(MMC5983_CONTROL_1,
+ MMC5983_CONTROL_1_BW_200 << MMC5983_CONTROL_1_BW);
+
+ /* Measure at 200Hz so we get recent samples by just reading
+ * the registers
+ */
+ ao_mmc5983_reg_write(MMC5983_CONTROL_2,
+ (1 << MMC5983_CONTROL_2_CMM_EN) |
+ (MMC5983_CONTROL_2_CM_FREQ_200HZ << MMC5983_CONTROL_2_CM_FREQ) |
+ (0 << MMC5983_CONTROL_2_EN_PRD_SET) |
+ (MMC5983_CONTROL_2_PRD_SET_1000));
+ ao_mmc5983_configured = 1;
+}
+
+/* Reboot the device by setting the SW_RST bit and waiting 10ms */
+static void
+ao_mmc5983_reboot(void)
+{
+ ao_mmc5983_configured = 0;