当前位置: 首页>>代码示例>>C++>>正文

C++ RATES_COPY函数代码示例

本文整理汇总了C++中RATES_COPY函数的典型用法代码示例。如果您正苦于以下问题：C++ RATES_COPY函数的具体用法？C++ RATES_COPY怎么用？C++ RATES_COPY使用的例子？那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。


在下文中一共展示了RATES_COPY函数的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: ahrs_icq_align

bool_t ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                      struct Int32Vect3 *lp_mag)
{

#if USE_MAGNETOMETER
  /* Compute an initial orientation from accel and mag directly as quaternion */
  ahrs_int_get_quat_from_accel_mag(&ahrs_icq.ltp_to_imu_quat,
                                   lp_accel, lp_mag);
  ahrs_icq.heading_aligned = TRUE;
#else
  /* Compute an initial orientation from accel and just set heading to zero */
  ahrs_int_get_quat_from_accel(&ahrs_icq.ltp_to_imu_quat, lp_accel);
  ahrs_icq.heading_aligned = FALSE;
  // supress unused arg warning
  lp_mag = lp_mag;
#endif

  /* Use low passed gyro value as initial bias */
  RATES_COPY(ahrs_icq.gyro_bias, *lp_gyro);
  RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro);
  INT_RATES_LSHIFT(ahrs_icq.high_rez_bias, ahrs_icq.high_rez_bias, 28);

  ahrs_icq.status = AHRS_ICQ_RUNNING;
  ahrs_icq.is_aligned = TRUE;

  return TRUE;
}
开发者ID:AE4317group07，项目名称:paparazzi，代码行数:27，代码来源:ahrs_int_cmpl_quat.c

示例2: store_filter_output

static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
  RATES_COPY(output[i].rate_est, ahrs_impl.body_rate);
#else
  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_imu_quat);
  RATES_COPY(output[i].rate_est, ahrs_impl.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
  RATES_COPY(output[i].bias_est, ahrs_impl.gyro_bias);
  //  memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
开发者ID:Abhi0204，项目名称:paparazzi，代码行数:11，代码来源:run_ahrs_on_flight_log.c

示例3: imu_aspirin2_event

void imu_aspirin2_event(void)
{
  mpu60x0_spi_event(&imu_aspirin2.mpu);
  if (imu_aspirin2.mpu.data_available) {
    /* HMC5883 has xzy order of axes in returned data */
    struct Int32Vect3 mag;
    mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
    mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
    mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#ifdef LISA_M_LONGITUDINAL_X
    RATES_ASSIGN(imu.gyro_unscaled,
                 imu_aspirin2.mpu.data_rates.rates.q,
                 -imu_aspirin2.mpu.data_rates.rates.p,
                 imu_aspirin2.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled,
                 imu_aspirin2.mpu.data_accel.vect.y,
                 -imu_aspirin2.mpu.data_accel.vect.x,
                 imu_aspirin2.mpu.data_accel.vect.z);
    VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#else
#ifdef LISA_S
#ifdef LISA_S_UPSIDE_DOWN
    RATES_ASSIGN(imu.gyro_unscaled,
                 imu_aspirin2.mpu.data_rates.rates.p,
                 -imu_aspirin2.mpu.data_rates.rates.q,
                 -imu_aspirin2.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled,
                 imu_aspirin2.mpu.data_accel.vect.x,
                 -imu_aspirin2.mpu.data_accel.vect.y,
                 -imu_aspirin2.mpu.data_accel.vect.z);
    VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
#else
    RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
    VECT3_COPY(imu.mag_unscaled, mag);
#endif
#else
    RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
    VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
#endif
    imu_aspirin2.mpu.data_available = FALSE;
    imu_aspirin2.gyro_valid = TRUE;
    imu_aspirin2.accel_valid = TRUE;
    imu_aspirin2.mag_valid = TRUE;
  }
}
开发者ID:1bitsquared，项目名称:paparazzi，代码行数:48，代码来源:imu_aspirin_2_spi.c

示例4: ahrs_icq_propagate

void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
{
  int32_t freq = (int32_t)(1. / dt);

  /* unbias gyro             */
  struct Int32Rates omega;
  RATES_DIFF(omega, *gyro, ahrs_icq.gyro_bias);

  /* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 2);
  RATES_ADD(ahrs_icq.imu_rate, omega);
  RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 3);
#else
  RATES_COPY(ahrs_icq.imu_rate, omega);
#endif

  /* add correction */
  RATES_ADD(omega, ahrs_icq.rate_correction);
  /* and zeros it */
  INT_RATES_ZERO(ahrs_icq.rate_correction);

  /* integrate quaternion */
  int32_quat_integrate_fi(&ahrs_icq.ltp_to_imu_quat, &ahrs_icq.high_rez_quat,
                          &omega, freq);
  int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat);

  // increase accel and mag propagation counters
  ahrs_icq.accel_cnt++;
  ahrs_icq.mag_cnt++;
}
开发者ID:AE4317group07，项目名称:paparazzi，代码行数:31，代码来源:ahrs_int_cmpl_quat.c

