C++ (Cpp) RAD_OF_EM7DEG Examples

Example #1

File: pprz_geodetic_int.c Project: 2seasuav/paparuzzi

void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
{

#if USE_SINGLE_PRECISION_LLA_ECEF
  /* convert our input to floating point */
  struct LlaCoor_f in_f;
  in_f.lon = RAD_OF_EM7DEG((float)in->lon);
  in_f.lat = RAD_OF_EM7DEG((float)in->lat);
  in_f.alt = M_OF_MM((float)in->alt);
  /* calls the floating point transformation */
  struct EcefCoor_f out_f;
  ecef_of_lla_f(&out_f, &in_f);
  /* convert the output to fixed point       */
  out->x = (int32_t)CM_OF_M(out_f.x);
  out->y = (int32_t)CM_OF_M(out_f.y);
  out->z = (int32_t)CM_OF_M(out_f.z);
#else // use double precision by default
  /* convert our input to floating point */
  struct LlaCoor_d in_d;
  in_d.lon = RAD_OF_EM7DEG((double)in->lon);
  in_d.lat = RAD_OF_EM7DEG((double)in->lat);
  in_d.alt = M_OF_MM((double)in->alt);
  /* calls the floating point transformation */
  struct EcefCoor_d out_d;
  ecef_of_lla_d(&out_d, &in_d);
  /* convert the output to fixed point       */
  out->x = (int32_t)CM_OF_M(out_d.x);
  out->y = (int32_t)CM_OF_M(out_d.y);
  out->z = (int32_t)CM_OF_M(out_d.z);
#endif

}

Example #2

File: pprz_geodetic_int.c Project: De-Night/paparazzi

void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla)
{

#if USE_SINGLE_PRECISION_TRIG
  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
#else // use double precision by default
  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
#endif

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

Example #3

File: pprz_geodetic_int.c Project: 2770862886/paparazzi

void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in)
{

  /* convert our input to floating point */
  struct LlaCoor_d in_d;
  in_d.lon = RAD_OF_EM7DEG((double)in->lon);
  in_d.lat = RAD_OF_EM7DEG((double)in->lat);
  in_d.alt = M_OF_MM((double)in->alt);
  /* calls the floating point transformation */
  struct EcefCoor_d out_d;
  ecef_of_lla_d(&out_d, &in_d);
  /* convert the output to fixed point       */
  out->x = (int32_t)CM_OF_M(out_d.x);
  out->y = (int32_t)CM_OF_M(out_d.y);
  out->z = (int32_t)CM_OF_M(out_d.z);

}