C++ (Cpp) enu_of_ecef_pos_i示例

示例#1

文件： pprz_geodetic_int.c 项目： De-Night/paparazzi

/** Convert a ECEF position to local NED.
 * @param[out] ned  NED position in meter << #INT32_POS_FRAC
 * @param[in]  def  local coordinate system definition
 * @param[in]  ecef ECEF position in cm
 */
void ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
{
  struct EnuCoor_i enu;
  enu_of_ecef_pos_i(&enu, def, ecef);
  ENU_OF_TO_NED(*ned, enu);
}

示例#2

文件： state.c 项目： Merafour/paparazzi

void stateCalcPositionEnu_f(void)
{
  if (bit_is_set(state.pos_status, POS_ENU_F)) {
    return;
  }

  int errno = 0;
  if (state.ned_initialized_f) {
    if (bit_is_set(state.pos_status, POS_NED_F)) {
      VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_NED_I)) {
      NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
      SetBit(state.pos_status, POS_NED_F);
      VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
      enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
    } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
      /* transform ecef_i -> enu_i -> enu_f, set status bits */
      enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      SetBit(state.pos_status, POS_ENU_I);
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
      enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
      /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
      ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
      SetBit(state.pos_status, POS_ECEF_I);
      enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      SetBit(state.pos_status, POS_ENU_I);
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    } else { /* could not get this representation,  set errno */
      errno = 1;
    }
  } else if (state.utm_initialized_f) {
    if (bit_is_set(state.pos_status, POS_ENU_I)) {
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_NED_F)) {
      VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_NED_I)) {
      NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
      SetBit(state.pos_status, POS_NED_F);
      VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
      ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
      /* transform lla_f -> utm_f -> enu, set status bits */
      utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
      SetBit(state.pos_status, POS_UTM_F);
      ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
      /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
      LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
      SetBit(state.pos_status, POS_LLA_F);
      utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
      SetBit(state.pos_status, POS_UTM_F);
      ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
    } else { /* could not get this representation,  set errno */
      errno = 2;
    }
  } else { /* ned coordinate system not initialized,  set errno */
    errno = 3;
  }
  if (errno) {
    //struct EnuCoor_f _enu_zero = {0.0f};
    //return _enu_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_ENU_F);
}