C++ (Cpp) vff_initの例

コード例 #1

ファイル: ins_int_extended.c プロジェクト: Fokker/paparazzi-1

void ins_init() {
#if USE_INS_NAV_INIT
  ins_ltp_initialised = TRUE;

  /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
  llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
  llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;

  struct EcefCoor_i ecef_nav0;
  ecef_of_lla_i(&ecef_nav0, &llh_nav0);

  ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
  ins_ltp_def.hmsl = NAV_ALT0;
#else
  ins_ltp_initialised  = FALSE;
#endif

  ins_baro_initialised = FALSE;
  init_median_filter(&baro_median);
  ins_update_on_agl = FALSE;
  init_median_filter(&sonar_median);
  ins_sonar_offset = INS_SONAR_OFFSET;
  vff_init(0., 0., 0., 0.);
  ins.vf_realign = FALSE;
  ins.hf_realign = FALSE;
#if USE_HFF
  b2_hff_init(0., 0., 0., 0.);
#endif
  INT32_VECT3_ZERO(ins_ltp_pos);
  INT32_VECT3_ZERO(ins_ltp_speed);
  INT32_VECT3_ZERO(ins_ltp_accel);
}

コード例 #2

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

void vff_realign(float z_meas)
{
  //vff.z = z_meas;
  //vff.zdot = 0.;
  //vff.offset = 0.;
  vff_init(z_meas, 0., 0., 0.);
}

コード例 #3

ファイル: ins_int.c プロジェクト: Aishwaryabr/paparazzi

void ins_init(void) {

#if USE_INS_NAV_INIT
  ins_init_origin_from_flightplan();
  ins_impl.ltp_initialized = TRUE;
#else
  ins_impl.ltp_initialized  = FALSE;
#endif

  // Bind to BARO_ABS message
  AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
  ins_impl.baro_initialized = FALSE;

#if USE_SONAR
  ins_impl.update_on_agl = FALSE;
  init_median_filter(&ins_impl.sonar_median);
  ins_impl.sonar_offset = INS_SONAR_OFFSET;
#endif

  ins.vf_realign = FALSE;
  ins.hf_realign = FALSE;

  /* init vertical and horizontal filters */
#if USE_VFF_EXTENDED
  vff_init(0., 0., 0., 0.);
#else
  vff_init(0., 0., 0.);
#endif
#if USE_HFF
  b2_hff_init(0., 0., 0., 0.);
#endif

  INT32_VECT3_ZERO(ins_impl.ltp_pos);
  INT32_VECT3_ZERO(ins_impl.ltp_speed);
  INT32_VECT3_ZERO(ins_impl.ltp_accel);

#if DOWNLINK
  register_periodic_telemetry(DefaultPeriodic, "INS", send_ins);
  register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z);
  register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
#endif
}

コード例 #4

ファイル: ins.c プロジェクト: MarkGriffin/paparazzi

void ins_init() {
#ifdef USE_INS_NAV_INIT
  ins_ltp_initialised = TRUE;

  /** FIXME: should use the same code than MOVE_WP in booz2_datalink.c */
  struct LlaCoor_i llh; /* Height above the ellipsoid */
  llh.lat = INT32_RAD_OF_DEG(NAV_LAT0);
  llh.lon = INT32_RAD_OF_DEG(NAV_LON0);
  //llh.alt = NAV_ALT0 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
  llh.alt = NAV_ALT0 + NAV_HMSL0;

  struct EcefCoor_i nav_init;
  ecef_of_lla_i(&nav_init, &llh);

  ltp_def_from_ecef_i(&ins_ltp_def, &nav_init);
  ins_ltp_def.hmsl = NAV_ALT0;
#else
  ins_ltp_initialised  = FALSE;
#endif
#ifdef USE_VFF
  ins_baro_initialised = FALSE;
#ifdef BOOZ2_SONAR
  ins_update_on_agl = FALSE;
#endif
  vff_init(0., 0., 0.);
#endif
  ins_vf_realign = FALSE;
  ins_hf_realign = FALSE;
#ifdef USE_HFF
  b2_hff_init(0., 0., 0., 0.);
#endif
  INT32_VECT3_ZERO(ins_ltp_pos);
  INT32_VECT3_ZERO(ins_ltp_speed);
  INT32_VECT3_ZERO(ins_ltp_accel);
  INT32_VECT3_ZERO(ins_enu_pos);
  INT32_VECT3_ZERO(ins_enu_speed);
  INT32_VECT3_ZERO(ins_enu_accel);
}

コード例 #5

ファイル: ins_int.c プロジェクト: F34140r/paparazzi

void ins_init() {
#if USE_INS_NAV_INIT
  ins_ltp_initialised = TRUE;

  /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
  llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
  llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;

  struct EcefCoor_i ecef_nav0;
  ecef_of_lla_i(&ecef_nav0, &llh_nav0);

  ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
  ins_ltp_def.hmsl = NAV_ALT0;
  stateSetLocalOrigin_i(&ins_ltp_def);
#else
  ins_ltp_initialised  = FALSE;
#endif
#if USE_VFF
  ins_baro_initialised = FALSE;
  vff_init(0., 0., 0.);
#endif
  ins.vf_realign = FALSE;
  ins.hf_realign = FALSE;
#if USE_HFF
  b2_hff_init(0., 0., 0., 0.);
#endif
  INT32_VECT3_ZERO(ins_ltp_pos);
  INT32_VECT3_ZERO(ins_ltp_speed);
  INT32_VECT3_ZERO(ins_ltp_accel);

  // TODO correct init
  ins.status = INS_RUNNING;

}

コード例 #6

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

void vff_init_zero(void)
{
  vff_init(0., 0., 0., 0.);
}

コード例 #7

ファイル: vf_float.c プロジェクト: 2seasuav/paparuzzi

void vff_realign(float z_meas)
{
  vff_init(z_meas, 0., 0.);
}

コード例 #8

ファイル: vf_extended_float.c プロジェクト: amtcvx/paparazzi

void vff_init_zero(void)
{
  vff_init(0.f, 0.f, 0.f, 0.f, 0.f);
}