2 * Copyright © 2022 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_bmi088.h>
23 #define AO_BMI088_SPI_SPEED ao_spi_speed(100000)
25 #define ao_bmi088_spi_get() ao_spi_get(AO_BMI088_SPI_BUS, AO_BMI088_SPI_SPEED)
26 #define ao_bmi088_spi_put() ao_spi_put(AO_BMI088_SPI_BUS)
28 #define ao_bmi088_acc_start() ao_spi_set_cs(AO_BMI088_ACC_CS_PORT, \
29 (1 << AO_BMI088_ACC_CS_PIN))
30 #define ao_bmi088_acc_end() ao_spi_clr_cs(AO_BMI088_ACC_CS_PORT, \
31 (1 << AO_BMI088_ACC_CS_PIN))
32 #define ao_bmi088_gyr_start() ao_spi_set_cs(AO_BMI088_GYR_CS_PORT, \
33 (1 << AO_BMI088_GYR_CS_PIN))
34 #define ao_bmi088_gyr_end() ao_spi_clr_cs(AO_BMI088_GYR_CS_PORT, \
35 (1 << AO_BMI088_GYR_CS_PIN))
38 _ao_bmi088_acc_reg_read(uint8_t addr)
42 ao_bmi088_acc_start();
43 ao_spi_send(&addr, 1, AO_BMI088_SPI_BUS);
44 ao_spi_recv(v, 2, AO_BMI088_SPI_BUS); /* part sends garbage for first byte */
50 _ao_bmi088_acc_reg_write(uint8_t addr, uint8_t value)
52 uint8_t d[2] = { addr, value };
53 ao_bmi088_acc_start();
54 ao_spi_send(d, 2, AO_BMI088_SPI_BUS);
59 _ao_bmi088_acc_sample_read(struct ao_bmi088_acc_sample *sample)
62 uint8_t addr = BMI088_ACC_DATA | 0x80;
64 ao_bmi088_acc_start();
65 ao_spi_send(&addr, 1, AO_BMI088_SPI_BUS);
66 ao_spi_recv(&dummy, 1, AO_BMI088_SPI_BUS); /* part sends garbage for first byte */
67 ao_spi_recv(sample, sizeof(struct ao_bmi088_acc_sample), AO_BMI088_SPI_BUS);
72 _ao_bmi088_gyr_read(uint8_t addr, void *data, uint8_t len)
75 ao_bmi088_gyr_start();
76 ao_spi_send(&addr, 1, AO_BMI088_SPI_BUS);
77 ao_spi_recv(data, len, AO_BMI088_SPI_BUS);
82 _ao_bmi088_gyr_reg_read(uint8_t addr)
85 _ao_bmi088_gyr_read(addr, &v, 1);
90 _ao_bmi088_gyr_reg_write(uint8_t addr, uint8_t value)
92 uint8_t d[2] = { addr, value };
93 ao_bmi088_gyr_start();
94 ao_spi_send(d, 2, AO_BMI088_SPI_BUS);
99 _ao_bmi088_gyr_sample_read(struct ao_bmi088_gyr_sample *sample)
101 _ao_bmi088_gyr_read(BMI088_GYRO_DATA, sample, sizeof (struct ao_bmi088_gyr_sample));
105 ao_bmi088_reset(void)
109 /* reset the two devices */
110 _ao_bmi088_acc_reg_write(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_RESET);
111 _ao_bmi088_gyr_reg_write(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_RESET);
113 /* wait 30ms (that's how long the gyro takes */
114 ao_delay(AO_MS_TO_TICKS(30));
116 /* force acc part to SPI mode */
117 ao_bmi088_acc_start();
119 ao_delay(AO_MS_TO_TICKS(1));
127 ao_bmi088_accel_good(int16_t pos, int16_t neg, int32_t good)
129 int32_t diff = (int32_t) pos - (int32_t) neg;
135 ao_bmi088_setup(void)
137 bool working = false;
138 struct ao_bmi088_acc_sample acc_pos, acc_neg;
144 /* Make sure the two devices are alive */
145 if (_ao_bmi088_acc_reg_read(BMI088_ACC_CHIP_ID) != BMI088_ACC_CHIP_ID_BMI088)
148 if (_ao_bmi088_gyr_reg_read(BMI088_GYRO_CHIP_ID) != BMI088_GYRO_CHIP_ID_BMI088)
151 /* Turn on the accelerometer */
153 _ao_bmi088_acc_reg_write(BMI088_ACC_PWR_CTRL, BMI088_ACC_PWR_CTRL_ON);
155 /* Wait 5ms after changing accel power mode */
156 ao_delay(AO_MS_TO_TICKS(5));
158 /* Accel self test. Procedure:
160 * 1) Set ±24g range by writing 0x03 to register ACC_RANGE (0x41)
161 * 2) Set ODR=1.6kHz, continuous sampling mode, “normal mode” (norm_avg4) by writing 0xA7 to
162 * register ACC_CONF (0x40)
163 * • Continuous filter function: set bit7 in ACC_CONF
164 * • “normal avg4 mode”: ACC_CONF |= 0x02<<4
165 * • ODR=1.6kHz: ACC_CONF |= 0x0C
167 * 4) Enable the positive self-test polarity (i.e. write 0x0D to register ACC_SELF_TEST (0x6D))
169 * 6) Read the accelerometer offset values for each axis (positive self-test response)
170 * 7) Enable the negative self-test polarity (i.e. write 0x09 to register ACC_SELF_TEST (0x6D))
172 * 9) Read the accelerometer offset values for each axis (negative self-test response)
173 * 10) Disable the self-test (i.e. write 0x00 to register ACC_SELF_TEST (0x6D))
174 * 11) Calculate difference of positive and negative self-test response and compare with the expected
175 * values (see table below)
176 * 12) Wait for > 50ms to let the sensor settle to normal mode steady state operation
179 _ao_bmi088_acc_reg_write(BMI088_ACC_RANGE, BMI088_ACC_RANGE_24);
181 _ao_bmi088_acc_reg_write(BMI088_ACC_CONF,
182 (BMI088_ACC_CONF_BWP_NORMAL << BMI088_ACC_CONF_BWP) |
183 (BMI088_ACC_CONF_ODR_1600 << BMI088_ACC_CONF_ODR));
185 ao_delay(AO_MS_TO_TICKS(2));
187 _ao_bmi088_acc_reg_write(BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_POSITIVE);
189 ao_delay(AO_MS_TO_TICKS(50));
191 _ao_bmi088_acc_sample_read(&acc_pos);
193 _ao_bmi088_acc_reg_write(BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_NEGATIVE);
195 ao_delay(AO_MS_TO_TICKS(50));
197 _ao_bmi088_acc_sample_read(&acc_neg);
199 _ao_bmi088_acc_reg_write(BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_OFF);
201 /* Self test X and Y must show at least 1000 mg difference,
202 * Z must show at least 500mg difference
204 if (!ao_bmi088_accel_good(acc_pos.x, acc_neg.x, (int32_t) ao_bmi_accel_to_sample(GRAVITY)))
207 if (!ao_bmi088_accel_good(acc_pos.y, acc_neg.y, (int32_t) ao_bmi_accel_to_sample(GRAVITY)))
210 if (!ao_bmi088_accel_good(acc_pos.z, acc_neg.z, (int32_t) ao_bmi_accel_to_sample(GRAVITY/2)))
216 * To trigger the self-test, bit #0 (‘bite_trig’) in address
217 * GYRO_SELF_TEST must be set. When the test is finished, bit
218 * #1 (‘bist_rdy’) will be set by the gyro and the test result
219 * can then be found in bit #2 (‘bist_fail’). A ‘0’ indicates
220 * that the test was passed without issues. If a failure
221 * occurred, the bit ‘bist_fail’ will be set to ‘1’.
224 _ao_bmi088_gyr_reg_write(BMI088_GYRO_SELF_TEST,
225 (1 << BMI088_GYRO_SELF_TEST_TRIG_BIST));
227 uint8_t gyro_self_test;
229 gyro_self_test = _ao_bmi088_gyr_reg_read(BMI088_GYRO_SELF_TEST);
230 if ((gyro_self_test & (1 << BMI088_GYRO_SELF_TEST_BIST_RDY)) != 0)
233 if ((gyro_self_test & (1 << BMI088_GYRO_SELF_TEST_BIST_FAIL)) != 0)
237 /* Put accel in desired mode */
239 /* 200 Hz sampling rate, normal filter */
240 _ao_bmi088_acc_reg_write(BMI088_ACC_CONF,
241 (BMI088_ACC_CONF_BWP_NORMAL << BMI088_ACC_CONF_BWP) |
242 (BMI088_ACC_CONF_ODR_200 << BMI088_ACC_CONF_ODR));
246 _ao_bmi088_acc_reg_write(BMI088_ACC_RANGE, BMI088_ACC_RANGE_24);
248 /* Put gyro in desired mode */
251 _ao_bmi088_gyr_reg_write(BMI088_GYRO_RANGE, BMI088_GYRO_RANGE_2000);
253 /* 200 Hz sampling rate, 64Hz filter */
254 _ao_bmi088_gyr_reg_write(BMI088_GYRO_BANDWIDTH, BMI088_GYRO_BANDWIDTH_200_64);
259 AO_SENSOR_ERROR(AO_DATA_BMI088);
264 struct ao_bmi088_sample ao_bmi088_current;
269 struct ao_bmi088_sample sample;
275 _ao_bmi088_acc_sample_read(&sample.acc);
276 _ao_bmi088_gyr_sample_read(&sample.gyr);
278 ao_arch_block_interrupts();
279 ao_bmi088_current = sample;
280 AO_DATA_PRESENT(AO_DATA_BMI088);
282 ao_arch_release_interrupts();
286 static struct ao_task ao_bmi088_task;
291 #ifdef AO_LOG_NORMALIZED
292 printf ("BMI088: %7d %7d %7d %7d %7d %7d\n",
293 ao_bmi088_along(&ao_bmi088_current),
294 ao_bmi088_across(&ao_bmi088_current),
295 ao_bmi088_through(&ao_bmi088_current),
296 ao_bmi088_roll(&ao_bmi088_current),
297 ao_bmi088_pitch(&ao_bmi088_current),
298 ao_bmi088_yaw(&ao_bmi088_current));
300 printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
301 ao_bmi088_current.acc.x,
302 ao_bmi088_current.acc.y,
303 ao_bmi088_current.acc.z,
304 ao_bmi088_current.gyr.x,
305 ao_bmi088_current.gyr.y,
306 ao_bmi088_current.gyr.z);
310 static const struct ao_cmds bmi_cmds[] = {
311 { ao_bmi088_show, "I\0Show BMI088 status" },
319 ao_spi_init_cs(AO_BMI088_ACC_CS_PORT, (1 << AO_BMI088_ACC_CS_PIN));
320 ao_spi_init_cs(AO_BMI088_GYR_CS_PORT, (1 << AO_BMI088_GYR_CS_PIN));
322 /* force acc part to SPI mode */
323 ao_bmi088_acc_start();
325 while ((ao_time() - then) < 2)
329 ao_add_task(&ao_bmi088_task, ao_bmi088, "bmi088");
331 ao_cmd_register(bmi_cmds);