C++ (Cpp) INT32_QUAT_INTEGRATE_FI Exemples

Exemple #1

Fichier : ahrs_int_cmpl.c Projet : cbeighley/paparazzi

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

}

Exemple #2

Fichier : ahrs_int_cmpl_quat.c Projet : jeffchenhr/dev-barometer

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

}