C++ (Cpp) b2_hff_init_y примеры использования

Пример #1

Файл: hf_float.c Проект: 2seasuav/paparuzzi

void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
{
  Rgps_pos = HFF_R_POS;
  Rgps_vel = HFF_R_SPEED;
  b2_hff_init_x(init_x, init_xdot);
  b2_hff_init_y(init_y, init_ydot);
#ifdef GPS_LAG
  /* init buffer for past mean accel values */
  acc_buf_r = 0;
  acc_buf_w = 0;
  acc_buf_n = 0;
  b2_hff_rb_put = b2_hff_rb;
  b2_hff_rb_last = b2_hff_rb;
  b2_hff_rb_last->rollback = false;
  b2_hff_rb_last->lag_counter = 0;
  b2_hff_state.lag_counter = GPS_LAG_N;
#ifdef SITL
  printf("GPS_LAG: %f\n", GPS_LAG);
  printf("GPS_LAG_N: %d\n", GPS_LAG_N);
  printf("GPS_DT_N: %d\n", GPS_DT_N);
  printf("DT_HFILTER: %f\n", DT_HFILTER);
  printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
#endif
#else
  b2_hff_rb_last = &b2_hff_state;
  b2_hff_state.lag_counter = 0;
#endif
  b2_hff_rb_n = 0;
  b2_hff_state.rollback = false;
  lag_counter_err = 0;
  save_counter = -1;
  past_save_counter = SAVE_DONE;
  b2_hff_ps_counter = 1;
  b2_hff_lost_counter = 0;
  b2_hff_lost_limit = HFF_LOST_LIMIT;

#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF, send_hff);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_DBG, send_hff_debug);
#ifdef GPS_LAG
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_GPS, send_hff_gps);
#endif
#endif

  init_butterworth_2_low_pass_int(&filter_x, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0);
  init_butterworth_2_low_pass_int(&filter_y, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0);
  init_butterworth_2_low_pass_int(&filter_z, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0);
}

Пример #2

Файл: hf_float.c Проект: Paolo-Maffei/lxyppc-tetrix

void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
  Rgps_pos = HFF_R_POS;
  Rgps_vel = HFF_R_SPEED;
  b2_hff_init_x(init_x, init_xdot);
  b2_hff_init_y(init_y, init_ydot);
  /* init buffer for mean accel calculation */
  acc_body.r = 0;
  acc_body.w = 0;
  acc_body.n = 0;
  acc_body.size = ACC_RB_MAXN;
#ifdef GPS_LAG
  /* init buffer for past mean accel values */
  acc_buf_r = 0;
  acc_buf_w = 0;
  acc_buf_n = 0;
  b2_hff_rb_put = b2_hff_rb;
  b2_hff_rb_last = b2_hff_rb;
  b2_hff_rb_last->rollback = FALSE;
  b2_hff_rb_last->lag_counter = 0;
  b2_hff_state.lag_counter = GPS_LAG_N;
#ifdef SITL
  printf("GPS_LAG: %f\n", GPS_LAG);
  printf("GPS_LAG_N: %d\n", GPS_LAG_N);
  printf("GPS_DT_N: %d\n", GPS_DT_N);
  printf("DT_HFILTER: %f\n", DT_HFILTER);
  printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
#endif
#else
  b2_hff_rb_last = &b2_hff_state;
  b2_hff_state.lag_counter = 0;
#endif
  b2_hff_rb_n = 0;
  b2_hff_state.rollback = FALSE;
  lag_counter_err = 0;
  save_counter = -1;
  past_save_counter = SAVE_DONE;
  b2_hff_ps_counter = 1;
  b2_hff_lost_counter = 0;
  b2_hff_lost_limit = HFF_LOST_LIMIT;
}