示例5: ahrs_align

void ahrs_align(void) {

#if USE_MAGNETOMETER
  /* Compute an initial orientation from accel and mag directly as quaternion */
  ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
  ahrs_impl.heading_aligned = TRUE;
#else
  /* Compute an initial orientation from accel and just set heading to zero */
  ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
  ahrs_impl.heading_aligned = FALSE;
#endif

  /* Convert initial orientation from quat to rotation matrix representations. */
  FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);

  /* Compute initial body orientation */
  compute_body_orientation_and_rates();

  /* used averaged gyro as initial value for bias */
  struct Int32Rates bias0;
  RATES_COPY(bias0, ahrs_aligner.lp_gyro);
  RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);

  ahrs.status = AHRS_RUNNING;
}
开发者ID:FatihRasimBahar，项目名称:paparazzi，代码行数:25，代码来源:ahrs_float_cmpl.c

示例6: ahrs_propagate

void ahrs_propagate(void) {

  /* unbias gyro             */
  struct Int32Rates omega;
  RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);

  /* low pass rate */
//#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  if (gyro_lowpass_filter > 1) {
	  RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter-1);
	  RATES_ADD(ahrs_impl.imu_rate, omega);
	  RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter);
  //#else
  } else {
	  RATES_COPY(ahrs_impl.imu_rate, omega);
  //#endif
  }

  /* add correction     */
  RATES_ADD(omega, ahrs_impl.rate_correction);
  /* and zeros it */
  INT_RATES_ZERO(ahrs_impl.rate_correction);

  /* integrate quaternion */
  INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
  INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);

  set_body_state_from_quat();

}
开发者ID:jeffchenhr，项目名称:dev-barometer，代码行数:30，代码来源:ahrs_int_cmpl_quat.c

示例7: imu_navstik_event

/**
 * Handle all the events of the Navstik IMU components.
 * When there is data available convert it to the correct axis and save it in the imu structure.
 */
void imu_navstik_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  /* MPU-60x0 event taks */
  mpu60x0_i2c_event(&imu_navstik.mpu);

  if (imu_navstik.mpu.data_available) {
    /* default orientation as should be printed on the pcb, z-down, ICs down */
    RATES_COPY(imu.gyro_unscaled, imu_navstik.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_navstik.mpu.data_accel.vect);

    imu_navstik.mpu.data_available = false;
    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
  }

  /* HMC58XX event task */
  hmc58xx_event(&imu_navstik.hmc);
  if (imu_navstik.hmc.data_available) {
    imu.mag_unscaled.x =  imu_navstik.hmc.data.vect.y;
    imu.mag_unscaled.y = -imu_navstik.hmc.data.vect.x;
    imu.mag_unscaled.z =  imu_navstik.hmc.data.vect.z;
    imu_navstik.hmc.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
  }
}
开发者ID:enacuavlab，项目名称:paparazzi，代码行数:34，代码来源:imu_navstik.c

