C++ (Cpp) INTEG_EULER_NORMALIZE Examples

Example #1

File: ahrs_int_cmpl_euler.c Project: Paolo-Maffei/lxyppc-tetrix

void ahrs_propagate(void) {

  /* unbias gyro             */
  struct Int32Rates uf_rate;
  RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias);
#if USE_NOISE_CUT
  static struct Int32Rates last_uf_rate = { 0, 0, 0 };
  if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
    /* low pass rate */
#if USE_NOISE_FILTER
    RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN);
    RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1);
#else
    RATES_ADD(ahrs.imu_rate, uf_rate);
    RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
#endif
#if USE_NOISE_CUT
  }
  RATES_COPY(last_uf_rate, uf_rate);
#endif

  /* integrate eulers */
  struct Int32Eulers euler_dot;
  INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
  EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot);

  /* low pass measurement */
  EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement);
  EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2);

  /* compute residual */
  EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.hi_res_euler);
  INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi);

  struct Int32Eulers correction;
  /* compute a correction */
  EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1);
  /* correct estimation */
  EULERS_ADD(ahrs_impl.hi_res_euler, correction);
  INTEG_EULER_NORMALIZE(ahrs_impl.hi_res_euler.psi);


  /* Compute LTP to IMU eulers      */
  EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);

  compute_imu_quat_and_rmat_from_euler();

  compute_body_orientation();

}

Example #2

File: ahrs_int_cmpl_euler.c Project: 2seasuav/paparuzzi

void ahrs_ice_propagate(struct Int32Rates *gyro)
{

  /* unbias gyro             */
  struct Int32Rates uf_rate;
  RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias);
#if USE_NOISE_CUT
  static struct Int32Rates last_uf_rate = { 0, 0, 0 };
  if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
    /* low pass rate */
#if USE_NOISE_FILTER
    RATES_SUM_SCALED(ahrs_ice.imu_rate, ahrs_ice.imu_rate, uf_rate, NOISE_FILTER_GAIN);
    RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, NOISE_FILTER_GAIN + 1);
#else
    RATES_ADD(ahrs_ice.imu_rate, uf_rate);
    RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, 2);
#endif
#if USE_NOISE_CUT
  }
  RATES_COPY(last_uf_rate, uf_rate);
#endif

  /* integrate eulers */
  struct Int32Eulers euler_dot;
  int32_eulers_dot_of_rates(&euler_dot, &ahrs_ice.ltp_to_imu_euler, &ahrs_ice.imu_rate);
  EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot);

  /* low pass measurement */
  EULERS_ADD(ahrs_ice.measure, ahrs_ice.measurement);
  EULERS_SDIV(ahrs_ice.measure, ahrs_ice.measure, 2);

  /* compute residual */
  EULERS_DIFF(ahrs_ice.residual, ahrs_ice.measure, ahrs_ice.hi_res_euler);
  INTEG_EULER_NORMALIZE(ahrs_ice.residual.psi);

  struct Int32Eulers correction;
  /* compute a correction */
  EULERS_SDIV(correction, ahrs_ice.residual, ahrs_ice.reinj_1);
  /* correct estimation */
  EULERS_ADD(ahrs_ice.hi_res_euler, correction);
  INTEG_EULER_NORMALIZE(ahrs_ice.hi_res_euler.psi);


  /* Compute LTP to IMU eulers      */
  EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE);
}