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

Merge pull request #202 from Mitchlol/LSM6DS3_Fix 

LSM6DS3 Fix
This commit is contained in:
Benjamin Vedder 2020-08-15 09:45:01 +02:00 committed by GitHub
parent f560f27871 349361ba36
commit 91bde64e71
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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)));
}