ogr_navsolve.c

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

  if ( GPS_Alm[n].ety != 0.0 )
    ta = atan2(sqrt(1.-pow( GPS_Alm[n].ety, 2.0))*sin(ea),cos(ea)-GPS_Alm[n].ety);
  else
    ta = ea;

//
//      TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
//
  r = GPS_Alm[n].sqra * GPS_Alm[n].sqra * 
      (1.0 - GPS_Alm[n].ety * GPS_Alm[n].ety * cos(ea));

//
//    R IS THE RADIUS OF SATELLITE ORBIT AT TIME T
//
  aol=ta+GPS_Alm[n].aop;

//
//    AOL IS THE ARGUMENT OF LATITUDE
//    LA IS THE LONGITUDE OF THE ASCENDING NODE
//
  la = GPS_Alm[n].lan + 
       (GPS_Alm[n].rra - OMEGAE) * d_toa - 
       GPS_Alm[n].toa * OMEGAE;
  xp = r * cos( aol);
  yp = r * sin( aol);

  res.x = xp * cos( la) - yp * cos( GPS_Alm[n].inc) * sin( la);
  res.y = xp * sin( la) + yp * cos( GPS_Alm[n].inc) * cos( la);
  res.z = yp * sin( GPS_Alm[n].inc);

  return (res);
}

#ifdef USE_OPENSRCGPS_NAVFIX

/*******************************************************************************
FUNCTION satpos_ephemeris()
RETURNS  satellite position in ECEF coordinates

PARAMETERS
  t : GPS system time at time of transmission
  n : PRN

PURPOSE
  This subroutine calculates the satellite position
  based on broadcast ephemeris data

     R     - RADIUS OF SATELLITE AT TIME T
     Crc   - RADIUS COSINE CORRECTION TERM
     Crs   - RADIUS SINE   CORRECTION TERM
     SLAT  - SATELLITE LATITUDE
     SLONG - SATELLITE LONGITUDE
     TOE   - TIME OF EPHEMERIS FROM START OF WEEKLY EPOCH
     ETY   - ORBITAL INITIAL ECCENTRICITY
     TOA   - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
     INC   - ORBITAL INCLINATION
     IDOT  - RATE OF INCLINATION ANGLE
     CUC   - ARGUMENT OF LATITUDE COSINE CORRECTION TERM
     CUS   - ARGUMENT OF LATITUDE SINE   CORRECTION TERM
     CIC   - INCLINATION COSINE CORRECTION TERM
     CIS   - INCLINATION SINE   CORRECTION TERM
     RRA   - RATE OF RIGHT ASCENSION
     SQA   - SQUARE ROOT OF SEMIMAJOR AXIS
     LAN   - LONGITUDE OF NODE AT WEEKLY EPOCH
     AOP   - ARGUMENT OF PERIGEE
     MA    - MEAN ANOMALY AT TOA
     DN    - MEAN MOTION DIFFERENCE

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

static ECEFT satpos_ephemeris( double t, int n)
{
  double Mk, 
         Ek, Ek_old, 
         true_anomaly, 
         aol, delr, delal, delinc,
         r, inc, Wk, xp, yp, 
         bclk,
         d_toc, 
         d_toe, 
         xn, yn, zn, xe, ye, xls, yls,
         zls, range1, ralt, tdot,
         xaz, yaz, b;
  ECEFT  res;

//
//     MA IS THE ANGLE FROM PERIGEE AT TOA
//
  d_toc = t - GPS_Eph[n].toc;

  if ( d_toc > 302400.0)
    d_toc -= 604800.0;
  else if ( d_toc < -302400.0)
    d_toc += 604800.0;

//  at this point relativistc correction not yet included
//  (eccentric anomaly 'Ek' is not yet known)
  bclk  = GPS_Eph[n].af0 + 
          GPS_Eph[n].af1 * d_toc +
          GPS_Eph[n].af2 * d_toc * d_toc - 
          GPS_Eph[n].tgd;

  d_toe = t - bclk - GPS_Eph[n].toe;

//
//  time from ephemeris reference epoch (GPS-Specs: variable t_k)
//
  if ( d_toe > 302400.0)
    d_toe -= 604800.0;
  else if ( d_toe < -302400.0)
    d_toe += 604800.0;

/*
 *  mean anomaly
 */
  Mk = GPS_Eph[n].ma + (GPS_Eph[n].n0 + GPS_Eph[n].dn) * d_toe;