示例8: imu_aspirin_event

void imu_aspirin_event(void)
{
  adxl345_spi_event(&imu_aspirin.acc_adxl);
  if (imu_aspirin.acc_adxl.data_available) {
    VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
    imu_aspirin.acc_adxl.data_available = FALSE;
    imu_aspirin.accel_valid = TRUE;
  }

  /* If the itg3200 I2C transaction has succeeded: convert the data */
  itg3200_event(&imu_aspirin.gyro_itg);
  if (imu_aspirin.gyro_itg.data_available) {
    RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates);
    imu_aspirin.gyro_itg.data_available = FALSE;
    imu_aspirin.gyro_valid = TRUE;
  }

  /* HMC58XX event task */
  hmc58xx_event(&imu_aspirin.mag_hmc);
  if (imu_aspirin.mag_hmc.data_available) {
#ifdef IMU_ASPIRIN_VERSION_1_0
    VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect);
#else // aspirin 1.5 with hmc5883
    imu.mag_unscaled.x =  imu_aspirin.mag_hmc.data.vect.y;
    imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x;
    imu.mag_unscaled.z =  imu_aspirin.mag_hmc.data.vect.z;
#endif
    imu_aspirin.mag_hmc.data_available = FALSE;
    imu_aspirin.mag_valid = TRUE;
  }
}
开发者ID:jornanke，项目名称:paparazzi，代码行数:31，代码来源:imu_aspirin.c

示例9: imu_navgo_event

void imu_navgo_event( void )
{

  // If the itg3200 I2C transaction has succeeded: convert the data
  itg3200_event();
  if (itg3200_data_available) {
    RATES_COPY(imu.gyro_unscaled, itg3200_data);
    itg3200_data_available = FALSE;
    gyr_valid = TRUE;
  }

  // If the adxl345 I2C transaction has succeeded: convert the data
  adxl345_event();
  if (adxl345_data_available) {
    VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z);
    adxl345_data_available = FALSE;
    acc_valid = TRUE;
  }

  // HMC58XX event task
  hmc58xx_event();
  if (hmc58xx_data_available) {
    VECT3_ASSIGN(imu.mag_unscaled, -hmc58xx_data.x, -hmc58xx_data.y, hmc58xx_data.z);
    hmc58xx_data_available = FALSE;
    mag_valid = TRUE;
  }

}
开发者ID:AshuLara，项目名称:lisa，代码行数:28，代码来源:imu_navgo.c

示例10: ahrs_propagate

void ahrs_propagate(void) {

  /* unbias gyro             */
  struct Int32Rates omega;
  RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);

  /* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  RATES_SMUL(ahrs.imu_rate, ahrs.imu_rate,2);
  RATES_ADD(ahrs.imu_rate, omega);
  RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 3);
#else
  RATES_COPY(ahrs.imu_rate, omega);
#endif

  /* add correction     */
  RATES_ADD(omega, ahrs_impl.rate_correction);
  /* and zeros it */
  INT_RATES_ZERO(ahrs_impl.rate_correction);

  /* integrate quaternion */
  INT32_QUAT_INTEGRATE_FI(ahrs.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
  INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat);

  compute_imu_euler_and_rmat_from_quat();

  compute_body_orientation();

}
开发者ID:cbeighley，项目名称:paparazzi，代码行数:29，代码来源:ahrs_int_cmpl.c

示例11: imu_mpu_hmc_event

