C++ (Cpp) ahrs_float_get_euler_from_accel_magの例

コード例 #1

ファイル: ahrs_float_cmpl_rmat.c プロジェクト: Ludo6431/paparazzi

void ahrs_align(void) {

  /* Compute an initial orientation using euler angles */
  ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
  /* Convert initial orientation in quaternion and rotation matrice representations. */
  FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
  FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
  /* Compute initial body orientation */
  compute_body_orientation_and_rates();

  /* used averaged gyro as initial value for bias */
  struct Int32Rates bias0;
  RATES_COPY(bias0, ahrs_aligner.lp_gyro);
  RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);

  ahrs.status = AHRS_RUNNING;

}

コード例 #2

ファイル: ahrs_float_dcm.c プロジェクト: rijesha/paparazzi

bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
                    struct FloatVect3 *lp_mag)
{
    /* Compute an initial orientation using euler angles */
    ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag);

    /* Convert initial orientation in quaternion and rotation matrice representations. */
    struct FloatRMat ltp_to_imu_rmat;
    float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);

    /* set filter dcm */
    set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);

    /* use averaged gyro as initial value for bias */
    ahrs_dcm.gyro_bias = *lp_gyro;

    ahrs_dcm.status = AHRS_DCM_RUNNING;
    ahrs_dcm.is_aligned = true;

    return true;
}

コード例 #3

ファイル: ahrs_float_dcm.c プロジェクト: jmavi/paparazzi_fix

bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                      struct Int32Vect3 *lp_mag)
{
  /* Compute an initial orientation using euler angles */
  ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag);

  /* Convert initial orientation in quaternion and rotation matrice representations. */
  struct FloatRMat ltp_to_imu_rmat;
  float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);

  /* set filter dcm */
  set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);

  /* use averaged gyro as initial value for bias */
  struct Int32Rates bias0;
  RATES_COPY(bias0, *lp_gyro);
  RATES_FLOAT_OF_BFP(ahrs_dcm.gyro_bias, bias0);

  ahrs_dcm.status = AHRS_DCM_RUNNING;
  ahrs_dcm.is_aligned = TRUE;

  return TRUE;
}