C++ (Cpp) stateSetLocalUtmOrigin_f 예제들

예제 #1

파일: common_nav.c 프로젝트: CheBuzz/paparazzi

/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
#ifdef GPS_USE_LATLONG
  /* Set the real UTM zone */
  nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;

  /* Recompute UTM coordinates in this zone */
  struct LlaCoor_f lla;
  lla.lat = gps.lla_pos.lat/1e7;
  lla.lon = gps.lla_pos.lon/1e7;
  struct UtmCoor_f utm;
  utm.zone = nav_utm_zone0;
  utm_of_lla_f(&utm, &lla);
  nav_utm_east0 = utm.east;
  nav_utm_north0 = utm.north;
#else
  nav_utm_zone0 = gps.utm_pos.zone;
  nav_utm_east0 = gps.utm_pos.east/100;
  nav_utm_north0 = gps.utm_pos.north/100;
#endif

  // reset state UTM ref
  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
  stateSetLocalUtmOrigin_f(&utm0);

  previous_ground_alt = ground_alt;
  ground_alt = gps.hmsl/1000.;
  return 0;
}

예제 #2

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

void ins_init() {
  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
  stateSetLocalUtmOrigin_f(&utm0);
  stateSetPositionUtm_f(&utm0);

  ins.status = INS_RUNNING;
}

예제 #3

파일: ins_float_invariant.c 프로젝트: HoplaHopla/paparazzi

void ins_reset_local_origin( void ) {
#if INS_UPDATE_FW_ESTIMATOR
  struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
  /* Recompute UTM coordinates in this zone */
  struct LlaCoor_f lla;
  lla.lat = gps.lla_pos.lat / 1e7;
  lla.lon = gps.lla_pos.lon / 1e7;
  utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
  utm_of_lla_f(&utm, &lla);
#else
  utm.zone = gps.utm_pos.zone;
  utm.east = gps.utm_pos.east / 100.0f;
  utm.north = gps.utm_pos.north / 100.0f;
#endif
  // ground_alt
  utm.alt = gps.hmsl / 1000.0f;
  // reset state UTM ref
  stateSetLocalUtmOrigin_f(&utm);
#else
  struct LtpDef_i ltp_def;
  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
  ltp_def.hmsl = gps.hmsl;
  stateSetLocalOrigin_i(&ltp_def);
#endif
}

예제 #4

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

/** Reset the UTM zone to current GPS fix */
unit_t nav_reset_utm_zone(void) {

  struct UtmCoor_f utm0_old;
  utm0_old.zone = nav_utm_zone0;
  utm0_old.north = nav_utm_north0;
  utm0_old.east = nav_utm_east0;
  utm0_old.alt = ground_alt;
  struct LlaCoor_f lla0;
  lla_of_utm_f(&lla0, &utm0_old);

#ifdef GPS_USE_LATLONG
  /* Set the real UTM zone */
  nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
#else
  nav_utm_zone0 = gps.utm_pos.zone;
#endif

  struct UtmCoor_f utm0;
  utm0.zone = nav_utm_zone0;
  utm_of_lla_f(&utm0, &lla0);

  nav_utm_east0 = utm0.east;
  nav_utm_north0 = utm0.north;

  stateSetLocalUtmOrigin_f(&utm0);

  return 0;
}

예제 #5

파일: ins_xsens.c 프로젝트: AULA-PPZ/paparazzi

void ins_init(void)
{
  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
  stateSetLocalUtmOrigin_f(&utm0);
  stateSetPositionUtm_f(&utm0);

  xsens_init();
}

예제 #6

파일: ins_float_invariant.c 프로젝트: hannwoei/paparazzi

void ins_init()
{

  // init position
#if INS_UPDATE_FW_ESTIMATOR
  struct UtmCoor_f utm0;
  utm0.north = (float)nav_utm_north0;
  utm0.east = (float)nav_utm_east0;
  utm0.alt = GROUND_ALT;
  utm0.zone = nav_utm_zone0;
  stateSetLocalUtmOrigin_f(&utm0);
  stateSetPositionUtm_f(&utm0);
#else
  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
  llh_nav0.lat = NAV_LAT0;
  llh_nav0.lon = 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);
  struct LtpDef_i ltp_def;
  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
  ltp_def.hmsl = NAV_ALT0;
  stateSetLocalOrigin_i(&ltp_def);
