C++ (Cpp) lla_of_ecef_i示例

示例#1

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

void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {

  /* store the origin of the tangeant plane */
  VECT3_COPY(def->ecef, *ecef);
  /* compute the lla representation of the origin */
  lla_of_ecef_i(&def->lla, &def->ecef);
  /* store the rotation matrix                    */

#if 1
  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#else
  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#endif


  def->ltp_of_ecef.m[0] = -sin_lon;
  def->ltp_of_ecef.m[1] =  cos_lon;
  def->ltp_of_ecef.m[2] =  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
  def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[5] =  cos_lat;
  def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[8] =  sin_lat;

}

示例#2

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

void stateCalcPositionLla_i(void) {
  if (bit_is_set(state.pos_status, POS_LLA_I))
    return;

  if (bit_is_set(state.pos_status, POS_LLA_F)) {
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
  }
  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
    /* transform ecef_f -> lla_f, set status bit, then convert to int */
    lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
    SetBit(state.pos_status, POS_LLA_F);
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_NED_F)) {
    /* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */
    ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
    SetBit(state.pos_status, POS_ECEF_F);
    lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
    SetBit(state.pos_status, POS_LLA_F);
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_NED_I)) {
    /* transform ned_i -> ecef_i -> lla_i, set status bits */
    /// @todo check if resolution is enough
    ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    SetBit(state.pos_status, POS_ECEF_I);
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */
  }
  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
    /* transform utm_f -> lla_f -> lla_i, set status bits */
    lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
    SetBit(state.pos_status, POS_LLA_F);
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  }
  else {
    /* could not get this representation,  set errno */
    //struct LlaCoor_i _lla_zero = {0};
    //return _lla_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_LLA_I);
}

示例#3

void waypoint_globalize(uint8_t wp_id)
{
    if (state.ned_initialized_i) {
        struct EcefCoor_i ecef;
        ecef_of_enu_pos_i(&ecef, &state.ned_origin_i, &waypoints[wp_id].enu_i);
        lla_of_ecef_i(&waypoints[wp_id].lla, &ecef);
        SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
    }
}

示例#4

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

void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
{

  /* store the origin of the tangent plane */
  VECT3_COPY(def->ecef, *ecef);
  /* compute the lla representation of the origin */
  lla_of_ecef_i(&def->lla, &def->ecef);
  /* store the rotation matrix                    */
  ltp_of_ecef_rmat_from_lla_i(&def->ltp_of_ecef, &def->lla);

}

示例#5

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

/**
 * Calculate LLA (int) from any other available representation.
 * Note that since LLA in float has bad precision this is the last choice.
 * So we mostly first convert to ECEF and then use lla_of_ecef_i
 * which provides higher precision but is currently using the double function internally.
 */
void stateCalcPositionLla_i(void)
{
  if (bit_is_set(state.pos_status, POS_LLA_I)) {
    return;
  }

  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
    /* transform ecef_f -> ecef_i -> lla_i, set status bits */
    ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
    SetBit(state.pos_status, POS_ECEF_I);
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
    /* transform ned_i -> ecef_i -> lla_i, set status bits */
    ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    SetBit(state.pos_status, POS_ECEF_I);
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
  } else if (bit_is_set(state.pos_status, POS_ENU_I) && state.ned_initialized_i) {
    /* transform enu_i -> ecef_i -> lla_i, set status bits */
    ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
    SetBit(state.pos_status, POS_ECEF_I);
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_i) {
    /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
    NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
    SetBit(state.pos_status, POS_NED_I);
    ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    SetBit(state.pos_status, POS_ECEF_I);
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
  } else if (bit_is_set(state.pos_status, POS_ENU_F) && state.ned_initialized_i) {
    /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
    ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
    SetBit(state.pos_status, POS_ENU_I);
    ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
    SetBit(state.pos_status, POS_ECEF_I);
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
    /* transform utm_f -> lla_f -> lla_i, set status bits */
    lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
    SetBit(state.pos_status, POS_LLA_F);
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  } else {
    /* could not get this representation,  set errno */
    //struct LlaCoor_i _lla_zero = {0};
    //return _lla_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_LLA_I);
}

示例#6

文件： pprz_geodetic_int.c 项目： WenFly123/openPPZ

void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {

  /* store the origin of the tangeant plane */
  /* 保存原始的切平面数据*/
  VECT3_COPY(def->ecef, *ecef);
  /* compute the lla representation of the origin */
  /* 计算LLA原始点的位置信息（lon:rad*1e7,lat:rad*1e7,alt：mm）*/
  lla_of_ecef_i(&def->lla, &def->ecef);
  /* store the rotation matrix                    */
  /* 存储旋转矩阵值*/

#if 1
  //计算经度和纬度的正弦和余弦的值（先转换成float型，再求sin和cos ,最后*<<20,求得整数值）
  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#else
  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#endif

/* 以下几位ECEF到lla坐标系的转换矩阵*/
  def->ltp_of_ecef.m[0] = -sin_lon;
  def->ltp_of_ecef.m[1] =  cos_lon;
  def->ltp_of_ecef.m[2] =  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
  def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[5] =  cos_lat;
  def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[8] =  sin_lat;

}