path: root/mbglib/common/gpsutils.c

/**************************************************************************
 *
 *  $Id: gpsutils.c 1.9.1.7 2016/08/11 12:45:54 martin TEST $
 *
 *  Copyright (c) Meinberg Funkuhren, Bad Pyrmont, Germany
 *
 *  Description:
 *    Utility functions useful with GPS data.
 *
 * -----------------------------------------------------------------------
 *  $Log: gpsutils.c $
 *  Revision 1.9.1.7  2016/08/11 12:45:54  martin
 *  Revision 1.9.1.6  2016/08/09 15:56:50Z  martin
 *  Fixed format bug.
 *  Revision 1.9.1.5  2016/07/22 09:57:11  martin
 *  Quieted some compiler warninges.
 *  Revision 1.9.1.4  2015/09/07 09:19:58  martin
 *  Revision 1.9.1.3  2015/08/27 16:24:41  martin
 *  Revision 1.9.1.2  2015/08/25 15:34:48  martin
 *  Use save string functions from str_util.c.
 *  Revision 1.9.1.1  2015/08/21 14:21:57  martin
 *  Revision 1.9  2013/01/30 16:10:08  martin
 *  Exclude some code from compiling by default, and
 *  thus don't require pcpslstr.h by default.
 *  Revision 1.8  2012/10/15 14:27:05Z  martin
 *  Exclude sprint_fixed_freq() from build except for Borland C / Windows.
 *  Revision 1.7  2010/07/15 09:32:09  martin
 *  Use DEG character definition from pcpslstr.h.
 *  Revision 1.6  2004/12/28 11:21:26Z  martin
 *  Omit trap if fixed_freq is 0.
 *  Use C99 fixed-size data types were required.
 *  Revision 1.5  2003/02/04 09:20:04Z  MARTIN
 *  New functions sprint_alt(), sprint_fixed_freq().
 *  Revision 1.4  2003/01/31 13:45:19  MARTIN
 *  sprint_pos_geo() returns N/A if position not valid.
 *  Revision 1.3  2002/12/12 16:07:04  martin
 *  New functions swap_pos_doubles(), sprint_dms(),
 *  and sprint_pos_geo().
 *  Revision 1.2  2001/02/05 09:39:12Z  MARTIN
 *  New file header.
 *  Change include file name to lower case.
 *  Source code cleanup.
 *
 **************************************************************************/

#define _GPSUTILS
 #include <gpsutils.h>
#undef _GPSUTILS

#if defined( USE_SPRINTF )
  #error USE_SPRINTF was obsoleted by USE_SNPRINTF. Please update project settings.
#endif

#if !defined( USE_SNPRINTF )
  #define USE_SNPRINTF 0
#endif

#if !defined( _USE_GPSUTILS_FULL )
  #if defined( MBG_TGT_WIN32 ) && defined( __BORLANDC__ )
    #define _USE_GPSUTILS_FULL  1
  #else
    #define _USE_GPSUTILS_FULL  0
  #endif
#endif

#if USE_SNPRINTF
  #include <str_util.h>
//  #include <pcpslstr.h>
#define DEG "deg"
#endif

#if  _USE_GPSUTILS_FULL
  #include <str_util.h>
  #include <math.h>
#endif

#include <stdio.h>
#include <string.h>


#if USE_SNPRINTF || _USE_GPSUTILS_FULL

static const char str_na[] = "N/A";

#endif



/*HDR*/
/**
 * @brief Swap the bytes of a single variable of type "double"
 *
 * The memory layout of a "double" on Meinberg bus level devices
 * and computers usually differs. This function can be used to
 * fix this and is usually called from inside API functions,
 * if required.
 *
 * @param[in,out] p   Pointer to a "double" to be swapped
 *
 * @see ::swap_eph_doubles
 * @see ::swap_alm_doubles
 * @see ::swap_utc_doubles
 * @see ::swap_iono_doubles
 * @see ::swap_pos_doubles
 */
void swap_double( double *p )
{
  uint16_t *wp1;
  uint16_t *wp2;
  uint16_t w;
  int i;

  wp1 = (uint16_t *) p;
  wp2 = ( (uint16_t *) p ) + 3;

  for ( i = 0; i < 2; i++ )
  {
    w = *wp1;
    *wp1 = *wp2;
    *wp2 = w;
    wp1++;
    wp2--;
  }

}  // swap_double



