void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb) {
gps.utm_pos.north = CM_OF_M(utm_north);
gps.utm_pos.east = CM_OF_M(utm_east);
//TODO set height above ellipsoid properly
gps.hmsl = utm_alt * 1000.;
gps.gspeed = CM_OF_M(gspeed);
gps.course = EM7RAD_OF_RAD(RadOfDeg(course/10.));
gps.ned_vel.z = -climb*100.;
gps.fix = GPS_FIX_3D;
gps_available = TRUE;
}
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_EM7RAD((double)in->lon);
in_d.lat = RAD_OF_EM7RAD((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);
}
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
}
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(<p_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(<p_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, <p_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, <p_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");
}
