mirror of https://github.com/rusefi/bldc.git
commit
91bde64e71
|
@ -162,12 +162,19 @@ static THD_FUNCTION(lsm6ds3_thread, arg) {
|
||||||
|
|
||||||
while (!chThdShouldTerminateX()) {
|
while (!chThdShouldTerminateX()) {
|
||||||
|
|
||||||
|
uint8_t txb[2];
|
||||||
uint8_t txb[1];
|
|
||||||
uint8_t rxb[12];
|
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;
|
txb[0] = LSM6DS3_ACC_GYRO_OUTX_L_G;
|
||||||
bool res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 12);
|
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 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 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;
|
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);
|
read_callback(tmp_accel, tmp_gyro, tmp_mag);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Delay between loops
|
// Delay between loops
|
||||||
chThdSleepMilliseconds((int)((1000.0 / rate_hz)));
|
chThdSleepMilliseconds((int)((1000.0 / rate_hz)));
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue