Exemplos de get_phi_theta_measurement_fom_accel em C++ (Cpp)

Exemplo n.º 1

Arquivo: ahrs_int_cmpl_euler.c Projeto: Paolo-Maffei/lxyppc-tetrix

void ahrs_update_accel(void) {

#if USE_NOISE_CUT || USE_NOISE_FILTER
  static struct Int32Vect3 last_accel = { 0, 0, 0 };
#endif
#if USE_NOISE_CUT
  if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) {
#endif
#if USE_NOISE_FILTER
    VECT3_SUM_SCALED(imu.accel, imu.accel, last_accel, NOISE_FILTER_GAIN);
    VECT3_SDIV(imu.accel, imu.accel, NOISE_FILTER_GAIN+1);
#endif
    get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel);
#if USE_NOISE_CUT
  }
  VECT3_COPY(last_accel, imu.accel);
#endif

}

Exemplo n.º 2

Arquivo: ahrs_int_cmpl_euler.c Projeto: Paolo-Maffei/lxyppc-tetrix

void ahrs_align(void) {

  get_phi_theta_measurement_fom_accel(&ahrs_impl.hi_res_euler.phi, &ahrs_impl.hi_res_euler.theta, ahrs_aligner.lp_accel);
  get_psi_measurement_from_mag(&ahrs_impl.hi_res_euler.psi,
                               ahrs_impl.hi_res_euler.phi/F_UPDATE, ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag);

  EULERS_COPY(ahrs_impl.measure, ahrs_impl.hi_res_euler);
  EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler);

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

  RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
  ahrs.status = AHRS_RUNNING;

}

Exemplo n.º 3

Arquivo: ahrs_int_cmpl_euler.c Projeto: 2seasuav/paparuzzi

bool ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                      struct Int32Vect3 *lp_mag)
{

  get_phi_theta_measurement_fom_accel(&ahrs_ice.hi_res_euler.phi,
                                      &ahrs_ice.hi_res_euler.theta, lp_accel);
  get_psi_measurement_from_mag(&ahrs_ice.hi_res_euler.psi,
                               ahrs_ice.hi_res_euler.phi / F_UPDATE,
                               ahrs_ice.hi_res_euler.theta / F_UPDATE, lp_mag);

  EULERS_COPY(ahrs_ice.measure, ahrs_ice.hi_res_euler);
  EULERS_COPY(ahrs_ice.measurement, ahrs_ice.hi_res_euler);

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

  RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro);

  ahrs_ice.status = AHRS_ICE_RUNNING;
  ahrs_ice.is_aligned = true;

  return true;
}