rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dev_fw_5_02' of https://github.com/vedderb/bldc into dev_fw_5_02

This commit is contained in:
Benjamin Vedder 2020-08-26 14:47:18 +02:00
parent ed788fbec5 9459083b3a
commit fbc8cf6cc1
2 changed files with 32 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
imu/bmi160_wrapper.c
View File

@ -54,16 +54,37 @@ static bool reset_init_bmi(BMI_STATE *s) {
bmi160_init(&(s->sensor));
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_200HZ;
s->sensor.accel_cfg.range = BMI160_ACCEL_RANGE_16G;
s->sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
s->sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_200HZ;
s->sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
s->sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
s->sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
if(s->rate_hz <= 25){
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_25HZ;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_25HZ;
}else if(s->rate_hz <= 50){
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_50HZ;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_50HZ;
}else if(s->rate_hz <= 100){
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_100HZ;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_100HZ;
}else if(s->rate_hz <= 200){
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_200HZ;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_200HZ;
}else if(s->rate_hz <= 400){
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_400HZ;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_400HZ;
}else if(s->rate_hz <= 800){
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_800HZ;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_800HZ;
}else{
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_1600HZ;
}
int8_t res = bmi160_set_sens_conf(&(s->sensor));
return res == BMI160_OK;

12
imu/lsm6ds3.c
View File

@ -162,12 +162,19 @@ static THD_FUNCTION(lsm6ds3_thread, arg) {
while (!chThdShouldTerminateX()) {
uint8_t txb[1];
uint8_t txb[2];
uint8_t rxb[12];
// Disable IMU writing to output registers
txb[0] = LSM6DS3_ACC_GYRO_CTRL3_C;
txb[1] = LSM6DS3_ACC_GYRO_BDU_BLOCK_UPDATE | LSM6DS3_ACC_GYRO_IF_INC_ENABLED;
i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
// Read IMU output registers
txb[0] = LSM6DS3_ACC_GYRO_OUTX_L_G;
bool res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 12);
// Parse 6 axis values
float gx = (float)((int16_t)((uint16_t)rxb[1] << 8) + rxb[0]) * 4.375 * (2000 / 125) / 1000;
float gy = (float)((int16_t)((uint16_t)rxb[3] << 8) + rxb[2]) * 4.375 * (2000 / 125) / 1000;
float gz = (float)((int16_t)((uint16_t)rxb[5] << 8) + rxb[4]) * 4.375 * (2000 / 125) / 1000;
@ -180,7 +187,6 @@ static THD_FUNCTION(lsm6ds3_thread, arg) {
read_callback(tmp_accel, tmp_gyro, tmp_mag);
}
// Delay between loops
chThdSleepMilliseconds((int)((1000.0 / rate_hz)));
}