#endif

  B.x = INS_H_X;
  B.y = INS_H_Y;
  B.z = INS_H_Z;

  // Bind to BARO_ABS message
  AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);

  // init state and measurements
  init_invariant_state();

  // init gains
  ins_impl.gains.lv   = INS_INV_LV;
  ins_impl.gains.lb   = INS_INV_LB;
  ins_impl.gains.mv   = INS_INV_MV;
  ins_impl.gains.mvz  = INS_INV_MVZ;
  ins_impl.gains.mh   = INS_INV_MH;
  ins_impl.gains.nx   = INS_INV_NX;
  ins_impl.gains.nxz  = INS_INV_NXZ;
  ins_impl.gains.nh   = INS_INV_NH;
  ins_impl.gains.ov   = INS_INV_OV;
  ins_impl.gains.ob   = INS_INV_OB;
  ins_impl.gains.rv   = INS_INV_RV;
  ins_impl.gains.rh   = INS_INV_RH;
  ins_impl.gains.sh   = INS_INV_SH;

  ins.status = INS_UNINIT;
  ins_impl.reset = FALSE;

#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
#endif
}

예제 #7

파일: ins_alt_float.c 프로젝트: evandersman/paparazzi

void ins_reset_altitude_ref(void) {
    struct UtmCoor_f utm = state.utm_origin_f;
    // ground_alt
    utm.alt = gps.hmsl / 1000.0f;
    // reset state UTM ref
    stateSetLocalUtmOrigin_f(&utm);
    // reset filter flag
    ins_impl.reset_alt_ref = TRUE;
}

예제 #8

파일: ins_xsens700.c 프로젝트: Abhi0204/paparazzi

void ins_init(void) {
  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
  stateSetLocalUtmOrigin_f(&utm0);
  stateSetPositionUtm_f(&utm0);

  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;

  xsens_init();
}

예제 #9

파일: ins.c 프로젝트: 2seasuav/paparuzzi

void WEAK ins_reset_local_origin(void)
{
#if USE_GPS
  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);

  // ground_alt
  utm.alt = gps.hmsl  / 1000.0f;

  // reset state UTM ref
  stateSetLocalUtmOrigin_f(&utm);
#endif
}

예제 #10

파일: ins.c 프로젝트: Abhi0204/paparazzi

void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm) {
  struct LlaCoor_f lla0;
  lla_of_utm_f(&lla0, utm);
#ifdef GPS_USE_LATLONG
  utm->zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
#else
  utm->zone = gps.utm_pos.zone;
#endif
  utm_of_lla_f(utm, &lla0);

  stateSetLocalUtmOrigin_f(utm);
}

예제 #11

파일: ins_xsens700.c 프로젝트: EwoudSmeur/paparazzi

void ins_xsens700_init(void)
{
  xsens700_init();

  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;

  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
  stateSetLocalUtmOrigin_f(&utm0);
  stateSetPositionUtm_f(&utm0);

  AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb);
}

예제 #12

파일: ins.c 프로젝트: 2seasuav/paparuzzi

void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
{
  struct LlaCoor_f lla0;
  lla_of_utm_f(&lla0, utm);
  if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
    utm->zone = gps.utm_pos.zone;
  }
  else {
    utm->zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
  }
  utm_of_lla_f(utm, &lla0);

  stateSetLocalUtmOrigin_f(utm);
}

예제 #13

파일: ins_float_invariant.c 프로젝트: HoplaHopla/paparazzi

void ins_reset_altitude_ref( void ) {
#if INS_UPDATE_FW_ESTIMATOR
  struct UtmCoor_f utm = state.utm_origin_f;
  utm.alt = gps.hmsl / 1000.0f;
  stateSetLocalUtmOrigin_f(&utm);
#else
  struct LlaCoor_i lla = {
    state.ned_origin_i.lla.lon,
    state.ned_origin_i.lla.lat,
    gps.lla_pos.alt
  };
  struct LtpDef_i ltp_def;
  ltp_def_from_lla_i(&ltp_def, &lla),
  ltp_def.hmsl = gps.hmsl;
  stateSetLocalOrigin_i(&ltp_def);
#endif
}

예제 #14

파일: ins_alt_float.c 프로젝트: HolyFox812/paparazzi

void ins_init() {

  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 };
  stateSetLocalUtmOrigin_f(&utm0);

  stateSetPositionUtm_f(&utm0);

  alt_kalman_init();

