示例#1
0
/** Convert a LLA to UTM.
 * @param[out] out  UTM in cm and mm hmsl alt
 * @param[in]  in   LLA in degrees*1e7 and mm above ellipsoid
 */
void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
{
#if USE_SINGLE_PRECISION_LLA_UTM
  /* convert our input to floating point */
  struct LlaCoor_f lla_f;
  LLA_FLOAT_OF_BFP(lla_f, *lla);
  /* calls the floating point transformation */
  struct UtmCoor_f utm_f;
  utm_f.zone = utm->zone;
  utm_of_lla_f(&utm_f, &lla_f);
  /* convert the output to fixed point       */
  UTM_BFP_OF_REAL(*utm, utm_f);
#else // use double precision by default
  /* convert our input to floating point */
  struct LlaCoor_d lla_d;
  LLA_DOUBLE_OF_BFP(lla_d, *lla);
  /* calls the floating point transformation */
  struct UtmCoor_d utm_d;
  utm_d.zone = utm->zone;
  utm_of_lla_d(&utm_d, &lla_d);
  /* convert the output to fixed point       */
  UTM_BFP_OF_REAL(*utm, utm_d);
#endif
}
示例#2
0
文件: ins.c 项目: coppolam/paparazzi
void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
{
  struct LlaCoor_f lla0;
  lla_of_utm_f(&lla0, utm);
  if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
    utm->zone = gps.utm_pos.zone;
  }
  else {
    utm->zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
  }
  utm_of_lla_f(utm, &lla0);

  struct UtmCoor_i utm_i;
  UTM_BFP_OF_REAL(utm_i, *utm);
  stateSetLocalUtmOrigin_i(&utm_i);
}