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); }