X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_mmc5983.c;h=d086fc3e5941aa941ad3a79b53205b6016c6cde9;hb=a3fd78224f222adc4cec394a2c8f4153d6e8571c;hp=7160ea1a2cf248150b77ef155104cb85a6aceddc;hpb=98e54df2385fcaf263de14f89a1f0728bfdd541a;p=fw%2Faltos diff --git a/src/drivers/ao_mmc5983.c b/src/drivers/ao_mmc5983.c index 7160ea1a..d086fc3e 100644 --- a/src/drivers/ao_mmc5983.c +++ b/src/drivers/ao_mmc5983.c @@ -22,7 +22,112 @@ #if HAS_MMC5983 -#define AO_MMC5983_SPI_SPEED ao_spi_speed(10000000) +#define DEBUG_MMC5983 0 + +struct ao_mmc5983_sample ao_mmc5983_current; +static struct ao_mmc5983_sample ao_mmc5983_offset; + +static uint8_t ao_mmc5983_configured; + +#ifdef MMC5983_I2C +#ifdef AO_MMC5983_I2C_INDEX + +static void +ao_mmc5983_start(void) { + ao_i2c_get(AO_MMC5983_I2C_INDEX); +} + +static void +ao_mmc5983_stop(void) { + ao_i2c_put(AO_MMC5983_I2C_INDEX); +} + +static void +ao_mmc5983_reg_write(uint8_t addr, uint8_t data) +{ + uint8_t d[2]; + + d[0] = addr; + d[1] = data; + + ao_mmc5983_start(); + ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR); + ao_i2c_send(d, 2, AO_MMC5983_I2C_INDEX, true); + ao_mmc5983_stop(); +} + +static uint8_t +ao_mmc5983_reg_read(uint8_t addr) +{ + uint8_t d[1]; + + ao_mmc5983_start(); + ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR); + d[0] = addr; + ao_i2c_send(d, 1, AO_MMC5983_I2C_INDEX, false); + ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR | 1); + ao_i2c_recv(d, 1, AO_MMC5983_I2C_INDEX, true); + ao_mmc5983_stop(); + return d[0]; +} + +static void +ao_mmc5983_raw(struct ao_mmc5983_raw *raw) +{ + ao_mmc5983_start(); + ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR); + raw->addr = MMC5983_X_OUT_0; + ao_i2c_send(&(raw->addr), 1, AO_MMC5983_I2C_INDEX, false); + ao_i2c_start(AO_MMC5983_I2C_INDEX, MMC5983_I2C_ADDR | 1); + ao_i2c_recv(&(raw->x0), sizeof(*raw) - 1, AO_MMC5983_I2C_INDEX, true); + ao_mmc5983_stop(); +} + +#else +#include + +static void +ao_mmc5983_reg_write(uint8_t addr, uint8_t data) +{ + uint8_t d[2]; + + d[0] = addr; + d[1] = data; + + ao_i2c_bit_start(MMC5983_I2C_ADDR); + ao_i2c_bit_send(d, 2); + ao_i2c_bit_stop(); +} + +static uint8_t +ao_mmc5983_reg_read(uint8_t addr) +{ + uint8_t d[1]; + + ao_i2c_bit_start(MMC5983_I2C_ADDR); + d[0] = addr; + ao_i2c_bit_send(d, 1); + ao_i2c_bit_restart(MMC5983_I2C_ADDR | 1); + ao_i2c_bit_recv(d, 1); + ao_i2c_bit_stop(); + return d[0]; +} + +static void +ao_mmc5983_raw(struct ao_mmc5983_raw *raw) +{ + ao_i2c_bit_start(MMC5983_I2C_ADDR); + raw->addr = MMC5983_X_OUT_0; + ao_i2c_bit_send(&(raw->addr), 1); + ao_i2c_bit_restart(MMC5983_I2C_ADDR | 1); + ao_i2c_bit_recv(&(raw->x0), sizeof(*raw) - 1); + ao_i2c_bit_stop(); +} + +#endif + +#else +#define AO_MMC5983_SPI_SPEED ao_spi_speed(2000000) static void ao_mmc5983_start(void) { @@ -39,9 +144,6 @@ ao_mmc5983_stop(void) { AO_MMC5983_SPI_INDEX); } -struct ao_mmc5983_sample ao_mmc5983_current; - -static uint8_t ao_mmc5983_configured; static void ao_mmc5983_reg_write(uint8_t addr, uint8_t data) @@ -77,35 +179,109 @@ ao_mmc5983_duplex(uint8_t *dst, uint8_t len) } static void -ao_mmc5983_sample(struct ao_mmc5983_sample *sample) +ao_mmc5983_raw(struct ao_mmc5983_raw *raw) +{ + raw->addr = MMC5983_X_OUT_0 | MMC5983_READ; + ao_mmc5983_duplex((uint8_t *) raw, sizeof (*raw)); +} +#endif + +/* Saturating subtraction. Keep the result within range + * of an int16_t + */ +static int16_t +sat_sub(int16_t a, int16_t b) +{ + int32_t v = (int32_t) a - (int32_t) b; + if (v < -32768) + v = -32768; + if (v > 32767) + v = 32767; + return (int16_t) v; +} + +/* Wait for a synchronous sample to finish */ +static void +ao_mmc5983_wait(void) { - struct ao_mmc5983_raw raw; + for (;;) { + uint8_t status = ao_mmc5983_reg_read(MMC5983_STATUS); + if ((status & (1 << MMC5983_STATUS_MEAS_M_DONE)) != 0) + break; + ao_delay(0); + } +} - raw.addr = MMC5983_X_OUT_0 | MMC5983_READ; - ao_mmc5983_duplex((uint8_t *) &raw, sizeof (raw)); +static void +ao_mmc5983_set(void) +{ + ao_mmc5983_reg_write(MMC5983_CONTROL_0, + (1 << MMC5983_CONTROL_0_SET)); +} - sample->x = raw.x0 << 10 | raw.x1 << 2 | ((raw.xyz2 >> 6) & 3); - sample->y = raw.y0 << 10 | raw.y1 << 2 | ((raw.xyz2 >> 4) & 3); - sample->z = raw.z0 << 10 | raw.z1 << 2 | ((raw.xyz2 >> 2) & 3); +static void +ao_mmc5983_reset(void) +{ + ao_mmc5983_reg_write(MMC5983_CONTROL_0, + (1 << MMC5983_CONTROL_0_RESET)); } -static uint8_t product_id; +static struct ao_mmc5983_raw raw; -static uint8_t -ao_mmc5983_setup(void) +/* Read the sensor values and convert to a sample struct */ +static void +ao_mmc5983_sample(struct ao_mmc5983_sample *s) { - if (ao_mmc5983_configured) - return 1; + ao_mmc5983_raw(&raw); - /* Place device in 3-wire mode */ - ao_mmc5983_reg_write(MMC5983_CONTROL_3, - 1 << MMC5983_CONTROL_3_SPI_3W); + /* Bias by 32768 to convert from uint16_t to int16_t */ + s->x = (int16_t) ((((uint16_t) raw.x0 << 8) | raw.x1) - 32768); + s->y = (int16_t) ((((uint16_t) raw.y0 << 8) | raw.y1) - 32768); + s->z = (int16_t) ((((uint16_t) raw.z0 << 8) | raw.z1) - 32768); +} - /* Check product ID */ - product_id = ao_mmc5983_reg_read(MMC5983_PRODUCT_ID); - if (product_id != MMC5983_PRODUCT_ID_PRODUCT) - AO_SENSOR_ERROR(AO_DATA_MMC5983); +/* 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 = (int16_t) (((int32_t) set.x + (int32_t) reset.x) / 2); + ao_mmc5983_offset.y = (int16_t) (((int32_t) set.y + (int32_t) reset.y) / 2); + ao_mmc5983_offset.z = (int16_t) (((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); @@ -115,9 +291,48 @@ ao_mmc5983_setup(void) */ ao_mmc5983_reg_write(MMC5983_CONTROL_2, (1 << MMC5983_CONTROL_2_CMM_EN) | - (MMC5983_CONTROL_2_CM_FREQ_200HZ << MMC5983_CONTROL_2_CM_FREQ)); - + (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; + + ao_mmc5983_reg_write(MMC5983_CONTROL_1, + 1 << MMC5983_CONTROL_1_SW_RST); + + /* Delay for power up time (10ms) */ + ao_delay(AO_MS_TO_TICKS(10)); +} + +/* Configure the device for operation */ +static uint8_t +ao_mmc5983_setup(void) +{ + uint8_t product_id; + + /* Reboot the device */ + ao_mmc5983_reboot(); + + /* Check product ID */ + product_id = ao_mmc5983_reg_read(MMC5983_PRODUCT_ID); + if (product_id != MMC5983_PRODUCT_ID_PRODUCT_I2C && + product_id != MMC5983_PRODUCT_ID_PRODUCT_SPI) + { + AO_SENSOR_ERROR(AO_DATA_MMC5983); + } + + /* Calibrate */ + ao_mmc5983_cal(); + + /* Start automatic sampling */ + ao_mmc5983_run(); + return 1; } @@ -129,7 +344,11 @@ ao_mmc5983(void) struct ao_mmc5983_sample sample; ao_mmc5983_setup(); for (;;) { - ao_mmc5983_sample(&sample); + if (ao_mmc5983_configured) + ao_mmc5983_sample(&sample); + sample.x = sat_sub(sample.x, ao_mmc5983_offset.x); + sample.y = sat_sub(sample.y, ao_mmc5983_offset.y); + sample.z = sat_sub(sample.z, ao_mmc5983_offset.z); ao_arch_block_interrupts(); ao_mmc5983_current = sample; AO_DATA_PRESENT(AO_DATA_MMC5983); @@ -143,12 +362,48 @@ static struct ao_task ao_mmc5983_task; static void ao_mmc5983_show(void) { - printf ("X: %d Z: %d Y: %d id %d\n", - ao_mmc5983_current.x, ao_mmc5983_current.z, ao_mmc5983_current.y, product_id); +#if DEBUG_MMC5983 + printf ("x0 %02x x1 %02x y0 %02x y1 %02x z0 %02x z1 %02x\n", + raw.x0, raw.x1, raw.y0, raw.y1, raw.z0, raw.z1); + + printf ("set.x %d set.y %d set.z %d\n", + set.x, set.y, set.z); + + printf ("reset.x %d reset.y %d reset.z %d\n", + reset.x, reset.y, reset.z); + + printf ("offset.x %d offset.y %d offset.z %d\n", + ao_mmc5983_offset.x, + ao_mmc5983_offset.y, + ao_mmc5983_offset.z); +#endif + printf ("MMC5983: %d %d %d\n", + ao_mmc5983_along(&ao_mmc5983_current), + ao_mmc5983_across(&ao_mmc5983_current), + ao_mmc5983_through(&ao_mmc5983_current)); } +#if DEBUG_MMC5983 +static void +ao_mmc5983_recal(void) +{ + printf("recal\n"); fflush(stdout); + ao_mmc5983_reboot(); + printf("reboot\n"); fflush(stdout); + ao_mmc5983_cal(); + printf("cal\n"); fflush(stdout); + ao_mmc5983_show(); + printf("show\n"); fflush(stdout); + ao_mmc5983_run(); + printf("run\n"); fflush(stdout); +} +#endif + static const struct ao_cmds ao_mmc5983_cmds[] = { { ao_mmc5983_show, "M\0Show MMC5983 status" }, +#if DEBUG_MMC5983 + { ao_mmc5983_recal, "m\0Recalibrate MMC5983" }, +#endif { 0, NULL } }; @@ -157,7 +412,21 @@ ao_mmc5983_init(void) { ao_mmc5983_configured = 0; +#ifndef MMC5983_I2C + ao_enable_input(AO_MMC5983_SPI_MISO_PORT, + AO_MMC5983_SPI_MISO_PIN, + AO_EXTI_MODE_PULL_NONE); + + ao_enable_output(AO_MMC5983_SPI_CLK_PORT, + AO_MMC5983_SPI_CLK_PIN, + 1); + + ao_enable_output(AO_MMC5983_SPI_MOSI_PORT, + AO_MMC5983_SPI_MOSI_PIN, + 0); + ao_spi_init_cs(AO_MMC5983_SPI_CS_PORT, (1 << AO_MMC5983_SPI_CS_PIN)); +#endif ao_add_task(&ao_mmc5983_task, ao_mmc5983, "mmc5983"); ao_cmd_register(&ao_mmc5983_cmds[0]);