コード例 #1
0
void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) {

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

}
コード例 #2
0
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
{

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

}
コード例 #3
0
ファイル: test_geodetic.c プロジェクト: flywewill/paparazzi
static void test_enu_of_ecef_int(void) {

  printf("\n--- enu_of_ecef int ---\n");
  struct EcefCoor_f ref_coor_f = { 4624497.0 , 116475.0, 4376563.0};
  struct LtpDef_f ltp_def_f;
  ltp_def_from_ecef_f(&ltp_def_f, &ref_coor_f);

  struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)),
				   rint(CM_OF_M(ref_coor_f.y)),
				   rint(CM_OF_M(ref_coor_f.z))};
  printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z);
  struct LtpDef_i ltp_def_i;
  ltp_def_from_ecef_i(&ltp_def_i, &ref_coor_i);
  printf("lla0 : (%d %d %d) (%f,%f,%f)\n", ltp_def_i.lla.lat, ltp_def_i.lla.lon, ltp_def_i.lla.alt,
	 DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)),
	 DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)),
	 M_OF_CM((double)ltp_def_i.lla.alt));

#define STEP    1000.
#define RANGE 100000.
  double sum_err = 0;
  struct FloatVect3 max_err;
  FLOAT_VECT3_ZERO(max_err);
  struct FloatVect3 offset;
  for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) {
    for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) {
      for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) {
	struct EcefCoor_f my_ecef_point_f = ref_coor_f;
	VECT3_ADD(my_ecef_point_f, offset);
	struct EnuCoor_f  my_enu_point_f;
	enu_of_ecef_point_f(&my_enu_point_f, &ltp_def_f, &my_ecef_point_f);
#if DEBUG
	printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n",
	       my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z,
	       my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z );
#endif

	struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)),
					      rint(CM_OF_M(my_ecef_point_f.y)),
					      rint(CM_OF_M(my_ecef_point_f.z))};;
	struct EnuCoor_i  my_enu_point_i;
	enu_of_ecef_point_i(&my_enu_point_i, &ltp_def_i, &my_ecef_point_i);

#if DEBUG
	//	printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z);
	printf("ecef to enu int   : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n",
	       M_OF_CM((double)my_ecef_point_i.x),
	       M_OF_CM((double)my_ecef_point_i.y),
	       M_OF_CM((double)my_ecef_point_i.z),
	       M_OF_CM((double)my_enu_point_i.x),
	       M_OF_CM((double)my_enu_point_i.y),
	       M_OF_CM((double)my_enu_point_i.z));
#endif

	float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x);
	if (fabs(ex) > max_err.x) max_err.x = fabs(ex);
	float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y);
	if (fabs(ey) > max_err.y) max_err.y = fabs(ey);
	float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z);
	if (fabs(ez) > max_err.z) max_err.z = fabs(ez);
	sum_err += ex*ex + ey*ey + ez*ez;
      }
    }
  }

  double nb_samples = (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1) *  (2*RANGE / STEP + 1);


  printf("enu_of_ecef int/float comparison:\n");
  printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z );
  printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples );
  printf("\n");


}