C++ (Cpp) UpdateMedianFilterVect3Int示例

示例#1

文件： imu_krooz.c 项目： OpenUAS/backup-files-quad

void imu_periodic( void )
{
  // Start reading the latest gyroscope data
  if (!imu_krooz.mpu.config.initialized)
    mpu60x0_i2c_start_configure(&imu_krooz.mpu);

  if (!imu_krooz.hmc.initialized)
    hmc58xx_start_configure(&imu_krooz.hmc);

  if (imu_krooz.meas_nb) {
    RATES_ASSIGN(imu.gyro_unscaled, imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
#if KROOZ_USE_MEDIAN_FILTER
    UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
#endif
    VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
#if KROOZ_USE_MEDIAN_FILTER
    UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
    RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
    VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
    imu_krooz.meas_nb = 0;

    imu_krooz.gyr_valid = TRUE;
    imu_krooz.acc_valid = TRUE;
  }

  //RunOnceEvery(10,imu_krooz_downlink_raw());
}

示例#2

文件： imu_krooz.c 项目： 2seasuav/paparuzzi

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

示例#3

文件： 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;
  }
}

示例#4

文件： imu_navgo.c 项目： 1bitsquared/paparazzi

void imu_navgo_event( void )
{

  // If the itg3200 I2C transaction has succeeded: convert the data
  itg3200_event(&imu_navgo.itg);
  if (imu_navgo.itg.data_available) {
    RATES_ASSIGN(imu.gyro_unscaled, -imu_navgo.itg.data.rates.q, imu_navgo.itg.data.rates.p, imu_navgo.itg.data.rates.r);
#if NAVGO_USE_MEDIAN_FILTER
    UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
#endif
    imu_navgo.itg.data_available = FALSE;
    imu_navgo.gyr_valid = TRUE;
  }

  // If the adxl345 I2C transaction has succeeded: convert the data
  adxl345_i2c_event(&imu_navgo.adxl);
  if (imu_navgo.adxl.data_available) {
    VECT3_ASSIGN(imu.accel_unscaled, imu_navgo.adxl.data.vect.y, -imu_navgo.adxl.data.vect.x, imu_navgo.adxl.data.vect.z);
#if NAVGO_USE_MEDIAN_FILTER
    UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
    imu_navgo.adxl.data_available = FALSE;
    imu_navgo.acc_valid = TRUE;
  }

  // HMC58XX event task
  hmc58xx_event(&imu_navgo.hmc);
  if (imu_navgo.hmc.data_available) {
    VECT3_COPY(imu.mag_unscaled, imu_navgo.hmc.data.vect);
#if NAVGO_USE_MEDIAN_FILTER
    UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
#endif
    imu_navgo.hmc.data_available = FALSE;
    imu_navgo.mag_valid = TRUE;
  }

}

示例#5

文件： imu_krooz_memsic.c 项目： 2seasuav/paparuzzi

void imu_periodic(void)
{
  // Start reading the latest gyroscope data
  if (!imu_krooz.mpu.config.initialized) {
    mpu60x0_i2c_start_configure(&imu_krooz.mpu);
  }

  if (!imu_krooz.hmc.initialized) {
    hmc58xx_start_configure(&imu_krooz.hmc);
  }

  uint32_t now_ts = get_sys_time_usec();

  if (imu_krooz.meas_nb) {
    RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb,
                 imu_krooz.rates_sum.p / imu_krooz.meas_nb,
                 imu_krooz.rates_sum.r / imu_krooz.meas_nb);

    RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
    imu_krooz.meas_nb = 0;
    imu_scale_gyro(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
  }

  if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
    imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
    imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
    imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;

#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
    UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
    VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
    VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
    VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
    VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);

    INT_VECT3_ZERO(imu_krooz.accel_sum);
    INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
  }

  RunOnceEvery(128, {axis_nb = 5;});

示例#6

文件： imu_krooz.c 项目： OpenUAS/backup-files-quad

void imu_krooz_event( void )
{
  // 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 the HMC5883 I2C transaction has succeeded: convert the data
  hmc58xx_event(&imu_krooz.hmc);
  if (imu_krooz.hmc.data_available) {
    VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect);
#if KROOZ_USE_MEDIAN_FILTER
    UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
#endif
    imu_krooz.hmc.data_available = FALSE;
    imu_krooz.mag_valid = TRUE;
  }
}