ogr_navsolve2.c

The OpenGPSRec receiver software runs on the real time operating system RTAI-Linux. It compiles with

 *
 * IN :   satellite ECEF XYZ
 * IN :   user ECEF XYZ
 * OUT: azimuth [rad]
 * OUT: elevation [rad]
 * OUT: calculation succeeded: stat = TRUE
 */
static void calcAzEl(double *Xs, double *Xu, 
  double *Az, double *El, int *stat)
{
  long i, k;
  double R, p, x, y, z, s;
  double e[3][3];
  double d[3];

  x = Xu[0];
  y = Xu[1];
  z = Xu[2];

  p = sqrt( x * x + y * y);

  if ( p == 0.0)
    *stat = FALSE;
  else
    *stat = TRUE;

  if ( !*stat)
    return;

  R = sqrt(x * x + y * y + z * z);

  e[0][0] = -(y / p);
  e[0][1] = x / p;
  e[0][2] = 0.0;
  e[1][0] = -(x * z / (p * R));
  e[1][1] = -(y * z / (p * R));
  e[1][2] = p / R;
  e[2][0] = x / R;
  e[2][1] = y / R;
  e[2][2] = z / R;

/* matrix multiply vector from user */
  for (k = 0; k <= 2; k++) 
  {
    d[k] = 0.0;
    for (i = 0; i <= 2; i++)
      d[k] += (Xs[i] - Xu[i]) * e[k][i];
    
  }

  s = d[2] / sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
  if (s == 1.0)
    *El = 0.5 * M_PI;
  else
    *El = atan(s / sqrt( 1.0 - s * s));

  if ( d[1] == 0.0 && d[0] > 0.0) 
    *Az = 0.5 * M_PI;
  else if ( d[1] == 0.0 && d[0] < 0.0) 
    *Az = 1.5 * M_PI;
  else
  {
    *Az = atan(d[0] / d[1]);
    if (d[1] < 0.0)
      *Az += M_PI;
    else if (d[1] > 0.0 && d[0] < 0.0)
      *Az += 2.0 * M_PI;
  }
  return;
}
#endif


/*
 * ion     iono correction coefficients from nav message
 *
 * Latu    user's latitude [rad]
 * Lonu    user's longitude [rad]
 * Az      SV azimuth [rad]
 * El    SV elevation [rad]
 * Ttr    SV signal transmission time [sec]
 * dTiono  Ionospheric delay [sec]
 */
static double ionocorr( IONO iono, double Latu, double Lonu, double Az, 
                        double El, double Ttr)
{
  double phi, 
         Lati, Loni, Latm, 
         T, F_, x, per, amp, 
         a0, a1, a2, a3, 
         b0, b1, b2, b3,
         res;

/* for clarity copy array */
  a0 = Iono.al0;
  a1 = Iono.al1;
  a2 = Iono.al2;
  a3 = Iono.al3;
  b0 = Iono.b0;
  b1 = Iono.b1;
  b2 = Iono.b2;
  b3 = Iono.b3;

/* convert from radians to semicircles */
  Latu /= M_PI;
  Lonu /= M_PI;
  Az   /= M_PI;
  El   /= M_PI;

/* calculation */
  phi  = 0.0137 / (El + 0.11) - 0.022;
  Lati = Latu + phi * cos( Az * M_PI);

  if ( Lati > 0.416)
    Lati = 0.416;
  else if (Lati < -0.416)
    Lati = -0.416;

  Loni = Lonu + phi * sin( Az * M_PI) / cos( Lati * M_PI);
  Latm = Lati + 0.064 * cos( (Loni - 1.617) * M_PI);
  T = 4.32e+4 * Loni + Ttr;

  while ( T >= 86400.0)
    T -= 86400.0;
  while ( T < 0.0)
    T += 86400.0;

  F_ = 1.0 + 16.0 * (0.53 - El) * (0.53 - El) * (0.53 - El);
  per = b0 + b1 * Latm + b2 * Latm * Latm + b3 * Latm * Latm * Latm;

  if ( per < 72000.0)
    per = 72000.0;

  x = 2 * M_PI * (T - 50400.0) / per;
  amp = a0 + 
        a1 * Latm + 
        a2 * Latm * Latm + 
        a3 * Latm * Latm * Latm;

  if ( amp < 0.0)
    amp = 0.0;

  if ( fabs( x) >= 1.57)
    res = F_ * 5.0e-9;
  else
    res = F_ * (5.0e-9 + amp * 
      (1.0 - x * x / 2.0 + x * x * x * x / 24.0));

  return (res);
}

