예제 #1
0
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]);
  }
}
예제 #2
0
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;
      }
    }
  }
}