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 #include <ao_i2c_bit.h>
36 ao_mmc5983_reg_write(uint8_t addr, uint8_t data)
43 ao_i2c_bit_start(MMC5983_I2C_ADDR);
44 ao_i2c_bit_send(d, 2);
49 ao_mmc5983_reg_read(uint8_t addr)
53 ao_i2c_bit_start(MMC5983_I2C_ADDR);
55 ao_i2c_bit_send(d, 1);
56 ao_i2c_bit_restart(MMC5983_I2C_ADDR | 1);
57 ao_i2c_bit_recv(d, 1);
63 ao_mmc5983_raw(struct ao_mmc5983_raw *raw)
65 ao_i2c_bit_start(MMC5983_I2C_ADDR);
66 raw->addr = MMC5983_X_OUT_0;
67 ao_i2c_bit_send(&(raw->addr), 1);
68 ao_i2c_bit_restart(MMC5983_I2C_ADDR | 1);
69 ao_i2c_bit_recv(&(raw->x0), sizeof(*raw) - 1);
74 #define AO_MMC5983_SPI_SPEED ao_spi_speed(2000000)
77 ao_mmc5983_start(void) {
78 ao_spi_get_bit(AO_MMC5983_SPI_CS_PORT,
79 AO_MMC5983_SPI_CS_PIN,
81 AO_MMC5983_SPI_SPEED);
85 ao_mmc5983_stop(void) {
86 ao_spi_put_bit(AO_MMC5983_SPI_CS_PORT,
87 AO_MMC5983_SPI_CS_PIN,
88 AO_MMC5983_SPI_INDEX);
93 ao_mmc5983_reg_write(uint8_t addr, uint8_t data)
100 ao_spi_send(d, 2, AO_MMC5983_SPI_INDEX);
105 ao_mmc5983_reg_read(uint8_t addr)
109 d[0] = addr | MMC5983_READ;
111 ao_spi_duplex(d, d, 2, AO_MMC5983_SPI_INDEX);
118 ao_mmc5983_duplex(uint8_t *dst, uint8_t len)
121 ao_spi_duplex(dst, dst, len, AO_MMC5983_SPI_INDEX);
126 ao_mmc5983_raw(struct ao_mmc5983_raw *raw)
128 raw->addr = MMC5983_X_OUT_0 | MMC5983_READ;
129 ao_mmc5983_duplex((uint8_t *) raw, sizeof (*raw));
133 /* Saturating subtraction. Keep the result within range
137 sat_sub(int16_t a, int16_t b)
139 int32_t v = (int32_t) a - (int32_t) b;
147 /* Wait for a synchronous sample to finish */
149 ao_mmc5983_wait(void)
152 uint8_t status = ao_mmc5983_reg_read(MMC5983_STATUS);
153 if ((status & (1 << MMC5983_STATUS_MEAS_M_DONE)) != 0)
162 ao_mmc5983_reg_write(MMC5983_CONTROL_0,
163 (1 << MMC5983_CONTROL_0_SET));
167 ao_mmc5983_reset(void)
169 ao_mmc5983_reg_write(MMC5983_CONTROL_0,
170 (1 << MMC5983_CONTROL_0_RESET));
173 static struct ao_mmc5983_raw raw;
175 /* Read the sensor values and convert to a sample struct */
177 ao_mmc5983_sample(struct ao_mmc5983_sample *s)
179 ao_mmc5983_raw(&raw);
181 /* Bias by 32768 to convert from uint16_t to int16_t */
182 s->x = (int16_t) ((((uint16_t) raw.x0 << 8) | raw.x1) - 32768);
183 s->y = (int16_t) ((((uint16_t) raw.y0 << 8) | raw.y1) - 32768);
184 s->z = (int16_t) ((((uint16_t) raw.z0 << 8) | raw.z1) - 32768);
187 /* Synchronously sample the sensors */
189 ao_mmc5983_sync_sample(struct ao_mmc5983_sample *v)
191 ao_mmc5983_reg_write(MMC5983_CONTROL_0,
192 (1 << MMC5983_CONTROL_0_TM_M));
194 ao_mmc5983_sample(v);
197 static struct ao_mmc5983_sample set, reset;
199 /* Calibrate the device by finding the zero point */
205 ao_delay(AO_MS_TO_TICKS(100));
207 /* Measure in 'SET' mode */
209 ao_delay(AO_MS_TO_TICKS(100));
210 ao_mmc5983_sync_sample(&set);
212 ao_delay(AO_MS_TO_TICKS(100));
214 /* Measure in 'RESET' mode */
216 ao_delay(AO_MS_TO_TICKS(100));
217 ao_mmc5983_sync_sample(&reset);
219 /* The zero point is the average of SET and RESET values */
220 ao_mmc5983_offset.x = (int16_t) (((int32_t) set.x + (int32_t) reset.x) / 2);
221 ao_mmc5983_offset.y = (int16_t) (((int32_t) set.y + (int32_t) reset.y) / 2);
222 ao_mmc5983_offset.z = (int16_t) (((int32_t) set.z + (int32_t) reset.z) / 2);
225 /* Configure the device to automatically sample at 200Hz */
229 /* Set bandwidth to 200Hz */
230 ao_mmc5983_reg_write(MMC5983_CONTROL_1,
231 MMC5983_CONTROL_1_BW_200 << MMC5983_CONTROL_1_BW);
233 /* Measure at 200Hz so we get recent samples by just reading
236 ao_mmc5983_reg_write(MMC5983_CONTROL_2,
237 (1 << MMC5983_CONTROL_2_CMM_EN) |
238 (MMC5983_CONTROL_2_CM_FREQ_200HZ << MMC5983_CONTROL_2_CM_FREQ) |
239 (0 << MMC5983_CONTROL_2_EN_PRD_SET) |
240 (MMC5983_CONTROL_2_PRD_SET_1000));
241 ao_mmc5983_configured = 1;
244 /* Reboot the device by setting the SW_RST bit and waiting 10ms */
246 ao_mmc5983_reboot(void)
248 ao_mmc5983_configured = 0;
250 ao_mmc5983_reg_write(MMC5983_CONTROL_1,
251 1 << MMC5983_CONTROL_1_SW_RST);
253 /* Delay for power up time (10ms) */
254 ao_delay(AO_MS_TO_TICKS(10));
257 /* Configure the device for operation */
259 ao_mmc5983_setup(void)
263 /* Reboot the device */
266 /* Check product ID */
267 product_id = ao_mmc5983_reg_read(MMC5983_PRODUCT_ID);
268 if (product_id != MMC5983_PRODUCT_ID_PRODUCT_I2C &&
269 product_id != MMC5983_PRODUCT_ID_PRODUCT_SPI)
271 AO_SENSOR_ERROR(AO_DATA_MMC5983);
277 /* Start automatic sampling */
283 struct ao_mmc5983_sample ao_mmc5983_current;
288 struct ao_mmc5983_sample sample;
291 if (ao_mmc5983_configured)
292 ao_mmc5983_sample(&sample);
293 sample.x = sat_sub(sample.x, ao_mmc5983_offset.x);
294 sample.y = sat_sub(sample.y, ao_mmc5983_offset.y);
295 sample.z = sat_sub(sample.z, ao_mmc5983_offset.z);
296 ao_arch_block_interrupts();
297 ao_mmc5983_current = sample;
298 AO_DATA_PRESENT(AO_DATA_MMC5983);
300 ao_arch_release_interrupts();
304 static struct ao_task ao_mmc5983_task;
307 ao_mmc5983_show(void)
310 printf ("x0 %02x x1 %02x y0 %02x y1 %02x z0 %02x z1 %02x\n",
311 raw.x0, raw.x1, raw.y0, raw.y1, raw.z0, raw.z1);
313 printf ("set.x %d set.y %d set.z %d\n",
314 set.x, set.y, set.z);
316 printf ("reset.x %d reset.y %d reset.z %d\n",
317 reset.x, reset.y, reset.z);
319 printf ("offset.x %d offset.y %d offset.z %d\n",
322 ao_mmc5983_offset.z);
324 printf ("MMC5983: %d %d %d\n",
325 ao_mmc5983_along(&ao_mmc5983_current),
326 ao_mmc5983_across(&ao_mmc5983_current),
327 ao_mmc5983_through(&ao_mmc5983_current));
332 ao_mmc5983_recal(void)
334 printf("recal\n"); fflush(stdout);
336 printf("reboot\n"); fflush(stdout);
338 printf("cal\n"); fflush(stdout);
340 printf("show\n"); fflush(stdout);
342 printf("run\n"); fflush(stdout);
346 static const struct ao_cmds ao_mmc5983_cmds[] = {
347 { ao_mmc5983_show, "M\0Show MMC5983 status" },
349 { ao_mmc5983_recal, "m\0Recalibrate MMC5983" },
355 ao_mmc5983_init(void)
357 ao_mmc5983_configured = 0;
360 ao_enable_output(AO_MMC5983_SPI_CS_PORT, AO_MMC5983_SPI_CS_PIN, 1);
362 ao_enable_input(AO_MMC5983_SPI_MISO_PORT,
363 AO_MMC5983_SPI_MISO_PIN,
364 AO_EXTI_MODE_PULL_NONE);
366 ao_enable_output(AO_MMC5983_SPI_CLK_PORT,
367 AO_MMC5983_SPI_CLK_PIN,
370 ao_enable_output(AO_MMC5983_SPI_MOSI_PORT,
371 AO_MMC5983_SPI_MOSI_PIN,
374 ao_spi_init_cs(AO_MMC5983_SPI_CS_PORT, (1 << AO_MMC5983_SPI_CS_PIN));
377 ao_add_task(&ao_mmc5983_task, ao_mmc5983, "mmc5983");
378 ao_cmd_register(&ao_mmc5983_cmds[0]);