#if 0
/*
 * ion     iono correction coefficients from nav message
 *
 * Latu    user's latitude [rad]
 * Lonu    user's longitude [rad]
 * Az      SV azimuth [rad]
 * El    SV elevation [rad]
 * Ttr    SV signal transmission time [sec]
 * dTiono  Ionospheric delay [sec]
 */
static void ionocorr_old( double *ion, double Latu, double Lonu, 
  double Az, double El, double Ttr, double *dTiono)
{
  double phi, 
         Lati, Loni, Latm, 
         T, F_, x, per, amp, 
         a0, a1, a2, a3, 
         b0, b1, b2, b3;

/* for clarity copy array */
  a0 = ion[0];
  a1 = ion[1];
  a2 = ion[2];
  a3 = ion[3];
  b0 = ion[4];
  b1 = ion[5];
  b2 = ion[6];
  b3 = ion[7];

//  printf("iono (a) = %.6e %.6e %.6e %.6e\n", a0, a1, a2, a3);
//  printf("iono (b) = %.6e %.6e %.6e %.6e\n", b0, b1, b2, b3);

/* convert from radians to semicircles */
  Latu /= M_PI;
  Lonu /= M_PI;
  Az   /= M_PI;
  El   /= M_PI;

/* calculation */
  phi  = 0.0137 / (El + 0.11) - 0.022;
  Lati = Latu + phi * cos( Az * M_PI);

  if (Lati > 0.416)
    Lati = 0.416;
  else if (Lati < -0.416)
    Lati = -0.416;

  Loni = Lonu + phi * sin( Az * M_PI) / cos( Lati * M_PI);
  Latm = Lati + 0.064 * cos( (Loni - 1.617) * M_PI);
  T = 4.32e+4 * Loni + Ttr;

  while ( T >= 86400.0)
    T -= 86400.0;
  while ( T < 0.0)
    T += 86400.0;

  F_ = 1.0 + 16.0 * (0.53 - El) * (0.53 - El) * (0.53 - El);
  per = b0 + b1 * Latm + b2 * Latm * Latm + b3 * Latm * Latm * Latm;

  if ( per < 72000.0)
    per = 72000.0;

  x = 2 * M_PI * (T - 50400.0) / per;
  amp = a0 + 
        a1 * Latm + 
        a2 * Latm * Latm + 
        a3 * Latm * Latm * Latm;

  if ( amp < 0.0)
    amp = 0.0;

  if ( fabs( x) >= 1.57)
    *dTiono = F_ * 5.0e-9;
  else
    *dTiono = F_ * (5.0e-9 + amp * 
      (1.0 - x * x / 2.0 + x * x * x * x / 24.0));

  return;
}
#endif

#if 0
/*  the following data should be available:
 *
 *   1. Pseudorange with receiver time of reception for each SV
 *   2. Ephemeris and almanac for each SV
 *   3. Iono coefficients
 */

