mirror of
https://github.com/UberGuidoZ/Flipper.git
synced 2024-12-23 23:10:16 +00:00
89 lines
2.8 KiB
C
89 lines
2.8 KiB
C
#include "bmi160.h"
|
|
|
|
#include <furi_hal.h>
|
|
|
|
#include "imu.h"
|
|
|
|
#define TAG "BMI160"
|
|
|
|
#define BMI160_DEV_ADDR (0x69 << 1)
|
|
|
|
static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
|
|
static const double G = 9.81;
|
|
|
|
struct bmi160_dev bmi160dev;
|
|
struct bmi160_sensor_data bmi160_accel;
|
|
struct bmi160_sensor_data bmi160_gyro;
|
|
|
|
int8_t bmi160_write_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint16_t len) {
|
|
if(furi_hal_i2c_write_mem(&furi_hal_i2c_handle_external, dev_addr, reg_addr, data, len, 50))
|
|
return BMI160_OK;
|
|
return BMI160_E_COM_FAIL;
|
|
}
|
|
|
|
int8_t bmi160_read_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* read_data, uint16_t len) {
|
|
if(furi_hal_i2c_read_mem(&furi_hal_i2c_handle_external, dev_addr, reg_addr, read_data, len, 50))
|
|
return BMI160_OK;
|
|
return BMI160_E_COM_FAIL;
|
|
}
|
|
|
|
bool bmi160_begin() {
|
|
FURI_LOG_I(TAG, "Init BMI160");
|
|
|
|
if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, BMI160_DEV_ADDR, 50)) {
|
|
FURI_LOG_E(TAG, "Device not ready!");
|
|
return false;
|
|
}
|
|
|
|
FURI_LOG_I(TAG, "Device ready!");
|
|
|
|
bmi160dev.id = BMI160_DEV_ADDR;
|
|
bmi160dev.intf = BMI160_I2C_INTF;
|
|
bmi160dev.read = bmi160_read_i2c;
|
|
bmi160dev.write = bmi160_write_i2c;
|
|
bmi160dev.delay_ms = furi_delay_ms;
|
|
|
|
if(bmi160_init(&bmi160dev) != BMI160_OK) {
|
|
FURI_LOG_E(TAG, "Initialization failure!");
|
|
FURI_LOG_E(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
|
|
return false;
|
|
}
|
|
|
|
bmi160dev.accel_cfg.odr = BMI160_ACCEL_ODR_400HZ;
|
|
bmi160dev.accel_cfg.range = BMI160_ACCEL_RANGE_4G;
|
|
bmi160dev.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
|
|
bmi160dev.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
|
|
bmi160dev.gyro_cfg.odr = BMI160_GYRO_ODR_400HZ;
|
|
bmi160dev.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
|
|
bmi160dev.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
|
|
bmi160dev.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
|
|
|
|
if(bmi160_set_sens_conf(&bmi160dev) != BMI160_OK) {
|
|
FURI_LOG_E(TAG, "Initialization failure!");
|
|
FURI_LOG_E(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
|
|
return false;
|
|
}
|
|
|
|
FURI_LOG_I(TAG, "Initialization success!");
|
|
FURI_LOG_I(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
|
|
|
|
return true;
|
|
}
|
|
|
|
int bmi160_read(double* vec) {
|
|
if(bmi160_get_sensor_data(
|
|
(BMI160_ACCEL_SEL | BMI160_GYRO_SEL), &bmi160_accel, &bmi160_gyro, &bmi160dev) !=
|
|
BMI160_OK) {
|
|
return 0;
|
|
}
|
|
|
|
vec[0] = ((double)bmi160_accel.x * 4 / 32768) * G;
|
|
vec[1] = ((double)bmi160_accel.y * 4 / 32768) * G;
|
|
vec[2] = ((double)bmi160_accel.z * 4 / 32768) * G;
|
|
vec[3] = ((double)bmi160_gyro.x * 2000 / 32768) * DEG_TO_RAD;
|
|
vec[4] = ((double)bmi160_gyro.y * 2000 / 32768) * DEG_TO_RAD;
|
|
vec[5] = ((double)bmi160_gyro.z * 2000 / 32768) * DEG_TO_RAD;
|
|
|
|
return ACC_DATA_READY | GYR_DATA_READY;
|
|
}
|