/*HDR*/
/**
 * @brief Swap the "double" fields in an ::EPH structure
 *
 * See comments for ::swap_double
 *
 * @param[in,out] p  Pointer to an ::EPH structure to be converted
 *
 * @see ::swap_double
 * @see ::swap_alm_doubles
 * @see ::swap_utc_doubles
 * @see ::swap_iono_doubles
 * @see ::swap_pos_doubles
 */
void swap_eph_doubles( EPH *p )
{
  swap_double( &p->sqrt_A );
  swap_double( &p->e );
  swap_double( &p->M0 );
  swap_double( &p->omega );
  swap_double( &p->i0 );
  swap_double( &p->OMEGA0 );
  swap_double( &p->OMEGADOT );

  swap_double( &p->deltan );
  swap_double( &p->idot );

  swap_double( &p->crc );
  swap_double( &p->crs );
  swap_double( &p->cuc );
  swap_double( &p->cus );
  swap_double( &p->cic );
  swap_double( &p->cis );

  swap_double( &p->af0 );
  swap_double( &p->af1 );
  swap_double( &p->af2 );

  swap_double( &p->tgd );

}  /* swap_eph_doubles */



/*HDR*/
/**
 * @brief Swap the "double" fields in an ::ALM structure
 *
 * See comments for ::swap_double
 *
 * @param[in,out] p  Pointer to an ::ALM structure to be converted
 *
 * @see ::swap_double
 * @see ::swap_eph_doubles
 * @see ::swap_utc_doubles
 * @see ::swap_iono_doubles
 * @see ::swap_pos_doubles
 */
void swap_alm_doubles( ALM *p )
{
  swap_double( &p->sqrt_A );
  swap_double( &p->e );
  swap_double( &p->deltai );
  swap_double( &p->OMEGA0 );
  swap_double( &p->OMEGADOT );
  swap_double( &p->omega );
  swap_double( &p->M0 );
  swap_double( &p->af0 );
  swap_double( &p->af1 );

}  /* swap_alm_doubles */



/*HDR*/
/**
 * @brief Swap the "double" fields in a ::UTC structure
 *
 * See comments for ::swap_double
 *
 * @param[in,out] p  Pointer to a ::UTC structure to be converted
 *
 * @see ::swap_double
 * @see ::swap_eph_doubles
 * @see ::swap_alm_doubles
 * @see ::swap_iono_doubles
 * @see ::swap_pos_doubles
 */
void swap_utc_doubles( UTC *p )
{
  swap_double( &p->A0 );
  swap_double( &p->A1 );

}  /* swap_utc_doubles */



/*HDR*/
/**
 * @brief Swap the "double" fields in a ::IONO structure
 *
 * See comments for ::swap_double
 *
 * @param[in,out] p  Pointer to a ::IONO structure to be converted
 *
 * @see ::swap_double
 * @see ::swap_eph_doubles
 * @see ::swap_alm_doubles
 * @see ::swap_utc_doubles
 * @see ::swap_pos_doubles
 */
void swap_iono_doubles( IONO *p )
{
  swap_double( &p->alpha_0 );
  swap_double( &p->alpha_1 );
  swap_double( &p->alpha_2 );
  swap_double( &p->alpha_3 );

  swap_double( &p->beta_0 );
  swap_double( &p->beta_1 );
  swap_double( &p->beta_2 );
  swap_double( &p->beta_3 );

}  /* swap_iono_doubles */



/*HDR*/
/**
 * @brief Swap the "double" fields in a ::POS structure
 *
 * See comments for ::swap_double
 *
 * @param[in,out] p  Pointer to a ::POS structure to be converted
 *
 * @see ::swap_double
 * @see ::swap_eph_doubles
 * @see ::swap_alm_doubles
 * @see ::swap_utc_doubles
 * @see ::swap_iono_doubles
 */