/*
 *  Ek : eccentric anomaly  
 *  Mk = Ek - sin( Ek)    solve by iteration
 */
  Ek = Mk;
  do
  {
    Ek_old = Ek;
    Ek = Mk + GPS_Eph[n].ety * sin( Ek);
  } while ( fabs( Ek_old - Ek) > 1.0e-9 );

/*
 *  true_anomaly (angle from perigee)
 */
  true_anomaly = atan2( sqrt( 1.0 - GPS_Eph[n].ety * 
                              GPS_Eph[n].ety) * sin( Ek),
                        cos( Ek) - GPS_Eph[n].ety);

/*
 *  aol IS THE ARGUMENT OF LATITUDE OF THE SATELLITE
 */
  aol = true_anomaly + GPS_Eph[n].w;

/*
 *  second harmonic perturbations of the orbit
 */

//  radius correction
  delr   = GPS_Eph[n].crc * cos( 2.0*aol) + 
           GPS_Eph[n].crs * sin( 2.0*aol);
//  arg of latitude correction
  delal  = GPS_Eph[n].cuc * cos( 2.0*aol) + 
           GPS_Eph[n].cus * sin( 2.0*aol);
//  correction to inclination
  delinc = GPS_Eph[n].cic * cos( 2.0*aol) + 
           GPS_Eph[n].cis * sin( 2.0*aol);

/*
 *  r IS THE RADIUS OF SATELLITE ORBIT AT TIME T
 */
  r   = GPS_Eph[n].sqra * GPS_Eph[n].sqra * 
        (1.0 - GPS_Eph[n].ety * cos( Ek)) + delr;
  aol += delal;
  inc = GPS_Eph[n].inc0 + delinc + GPS_Eph[n].idot * d_toe;

/*
 *  position in orbital plane
 */
  xp = r * cos( aol);
  yp = r * sin( aol);

/*
 *  Wk IS THE CORRECTED LONGITUDE OF THE ASCENDING NODE
 */
  Wk = GPS_Eph[n].W0 + 
       (GPS_Eph[n].Wdot - OMEGAE) * d_toe -
       OMEGAE * GPS_Eph[n].toe;

/*
 *  position in Earth-centered, Earth-fixed coordinates
 */
  res.x  = xp * cos( Wk) - yp * cos( inc) * sin( Wk);
  res.y  = xp * sin( Wk) + yp * cos( inc) * cos( Wk);
  res.z  = yp * sin( inc);

/*
 *  include relativistic correction term
 */
  res.tb = bclk + FCONST * GPS_Eph[n].ety * GPS_Eph[n].sqra * sin( Ek);

  res.az = 0.0;
  res.el = 0.0;

  if ( rec_pos_xyz.x || rec_pos_xyz.y || rec_pos_xyz.z)
  {
//               fprintf(stream,"x=%20.10lf,y=%20.10lf,z=%20.10lf\n",rec_pos_xyz.x,
//               rec_pos_xyz.y,rec_pos_xyz.z);

/*
 *  CALCULATE THE POSITION OF THE RECEIVER
 */
    xn  = -cos( rec_pos_llh.lon) * sin( rec_pos_llh.lat);
    yn  = -sin( rec_pos_llh.lon) * sin( rec_pos_llh.lat);
    zn  =  cos( rec_pos_llh.lat);

    xe  = -sin( rec_pos_llh.lon);
    ye  =  cos( rec_pos_llh.lon);

/*
 *  DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
 */
    xls = res.x - rec_pos_xyz.x;
    yls = res.y - rec_pos_xyz.y;
    zls = res.z - rec_pos_xyz.z;

    range1 = sqrt( xls*xls + yls*yls + zls*zls);

    ralt = sqrt( rec_pos_xyz.x * rec_pos_xyz.x + 
                 rec_pos_xyz.y * rec_pos_xyz.y + 
                 rec_pos_xyz.z * rec_pos_xyz.z);

    tdot = ( rec_pos_xyz.x * xls + 
             rec_pos_xyz.y * yls + 
             rec_pos_xyz.z * zls) / range1 / ralt;

    xls = xls / range1;
    yls = yls / range1;
    zls = zls / range1;

    if ( tdot >= 1.0 )
      b = 0.0;
    else if ( tdot <= -1.0 )
      b = PI;
    else
      b = acos( tdot);

    res.el = PI / 2.0 - b;

    xaz = xe * xls + ye * yls;
    yaz = xn * xls + yn * yls + zn * zls;

    res.az = atan2( xaz, yaz);
  } // ---   if ( rec_pos_xyz.x || rec_pos_xyz.y || rec_pos_xyz.z) ---

  return (res);
}

#endif 

/*******************************************************************************
FUNCTION velocity()
RETURNS  None.

PARAMETERS None.

PURPOSE  To convert velocity from ecef to local level axes

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
void velocity( LLH rp_llh)
{
  double  pt, pb,
          dxn, dyn, dzn,
          dn_mag, dxe, dye, de_mag,
          north, east;

  pt = (EARTH_MAJ_AXIS * EARTH_MAJ_AXIS -
        EARTH_MIN_AXIS*EARTH_MIN_AXIS) *
       sin( rp_llh.lat) * cos( rp_llh.lat);
  pb = sqrt( EARTH_MAJ_AXIS * EARTH_MAJ_AXIS *
        cos( rp_llh.lat) * cos( rp_llh.lat) +
        EARTH_MIN_AXIS * EARTH_MIN_AXIS *
        sin( rp_llh.lat) * sin( rp_llh.lat));

  dxn = EARTH_MAJ_AXIS * EARTH_MAJ_AXIS * pt / pow( pb, 3.0) *
    cos( rp_llh.lat) * cos( rp_llh.lon) -
    (EARTH_MAJ_AXIS * EARTH_MAJ_AXIS/pb + rp_llh.hae) *
    sin( rp_llh.lat) * cos( rp_llh.lon);

  dyn = EARTH_MAJ_AXIS * EARTH_MAJ_AXIS * pt / pow( pb, 3.0) *
    cos( rp_llh.lat) * sin( rp_llh.lon) -
    (EARTH_MAJ_AXIS * EARTH_MAJ_AXIS / pb + rp_llh.hae) *
    sin( rp_llh.lat) * sin( rp_llh.lon);

  dzn = EARTH_MIN_AXIS * EARTH_MIN_AXIS * pt / pow( pb, 3.0) *
    sin( rp_llh.lat) + (EARTH_MIN_AXIS * EARTH_MIN_AXIS / pb + rp_llh.hae) *
    cos( rp_llh.lat);

  dn_mag = sqrt(dxn*dxn + dyn*dyn + dzn*dzn);     // north magnitude

  if ( dn_mag == 0.0)
    north = 0.0;
  else
    north = (rpvt.xv*dxn + rpvt.yv*dyn + rpvt.zv*dzn)/dn_mag;

  dxe = -(EARTH_MAJ_AXIS*EARTH_MAJ_AXIS/pb + rp_llh.hae) *
    cos( rp_llh.lat) * sin( rp_llh.lon);

  dye = (EARTH_MAJ_AXIS*EARTH_MAJ_AXIS/pb + rp_llh.hae) *
    cos( rp_llh.lat) * cos( rp_llh.lon);

  de_mag = sqrt(dxe*dxe + dye*dye);             // east magnitude

  if ( de_mag==0.0)
    east = 0.0;
  else
    east = (rpvt.xv*dxe + rpvt.yv*dye) / de_mag;

#if 0
  dxu = cos( rp_llh.lat) * cos( rp_llh.lon);
  dyu = cos( rp_llh.lat) * sin( rp_llh.lon);
  dzu = sin( rp_llh.lat);

  up = rpvt.xv*dxu + rpvt.yv*dyu + rpvt.zv*dzu;   // up magnitude
#endif

  Rcvr_Info.speed   = sqrt( north*north + east*east);
  Rcvr_Info.heading = atan2( east, north);

//  if ( out_vel)
//    fprintf( stream, "vel north=%lf, east=%lf, up=%lf\n",
//      north, east, up);

  return;
}

#if 0
/*
 *  find start of preamble pattern in msg[].buffer
 *  msg[ch].preamidx points to preamble of first subframe
 */