static void stdalone_input_phase( double *Xlla, double *Trc, int *SV, 
  double eph[32][16], double clk[32][5], double ion[], double *Praw, 
  INT16 tr_ch[], INT16 n_tr, double transmit_time[])
{
  int i, prn;
 
/* GPS time of reception (sampled at TIC) */

/* *** CHECKME: n_tr > 0, 
 *              NavData.tow_tic_count[tr_ch[i]] the same 
 *              for i=0,...,n_tr-1
 */

//  for (i=0;i<n_tr;i++)
//    printf( "tr_ch = %d, tow = %.9e  ", tr_ch[i], NavData.tow_tic_count[tr_ch[i]] / 10.0);
//  printf( "\n");
//  printf( "tow (param) = %.9e\n", Ctrl->tow_tic_count / 10.0);

/*
 * NavData.tow_tic_count[tr_ch[0]],...,NavData.tow_tic_count[tr_ch[n_tr-1]]
 * contain the same value! Just take the first....
 */
  *Trc = NavData.tow_tic_count[tr_ch[0]] / 10.0;

//  printf( "Trc %.9e\n", *Trc);

/* iono coefficients */
  ion[0] = Iono.al0;
  ion[1] = Iono.al1;
  ion[2] = Iono.al2;
  ion[3] = Iono.al3;
  ion[4] = Iono.b0;
  ion[5] = Iono.b1;
  ion[6] = Iono.b2;
  ion[7] = Iono.b3;

/* read pseudoranges */
  for (prn = 1; prn <= 32; prn++)
  {
    SV[prn-1]   = FALSE;
    Praw[prn-1] = 0.0;
  }  

  for (i = 0; i < n_tr; i++)
  {
    prn = Chan[tr_ch[i]].prn;

    SV[prn-1]   = TRUE;
//    Praw[prn-1] = (transmit_time[i] - *Trc) * LIGHTVEL;
    Praw[prn-1] = (*Trc - transmit_time[i]) * LIGHTVEL;

//    printf( " SV[%d] = %d\n", prn, SV[prn-1]);

/* ephemeris- and clock data */
#if 0
  Eph(1)  = EphData(Idx).Crs ;            % ampl. of sine harm. corr. to orbit radius
  Eph(2)  = EphData(Idx).DeltaN / pi;     % mean motion difference
  Eph(3)  = EphData(Idx).M0 / pi;         % mean anomaly at ref. time
  Eph(4)  = EphData(Idx).Cuc ;            % ampl. of cosine harm. corr. to arg. of  lat.
  Eph(5)  = EphData(Idx).Eccentricity;    % eccentricity
  Eph(6)  = EphData(Idx).Cus;             % ampl. of sine harm. corr. to arg. of  lat.
  Eph(7)  = EphData(Idx).SqrtA;           % square root of semi-major axis
  Eph(8)  = EphData(Idx).TimeOfEphemeris; % time of ephemeris           
  Eph(9)  = EphData(Idx).Cic;             % ampl. of cosine harm. corr. to incl. angle
  Eph(10) = EphData(Idx).OMEGA / pi;      % longitde of asc. node of orb. plane
  Eph(11) = EphData(Idx).Cis;             % ampl. of sine harm. corr. to incl. angle
  Eph(12) = EphData(Idx).InclinationRad / pi;  % incl. angle at ref. time
  Eph(13) = EphData(Idx).Crc;             % ampl. of cosine harm. corr. to orbit radius
  Eph(14) = EphData(Idx).omega / pi;      % argument of perigee
  Eph(15) = EphData(Idx).OMEGADOT / pi;   % rate of right ascension
  Eph(16) = EphData(Idx).IDOT / pi;       % rate of incl. angle

  INT16  iode,             // issue of date (ephemeris)
         iodc,             // issue of date (clock)
         ura,
         valid,
         health,
         week;
  double dn,               // mean motion - correction
         sqra,             // sqrt of semi-major axis
         W0,               // longitude of asc node
         Wdot,             // date of right ascension
         w,                // argument of perigee
         tgd,              // group dela differential
         toe,              // reference time ephemeris
         toc,              // sat clock data reference time [sec]
         inc0,             // inclination correction
         idot,
         cuc, cus, crc, crs, cic, cis, // 2nrd harmonic perturbation
         ma,               // mean anomaly
         n0,               // computed mean motion
         ety,              // eccentricity
         af0, af1, af2;    // clock correction
#endif

    eph[prn-1][0]  = GPS_Eph[prn].crs;
    eph[prn-1][1]  = GPS_Eph[prn].dn / PI;
    eph[prn-1][2]  = GPS_Eph[prn].ma / PI;
    eph[prn-1][3]  = GPS_Eph[prn].cuc;
    eph[prn-1][4]  = GPS_Eph[prn].ety;
    eph[prn-1][5]  = GPS_Eph[prn].cus;
    eph[prn-1][6]  = GPS_Eph[prn].sqra;
    eph[prn-1][7]  = GPS_Eph[prn].toe;
    eph[prn-1][8]  = GPS_Eph[prn].cic;
    eph[prn-1][9]  = GPS_Eph[prn].W0 / PI;
    eph[prn-1][10] = GPS_Eph[prn].cis;
    eph[prn-1][11] = GPS_Eph[prn].inc0 / PI;
    eph[prn-1][12] = GPS_Eph[prn].crc;
    eph[prn-1][13] = GPS_Eph[prn].w / PI;
    eph[prn-1][14] = GPS_Eph[prn].Wdot / PI;
    eph[prn-1][15] = GPS_Eph[prn].idot / PI;

//    printf( "prn = %d, sqra = %e\n", prn, eph[prn-1][6]);

#if 0
      Clk(prn,1)  = EphData(Idx).TGD;             % group delay differential (T_GD)
      Clk(prn,2)  = EphData(Idx).TimeOfClock;     % clock data ref time (t_oc)     
      Clk(prn,3)  = EphData(Idx).ClockDriftRate;  % drift rate
      Clk(prn,4)  = EphData(Idx).ClockDrift;      % drift
      Clk(prn,5)  = EphData(Idx).ClockBias;       % bias
#endif

    clk[prn-1][0]  = GPS_Eph[prn].tgd;
    clk[prn-1][1]  = GPS_Eph[prn].toc;
    clk[prn-1][2]  = GPS_Eph[prn].af2;
    clk[prn-1][3]  = GPS_Eph[prn].af1;
    clk[prn-1][4]  = GPS_Eph[prn].af0;

  }  // --- for (n = 1; n <= n_tr; n++) ---

/* user input of start position */

/* Start position Lat [deg.dec], Lon [deg.dec], Alt [m] */
  Xlla[0] = Rcvr_Info.llh.lat;
  Xlla[1] = Rcvr_Info.llh.lon;
  Xlla[2] = Rcvr_Info.llh.hae;   // height above ellipsoid

  return;
}
#endif


