40 lines
954 B
C
40 lines
954 B
C
#include "bmi270.h"
|
|
#include "FreeRTOS.h"
|
|
#include "i2c.h"
|
|
#include "pins.h"
|
|
#include <stdint.h>
|
|
|
|
void bmi270_init(void) {
|
|
uint8_t id;
|
|
i2c_read_reg(IMU_I2C_ADDRESS, BMI270_CHIP_ID, &id);
|
|
|
|
if (id != 0x24)
|
|
return;
|
|
|
|
// soft reset
|
|
i2c_write_reg(IMU_I2C_ADDRESS, 0x7E, 0xB6);
|
|
vTaskDelay(pdMS_TO_TICKS(10));
|
|
|
|
// power on accel + gyro
|
|
i2c_write_reg(IMU_I2C_ADDRESS, BMI270_PWR_CTRL, 0x0E);
|
|
|
|
// accel config (100 Hz-ish)
|
|
i2c_write_reg(IMU_I2C_ADDRESS, BMI270_ACC_CONF, 0xA8);
|
|
|
|
// gyro config
|
|
i2c_write_reg(IMU_I2C_ADDRESS, BMI270_GYR_CONF, 0xA9);
|
|
}
|
|
|
|
void bmi270_read(bmi270_data_t *d) {
|
|
uint8_t buf[12];
|
|
|
|
i2c_read_reg_multi(IMU_I2C_ADDRESS, BMI270_ACC_X_L, buf, 12);
|
|
|
|
d->ax = (int16_t)(buf[1] << 8 | buf[0]);
|
|
d->ay = (int16_t)(buf[3] << 8 | buf[2]);
|
|
d->az = (int16_t)(buf[5] << 8 | buf[4]);
|
|
|
|
d->gx = (int16_t)(buf[7] << 8 | buf[6]);
|
|
d->gy = (int16_t)(buf[9] << 8 | buf[8]);
|
|
d->gz = (int16_t)(buf[11] << 8 | buf[10]);
|
|
} |