void swap_pos_doubles( POS *p )
{
  int i;

  for ( i = 0; i < N_XYZ; i++ )
    swap_double( &p->xyz[i] );

  for ( i = 0; i < N_LLA; i++ )
    swap_double( &p->lla[i] );

  swap_double( &p->longitude.sec );
  swap_double( &p->latitude.sec );

}  /* swap_pos_doubles */



#if USE_SNPRINTF

/*HDR*/
/**
 * @brief Print the ::DMS part of a geo position into a string buffer
 *
 * @param[out] s        The string buffer to be filled
 * @param[in]  max_len  Size of the output buffer for 0-terminated string
 * @param[in]  p        Pointer to a ::DMS structure to be printed
 * @param[in]  prec     Precision, i.e. number of fractions of the seconds
 *
 * @return Length of the string in the buffer
 *
 * @see snprint_dms
 * @see snprint_alt
 * @see snprint_pos_geo
 * @see snprint_fixed_freq
 */
size_t snprint_dms( char *s, size_t max_len, const DMS *p, int prec )
{
  size_t n = snprintf_safe( s, max_len, "%c %i" DEG "%02i'%02.*f\"",
                            p->prefix, p->deg, p->min,
                            prec, p->sec );

  return n;

}  // snprint_dms



/*HDR*/
/**
 * @brief Print the altitude part of a geo position into a string buffer
 *
 * @param[out] s        The string buffer to be filled
 * @param[in]  max_len  Size of the output buffer for 0-terminated string
 * @param[in]  alt      The altitude value to be printed, in [m]
 *
 * @return Length of the string in the buffer
 *
 * @see snprint_dms
 * @see snprint_pos_geo
 * @see snprint_fixed_freq
 */
size_t snprint_alt( char *s, size_t max_len, double alt )
{
  size_t n = snprintf_safe( s, max_len, "%.0fm", alt );

  return n;

}  // snprint_alt



/*HDR*/
/**
 * @brief Print a geo position in ::POS format into a string buffer
 *
 * @param[out] s        The string buffer to be filled
 * @param[in]  max_len  Size of the output buffer for 0-terminated string
 * @param[in]  p        Pointer to a ::POS structure to be printed
 * @param[in]  sep      Separator character for the ::DMS part
 * @param[in]  prec     Precision, i.e. number of fractions of the seconds of the ::DMS part
 *
 * @return Length of the string in the buffer
 *
 * @see snprint_dms
 * @see snprint_alt
 * @see snprint_fixed_freq
 */
size_t snprint_pos_geo( char *s, size_t max_len, const POS *p, char sep, int prec )
{
  size_t n = 0;

  if ( p->lla[LON] && p->lla[LAT] && p->lla[ALT] )
  {
    n += snprint_dms( &s[n], max_len - n, &p->latitude, prec );
    n += snprintf_safe( &s[n], max_len - n, "%c", sep );
    n += snprint_dms( &s[n], max_len - n, &p->longitude, prec );
    n += snprintf_safe( &s[n], max_len - n, "%c", sep );
    n += snprint_alt( &s[n], max_len - n, p->lla[ALT] );
  }
  else
    n = sn_cpy_str_safe( s, max_len, str_na );

  return n;

}  // sprint_pos_geo

#endif  // USE_SNPRINTF



#if _USE_GPSUTILS_FULL

/*HDR*/
/**
 * @brief Print a formatted ::FIXED_FREQ_INFO into a string buffer
 *
 * @param[out] s        The string buffer to be filled
 * @param[in]  max_len  Size of the output buffer for 0-terminated string
 * @param[in]  p_ff     Pointer to a ::FIXED_FREQ_INFO structure to be printed
 *
 * @return Length of the string in the buffer
 *
 * @see snprint_dms
 * @see snprint_alt
 * @see snprint_pos_geo
 */
