C++ (Cpp) FLOAT_QUAT_INTEGRATE 예제들

예제 #1

파일: ahrs_float_cmpl.c 프로젝트: 1bitsquared/paparazzi

void ahrs_propagate(void) {

  /* converts gyro to floating point */
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
  /* unbias measurement */
  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);

#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  const float alpha = 0.1;
  FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
#else
  RATES_COPY(ahrs_impl.imu_rate,gyro_float);
#endif

  /* add correction     */
  struct FloatRates omega;
  RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
  /* and zeros it */
  FLOAT_RATES_ZERO(ahrs_impl.rate_correction);

  const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
#if AHRS_PROPAGATE_RMAT
  FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt);
  float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat);
  FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
  FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt);
  FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
  FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
#endif
  compute_body_orientation_and_rates();

}

예제 #2

파일: ahrs_on_synth.c 프로젝트: AshuLara/paparazzi

static void traj_static_sine_update(void) {
  

  aos.imu_rates.p = RadOfDeg(200)*cos(aos.time);
  aos.imu_rates.q = RadOfDeg(200)*cos(0.7*aos.time+2);
  aos.imu_rates.r = RadOfDeg(200)*cos(0.8*aos.time+1);
  FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
  FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);

}

예제 #3

파일: ahrs_on_synth.c 프로젝트: AshuLara/paparazzi

static void traj_step_phi_2nd_order_update(void) {

  if (aos.time > 15) {
    const float omega = RadOfDeg(100);
    const float xi = 0.9;
    struct FloatRates raccel;
    RATES_ASSIGN(raccel, -2.*xi*omega*aos.imu_rates.p - omega*omega*(aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., 0.);
    FLOAT_RATES_INTEGRATE_FI(aos.imu_rates, raccel, aos.dt);
    FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
    FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);
  }

}

예제 #4

파일: ahrs_float_mlkf.c 프로젝트: Aishwaryabr/paparazzi

static inline void propagate_ref(void) {
  /* converts gyro to floating point */
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);

  /* unbias measurement */
  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);

#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  /* lowpass angular rates */
  const float alpha = 0.1;
  FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
                      (1.-alpha), gyro_float, alpha);
#else
  RATES_COPY(ahrs_impl.imu_rate, gyro_float);
#endif

  /* propagate reference quaternion */
  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
  FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, ahrs_impl.imu_rate, dt);

}