Ejemplos de float_rates_integrate_fi en C++ (Cpp)

Ejemplo n.º 1

Archivo: stabilization_indi.c Proyecto: Merafour/paparazzi

// This is a simple second order low pass filter
static void stabilization_indi_second_order_filter(struct IndiFilter *filter, struct FloatRates *input)
{
  float_rates_integrate_fi(&filter->x, &filter->dx, 1.0 / PERIODIC_FREQUENCY);
  float_rates_integrate_fi(&filter->dx, &filter->ddx, 1.0 / PERIODIC_FREQUENCY);

  filter->ddx.p = -filter->dx.p * 2 * filter->zeta * filter->omega   + (input->p - filter->x.p) * filter->omega2;
  filter->ddx.q = -filter->dx.q * 2 * filter->zeta * filter->omega   + (input->q - filter->x.q) * filter->omega2;
  filter->ddx.r = -filter->dx.r * 2 * filter->zeta * filter->omega_r + (input->r - filter->x.r) * filter->omega2_r;
}

Ejemplo n.º 2

Archivo: stabilization_attitude_quat_indi.c Proyecto: robinjanssen/paparazzi

// This is a simple second order low pass filter
void stabilization_indi_second_order_filter(struct FloatRates *input, struct FloatRates *filter_ddx,
    struct FloatRates *filter_dx, struct FloatRates *filter_x, float omega, float zeta, float omega_r)
{
  float_rates_integrate_fi(filter_x, filter_dx, 1.0 / PERIODIC_FREQUENCY);
  float_rates_integrate_fi(filter_dx, filter_ddx, 1.0 / PERIODIC_FREQUENCY);
  float omega2 = omega * omega;
  float omega2_r = omega_r * omega_r;

  filter_ddx->p = -filter_dx->p * 2 * zeta * omega   + (input->p - filter_x->p) * omega2;    \
  filter_ddx->q = -filter_dx->q * 2 * zeta * omega   + (input->q - filter_x->q) * omega2;    \
  filter_ddx->r = -filter_dx->r * 2 * zeta * omega_r + (input->r - filter_x->r) * omega2_r;
}

Ejemplo n.º 3

Archivo: ahrs_on_synth.c Proyecto: EwoudSmeur/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);
  }

}