size_t snprint_fixed_freq( char *s, size_t max_len, FIXED_FREQ_INFO *p_ff )
{
  double freq;
  int range;
  ushort unit;
  ushort format;
  size_t n = 0;

  // Before re-calculating frequency, range is the base 10 exponent
  // to the frequency value which is represented in kHz.
  // After calculating range from real frequency, range is represented
  // as follows:
  //    range  display  format  divisor    format index calculation
  //     -3    100mHz   1.000  [/1e-3]   -3 % 3 = -3 + 3 = 0 % 3 = 0
  //     -2    100mHz   10.00  [/1e-3]   -2 % 3 = -2 + 3 = 1 % 3 = 1
  //     -1    100mHz   100.0  [/1e-3]   -1 % 3 = -1 + 3 = 2 % 3 = 2
  //      0    1Hz      1.000  [/1e0]     0 % 3 =  0 + 3 = 3 % 3 = 0
  //      1    10Hz     10.00  [/1e0]     1 % 3 =  1 + 3 = 1 % 3 = 1
  //      2    100Hz    100.0  [/1e0]     2 % 3 =  2 + 3 = 2 % 3 = 2
  //      3    1kHz     1.000  [/1e3]     3 % 3 =  0 + 3 = 3 % 3 = 0
  //      4    10kHz    10.00  [/1e3]     4 % 3 =  1 + 3 = 4 % 3 = 1
  //      5    100kHz   100.0  [/1e3]     5 % 3 =  2 + 3 = 5 % 3 = 2
  //      6    1MHz     1.000  [/1e6]     6 % 3 =  0 + 3 = 3 % 3 = 0
  //      7    10MHz    10.00  [/1e6]     7 % 3 =  1 + 3 = 4 % 3 = 1
  //      8    100MHz   100.0  [/1e6]     8 % 3 =  2 + 3 = 5 % 3 = 2

  // format string for fp output
  static const char *fmt_str[] =
  {
    "%4.3lf%s",
    "%4.2lf%s",
    "%4.1lf%s",
  };

  // Unit index and divisor are calculated as follows:
  // range  unit        index calculation            divisor calculation
  // -3      mHz   ( int )( ( -3 + 3 ) / 3 ) =  0
  // -2      mHz   ( int )( ( -2 + 3 ) / 3 ) =  0
  // -1      mHz   ( int )( ( -1 + 3 ) / 3 ) =  0    / 10e-3 = 10e( 3 * 0 - 3 )
  //  0      Hz    ( int )( (  0 + 3 ) / 3 ) =  1
  //  1      Hz    ( int )( (  1 + 3 ) / 3 ) =  1
  //  2      Hz    ( int )( (  2 + 3 ) / 3 ) =  1    / 1e0   = 10e( 3 * 1 - 3 )
  //  3      kHz   ( int )( (  3 + 3 ) / 3 ) =  2
  //  4      kHz   ( int )( (  4 + 3 ) / 3 ) =  2
  //  5      kHz   ( int )( (  5 + 3 ) / 3 ) =  2    / 10e3  = 10e( 3 * 2 - 3 )
  //  6      MHz   ( int )( (  6 + 3 ) / 3 ) =  3
  //  7      MHz   ( int )( (  7 + 3 ) / 3 ) =  3
  //  8      MHz   ( int )( (  8 + 3 ) / 3 ) =  3    / 10e6   =10e( 3 * 3 - 3 )

  // unit string
  static const char *unit_str[] =
  {
    "mHz",
    "Hz",
    "kHz",
    "MHz"
  };


  if ( p_ff->khz_val )
  {
    // calculate frequency in Hz
    freq = ( double ) p_ff->khz_val * pow( 10, ( p_ff->range + 3 ) );

    // calculate decimal exponent
    range = ( ( int ) log10( freq ) );

    // check whether range is in the allowed range
    // if so display frequency in broken format
    if ( ( range >= -3 ) && ( range <= 8 ) )
    {
      // calculate format index ( see above )
      format = ( ( ( range % 3 ) + 3 ) % 3 );

      // calculate unit index
      unit =  ( ushort )( range + 3 ) / 3;

      // calculate display value
      freq = freq / pow( 10, ( ( 3 * unit ) - 3 ) );
      n = snprintf_safe( s, max_len, fmt_str[format], freq, unit_str[unit] );
    }
    else
    {
      // out of range display fequency in Hz
      n = snprintf_safe( s, max_len, "%fHz", freq );
    }
  }
  else
    n = sn_cpy_str_safe( s, max_len, str_na );

  return n;

}  // snprint_fixed_freq

#endif  // _USE_GPSUTILS_FULL