예제 #1
0
void ahrs_update_gps(void) {

  if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) {
    ins_gps_fix_once = TRUE;

#if INS_UPDATE_FW_ESTIMATOR
    if (state.utm_initialized_f) {
      // position (local ned)
      ins_impl.meas.pos_gps.x = (gps.utm_pos.north / 100.0f) - state.utm_origin_f.north;
      ins_impl.meas.pos_gps.y = (gps.utm_pos.east / 100.0f) - state.utm_origin_f.east;
      ins_impl.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.0f);
      // speed
      ins_impl.meas.speed_gps.x = gps.ned_vel.x / 100.0f;
      ins_impl.meas.speed_gps.y = gps.ned_vel.y / 100.0f;
      ins_impl.meas.speed_gps.z = gps.ned_vel.z / 100.0f;
    }
#else
    if (state.ned_initialized_f) {
      struct EcefCoor_f ecef_pos, ecef_vel;
      ECEF_FLOAT_OF_BFP(ecef_pos, gps.ecef_pos);
      ned_of_ecef_point_f(&ins_impl.meas.pos_gps, &state.ned_origin_f, &ecef_pos);
      ECEF_FLOAT_OF_BFP(ecef_vel, gps.ecef_vel);
      ned_of_ecef_vect_f(&ins_impl.meas.speed_gps, &state.ned_origin_f, &ecef_vel);
    }
#endif
  }

}
예제 #2
0
void ned_of_lla_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct LlaCoor_f* lla) {
  struct EcefCoor_f ecef;
  ecef_of_lla_f(&ecef,lla);
  ned_of_ecef_point_f(ned,def,&ecef);
}
예제 #3
0
파일: state.c 프로젝트: Merafour/paparazzi
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);
}