예제 #1
0
// Utility function: converts lla to local point
bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) {
  // return FALSE if there is no valid local coordinate
  // FIXME we should only test if local frame is initialized, not valid
  if (!stateIsLocalCoordinateValid()) return FALSE;

  // change geoid alt to ellipsoid alt
  lla->alt = lla->alt - state.ned_origin_f.hmsl + state.ned_origin_f.lla.alt;
  //Compute ENU components from LLA with respect to ltp origin
  struct EnuCoor_f tmp_enu_point;
  enu_of_lla_point_f(&tmp_enu_point, &state.ned_origin_f, lla);

  //Bound the new waypoint with max distance from home
  struct EnuCoor_f home;
  ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]);
  struct FloatVect2 vect_from_home;
  VECT2_DIFF(vect_from_home, tmp_enu_point, home);
  //Saturate the mission wp not to overflow max_dist_from_home
  //including a buffer zone before limits
  float dist_to_home = float_vect2_norm(&vect_from_home);
  dist_to_home += BUFFER_ZONE_DIST;
  if (dist_to_home > MAX_DIST_FROM_HOME) {
    VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
  }
  // set new point
  VECT2_SUM(*point, home, vect_from_home);
  point->z = tmp_enu_point.z;

  return TRUE;
}
예제 #2
0
파일: state.c 프로젝트: Merafour/paparazzi
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_pos_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_pos_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);
}