#if USE_BAROMETER
  ins_qfe = 0;;
  ins_baro_initialised = FALSE;
  ins_baro_alt = 0.;
#endif
  ins.vf_realign = FALSE;

  EstimatorSetAlt(0.);

}

예제 #15

파일: ins_gps_passthrough_utm.c 프로젝트: AULA-PPZ/paparazzi

void ins_reset_local_origin(void)
{
  struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
  /* Recompute UTM coordinates in this zone */
  struct LlaCoor_f lla;
  LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
  utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
  utm_of_lla_f(&utm, &lla);
#else
  utm.zone = gps.utm_pos.zone;
  utm.east = gps.utm_pos.east / 100.0f;
  utm.north = gps.utm_pos.north / 100.0f;
#endif
  // ground_alt
  utm.alt = gps.hmsl / 1000.0f;
  // reset state UTM ref
  stateSetLocalUtmOrigin_f(&utm);
}

예제 #16

파일: ins_alt_float.c 프로젝트: evandersman/paparazzi

void ins_init(void) {

    struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 };
    stateSetLocalUtmOrigin_f(&utm0);

    stateSetPositionUtm_f(&utm0);

    alt_kalman_init();

#if USE_BAROMETER
    ins_impl.qfe = 0.0f;
    ins_impl.baro_initialized = FALSE;
    ins_impl.baro_alt = 0.0f;
    // Bind to BARO_ABS message
    AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
#endif
    ins_impl.reset_alt_ref = FALSE;

    alt_kalman(0.0f);

    ins.status = INS_RUNNING;
}

예제 #17

파일: ins_float_invariant.c 프로젝트: hannwoei/paparazzi

void ins_reset_altitude_ref(void)
{
#if INS_UPDATE_FW_ESTIMATOR
  struct UtmCoor_f utm = state.utm_origin_f;
  utm.alt = gps.hmsl / 1000.0f;
  stateSetLocalUtmOrigin_f(&utm);
#else
  struct LlaCoor_i lla = {
    .lat = state.ned_origin_i.lla.lat,
    .lon = state.ned_origin_i.lla.lon,
    .alt = gps.lla_pos.alt
  };
  struct LtpDef_i ltp_def;
  ltp_def_from_lla_i(&ltp_def, &lla);
  ltp_def.hmsl = gps.hmsl;
  stateSetLocalOrigin_i(&ltp_def);
#endif
}

void ahrs_init(void)
{
  ahrs.status = AHRS_UNINIT;
}

void ahrs_align(void)
{
  /* Compute an initial orientation from accel and mag directly as quaternion */
  ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);

  /* use average gyro as initial value for bias */
  struct FloatRates bias0;
  RATES_COPY(bias0, ahrs_aligner.lp_gyro);
  RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0);

  // ins and ahrs are now running
  ahrs.status = AHRS_RUNNING;
  ins.status = INS_RUNNING;
}

예제 #18

파일: ins_alt_float.c 프로젝트: evandersman/paparazzi

/** Reset the geographic reference to the current GPS fix */
void ins_reset_local_origin(void) {
    struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
    /* Recompute UTM coordinates in this zone */
    struct LlaCoor_f lla;
    lla.lat = gps.lla_pos.lat / 1e7;
    lla.lon = gps.lla_pos.lon / 1e7;
    utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
    utm_of_lla_f(&utm, &lla);
#else
    utm.zone = gps.utm_pos.zone;
    utm.east = gps.utm_pos.east / 100.0f;
    utm.north = gps.utm_pos.north / 100.0f;
#endif
    // ground_alt
    utm.alt = gps.hmsl  /1000.0f;

    // reset state UTM ref
    stateSetLocalUtmOrigin_f(&utm);

    // reset filter flag
    ins_impl.reset_alt_ref = TRUE;
}

예제 #19

파일: ins_gps_passthrough_utm.c 프로젝트: AULA-PPZ/paparazzi

void ins_reset_altitude_ref(void)
{
  struct UtmCoor_f utm = state.utm_origin_f;
  utm.alt = gps.hmsl / 1000.0f;
  stateSetLocalUtmOrigin_f(&utm);
}

예제 #20

파일: ins_gps_passthrough.c 프로젝트: oywjun/openPPZ

void ins_init() {
    struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
    stateSetLocalUtmOrigin_f(&utm0);//通过UTM坐标设置本地坐标
    stateSetPositionUtm_f(&utm0);
}