void stateCalcPositionNed_i(void) { if (bit_is_set(state.pos_status, POS_NED_I)) { return; } int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ned_f, set status bit, then convert to int */ ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_ECEF_F); ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_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_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> ned_f -> ned_i, set status bits */ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, 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); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else { /* could not get this representation, set errno */ errno = 2; } } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { //struct NedCoor_i _ned_zero = {0}; //return _ned_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_NED_I); }
int main() { struct LtpDef_i ref; struct EcefCoor_i ecef_ref; ecef_ref.x = -241887298; ecef_ref.y = 538037726; ecef_ref.z = 241732905; ltp_def_from_ecef_i(&ref,&ecef_ref); struct EcefCoor_i pos1; pos1.x = -241887285; pos1.y = 538037716; pos1.z = 241732895; struct NedCoor_i result1; ned_of_ecef_pos_i(&result1,&ref,&ecef_ref); printf("x %f, ",POS_FLOAT_OF_BFP(result1.x)); printf("y %f, ",POS_FLOAT_OF_BFP(result1.y)); printf("z %f\n",POS_FLOAT_OF_BFP(result1.z)); struct NedCoor_i result; ned_of_ecef_pos_i(&result,&ref,&pos1); printf("x %f, ",POS_FLOAT_OF_BFP(result.x)); printf("y %f, ",POS_FLOAT_OF_BFP(result.y)); printf("z %f\n",POS_FLOAT_OF_BFP(result.z)); struct NedCoor_i ned_tar1, ned_tar2; ned_tar1.x = 100 + POS_FLOAT_OF_BFP(result.x) * 100; ned_tar1.y = POS_FLOAT_OF_BFP(result.y) * 100; ned_tar1.z = 100 + POS_FLOAT_OF_BFP(result.z) * 100; ned_tar2.x = -300; ned_tar2.y = 0; ned_tar2.z = 100; struct EcefCoor_i ecef_tar1, ecef_tar2; ecef_of_ned_point_i(&ecef_tar1,&ref,&ned_tar1); ecef_of_ned_point_i(&ecef_tar2,&ref,&ned_tar2); printf("ecef_tar1 x %d y %d z %d\n",ecef_tar1.x,ecef_tar1.y,ecef_tar1.z); printf("ecef_tar2 x %d y %d z %d\n",ecef_tar2.x,ecef_tar2.y,ecef_tar2.z); ned_of_ecef_pos_i(&result,&ref,&ecef_tar1); printf("x %f, ",POS_FLOAT_OF_BFP(result.x)); printf("y %f, ",POS_FLOAT_OF_BFP(result.y)); printf("z %f\n",POS_FLOAT_OF_BFP(result.z)); ned_of_ecef_pos_i(&result,&ref,&ecef_tar2); printf("x %f, ",POS_FLOAT_OF_BFP(result.x)); printf("y %f, ",POS_FLOAT_OF_BFP(result.y)); printf("z %f\n",POS_FLOAT_OF_BFP(result.z)); ned_of_ecef_point_i(&result,&ref,&ecef_tar1); printf("x %f, ",POS_FLOAT_OF_BFP(result.x)); printf("y %f, ",POS_FLOAT_OF_BFP(result.y)); printf("z %f\n",POS_FLOAT_OF_BFP(result.z)); ned_of_ecef_point_i(&result,&ref,&ecef_tar2); printf("x %f, ",POS_FLOAT_OF_BFP(result.x)); printf("y %f, ",POS_FLOAT_OF_BFP(result.y)); printf("z %f\n",POS_FLOAT_OF_BFP(result.z)); return 0; }