Esempio n. 1
0
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);
}
Esempio n. 2
0
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;
}