C++ (Cpp) ahrs_int_get_quat_from_accel Exemples

Exemple #1

Fichier : ahrs_int_cmpl_quat.c Projet : AE4317group07/paparazzi

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;
}

Exemple #2

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

void ahrs_align(void) {

#if USE_MAGNETOMETER
  /* Compute an initial orientation from accel and mag directly as quaternion */
  ahrs_int_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_int_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
  ahrs_impl.heading_aligned = FALSE;
#endif

  set_body_state_from_quat();

  /* 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;
}