2 * Copyright © 2021 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
20 #include <ao_mmc5983.h>
25 #define DEBUG_MMC5983 0
27 struct ao_mmc5983_sample ao_mmc5983_current;
28 static struct ao_mmc5983_sample ao_mmc5983_offset;
30 static uint8_t ao_mmc5983_configured;
33 #ifdef AO_MMC5983_I2C_INDEX
36 ao_mmc5983_start(void) {
37 ao_i2c_get(AO_MMC5983_I2C_INDEX);
41 ao_mmc5983_stop(void) {
42 ao_i2c_put(AO_MMC5983_I2C_INDEX);
46 ao_mmc5983_reg_write(uint8_t addr, uint8_t data)
54 ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR);
55 ao_i2c_send(d, 2, AO_MMC5983_I2C_INDEX, true);
60 ao_mmc5983_reg_read(uint8_t addr)
65 ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR);
67 ao_i2c_send(d, 1, AO_MMC5983_I2C_INDEX, false);
68 ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR | 1);
69 ao_i2c_recv(d, 1, AO_MMC5983_I2C_INDEX, true);
75 ao_mmc5983_raw(struct ao_mmc5983_raw *raw)
78 ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR);
79 raw->addr = MMC5983_X_OUT_0;
80 ao_i2c_send(&(raw->addr), 1, AO_MMC5983_I2C_INDEX, false);
81 ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR | 1);
82 ao_i2c_recv(&(raw->x0), sizeof(*raw) - 1, AO_MMC5983_I2C_INDEX, true);
87 #include <ao_i2c_bit.h>
90 ao_mmc5983_reg_write(uint8_t addr, uint8_t data)
97 ao_i2c_bit_start(MMC5983_I2C_ADDR);
98 ao_i2c_bit_send(d, 2);
103 ao_mmc5983_reg_read(uint8_t addr)
107 ao_i2c_bit_start(MMC5983_I2C_ADDR);
109 ao_i2c_bit_send(d, 1);
110 ao_i2c_bit_restart(MMC5983_I2C_ADDR | 1);
111 ao_i2c_bit_recv(d, 1);
117 ao_mmc5983_raw(struct ao_mmc5983_raw *raw)
119 ao_i2c_bit_start(MMC5983_I2C_ADDR);
120 raw->addr = MMC5983_X_OUT_0;
121 ao_i2c_bit_send(&(raw->addr), 1);
122 ao_i2c_bit_restart(MMC5983_I2C_ADDR | 1);
123 ao_i2c_bit_recv(&(raw->x0), sizeof(*raw) - 1);
130 #define AO_MMC5983_SPI_SPEED ao_spi_speed(2000000)
133 ao_mmc5983_start(void) {
134 ao_spi_get_bit(AO_MMC5983_SPI_CS_PORT,
135 AO_MMC5983_SPI_CS_PIN,
136 AO_MMC5983_SPI_INDEX,
137 AO_MMC5983_SPI_SPEED);
141 ao_mmc5983_stop(void) {
142 ao_spi_put_bit(AO_MMC5983_SPI_CS_PORT,
143 AO_MMC5983_SPI_CS_PIN,
144 AO_MMC5983_SPI_INDEX);
149 ao_mmc5983_reg_write(uint8_t addr, uint8_t data)
156 ao_spi_send(d, 2, AO_MMC5983_SPI_INDEX);
161 ao_mmc5983_reg_read(uint8_t addr)
165 d[0] = addr | MMC5983_READ;
167 ao_spi_duplex(d, d, 2, AO_MMC5983_SPI_INDEX);
174 ao_mmc5983_duplex(uint8_t *dst, uint8_t len)
177 ao_spi_duplex(dst, dst, len, AO_MMC5983_SPI_INDEX);
182 ao_mmc5983_raw(struct ao_mmc5983_raw *raw)
184 raw->addr = MMC5983_X_OUT_0 | MMC5983_READ;
185 ao_mmc5983_duplex((uint8_t *) raw, sizeof (*raw));
189 /* Saturating subtraction. Keep the result within range
193 sat_sub(int16_t a, int16_t b)
195 int32_t v = (int32_t) a - (int32_t) b;
203 /* Wait for a synchronous sample to finish */
205 ao_mmc5983_wait(void)
208 uint8_t status = ao_mmc5983_reg_read(MMC5983_STATUS);
209 if ((status & (1 << MMC5983_STATUS_MEAS_M_DONE)) != 0)
218 ao_mmc5983_reg_write(MMC5983_CONTROL_0,
219 (1 << MMC5983_CONTROL_0_SET));
223 ao_mmc5983_reset(void)
225 ao_mmc5983_reg_write(MMC5983_CONTROL_0,
226 (1 << MMC5983_CONTROL_0_RESET));
229 static struct ao_mmc5983_raw raw;
231 /* Read the sensor values and convert to a sample struct */
233 ao_mmc5983_sample(struct ao_mmc5983_sample *s)
235 ao_mmc5983_raw(&raw);
237 /* Bias by 32768 to convert from uint16_t to int16_t */
238 s->x = (int16_t) ((((uint16_t) raw.x0 << 8) | raw.x1) - 32768);
239 s->y = (int16_t) ((((uint16_t) raw.y0 << 8) | raw.y1) - 32768);
240 s->z = (int16_t) ((((uint16_t) raw.z0 << 8) | raw.z1) - 32768);
243 /* Synchronously sample the sensors */
245 ao_mmc5983_sync_sample(struct ao_mmc5983_sample *v)
247 ao_mmc5983_reg_write(MMC5983_CONTROL_0,
248 (1 << MMC5983_CONTROL_0_TM_M));
250 ao_mmc5983_sample(v);
253 static struct ao_mmc5983_sample set, reset;
255 /* Calibrate the device by finding the zero point */
261 ao_delay(AO_MS_TO_TICKS(100));
263 /* Measure in 'SET' mode */
265 ao_delay(AO_MS_TO_TICKS(100));
266 ao_mmc5983_sync_sample(&set);
268 ao_delay(AO_MS_TO_TICKS(100));
270 /* Measure in 'RESET' mode */
272 ao_delay(AO_MS_TO_TICKS(100));
273 ao_mmc5983_sync_sample(&reset);
275 /* The zero point is the average of SET and RESET values */
276 ao_mmc5983_offset.x = (int16_t) (((int32_t) set.x + (int32_t) reset.x) / 2);
277 ao_mmc5983_offset.y = (int16_t) (((int32_t) set.y + (int32_t) reset.y) / 2);
278 ao_mmc5983_offset.z = (int16_t) (((int32_t) set.z + (int32_t) reset.z) / 2);
281 /* Configure the device to automatically sample at 200Hz */
285 /* Set bandwidth to 200Hz */
286 ao_mmc5983_reg_write(MMC5983_CONTROL_1,
287 MMC5983_CONTROL_1_BW_200 << MMC5983_CONTROL_1_BW);
289 /* Measure at 200Hz so we get recent samples by just reading
292 ao_mmc5983_reg_write(MMC5983_CONTROL_2,
293 (1 << MMC5983_CONTROL_2_CMM_EN) |
294 (MMC5983_CONTROL_2_CM_FREQ_200HZ << MMC5983_CONTROL_2_CM_FREQ) |
295 (0 << MMC5983_CONTROL_2_EN_PRD_SET) |
296 (MMC5983_CONTROL_2_PRD_SET_1000));
297 ao_mmc5983_configured = 1;
300 /* Reboot the device by setting the SW_RST bit and waiting 10ms */
302 ao_mmc5983_reboot(void)
304 ao_mmc5983_configured = 0;
306 ao_mmc5983_reg_write(MMC5983_CONTROL_1,
307 1 << MMC5983_CONTROL_1_SW_RST);
309 /* Delay for power up time (10ms) */
310 ao_delay(AO_MS_TO_TICKS(10));
313 /* Configure the device for operation */
315 ao_mmc5983_setup(void)
319 /* Reboot the device */
322 /* Check product ID */
323 product_id = ao_mmc5983_reg_read(MMC5983_PRODUCT_ID);
324 if (product_id != MMC5983_PRODUCT_ID_PRODUCT_I2C &&
325 product_id != MMC5983_PRODUCT_ID_PRODUCT_SPI)
327 AO_SENSOR_ERROR(AO_DATA_MMC5983);
333 /* Start automatic sampling */
339 struct ao_mmc5983_sample ao_mmc5983_current;
344 struct ao_mmc5983_sample sample;
347 if (ao_mmc5983_configured)
348 ao_mmc5983_sample(&sample);
349 sample.x = sat_sub(sample.x, ao_mmc5983_offset.x);
350 sample.y = sat_sub(sample.y, ao_mmc5983_offset.y);
351 sample.z = sat_sub(sample.z, ao_mmc5983_offset.z);
352 ao_arch_block_interrupts();
353 ao_mmc5983_current = sample;
354 AO_DATA_PRESENT(AO_DATA_MMC5983);
356 ao_arch_release_interrupts();
360 static struct ao_task ao_mmc5983_task;
363 ao_mmc5983_show(void)
366 printf ("x0 %02x x1 %02x y0 %02x y1 %02x z0 %02x z1 %02x\n",
367 raw.x0, raw.x1, raw.y0, raw.y1, raw.z0, raw.z1);
369 printf ("set.x %d set.y %d set.z %d\n",
370 set.x, set.y, set.z);
372 printf ("reset.x %d reset.y %d reset.z %d\n",
373 reset.x, reset.y, reset.z);
375 printf ("offset.x %d offset.y %d offset.z %d\n",
378 ao_mmc5983_offset.z);
380 printf ("MMC5983: %d %d %d\n",
381 ao_mmc5983_along(&ao_mmc5983_current),
382 ao_mmc5983_across(&ao_mmc5983_current),
383 ao_mmc5983_through(&ao_mmc5983_current));
388 ao_mmc5983_recal(void)
390 printf("recal\n"); fflush(stdout);
392 printf("reboot\n"); fflush(stdout);
394 printf("cal\n"); fflush(stdout);
396 printf("show\n"); fflush(stdout);
398 printf("run\n"); fflush(stdout);
402 static const struct ao_cmds ao_mmc5983_cmds[] = {
403 { ao_mmc5983_show, "M\0Show MMC5983 status" },
405 { ao_mmc5983_recal, "m\0Recalibrate MMC5983" },
411 ao_mmc5983_init(void)
413 ao_mmc5983_configured = 0;
416 ao_enable_input(AO_MMC5983_SPI_MISO_PORT,
417 AO_MMC5983_SPI_MISO_PIN,
418 AO_EXTI_MODE_PULL_NONE);
420 ao_enable_output(AO_MMC5983_SPI_CLK_PORT,
421 AO_MMC5983_SPI_CLK_PIN,
424 ao_enable_output(AO_MMC5983_SPI_MOSI_PORT,
425 AO_MMC5983_SPI_MOSI_PIN,
428 ao_spi_init_cs(AO_MMC5983_SPI_CS_PORT, (1 << AO_MMC5983_SPI_CS_PIN));
431 ao_add_task(&ao_mmc5983_task, ao_mmc5983, "mmc5983");
432 ao_cmd_register(&ao_mmc5983_cmds[0]);