Ejemplos de get_psi_measurement_from_mag en C++ (Cpp)

Ejemplo n.º 1

Archivo: ahrs_int_cmpl_euler.c Proyecto: 2seasuav/paparuzzi

void ahrs_ice_update_mag(struct Int32Vect3 *mag)
{

  get_psi_measurement_from_mag(&ahrs_ice.measurement.psi, ahrs_ice.ltp_to_imu_euler.phi, ahrs_ice.ltp_to_imu_euler.theta,
                               mag);

}

Ejemplo n.º 2

Archivo: ahrs_int_cmpl_euler.c Proyecto: 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;

}

Ejemplo n.º 3

Archivo: ahrs_int_cmpl_euler.c Proyecto: 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;
}

Ejemplo n.º 4

Archivo: ahrs_int_cmpl_euler.c Proyecto: Paolo-Maffei/lxyppc-tetrix

void ahrs_update_mag(void) {

  get_psi_measurement_from_mag(&ahrs_impl.measurement.psi, ahrs.ltp_to_imu_euler.phi, ahrs.ltp_to_imu_euler.theta, imu.mag);

}