Ejemplos de FLOAT_RATES_LIN_CMB en C++ (Cpp)

Ejemplo n.º 1

Archivo: ahrs_float_cmpl.c Proyecto: 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();

}

Ejemplo n.º 2

Archivo: ahrs_float_mlkf.c Proyecto: josibarns/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 only if rate is non null */
  const float no = FLOAT_RATES_NORM(ahrs_impl.imu_rate);
  if (no > FLT_MIN) {
    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
    const float a  = 0.5*no*dt;
    const float ca = cosf(a);
    const float sa_ov_no = sinf(a)/no;
    const float dp = sa_ov_no*ahrs_impl.imu_rate.p;
    const float dq = sa_ov_no*ahrs_impl.imu_rate.q;
    const float dr = sa_ov_no*ahrs_impl.imu_rate.r;
    const float qi = ahrs_impl.ltp_to_imu_quat.qi;
    const float qx = ahrs_impl.ltp_to_imu_quat.qx;
    const float qy = ahrs_impl.ltp_to_imu_quat.qy;
    const float qz = ahrs_impl.ltp_to_imu_quat.qz;
    ahrs_impl.ltp_to_imu_quat.qi = ca*qi - dp*qx - dq*qy - dr*qz;
    ahrs_impl.ltp_to_imu_quat.qx = dp*qi + ca*qx + dr*qy - dq*qz;
    ahrs_impl.ltp_to_imu_quat.qy = dq*qi - dr*qx + ca*qy + dp*qz;
    ahrs_impl.ltp_to_imu_quat.qz = dr*qi + dq*qx - dp*qy + ca*qz;

    //    printf("%f\n",  ahrs_impl.ltp_to_imu_quat.qi);
  }

}

Ejemplo n.º 3

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

}

Ejemplo n.º 4

Archivo: ahrs_float_cmpl.c Proyecto: jmavi/paparazzi_fix

void ahrs_fc_propagate(struct Int32Rates *gyro, float dt)
{

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

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

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

#if AHRS_PROPAGATE_RMAT
  float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt);
  float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat);
  float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
  float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt);
  float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
  float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
#endif

  // increase accel and mag propagation counters
  ahrs_fc.accel_cnt++;
  ahrs_fc.mag_cnt++;
}