static void ms5611_read_prom(void)
{
for (uint8_t i = 0; i < MS5611_PROM_SIZE; ++i) {
uint8_t txbuf[1] = {0xA0 | i << 1}; // prom register
uint8_t rxbuf[2];
sc_i2c_transmit(i2cn, txbuf, sizeof(txbuf), rxbuf, sizeof(rxbuf));
ms5611_prom[i] = (uint16_t) (((uint16_t)rxbuf[0] << 8) | rxbuf[1]);
}
}
void sc_lsm9ds0_read(sc_float *acc, sc_float *magn, sc_float *gyro)
{
uint8_t txbuf[1];
uint8_t rxbuf[6];
uint8_t sensors_done = 0;
uint8_t i;
const uint8_t all_done = SENSOR_RDY_ACC | SENSOR_RDY_MAGN | SENSOR_RDY_GYRO;
// Read all sensors at least once
while (sensors_done != all_done){
// Wait for data ready signal
chBSemWait(&lsm9ds0_drdy_sem);
#ifdef SC_WAR_ISSUE_1
// WAR for broken DRDY_G pin
if (sensors_ready & SENSOR_RDY_ACC) {
chMtxLock(&data_mtx);
sensors_ready |= SENSOR_RDY_GYRO;
chMtxUnlock();
}
#endif
while (sensors_ready && sensors_done != all_done) {
if (sensors_ready & SENSOR_RDY_ACC) {
txbuf[0] = LSM9DS0_OUT_X_L_A | LSM9DS0_SUBADDR_AUTO_INC_BIT;
sc_i2c_transmit(i2cn_xm, txbuf, 1, rxbuf, 6);
for (i = 0; i < 3; ++i) {
acc[i] = ((int16_t)(((uint16_t)rxbuf[2*i + 1]) << 8 | rxbuf[2*i])) *
LSM9DS0_ACC_SENSITIVITY;
}
chMtxLock(&data_mtx);
sensors_ready &= ~SENSOR_RDY_ACC;
chMtxUnlock();
sensors_done |= SENSOR_RDY_ACC;
}
if (sensors_ready & SENSOR_RDY_MAGN) {
txbuf[0] = LSM9DS0_OUT_X_L_M | LSM9DS0_SUBADDR_AUTO_INC_BIT;
sc_i2c_transmit(i2cn_xm, txbuf, 1, rxbuf, 6);
for (i = 0; i < 3; ++i) {
magn[i] = ((int16_t)(((uint16_t)rxbuf[2*i + 1]) << 8 | rxbuf[2*i])) *
LSM9DS0_MAGN_SENSITIVITY;
}
chMtxLock(&data_mtx);
sensors_ready &= ~SENSOR_RDY_MAGN;
chMtxUnlock();
sensors_done |= SENSOR_RDY_MAGN;
}
if (sensors_ready & SENSOR_RDY_GYRO) {
txbuf[0] = LSM9DS0_OUT_X_L_G | LSM9DS0_SUBADDR_AUTO_INC_BIT;
sc_i2c_transmit(i2cn_g, txbuf, 1, rxbuf, 6);
for (i = 0; i < 3; ++i) {
gyro[i] = ((int16_t)(((uint16_t)rxbuf[2*i + 1]) << 8 | rxbuf[2*i])) *
LSM9DS0_GYRO_SENSITIVITY;
}
chMtxLock(&data_mtx);
sensors_ready &= ~SENSOR_RDY_GYRO;
chMtxUnlock();
sensors_done |= SENSOR_RDY_GYRO;
}
}
}
}