--- /dev/null
+/*
+ * Copyright © 2022 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_bmi088.h>
+#include <ao_data.h>
+
+#define AO_BMI088_SPI_SPEED ao_spi_speed(100000)
+
+#define ao_bmi088_spi_get() ao_spi_get(AO_BMI088_SPI_BUS, AO_BMI088_SPI_SPEED)
+#define ao_bmi088_spi_put() ao_spi_put(AO_BMI088_SPI_BUS)
+
+#define ao_bmi088_acc_start() ao_spi_set_cs(AO_BMI088_ACC_CS_PORT, \
+ (1 << AO_BMI088_ACC_CS_PIN))
+#define ao_bmi088_acc_end() ao_spi_clr_cs(AO_BMI088_ACC_CS_PORT, \
+ (1 << AO_BMI088_ACC_CS_PIN))
+#define ao_bmi088_gyr_start() ao_spi_set_cs(AO_BMI088_GYR_CS_PORT, \
+ (1 << AO_BMI088_GYR_CS_PIN))
+#define ao_bmi088_gyr_end() ao_spi_clr_cs(AO_BMI088_GYR_CS_PORT, \
+ (1 << AO_BMI088_GYR_CS_PIN))
+
+static uint8_t
+_ao_bmi088_acc_reg_read(uint8_t addr)
+{
+ uint8_t v[2];
+ addr |= 0x80;
+ ao_bmi088_acc_start();
+ ao_spi_send(&addr, 1, AO_BMI088_SPI_BUS);
+ ao_spi_recv(v, 2, AO_BMI088_SPI_BUS); /* part sends garbage for first byte */
+ ao_bmi088_acc_end();
+ return v[1];
+}
+
+static void
+_ao_bmi088_acc_reg_write(uint8_t addr, uint8_t value)
+{
+ uint8_t d[2] = { addr, value };
+ ao_bmi088_acc_start();
+ ao_spi_send(d, 2, AO_BMI088_SPI_BUS);
+ ao_bmi088_acc_end();
+}
+
+static void
+_ao_bmi088_acc_sample_read(struct ao_bmi088_acc_sample *sample)
+{
+ uint8_t dummy;
+ uint8_t addr = BMI088_ACC_DATA | 0x80;
+
+ ao_bmi088_acc_start();
+ ao_spi_send(&addr, 1, AO_BMI088_SPI_BUS);
+ ao_spi_recv(&dummy, 1, AO_BMI088_SPI_BUS); /* part sends garbage for first byte */
+ ao_spi_recv(sample, sizeof(struct ao_bmi088_acc_sample), AO_BMI088_SPI_BUS);
+ ao_bmi088_acc_end();
+}
+
+static void
+_ao_bmi088_gyr_read(uint8_t addr, void *data, uint8_t len)
+{
+ addr |= 0x80;
+ ao_bmi088_gyr_start();
+ ao_spi_send(&addr, 1, AO_BMI088_SPI_BUS);
+ ao_spi_recv(data, len, AO_BMI088_SPI_BUS);
+ ao_bmi088_gyr_end();
+}
+
+static uint8_t
+_ao_bmi088_gyr_reg_read(uint8_t addr)
+{
+ uint8_t v;
+ _ao_bmi088_gyr_read(addr, &v, 1);
+ return v;
+}
+
+static void
+_ao_bmi088_gyr_reg_write(uint8_t addr, uint8_t value)
+{
+ uint8_t d[2] = { addr, value };
+ ao_bmi088_gyr_start();
+ ao_spi_send(d, 2, AO_BMI088_SPI_BUS);
+ ao_bmi088_gyr_end();
+}
+
+static void
+_ao_bmi088_gyr_sample_read(struct ao_bmi088_gyr_sample *sample)
+{
+ _ao_bmi088_gyr_read(BMI088_GYRO_DATA, sample, sizeof (struct ao_bmi088_gyr_sample));
+}
+
+static void
+ao_bmi088_reset(void)
+{
+ ao_bmi088_spi_get();
+
+ /* reset the two devices */
+ _ao_bmi088_acc_reg_write(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_RESET);
+ _ao_bmi088_gyr_reg_write(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_RESET);
+
+ /* wait 30ms (that's how long the gyro takes */
+ ao_delay(AO_MS_TO_TICKS(30));
+
+ /* force acc part to SPI mode */
+ ao_bmi088_acc_start();
+
+ ao_delay(AO_MS_TO_TICKS(1));
+
+ ao_bmi088_acc_end();
+
+ ao_bmi088_spi_put();
+}
+
+static bool
+ao_bmi088_accel_good(int16_t pos, int16_t neg, int32_t good)
+{
+ int32_t diff = (int32_t) pos - (int32_t) neg;
+
+ return diff >= good;
+}
+
+static void
+ao_bmi088_setup(void)
+{
+ bool working = false;
+ struct ao_bmi088_acc_sample acc_pos, acc_neg;
+
+ ao_bmi088_reset();
+
+ ao_bmi088_spi_get();
+
+ /* Make sure the two devices are alive */
+ if (_ao_bmi088_acc_reg_read(BMI088_ACC_CHIP_ID) != BMI088_ACC_CHIP_ID_BMI088)
+ goto failure;
+
+ if (_ao_bmi088_gyr_reg_read(BMI088_GYRO_CHIP_ID) != BMI088_GYRO_CHIP_ID_BMI088)
+ goto failure;
+
+ /* Turn on the accelerometer */
+
+ _ao_bmi088_acc_reg_write(BMI088_ACC_PWR_CTRL, BMI088_ACC_PWR_CTRL_ON);
+
+ /* Wait 5ms after changing accel power mode */
+ ao_delay(AO_MS_TO_TICKS(5));
+
+ /* Accel self test. Procedure:
+ *
+ * 1) Set ±24g range by writing 0x03 to register ACC_RANGE (0x41)
+ * 2) Set ODR=1.6kHz, continuous sampling mode, “normal mode” (norm_avg4) by writing 0xA7 to
+ * register ACC_CONF (0x40)
+ * • Continuous filter function: set bit7 in ACC_CONF
+ * • “normal avg4 mode”: ACC_CONF |= 0x02<<4
+ * • ODR=1.6kHz: ACC_CONF |= 0x0C
+ * 3) Wait for > 2 ms
+ * 4) Enable the positive self-test polarity (i.e. write 0x0D to register ACC_SELF_TEST (0x6D))
+ * 5) Wait for > 50ms
+ * 6) Read the accelerometer offset values for each axis (positive self-test response)
+ * 7) Enable the negative self-test polarity (i.e. write 0x09 to register ACC_SELF_TEST (0x6D))
+ * 8) Wait for > 50ms
+ * 9) Read the accelerometer offset values for each axis (negative self-test response)
+ * 10) Disable the self-test (i.e. write 0x00 to register ACC_SELF_TEST (0x6D))
+ * 11) Calculate difference of positive and negative self-test response and compare with the expected
+ * values (see table below)
+ * 12) Wait for > 50ms to let the sensor settle to normal mode steady state operation
+ */
+
+ _ao_bmi088_acc_reg_write(BMI088_ACC_RANGE, BMI088_ACC_RANGE_24);
+
+ _ao_bmi088_acc_reg_write(BMI088_ACC_CONF,
+ (BMI088_ACC_CONF_BWP_NORMAL << BMI088_ACC_CONF_BWP) |
+ (BMI088_ACC_CONF_ODR_1600 << BMI088_ACC_CONF_ODR));
+
+ ao_delay(AO_MS_TO_TICKS(2));
+
+ _ao_bmi088_acc_reg_write(BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_POSITIVE);
+
+ ao_delay(AO_MS_TO_TICKS(50));
+
+ _ao_bmi088_acc_sample_read(&acc_pos);
+
+ _ao_bmi088_acc_reg_write(BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_NEGATIVE);
+
+ ao_delay(AO_MS_TO_TICKS(50));
+
+ _ao_bmi088_acc_sample_read(&acc_neg);
+
+ _ao_bmi088_acc_reg_write(BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_OFF);
+
+ /* Self test X and Y must show at least 1000 mg difference,
+ * Z must show at least 500mg difference
+ */
+ if (!ao_bmi088_accel_good(acc_pos.x, acc_neg.x, (int32_t) ao_bmi_accel_to_sample(GRAVITY)))
+ goto failure;
+
+ if (!ao_bmi088_accel_good(acc_pos.y, acc_neg.y, (int32_t) ao_bmi_accel_to_sample(GRAVITY)))
+ goto failure;
+
+ if (!ao_bmi088_accel_good(acc_pos.z, acc_neg.z, (int32_t) ao_bmi_accel_to_sample(GRAVITY/2)))
+ goto failure;
+
+ /*
+ * Self-test gyro
+ *
+ * To trigger the self-test, bit #0 (‘bite_trig’) in address
+ * GYRO_SELF_TEST must be set. When the test is finished, bit
+ * #1 (‘bist_rdy’) will be set by the gyro and the test result
+ * can then be found in bit #2 (‘bist_fail’). A ‘0’ indicates
+ * that the test was passed without issues. If a failure
+ * occurred, the bit ‘bist_fail’ will be set to ‘1’.
+ */
+
+ _ao_bmi088_gyr_reg_write(BMI088_GYRO_SELF_TEST,
+ (1 << BMI088_GYRO_SELF_TEST_TRIG_BIST));
+
+ uint8_t gyro_self_test;
+ for(;;) {
+ gyro_self_test = _ao_bmi088_gyr_reg_read(BMI088_GYRO_SELF_TEST);
+ if ((gyro_self_test & (1 << BMI088_GYRO_SELF_TEST_BIST_RDY)) != 0)
+ break;
+ }
+ if ((gyro_self_test & (1 << BMI088_GYRO_SELF_TEST_BIST_FAIL)) != 0)
+ goto failure;
+
+
+ /* Put accel in desired mode */
+
+ /* 200 Hz sampling rate, normal filter */
+ _ao_bmi088_acc_reg_write(BMI088_ACC_CONF,
+ (BMI088_ACC_CONF_BWP_NORMAL << BMI088_ACC_CONF_BWP) |
+ (BMI088_ACC_CONF_ODR_200 << BMI088_ACC_CONF_ODR));
+
+
+ /* 24 g range */
+ _ao_bmi088_acc_reg_write(BMI088_ACC_RANGE, BMI088_ACC_RANGE_24);
+
+ /* Put gyro in desired mode */
+
+ /* 2000°/s range */
+ _ao_bmi088_gyr_reg_write(BMI088_GYRO_RANGE, BMI088_GYRO_RANGE_2000);
+
+ /* 200 Hz sampling rate, 64Hz filter */
+ _ao_bmi088_gyr_reg_write(BMI088_GYRO_BANDWIDTH, BMI088_GYRO_BANDWIDTH_200_64);
+
+ working = true;
+failure:
+ if (!working)
+ AO_SENSOR_ERROR(AO_DATA_BMI088);
+
+ ao_bmi088_spi_put();
+}
+
+struct ao_bmi088_sample ao_bmi088_current;
+
+static void
+ao_bmi088(void)
+{
+ struct ao_bmi088_sample sample;
+
+ ao_bmi088_setup();
+ for (;;)
+ {
+ ao_bmi088_spi_get();
+ _ao_bmi088_acc_sample_read(&sample.acc);
+ _ao_bmi088_gyr_sample_read(&sample.gyr);
+ ao_bmi088_spi_put();
+ ao_arch_block_interrupts();
+ ao_bmi088_current = sample;
+ AO_DATA_PRESENT(AO_DATA_BMI088);
+ AO_DATA_WAIT();
+ ao_arch_release_interrupts();
+ }
+}
+
+static struct ao_task ao_bmi088_task;
+
+static void
+ao_bmi088_show(void)
+{
+ printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
+ ao_bmi088_current.acc.x,
+ ao_bmi088_current.acc.y,
+ ao_bmi088_current.acc.z,
+ ao_bmi088_current.gyr.x,
+ ao_bmi088_current.gyr.y,
+ ao_bmi088_current.gyr.z);
+}
+
+static const struct ao_cmds bmi_cmds[] = {
+ { ao_bmi088_show, "I\0Show BMI088 status" },
+ { 0, 0 }
+};
+
+void
+ao_bmi088_init(void)
+{
+ AO_TICK_TYPE then;
+ ao_spi_init_cs(AO_BMI088_ACC_CS_PORT, (1 << AO_BMI088_ACC_CS_PIN));
+ ao_spi_init_cs(AO_BMI088_GYR_CS_PORT, (1 << AO_BMI088_GYR_CS_PIN));
+
+ /* force acc part to SPI mode */
+ ao_bmi088_acc_start();
+ then = ao_time();
+ while ((ao_time() - then) < 2)
+ ;
+ ao_bmi088_acc_end();
+
+ ao_add_task(&ao_bmi088_task, ao_bmi088, "bmi088");
+
+ ao_cmd_register(bmi_cmds);
+}
--- /dev/null
+/*
+ * Copyright © 2022 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.
+ */
+
+#ifndef _AO_BMI088_H_
+#define _AO_BMI088_H_
+
+#include <math.h>
+
+struct ao_bmi088_acc_sample {
+ int16_t x;
+ int16_t y;
+ int16_t z;
+};
+
+struct ao_bmi088_gyr_sample {
+ int16_t x;
+ int16_t y;
+ int16_t z;
+};
+
+struct ao_bmi088_sample {
+ struct ao_bmi088_acc_sample acc;
+ struct ao_bmi088_gyr_sample gyr;
+};
+
+extern struct ao_bmi088_sample ao_bmi088_current;
+
+void
+ao_bmi088_init(void);
+
+#define BMI088_ACC_CHIP_ID 0x00
+#define BMI088_ACC_CHIP_ID_BMI088 0x1e
+
+#define BMI088_ACC_ERR_REG 0x02
+# define BMI088_ACC_ERR_REG_ERROR_CODE 2
+# define BMI088_ACC_ERR_REG_FATAL_ERR 0
+
+#define BMI088_ACC_STATUS 0x03
+# define BMI088_ACC_STATUS_ACC_DRDY 7
+
+#define BMI088_ACC_DATA 0x12
+#define BMI088_ACC_TIME 0x18
+
+#define BMI088_ACC_INT_STAT_1 0x1d
+# define BMI088_ACC_INT_STAT_1_ACC_DRDY 7
+
+#define BMI088_ACC_TEMP 0x22
+
+#define BMI088_ACC_CONF 0x40
+# define BMI088_ACC_CONF_BWP 4
+# define BMI088_ACC_CONF_BWP_OSR4 0x08
+# define BMI088_ACC_CONF_BWP_OSR2 0x08
+# define BMI088_ACC_CONF_BWP_NORMAL 0x0a
+# define BMI088_ACC_CONF_ODR 0
+# define BMI088_ACC_CONF_ODR_12_5 0x05
+# define BMI088_ACC_CONF_ODR_25 0x06
+# define BMI088_ACC_CONF_ODR_50 0x07
+# define BMI088_ACC_CONF_ODR_100 0x08
+# define BMI088_ACC_CONF_ODR_200 0x09
+# define BMI088_ACC_CONF_ODR_400 0x0a
+# define BMI088_ACC_CONF_ODR_800 0x0b
+# define BMI088_ACC_CONF_ODR_1600 0x0c
+
+#define BMI088_ACC_RANGE 0x41
+# define BMI088_ACC_RANGE_3 0x00
+# define BMI088_ACC_RANGE_6 0x01
+# define BMI088_ACC_RANGE_12 0x02
+# define BMI088_ACC_RANGE_24 0x03
+
+#define BMI088_INT1_IO_CONF 0x53
+# define BMI088_INT1_IO_CONF_INT1_IN 4
+# define BMI088_INT1_IO_CONF_INT1_OUT 3
+# define BMI088_INT1_IO_CONF_INT1_OD 2
+# define BMI088_INT1_IO_CONF_INT1_LVL 1
+
+#define BMI088_INT2_IO_CONF 0x54
+# define BMI088_INT2_IO_CONF_INT2_IN 4
+# define BMI088_INT2_IO_CONF_INT2_OUT 3
+# define BMI088_INT2_IO_CONF_INT2_OD 2
+# define BMI088_INT2_IO_CONF_INT2_LVL 1
+
+#define BMI088_INT2_INT2_MAP_DATA 0x58
+# define BMI088_INT2_INT2_MAP_DATA_INT2_DRDY 6
+# define BMI088_INT2_INT2_MAP_DATA_INT1_DRDY 2
+
+#define BMI088_ACC_SELF_TEST 0x6d
+# define BMI088_ACC_SELF_TEST_OFF 0x00
+# define BMI088_ACC_SELF_TEST_POSITIVE 0x0d
+# define BMI088_ACC_SELF_TEST_NEGATIVE 0x09
+
+#define BMI088_ACC_PWR_CONF 0x7c
+# define BMI088_ACC_PWR_CONF_SUSPEND 0x03
+# define BMI088_ACC_PWR_CONF_ACTIVE 0x00
+
+#define BMI088_ACC_PWR_CTRL 0x7d
+# define BMI088_ACC_PWR_CTRL_OFF 0x00
+# define BMI088_ACC_PWR_CTRL_ON 0x04
+
+#define BMI088_ACC_SOFTRESET 0x7e
+# define BMI088_ACC_SOFTRESET_RESET 0xb6
+
+#define BMI088_GYRO_CHIP_ID 0x00
+# define BMI088_GYRO_CHIP_ID_BMI088 0x0f
+
+#define BMI088_GYRO_DATA 0x02
+#define BMI088_GYRO_DATA_X 0x02
+#define BMI088_GYRO_DATA_Y 0x04
+#define BMI088_GYRO_DATA_Z 0x06
+
+#define BMI088_GYRO_INT_STAT_1 0x0a
+# define BMI088_GYRO_INT_STAT_1_GYRO_DRDY 7
+
+#define BMI088_GYRO_RANGE 0x0f
+# define BMI088_GYRO_RANGE_2000 0x00
+# define BMI088_GYRO_RANGE_1000 0x01
+# define BMI088_GYRO_RANGE_500 0x02
+# define BMI088_GYRO_RANGE_250 0x03
+# define BMI088_GYRO_RANGE_125 0x04
+
+#define BMI088_GYRO_BANDWIDTH 0x10
+# define BMI088_GYRO_BANDWIDTH_2000_532 0x00
+# define BMI088_GYRO_BANDWIDTH_2000_230 0x01
+# define BMI088_GYRO_BANDWIDTH_1000_116 0x02
+# define BMI088_GYRO_BANDWIDTH_400_47 0x03
+# define BMI088_GYRO_BANDWIDTH_200_23 0x04
+# define BMI088_GYRO_BANDWIDTH_100_12 0x05
+# define BMI088_GYRO_BANDWIDTH_200_64 0x06
+# define BMI088_GYRO_BANDWIDTH_100_32 0x07
+
+#define BMI088_GYRO_LPM1 0x11
+# define BMI088_GYRO_LPM1_NORMAL 0x00
+# define BMI088_GYRO_LPM1_SUSPEND 0x80
+# define BMI088_GYRO_LPM1_DEEP_SUSPEND 0x20
+
+#define BMI088_GYRO_SOFTRESET 0x14
+# define BMI088_GYRO_SOFTRESET_RESET 0xb6
+
+#define BMI088_GYRO_INT_CTRL 0x15
+# define BMI088_GYRO_INT_CTRL_DISABLE 0x00
+# define BMI088_GYRO_INT_CTRL_ENABLE 0x80
+
+#define BMI088_INT3_INT4_IO_CONF 0x16
+# define BMI088_INT3_INT4_IO_CONF_INT4_OD 3
+# define BMI088_INT3_INT4_IO_CONF_INT4_LVL 2
+# define BMI088_INT3_INT4_IO_CONF_INT3_OD 1
+# define BMI088_INT3_INT4_IO_CONF_INT3_LVL 0
+
+#define BMI088_INT3_INT4_IO_MAP 0x18
+# define BMI088_INT3_INT4_IO_MAP_NONE 0x00
+# define BMI088_INT3_INT4_IO_MAP_INT3 0x01
+# define BMI088_INT3_INT4_IO_MAP_INT4 0x80
+# define BMI088_INT3_INT4_IO_MAP_INT3_INT4 0x81
+
+#define BMI088_GYRO_SELF_TEST 0x3c
+# define BMI088_GYRO_SELF_TEST_RATE_OK 4
+# define BMI088_GYRO_SELF_TEST_BIST_FAIL 2
+# define BMI088_GYRO_SELF_TEST_BIST_RDY 1
+# define BMI088_GYRO_SELF_TEST_TRIG_BIST 0
+
+#define BMI088_GYRO_FULLSCALE ((float) 2000.0f * (float) M_PI / 180.0f)
+
+static inline float
+ao_bmi088_gyro(float sensor) {
+ return sensor * ((float) (BMI088_GYRO_FULLSCALE / 32767.0));
+}
+
+#define ao_bmi_gyro_to_sample(gyro) ((gyro) * (32767.0f / (BMI088_GYRO_FULLSCALE
+
+#define BMI088_ACCEL_FULLSCALE 24
+
+static inline float
+ao_bmi088_accel(int16_t sensor) {
+ return (float) sensor * ((float) (BMI088_ACCEL_FULLSCALE * GRAVITY / 32767.0));
+}
+
+#define ao_bmi_accel_to_sample(accel) ((accel) * (32767.0f / (BMI088_ACCEL_FULLSCALE * GRAVITY)))
+
+#endif /* _AO_BMI088_H_ */
projects / fw / altos / commitdiff