예제 #1
0
void imu_krooz_event(void)
{
  if (imu_krooz.mpu_eoc) {
    mpu60x0_i2c_read(&imu_krooz.mpu);
    imu_krooz.mpu_eoc = false;
  }

  // If the MPU6050 I2C transaction has succeeded: convert the data
  mpu60x0_i2c_event(&imu_krooz.mpu);
  if (imu_krooz.mpu.data_available) {
    RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
    VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
    imu_krooz.meas_nb++;
    imu_krooz.mpu.data_available = false;
  }

  if (imu_krooz.hmc_eoc) {
    hmc58xx_read(&imu_krooz.hmc);
    imu_krooz.hmc_eoc = false;
  }

  // If the HMC5883 I2C transaction has succeeded: convert the data
  hmc58xx_event(&imu_krooz.hmc);
  if (imu_krooz.hmc.data_available) {
    VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
    UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
    imu_krooz.hmc.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, get_sys_time_usec(), &imu.mag);
  }
}
예제 #2
0
파일: imu_krooz.c 프로젝트: SayCV/paparazzi
void imu_krooz_event( void )
{
  if (imu_krooz.mpu_eoc) {
    mpu60x0_i2c_read(&imu_krooz.mpu);
    imu_krooz.mpu_eoc = FALSE;
  }

  // If the MPU6050 I2C transaction has succeeded: convert the data
  mpu60x0_i2c_event(&imu_krooz.mpu);
  if (imu_krooz.mpu.data_available) {
    RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
    VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
    imu_krooz.meas_nb++;
    imu_krooz.mpu.data_available = FALSE;
  }

  if (imu_krooz.hmc_eoc) {
    hmc58xx_read(&imu_krooz.hmc);
    imu_krooz.hmc_eoc = FALSE;
  }

  // If the HMC5883 I2C transaction has succeeded: convert the data
  hmc58xx_event(&imu_krooz.hmc);
  if (imu_krooz.hmc.data_available) {
    VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
    UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
    imu_krooz.hmc.data_available = FALSE;
    imu_krooz.mag_valid = TRUE;
  }
}