void find_preamble( int ch)
{
  INT16 i, j, k, idx1, idx2, fifo = 0, 
        hist1[300],   // histogram buffer to find preamble pattern
        hist2[300],   // same for inverted preambles
        sfid[5];      // subframe id

  for( i=0; i<300; i++)
  {
    hist1[i] = 0;
    hist2[i] = 0;
  }

  for( i=0; i<1500; i++)
  {
    fifo = (fifo << 1) | (Msg[ch].buffer[i] != 0);
    fifo &= 0xff;

    if ( fifo == 0x8b)        // pattern matches preamble?
      hist1[i%300] += 1; 
    else if ( fifo == (~0x8b & 0xff))  // nav bits may be inverted!
      hist2[i%300] += 1; 
  }

//  find max value in hist array, 
  idx1 = maxidx( hist1, 300);
  idx2 = maxidx( hist2, 300);

  assert( idx1 >= 0 && idx1 < 300);
  assert( idx2 >= 0 && idx2 < 300);

  if (hist1[idx1] >= hist2[idx2])
  {
//  preamidx points to _start_ of preamble, 
//  idx[1|2] point to end (last bit)!
    msg[ch].preamidx = (idx1 + 293) % 300;
    msg[ch].inverted = 0;
//    printf( "ch %d - nof pream patterns : %d\n", ch, hist1[idx1]);
  }
  else
  {
    msg[ch].preamidx = (idx2 + 293) % 300;
    msg[ch].inverted = 1;
//    printf( "ch %d - nof pream patterns : %d\n", ch, hist2[idx2]);
  }

#if 0
  printf( "\n");
  printf( "\n");
  for (i=0;i<60;i++)
    printf( "%d", msg[ch].buffer[(msg[ch].preamidx+i)%1500]);
  printf( "\n");
#endif

//
//  find subframe id for each subframe
//
  k = -1;
  idx1 = msg[ch].preamidx + 49;  // gpsbit20-22: sfid
  idx2 = msg[ch].preamidx + 29;  // gpsbit30: D30
  for( i=0; i<5; i++)            
  {
    sfid[i] = 0;
    for( j=0; j<3; j++)
    {
      sfid[i] = (sfid[i] << 1) | (msg[ch].buffer[idx1%1500] != 0);
      idx1 += 1;
    }

    if ( msg[ch].buffer[idx2%1500])
      sfid[i] = (~sfid[i]) & 0x7;

//    assert( sfid[i] >= 0 && sfid[i] < 8);

    idx1 += 297;
    idx2 += 300;

    if ( sfid[i] == 1)
      k = i;

  }

//
//  if no consistency, discard all
//
  if (!( k >= 0 && sfid[k] == 1 && 
         sfid[(k+1)%5] == 2 && sfid[(k+2)%5] == 3 && 
         sfid[(k+3)%5] == 4 && sfid[(k+4)%5] == 5))
  {
    msg[ch].preamidx = -1;
//    printf("sfid = %d/%d/%d/%d/%d\n", 
//      sfid[k], sfid[(k+1)%5], sfid[(k+2)%5], 
//      sfid[(k+3)%5], sfid[(k+4)%5]);
  }

  if ( k >= 0 && msg[ch].preamidx >= 0)
    msg[ch].preamidx = (msg[ch].preamidx + k*300) % 1500;

  return;
}

#endif

/*******************************************************************************
FUNCTION ecef_to_llh()
RETURNS  position in longitude/latitude/height coordinates

PARAMETERS
  Position in Earth-centered, earth-fixed coordinates

PURPOSE
  transform from Earth-centered, earth-fixed coordinates to
  longitude/latitude/height coordinates

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

LLH ecef_to_llh( ECEF pos)
{
  double p, n, 
         thet, 
         esq, 
         epsq;
  LLH res;

  if ( pos.x == 0.0 && pos.y == 0.0 && pos.z == 0.0)
  {
    res.lon = 0.0;
    res.lat = 0.0;
    res.hae = 0.0;
    return (res);
  }

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