void imu_mpu_hmc_event(void)
{
    uint32_t now_ts = get_sys_time_usec();

    mpu60x0_spi_event(&imu_mpu_hmc.mpu);
    if (imu_mpu_hmc.mpu.data_available) {
        RATES_COPY(imu.gyro_unscaled, imu_mpu_hmc.mpu.data_rates.rates);
        VECT3_COPY(imu.accel_unscaled, imu_mpu_hmc.mpu.data_accel.vect);
        imu_mpu_hmc.mpu.data_available = false;
        imu_scale_gyro(&imu);
        imu_scale_accel(&imu);
        AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
        AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
    }

    /* HMC58XX event task */
    hmc58xx_event(&imu_mpu_hmc.hmc);
    if (imu_mpu_hmc.hmc.data_available) {
        /* mag rotated by 90deg around z axis relative to MPU */
        imu.mag_unscaled.x =  imu_mpu_hmc.hmc.data.vect.y;
        imu.mag_unscaled.y = -imu_mpu_hmc.hmc.data.vect.x;
        imu.mag_unscaled.z =  imu_mpu_hmc.hmc.data.vect.z;
        imu_mpu_hmc.hmc.data_available = false;
        imu_scale_mag(&imu);
        AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
    }
}
开发者ID:coppolam，项目名称:paparazzi，代码行数:27，代码来源:imu_mpu6000_hmc5883.c

示例12: ahrs_fc_align

bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                     struct Int32Vect3 *lp_mag)
{

#if USE_MAGNETOMETER
  /* Compute an initial orientation from accel and mag directly as quaternion */
  ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_imu_quat, lp_accel, lp_mag);
  ahrs_fc.heading_aligned = TRUE;
#else
  /* Compute an initial orientation from accel and just set heading to zero */
  ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_imu_quat, lp_accel);
  ahrs_fc.heading_aligned = FALSE;
#endif

  /* Convert initial orientation from quat to rotation matrix representations. */
  float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);

  /* used averaged gyro as initial value for bias */
  struct Int32Rates bias0;
  RATES_COPY(bias0, *lp_gyro);
  RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0);

  ahrs_fc.status = AHRS_FC_RUNNING;
  ahrs_fc.is_aligned = TRUE;

  return TRUE;
}
开发者ID:jmavi，项目名称:paparazzi_fix，代码行数:27，代码来源:ahrs_float_cmpl.c

示例13: ahrs_propagate

void ahrs_propagate(void) {

  /* converts gyro to floating point */
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
  /* unbias measurement */
  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);

#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  const float alpha = 0.1;
  FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
#else
  RATES_COPY(ahrs_impl.imu_rate,gyro_float);
#endif

  /* add correction     */
  struct FloatRates omega;
  RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
  /* and zeros it */
  FLOAT_RATES_ZERO(ahrs_impl.rate_correction);

  const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
#if AHRS_PROPAGATE_RMAT
  FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt);
  float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat);
  FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
  FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt);
  FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
  FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
#endif
  compute_body_orientation_and_rates();

}
开发者ID:1bitsquared，项目名称:paparazzi，代码行数:35，代码来源:ahrs_float_cmpl.c

示例14: ahrs_align

void ahrs_align(void) {


  /* Compute an initial orientation using euler angles */
  ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
  /* Convert initial orientation in quaternion and rotation matrice representations. */
  compute_imu_quat_and_rmat_from_euler();

  compute_body_orientation();

  /* Use low passed gyro value as initial bias */
  RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
  RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
  INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28);

  ahrs.status = AHRS_RUNNING;

}
开发者ID:cbeighley，项目名称:paparazzi，代码行数:18，代码来源:ahrs_int_cmpl.c

示例15: compute_body_orientation_and_rates

/*
 * Compute body orientation and rates from imu orientation and rates
 */
void compute_body_orientation_and_rates(void) {

  /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */

  QUAT_COPY(ahrs_float.ltp_to_body_quat, ahrs_float.ltp_to_imu_quat);
  EULERS_COPY(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_imu_euler);
  RMAT_COPY(ahrs_float.ltp_to_body_rmat, ahrs_float.ltp_to_imu_rmat);
  RATES_COPY(ahrs_float.body_rate, ahrs_float.imu_rate);
}
开发者ID:Ludo6431，项目名称:paparazzi，代码行数:12，代码来源:ahrs_sim.c


注：本文中的RATES_COPY函数示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。