void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) { /* store the origin of the tangeant plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ lla_of_ecef_i(&def->lla, &def->ecef); /* store the rotation matrix */ #if 1 int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC)); int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC)); #else int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC)); int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC)); #endif def->ltp_of_ecef.m[0] = -sin_lon; def->ltp_of_ecef.m[1] = cos_lon; def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */ def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[5] = cos_lat; def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[8] = sin_lat; }
void stateCalcPositionLla_i(void) { if (bit_is_set(state.pos_status, POS_LLA_I)) return; if (bit_is_set(state.pos_status, POS_LLA_F)) { LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> lla_f, set status bit, then convert to int */ lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_LLA_F); LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); SetBit(state.pos_status, POS_ECEF_F); lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_LLA_F); LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> lla_i, set status bits */ /// @todo check if resolution is enough ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */ } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> lla_f -> lla_i, set status bits */ lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f); SetBit(state.pos_status, POS_LLA_F); LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else { /* could not get this representation, set errno */ //struct LlaCoor_i _lla_zero = {0}; //return _lla_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_LLA_I); }
void waypoint_globalize(uint8_t wp_id) { if (state.ned_initialized_i) { struct EcefCoor_i ecef; ecef_of_enu_pos_i(&ecef, &state.ned_origin_i, &waypoints[wp_id].enu_i); lla_of_ecef_i(&waypoints[wp_id].lla, &ecef); SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); } }
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef) { /* store the origin of the tangent plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ lla_of_ecef_i(&def->lla, &def->ecef); /* store the rotation matrix */ ltp_of_ecef_rmat_from_lla_i(&def->ltp_of_ecef, &def->lla); }
/** * Calculate LLA (int) from any other available representation. * Note that since LLA in float has bad precision this is the last choice. * So we mostly first convert to ECEF and then use lla_of_ecef_i * which provides higher precision but is currently using the double function internally. */ void stateCalcPositionLla_i(void) { if (bit_is_set(state.pos_status, POS_LLA_I)) { return; } if (bit_is_set(state.pos_status, POS_ECEF_I)) { lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ecef_i -> lla_i, set status bits */ ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) { /* transform ned_i -> ecef_i -> lla_i, set status bits */ ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ENU_I) && state.ned_initialized_i) { /* transform enu_i -> ecef_i -> lla_i, set status bits */ ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_i) { /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); SetBit(state.pos_status, POS_NED_I); ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ENU_F) && state.ned_initialized_i) { /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> lla_f -> lla_i, set status bits */ lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f); SetBit(state.pos_status, POS_LLA_F); LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else { /* could not get this representation, set errno */ //struct LlaCoor_i _lla_zero = {0}; //return _lla_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_LLA_I); }
void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) { /* store the origin of the tangeant plane */ /* 保存原始的切平面数据*/ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ /* 计算LLA原始点的位置信息(lon:rad*1e7,lat:rad*1e7,alt:mm)*/ lla_of_ecef_i(&def->lla, &def->ecef); /* store the rotation matrix */ /* 存储旋转矩阵值*/ #if 1 //计算经度和纬度的正弦和余弦的值(先转换成float型,再求sin和cos ,最后*<<20,求得整数值) int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC)); int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC)); #else int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC)); int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC)); #endif /* 以下几位ECEF到lla坐标系的转换矩阵*/ def->ltp_of_ecef.m[0] = -sin_lon; def->ltp_of_ecef.m[1] = cos_lon; def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */ def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[5] = cos_lat; def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[8] = sin_lat; }