void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
struct EnuCoor_i enu;
enu_of_ecef_point_i(&enu, def, ecef);
ENU_OF_TO_NED(*ned, enu);
}
void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
struct EnuCoor_i enu;
enu_of_ecef_point_i(&enu, def, ecef);//先得到enu坐标信息
ENU_OF_TO_NED(*ned, enu);//enu坐标转换到ned坐标，z轴取反即可
}
/** Convert a ECEF position to local ENU.
* @param[out] enu ENU position in meter << #INT32_POS_FRAC
* @param[in] def local coordinate system definition
* @param[in] ecef ECEF position in cm
*/
void enu_of_ecef_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
{
struct EnuCoor_i enu_cm;
enu_of_ecef_point_i(&enu_cm, def, ecef);
/* enu = (enu_cm / 100) << INT32_POS_FRAC
* to loose less range:
* enu_cm = enu << (INT32_POS_FRAC-2) / 25
* which puts max enu output Q23.8 range to 8388km / 25 = 335km
*/
INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC - 2);
VECT3_SDIV(*enu, *enu, 25);
}
void follow_change_wp( unsigned char* buffer ) {
struct EcefCoor_i new_pos;
struct EnuCoor_i enu;
new_pos.x = DL_REMOTE_GPS_ecef_x(buffer);
new_pos.y = DL_REMOTE_GPS_ecef_y(buffer);
new_pos.z = DL_REMOTE_GPS_ecef_z(buffer);
// Translate to ENU
enu_of_ecef_point_i(&enu, &ins_impl.ltp_def, &new_pos);
INT32_VECT3_SCALE_2(enu, enu, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
// TODO: Add the angle to the north
// Update the offsets
enu.x += POS_BFP_OF_REAL(FOLLOW_OFFSET_X);
enu.y += POS_BFP_OF_REAL(FOLLOW_OFFSET_Y);
enu.z += POS_BFP_OF_REAL(FOLLOW_OFFSET_Z);
// TODO: Remove the angle to the north
// Move the waypoint
VECT3_COPY(waypoints[FOLLOW_WAYPOINT_ID], enu);
}
void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
struct EcefCoor_i ecef;
ecef_of_lla_i(&ecef,lla);
enu_of_ecef_point_i(enu,def,&ecef);
}
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");
}
void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
struct EcefCoor_i ecef;
ecef_of_lla_i(&ecef,lla);//先将lla坐标系转换成ecef坐标系
enu_of_ecef_point_i(enu,def,&ecef);//再由ecef坐标系转换成enu坐标系（点）
}
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_point_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_point_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);
}
