C++ (Cpp) stateSetPositionUtm_f示例

示例#1

文件： 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;
}

示例#2

文件： ins_alt_float.c 项目： HolyFox812/paparazzi

void ins_update_gps(void) {
#if USE_GPS
  struct UtmCoor_f utm;
  utm.east = gps.utm_pos.east / 100.;
  utm.north = gps.utm_pos.north / 100.;
  utm.zone = nav_utm_zone0;

#if !USE_BAROMETER
  float falt = gps.hmsl / 1000.;
  EstimatorSetAlt(falt);
#endif
  utm.alt = ins_alt;
  // set position
  stateSetPositionUtm_f(&utm);

  struct NedCoor_f ned_vel = {
    gps.ned_vel.x / 100.,
    gps.ned_vel.y / 100.,
    gps.ned_vel.z / 100.
  };
  // set velocity
  stateSetSpeedNed_f(&ned_vel);

#endif
}

示例#3

文件： ins_alt_float.c 项目： HolyFox812/paparazzi

void ins_update_baro() {
#if USE_BAROMETER
  // TODO update kalman filter with baro struct
  if (baro.status == BS_RUNNING) {
    if (!ins_baro_initialised) {
      ins_qfe = baro.absolute;
      ins_baro_initialised = TRUE;
    }
    if (ins.vf_realign) {
      ins.vf_realign = FALSE;
      ins_qfe = baro.absolute;
    }
    else { /* not realigning, so normal update with baro measurement */
      /* altitude decreases with increasing baro.absolute pressure */
      ins_baro_alt = ground_alt - (baro.absolute - ins_qfe) * INS_BARO_SENS;
      /* run the filter */
      EstimatorSetAlt(ins_baro_alt);
      /* set new altitude, just copy old horizontal position */
      struct UtmCoor_f utm;
      UTM_COPY(utm, *stateGetPositionUtm_f());
      utm.alt = ins_alt;
      stateSetPositionUtm_f(&utm);
    }
  }
#endif
}

示例#4

文件： 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();
}

示例#5

文件： 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
}

示例#6

文件： 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();
}

示例#7

文件： 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);
}

示例#8

文件： ins_xsens700.c 项目： CodeMining/paparazzi

void ins_xsens_update_gps(struct GpsState *gps_s)
{
  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
  utm.alt = gps_s->hmsl / 1000.;

  // set position
  stateSetPositionUtm_f(&utm);

  struct NedCoor_f ned_vel = {
    gps_s->ned_vel.x / 100.,
    gps_s->ned_vel.y / 100.,
    gps_s->ned_vel.z / 100.
  };
  // set velocity
  stateSetSpeedNed_f(&ned_vel);
}

示例#9

文件： ins_xsens700.c 项目： EwoudSmeur/paparazzi

static void gps_cb(uint8_t sender_id __attribute__((unused)),
                   uint32_t stamp __attribute__((unused)),
                   struct GpsState *gps_s)
{
  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);

  // set position
  stateSetPositionUtm_f(&utm);

  struct NedCoor_f ned_vel = {
    gps_s->ned_vel.x / 100.,
    gps_s->ned_vel.y / 100.,
    gps_s->ned_vel.z / 100.
  };
  // set velocity
  stateSetSpeedNed_f(&ned_vel);
}

示例#10

文件： ins_gps_passthrough.c 项目： Aishwaryabr/paparazzi

void ins_update_gps(void) {
  struct UtmCoor_f utm;
  utm.east = gps.utm_pos.east / 100.;
  utm.north = gps.utm_pos.north / 100.;
  utm.zone = nav_utm_zone0;
  utm.alt = gps.hmsl / 1000.;

  // set position
  stateSetPositionUtm_f(&utm);

  struct NedCoor_f ned_vel = {
    gps.ned_vel.x / 100.,
    gps.ned_vel.y / 100.,
    gps.ned_vel.z / 100.
  };
  // set velocity
  stateSetSpeedNed_f(&ned_vel);
}

示例#11

文件： 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.);

}

示例#12

文件： 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;
}

示例#13

文件： 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);
}