/* 
 * IN:   ephemeris  eph   
 *       satellite GPS time  Ttr
 * OUT:  relativistic correction term  Trel
 *       satellite position  X  
 */
ECEF satellite_position( INT16 prn, double Ttr, double *Trel)
{
  double     ec, A, T, n0, n, M, E, Eold, 
             snu, cnu, nu, phi, du, dr, di, u, r, i,
             Xdash, Ydash, Wc;
  ECEF       res;
  EPHEMERIS *eph;

//  select ephemeris data
  eph = &GPS_Eph[prn];

  A    = eph->sqra * eph->sqra;

#if 0
  Crs  = eph->crs;
  dn   = eph->dn;
  M0   = eph->ma;
  Cuc  = eph->cuc;
  ec   = eph->ety;
  Cus  = eph->cus;
  Toe  = eph->toe;
  Cic  = eph->cic;
  W0   = eph->W0;
  Cis  = eph->cis;
  i0   = eph->inc0;
  Crc  = eph->crc;
  w    = eph->w;
  Wdot = eph->Wdot;
  Idot = eph->idot;
#endif

#if 0
  if ( first == 0)
  {
    first = 1;  
    printf(" dn=%e M0=%e A=%e Toe=%e W0=%e i0%e w=%e Wdot=%e Idot=%e\n", 
      dn, M0, A, Toe, W0, i0, w, Wdot, Idot);
  }
#endif

  T = Ttr - eph->toe;
  if ( T > 302400.0)
    T -= 604800.0;
  else if (T < -302400.0)
    T += 604800.0;

//  printf(" T=%.12e Toe=%.12e\n", T, Toe);

  n0 = sqrt( GRAVCONST / (A * A * A));
  n = n0 + eph->dn;

  M = eph->ma + n * T;  
  E = M;   /* start value for E */

//  printf(" dn=%.9e Mo=%.9e ec=%.9e A=%.9e Toe=%.9e W0=%.9e i0=%.9e w=%.9e Wdot=%.9e idot=%.9e\n", 
//    dn, M0, ec, A, Toe, W0, i0, w, Wdot, Idot);

  ec = eph->ety;

#if 0
  if ( prn == SelPRN)
  {
    printf(" dn=%.8e M0=%.8e ec=%.8e A=%.8e Toe=%.8e W0=%.8e i0%.8e w=%.8e Wdot=%.8e Idot=%.8e\n", 
      eph->dn, eph->ma, ec, A, eph->toe, eph->W0, eph->inc0, eph->w, eph->Wdot, eph->idot);
  }
#endif

  do 
  {
    Eold = E;
    E = M + ec * sin( E);
  } while ( fabs( E - Eold) >= 1.0e-8);

//  printf(" M0=%.12e n=%.12e T=%.12e Ttr=%.12e Toe=%.12e E=%.12e\n", M0, n, T, Ttr, Toe, E);
//  printf(" E=%.9e M=%.9e ec=%.9e\n", E, M, ec);

  snu = sqrt(1 - ec * ec) * sin( E) / (1 - ec * cos( E));
  cnu = (cos( E) - ec) / (1 - ec * cos( E));

  nu = atan2( snu, cnu);

  phi = nu + eph->w;

  du = eph->cuc * cos( 2 * phi) + eph->cus * sin( 2 * phi);
  dr = eph->crc * cos( 2 * phi) + eph->crs * sin( 2 * phi);
  di = eph->cic * cos( 2 * phi) + eph->cis * sin( 2 * phi);

  u = phi + du;
  r = A * (1 - ec * cos( E)) + dr;
  i = eph->inc0 + eph->idot * T + di;

  Xdash = r * cos( u);
  Ydash = r * sin( u);

  Wc = eph->W0 + (eph->Wdot - OMEGAE) * T - OMEGAE * eph->toe;

  res.x = Xdash * cos( Wc) - Ydash * cos( i) * sin( Wc);
  res.y = Xdash * sin( Wc) + Ydash * cos( i) * cos( Wc);
  res.z = Ydash * sin( i);

#if 0
  if ( prn == SelPRN)
  {
    printf(" du=%.8e dr=%.8e di=%.8e Xd=%.8e Yd=%.8e Wc=%.8e X=[%.8e %.8e %.8e]\n", 
      du, dr, di, Xdash, Ydash, Wc, res.x, res.y, res.z);
  }
#endif

  *Trel = FCONST * ec * eph->sqra * sin( E);   /* relativistic correction term */

  return (res);
}

#if 0