diff options
Diffstat (limited to 'src/drivers/ao_bmx160.c')
| -rw-r--r-- | src/drivers/ao_bmx160.c | 422 |
1 files changed, 422 insertions, 0 deletions
diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c new file mode 100644 index 00000000..4c3c2a9f --- /dev/null +++ b/src/drivers/ao_bmx160.c @@ -0,0 +1,422 @@ +/* + * Copyright © 2019 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_bmx160.h> +#include <ao_exti.h> + +static uint8_t ao_bmx160_configured; + +#define ao_bmx160_spi_get() ao_spi_get(AO_BMX160_SPI_BUS, AO_SPI_SPEED_8MHz) +#define ao_bmx160_spi_put() ao_spi_put(AO_BMX160_SPI_BUS) + +#define ao_bmx160_spi_start() ao_spi_set_cs(AO_BMX160_SPI_CS_PORT, \ + (1 << AO_BMX160_SPI_CS_PIN)) + +#define ao_bmx160_spi_end() ao_spi_clr_cs(AO_BMX160_SPI_CS_PORT, \ + (1 << AO_BMX160_SPI_CS_PIN)) + +static void +_ao_bmx160_reg_write(uint8_t addr, uint8_t value) +{ + uint8_t d[2] = { addr, value }; + ao_bmx160_spi_start(); + ao_spi_send(d, 2, AO_BMX160_SPI_BUS); + ao_bmx160_spi_end(); +} + +static void +_ao_bmx160_read(uint8_t addr, void *data, uint8_t len) +{ + addr |= 0x80; + ao_bmx160_spi_start(); + ao_spi_send(&addr, 1, AO_BMX160_SPI_BUS); + ao_spi_recv(data, len, AO_BMX160_SPI_BUS); + ao_bmx160_spi_end(); +} + +static uint8_t +_ao_bmx160_reg_read(uint8_t addr) +{ + uint8_t value; + addr |= 0x80; + ao_bmx160_spi_start(); + ao_spi_send(&addr, 1, AO_BMX160_SPI_BUS); + ao_spi_recv(&value, 1, AO_BMX160_SPI_BUS); + ao_bmx160_spi_end(); + return value; +} + +static void +_ao_bmx160_cmd(uint8_t cmd) +{ + _ao_bmx160_reg_write(BMX160_CMD, cmd); + ao_delay(AO_MS_TO_TICKS(100)); +} + +static void +_ao_bmx160_mag_setup(void) +{ + _ao_bmx160_reg_write(BMX160_MAG_IF_0, 0x80); +} + +static void +_ao_bmm150_wait_manual(void) +{ + while (_ao_bmx160_reg_read(BMX160_STATUS) & (1 << BMX160_STATUS_MAG_MAN_OP)) + ; +} + +static void +_ao_bmm150_reg_write(uint8_t addr, uint8_t data) +{ + _ao_bmx160_reg_write(BMX160_MAG_IF_3, data); + _ao_bmx160_reg_write(BMX160_MAG_IF_2, addr); + _ao_bmm150_wait_manual(); +} + +static uint8_t +_ao_bmm150_reg_read(uint8_t addr) +{ + _ao_bmx160_reg_write(BMX160_MAG_IF_1, addr); + _ao_bmm150_wait_manual(); + return _ao_bmx160_reg_read(BMX160_DATA_0); +} + +static void +_ao_bmx160_sample(struct ao_bmx160_sample *sample) +{ + _ao_bmx160_read(BMX160_MAG_X_0_7, sample, sizeof (*sample)); +#if __BYTE_ORDER != __LITTLE_ENDIAN + int i = sizeof (*sample) / 2; + uint16_t *d = (uint16_t *) sample; + + /* byte swap */ + while (i--) { + uint16_t t = *d; + *d++ = (t >> 8) | (t << 8); + } +#endif +} + +#define G 981 /* in cm/s² */ + +#if 0 +static int16_t /* cm/s² */ +ao_bmx160_accel(int16_t v) +{ + return (int16_t) ((v * (int32_t) (16.0 * 980.665 + 0.5)) / 32767); +} + +static int16_t /* deg*10/s */ +ao_bmx160_gyro(int16_t v) +{ + return (int16_t) ((v * (int32_t) 20000) / 32767); +} + +static uint8_t +ao_bmx160_accel_check(int16_t normal, int16_t test) +{ + int16_t diff = test - normal; + + if (diff < BMX160_ST_ACCEL(16) / 4) { + return 1; + } + if (diff > BMX160_ST_ACCEL(16) * 4) { + return 1; + } + return 0; +} + +static uint8_t +ao_bmx160_gyro_check(int16_t normal, int16_t test) +{ + int16_t diff = test - normal; + + if (diff < 0) + diff = -diff; + if (diff < BMX160_ST_GYRO(2000) / 4) { + return 1; + } + if (diff > BMX160_ST_GYRO(2000) * 4) { + return 1; + } + return 0; +} +#endif + +static void +_ao_bmx160_wait_alive(void) +{ + uint8_t i; + + /* Wait for the chip to wake up */ + for (i = 0; i < 30; i++) { + ao_delay(AO_MS_TO_TICKS(100)); + if (_ao_bmx160_reg_read(BMX160_CHIPID) == BMX160_CHIPID_BMX160) + break; + } + if (i == 30) + ao_panic(AO_PANIC_SELF_TEST_BMX160); +} + +#define ST_TRIES 10 +#define MAG_TRIES 10 + +static void +_ao_bmx160_setup(void) +{ + if (ao_bmx160_configured) + return; + + /* Make sure the chip is responding */ + _ao_bmx160_wait_alive(); + + /* Reboot */ + _ao_bmx160_cmd(BMX160_CMD_SOFTRESET); + + /* Force SPI mode */ + _ao_bmx160_reg_write(BMX160_NV_CONF, 1 << BMX160_NV_CONF_SPI_EN); + + /* Configure accelerometer: + * + * undersampling disabled + * normal filter + * 200Hz sampling rate + * 16g range + * + * This yields a 3dB cutoff frequency of 80Hz + */ + _ao_bmx160_reg_write(BMX160_ACC_CONF, + (0 << BMX160_ACC_CONF_ACC_US) | + (BMX160_ACC_CONF_ACC_BWP_NORMAL << BMX160_ACC_CONF_ACC_BWP) | + (BMX160_ACC_CONF_ACC_ODR_200 << BMX160_ACC_CONF_ACC_ODR)); + _ao_bmx160_reg_write(BMX160_ACC_RANGE, + BMX160_ACC_RANGE_16G); + + /* Configure gyro: + * + * 200Hz sampling rate + * Normal filter mode + * ±2000°/s + */ + _ao_bmx160_reg_write(BMX160_GYR_CONF, + (BMX160_GYR_CONF_GYR_BWP_NORMAL << BMX160_GYR_CONF_GYR_BWP) | + (BMX160_GYR_CONF_GYR_ODR_200 << BMX160_GYR_CONF_GYR_ODR)); + _ao_bmx160_reg_write(BMX160_GYR_RANGE, + BMX160_GYR_RANGE_2000); + + + /* Configure magnetometer: + * + * 30Hz sampling rate + * power on + * axes enabled + */ + _ao_bmx160_cmd(BMX160_CMD_MAG_IF_SET_PMU_MODE(BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_NORMAL)); + + /* Enter setup mode */ + _ao_bmx160_mag_setup(); + + /* Place in suspend mode to reboot the chip */ + _ao_bmm150_reg_write(BMM150_POWER_MODE, + (0 << BMM150_POWER_MODE_POWER_CONTROL)); + + /* Power on */ + _ao_bmm150_reg_write(BMM150_POWER_MODE, + (1 << BMM150_POWER_MODE_POWER_CONTROL)); + + /* Set data rate and place in sleep mode */ + _ao_bmm150_reg_write(BMM150_CONTROL, + (BMM150_CONTROL_DATA_RATE_30 << BMM150_CONTROL_DATA_RATE) | + (BMM150_CONTROL_OP_MODE_SLEEP << BMM150_CONTROL_OP_MODE)); + + /* enable all axes (should already be enabled) */ + _ao_bmm150_reg_write(BMM150_INT_CONF, + (0 << BMM150_INT_CONF_X_DISABLE) | + (0 << BMM150_INT_CONF_Y_DISABLE) | + (0 << BMM150_INT_CONF_Z_DISABLE)); + + /* Set repetition values (?) */ + _ao_bmm150_reg_write(BMM150_REPXY, BMM150_REPXY_VALUE(9)); + _ao_bmm150_reg_write(BMM150_REPZ, BMM150_REPZ_VALUE(15)); + + /* To get data out of the magnetometer, set the control op mode to 'forced', then read + * from the data registers + */ + _ao_bmx160_reg_write(BMX160_MAG_IF_3, (BMM150_CONTROL_OP_MODE_FORCED << BMM150_CONTROL_OP_MODE)); + _ao_bmx160_reg_write(BMX160_MAG_IF_2, BMM150_CONTROL); + _ao_bmx160_reg_write(BMX160_MAG_IF_1, BMM150_DATA_X_0_4); + + /* Set data rate to 200Hz */ + _ao_bmx160_reg_write(BMX160_MAG_CONF, + (BMX160_MAG_CONF_MAG_ODR_200 << BMX160_MAG_CONF_MAG_ODR)); + + /* Put magnetometer interface back into 'normal mode' + */ + _ao_bmx160_reg_write(BMX160_MAG_IF_0, + (0 << BMX160_MAG_IF_0_MAG_MANUAL_EN) | + (0 << BMX160_MAG_IF_0_MAG_OFFSET) | + (0 << BMX160_MAG_IF_0_MAG_RD_BURST)); + + /* Enable acc and gyr + */ + + _ao_bmx160_cmd(BMX160_CMD_ACC_SET_PMU_MODE(BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL)); + _ao_bmx160_cmd(BMX160_CMD_GYR_SET_PMU_MODE(BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL)); + ao_bmx160_configured = 1; +} + +struct ao_bmx160_sample ao_bmx160_current; + +static void +ao_bmx160(void) +{ + struct ao_bmx160_sample sample; + + /* ao_bmx160_init already grabbed the SPI bus and mutex */ + _ao_bmx160_setup(); + ao_bmx160_spi_put(); + for (;;) + { + ao_bmx160_spi_get(); + _ao_bmx160_sample(&sample); + ao_bmx160_spi_put(); + ao_arch_block_interrupts(); + ao_bmx160_current = sample; + AO_DATA_PRESENT(AO_DATA_BMX160); + AO_DATA_WAIT(); + ao_arch_release_interrupts(); + } +} + +static struct ao_task ao_bmx160_task; + +static void +ao_bmx160_show(void) +{ + printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d Mag: %7d %7d %7d\n", + ao_bmx160_current.acc_x, + ao_bmx160_current.acc_y, + ao_bmx160_current.acc_z, + ao_bmx160_current.gyr_x, + ao_bmx160_current.gyr_y, + ao_bmx160_current.gyr_z, + ao_bmx160_current.mag_x, + ao_bmx160_current.mag_y, + ao_bmx160_current.mag_z); +} + +#if BMX160_TEST + +static void +ao_bmx160_read(void) +{ + uint8_t addr; + uint8_t val; + + addr = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_bmx160_spi_get(); + val = _ao_bmx160_reg_read(addr); + ao_bmx160_spi_put(); + printf("Addr %02x val %02x\n", addr, val); +} + +static void +ao_bmx160_write(void) +{ + uint8_t addr; + uint8_t val; + + addr = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + val = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + printf("Addr %02x val %02x\n", addr, val); + ao_bmx160_spi_get(); + _ao_bmx160_reg_write(addr, val); + ao_bmx160_spi_put(); +} + +static void +ao_bmm150_read(void) +{ + uint8_t addr; + uint8_t val; + + addr = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_bmx160_spi_get(); + val = _ao_bmm150_reg_read(addr); + ao_bmx160_spi_put(); + printf("Addr %02x val %02x\n", addr, val); +} + +static void +ao_bmm150_write(void) +{ + uint8_t addr; + uint8_t val; + + addr = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + val = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + printf("Addr %02x val %02x\n", addr, val); + ao_bmx160_spi_get(); + _ao_bmm150_reg_write(addr, val); + ao_bmx160_spi_put(); +} + +#endif /* BMX160_TEST */ + +static const struct ao_cmds ao_bmx160_cmds[] = { + { ao_bmx160_show, "I\0Show BMX160 status" }, +#if BMX160_TEST + { ao_bmx160_read, "R <addr>\0Read BMX160 register" }, + { ao_bmx160_write, "W <addr> <val>\0Write BMX160 register" }, + { ao_bmm150_read, "M <addr>\0Read BMM150 register" }, + { ao_bmm150_write, "N <addr> <val>\0Write BMM150 register" }, +#endif + { 0, NULL } +}; + +void +ao_bmx160_init(void) +{ + ao_add_task(&ao_bmx160_task, ao_bmx160, "bmx160"); + + ao_spi_init_cs(AO_BMX160_SPI_CS_PORT, (1 << AO_BMX160_SPI_CS_PIN)); + + /* Pretend to be the bmx160 task. Grab the SPI bus right away and + * hold it for the task so that nothing else uses the SPI bus before + * we get the I2C mode disabled in the chip + */ + + ao_cur_task = &ao_bmx160_task; + ao_bmx160_spi_get(); + ao_cur_task = NULL; + ao_cmd_register(&ao_bmx160_cmds[0]); +} |