X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_hmc5883.c;h=782d03f447e7a79a619b0e79b4814c0fcb4c9ae2;hp=43d04f70c5edae83f3e1861bfc5aa0dcc6cf1c7a;hb=760b1f02c178c600226f39b5e66d8cbadbf4a29b;hpb=6a973f788563ccc66b01cc7557a004dabef18d09 diff --git a/src/drivers/ao_hmc5883.c b/src/drivers/ao_hmc5883.c index 43d04f70..782d03f4 100644 --- a/src/drivers/ao_hmc5883.c +++ b/src/drivers/ao_hmc5883.c @@ -19,106 +19,136 @@ #include #include -static uint8_t ao_hmc5883_wake; -static uint8_t ao_hmc5883_configured; +#if HAS_HMC5883 -static void -ao_hmc5883_isr(void) -{ - ao_exti_disable(&AO_HMC5883_INT_PORT, AO_HMC5883_INT_PIN); - ao_hmc5883_wake = 1; - ao_wakeup(&ao_hmc5883_wake); -} +static uint8_t ao_hmc5883_configured; -static uint8_t ao_hmc5883_addr_reg; +static uint8_t ao_hmc5883_addr; static void -ao_hmc5883_update_addr (uint8_t len) +ao_hmc5883_reg_write(uint8_t addr, uint8_t data) { - ao_hmc5883_addr_reg += len; - if (ao_hmc5883_addr_reg == 9) - ao_hmc5883_addr_reg = 3; - else if (ao_hmc5883_addr_reg > 12) - ao_hmc5883_addr_reg = 0; - -} + uint8_t d[2]; -static void -ao_hmc5883_write(uint8_t addr, uint8_t *data, uint8_t len) -{ + d[0] = addr; + d[1] = data; ao_i2c_get(AO_HMC5883_I2C_INDEX); ao_i2c_start(AO_HMC5883_I2C_INDEX, HMC5883_ADDR_WRITE); - ao_i2c_send(&addr, 1, AO_HMC5883_I2C_INDEX, FALSE); - ao_hmc5883_addr_reg = addr; - ao_i2c_send(data, len, AO_HMC5883_I2C_INDEX, TRUE); - ao_hmc5883_update_addr(len); + ao_i2c_send(d, 2, AO_HMC5883_I2C_INDEX, TRUE); ao_i2c_put(AO_HMC5883_I2C_INDEX); + ao_hmc5883_addr = addr + 1; } static void ao_hmc5883_read(uint8_t addr, uint8_t *data, uint8_t len) { ao_i2c_get(AO_HMC5883_I2C_INDEX); - if (addr != ao_hmc5883_addr_reg) { + if (addr != ao_hmc5883_addr) { ao_i2c_start(AO_HMC5883_I2C_INDEX, HMC5883_ADDR_WRITE); ao_i2c_send(&addr, 1, AO_HMC5883_I2C_INDEX, FALSE); - ao_hmc5883_addr_reg = addr; } ao_i2c_start(AO_HMC5883_I2C_INDEX, HMC5883_ADDR_READ); ao_i2c_recv(data, len, AO_HMC5883_I2C_INDEX, TRUE); - ao_hmc5883_update_addr(len); ao_i2c_put(AO_HMC5883_I2C_INDEX); + ao_hmc5883_addr = 0xff; +} + +static uint8_t ao_hmc5883_done; + +static void +ao_hmc5883_isr(void) +{ + ao_exti_disable(AO_HMC5883_INT_PORT, AO_HMC5883_INT_PIN); + ao_hmc5883_done = 1; + ao_wakeup(&ao_hmc5883_done); +} + +static uint32_t ao_hmc5883_missed_irq; + +void +ao_hmc5883_sample(struct ao_hmc5883_sample *sample) +{ + uint16_t *d = (uint16_t *) sample; + int i = sizeof (*sample) / 2; + uint8_t single = HMC5883_MODE_SINGLE; + + ao_hmc5883_done = 0; + ao_exti_enable(AO_HMC5883_INT_PORT, AO_HMC5883_INT_PIN); + ao_hmc5883_reg_write(HMC5883_MODE, HMC5883_MODE_SINGLE); + + ao_alarm(AO_MS_TO_TICKS(10)); + ao_arch_block_interrupts(); + while (!ao_hmc5883_done) + if (ao_sleep(&ao_hmc5883_done)) + ++ao_hmc5883_missed_irq; + ao_arch_release_interrupts(); + ao_clear_alarm(); + + ao_hmc5883_read(HMC5883_X_MSB, (uint8_t *) sample, sizeof (struct ao_hmc5883_sample)); +#if __BYTE_ORDER == __LITTLE_ENDIAN + /* byte swap */ + while (i--) { + uint16_t t = *d; + *d++ = (t >> 8) | (t << 8); + } +#endif } static uint8_t ao_hmc5883_setup(void) { + uint8_t d; uint8_t present; + if (ao_hmc5883_configured) return 1; - /* Enable the EXTI interrupt for the appropriate pin */ - ao_enable_port(AO_HMC5883_INT_PORT); - ao_exti_setup(&AO_HMC5883_INT_PORT, AO_HMC5883_INT_PIN, - AO_EXTI_MODE_FALLING, ao_hmc5883_isr); - - ao_hmc5883_addr_reg = 0xff; - ao_i2c_get(AO_HMC5883_I2C_INDEX); present = ao_i2c_start(AO_HMC5883_I2C_INDEX, HMC5883_ADDR_READ); - ao_i2c_recv(NULL, 0, AO_HMC5883_I2C_INDEX, TRUE); + ao_i2c_recv(&d, 1, AO_HMC5883_I2C_INDEX, TRUE); ao_i2c_put(AO_HMC5883_I2C_INDEX); + if (!present) - return 0; + ao_panic(AO_PANIC_SELF_TEST_HMC5883); + + ao_hmc5883_reg_write(HMC5883_CONFIG_A, + (HMC5883_CONFIG_A_MA_8 << HMC5883_CONFIG_A_MA) | + (HMC5883_CONFIG_A_DO_15 << HMC5883_CONFIG_A_DO) | + (HMC5883_CONFIG_A_MS_NORMAL << HMC5883_CONFIG_A_MS)); + + ao_hmc5883_reg_write(HMC5883_CONFIG_B, + (HMC5883_CONFIG_B_GN_1_3 << HMC5883_CONFIG_B_GN)); + ao_hmc5883_configured = 1; return 1; } +struct ao_hmc5883_sample ao_hmc5883_current; + static void -ao_hmc5883_show(void) +ao_hmc5883(void) { - uint8_t addr, data; - - for (addr = 0x00; addr <= 0x7f; addr++) - { - ao_i2c_get(AO_HMC5883_I2C_INDEX); - data = ao_i2c_start(AO_HMC5883_I2C_INDEX, addr << 1); - ao_i2c_recv(NULL, 0, AO_HMC5883_I2C_INDEX, TRUE); - ao_i2c_put(AO_HMC5883_I2C_INDEX); - if (data) - printf("address %02x responds\n", addr << 1); - } - if (!ao_hmc5883_setup()) { - printf("hmc5883 not present\n"); - return; - } - for (addr = 0; addr <= 12; addr++) { - ao_hmc5883_read(addr, &data, 1); - printf ("hmc5883 register %2d: %02x\n", - addr, data); + ao_hmc5883_setup(); + for (;;) { + ao_hmc5883_sample(&ao_hmc5883_current); + ao_arch_critical( + AO_DATA_PRESENT(AO_DATA_HMC5883); + AO_DATA_WAIT(); + ); } } +static struct ao_task ao_hmc5883_task; + +static void +ao_hmc5883_show(void) +{ + struct ao_data sample; + ao_data_get(&sample); + printf ("X: %d Y: %d Z: %d missed irq: %lu\n", + sample.hmc5883.x, sample.hmc5883.y, sample.hmc5883.z, ao_hmc5883_missed_irq); +} + static const struct ao_cmds ao_hmc5883_cmds[] = { { ao_hmc5883_show, "M\0Show HMC5883 status" }, { 0, NULL } @@ -129,5 +159,14 @@ ao_hmc5883_init(void) { ao_hmc5883_configured = 0; + ao_enable_port(AO_HMC5883_INT_PORT); + ao_exti_setup(AO_HMC5883_INT_PORT, + AO_HMC5883_INT_PIN, + AO_EXTI_MODE_FALLING | AO_EXTI_MODE_PULL_UP, + ao_hmc5883_isr); + + ao_add_task(&ao_hmc5883_task, ao_hmc5883, "hmc5883"); ao_cmd_register(&ao_hmc5883_cmds[0]); } + +#endif