ogr_solvessvl.c

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

  {
    if (i + 1 != r)   
    {
      i1++;
      j1 = 0;
      for (j = 1; j <= 4; j++) 
      {
        if (j != c_) 
        {
          j1++;
          B[i1-1][j1-1] = A[i][j-1];
        }
      }
    }
  }
  return ( B[0][0] * (B[1][1] * B[2][2] - B[1][2] * B[2][1]) + 
           B[1][0] * (B[2][1] * B[0][2] - B[0][1] * B[2][2]) + 
           B[2][0] * (B[0][1] * B[1][2] - B[0][2] * B[1][1]));
} 

#define ITERMAX 20

/*
 *
 *  IN  : Xs   array with 3 columns and 32 rows for the coordinates of the sat's
 *  IN  : SV   valid prn's
 *  IN  : P    pseudoranges
 *  IN  : Xr   input of initial final position
 *  OUT : Cr   receiver clock error
 */
static int solve( int n_max, ECEF *rcvr_pos, ECEF xmit_pos[], 
  double *P, double *Cr)
{
  int retcode = 0;
  long it = 0, i, j, k, n;
  double R[NOFCHN], L[NOFCHN],
         A[NOFCHN][4],
         AL[4],
         AA[4][4], 
         AAi[4][4],
         det,
         D[4],
         Xs[NOFCHN][3],  // sat position
         Xr[3];          // rcvr position

  Xr[0] = rcvr_pos->x;
  Xr[1] = rcvr_pos->y;
  Xr[2] = rcvr_pos->z;

  for ( n = 0; n < n_max; n++) 
  {
    Xs[n][0] = xmit_pos[n].x;
    Xs[n][1] = xmit_pos[n].y;
    Xs[n][2] = xmit_pos[n].z;
  }

  do 
  {   /* iterations */

    it++;   /* increase iteration counter */

    for (n = 0; n < n_max; n++) 
    {
      R[n] = sqrt((Xr[0] - Xs[n][0]) * (Xr[0] - Xs[n][0]) +
                  (Xr[1] - Xs[n][1]) * (Xr[1] - Xs[n][1]) +
                  (Xr[2] - Xs[n][2]) * (Xr[2] - Xs[n][2]));

/* range from receiver to satellite */
      L[n] = P[n] - R[n];   /* range residual value */

/* A is the geometry matrix or model matrix */
      for ( k = 0; k < 3; k++)
        A[n][k] = (Xr[k] - Xs[n][k]) / R[n];

/* normalised user->satellite vectors */
      A[n][3] = -1.0;   
    }

    for (k = 0; k < 4; k++) 
    {     /*  calculate A.L  */
      AL[k] = 0.0;
      for (n = 0; n < n_max; n++) 
        AL[k] += A[n][k] * L[n];
    }

    for (k = 0; k < 4; k++) 
    {
      for (i = 0; i < 4; i++) 
      {     /*  calculate A.A  */
        AA[k][i] = 0.0;
        for ( n = 0; n < n_max; n++) 
          AA[k][i] += A[n][k] * A[n][i];
      }
    }

/* invert A.A */
    det = AA[0][0] * sub( AA, 1L, 1L) - AA[1][0] * sub( AA, 2L, 1L) + 
          AA[2][0] * sub( AA, 3L, 1L) - AA[3][0] * sub( AA, 4L, 1L);

    if ( det == 0.0)
/* there is something wrong if more than 6 iterations are required */
      retcode = OGRERR_DET_ZERO;
    else 
    {
      retcode = 0;

      for (k = 1; k <= 4; k++)
      {
        for (i = 1; i <= 4; i++) 
        {
          n = k + i;
          if (n & 1)
            j = -1;
          else
            j = 1;
          AAi[k-1][i-1] = j * sub( AA, i, k) / det;
        }
      }

/* calculate (invA.A).(A.L) */
      for (k = 0; k < 4; k++) 
      {
        D[k] = 0.0;
        for (i = 0; i < 4; i++)
          D[k] += AAi[k][i] * AL[i];
      }

/* update position */
      for (k = 0; k < 3; k++)
        Xr[k] += D[k];

    }
  } while ( it != ITERMAX && retcode == 0 &&   
            fabs( D[0]) + fabs( D[1]) + fabs( D[2]) >= 1.0e-2);
  

  rcvr_pos->x = Xr[0];
  rcvr_pos->y = Xr[1];
  rcvr_pos->z = Xr[2];

  *Cr = D[3];   /* receiver clock error */

  if ( it == ITERMAX) 
    retcode = OGRERR_MAXITER_REACHED;

  return (retcode);
} 

/*
 *  calculate receiver position from pseudorange
 */
static int calc_rcvr_pos( INT16 n_max, INT16 tr_ch[], 
  double transmit_time[], ECEF *rcvr_pos, double *Trc)
{
  INT16 n;
  int prn, loop, retcode = 0;
  double dTclck, dTiono, dRtrop, 
         p_cor[NOFCHN], 
         p_raw[NOFCHN];   // max. 12 pseudorange observations
  ECEF tmp_pos, xmit_pos[NOFCHN];
  LLH rcvr_llh;
  EPHEMERIS *eph;

  static double Ttr, tau, Trel, Tcor, Az, El, Cr, alpha;

/*  convert lat, ln, alt to ECEF X, Y, Z  */
//  lla2xyz( Xlla, Xr);

//  gotoxy(1,24);
//  printf("Xlla = [%e %e %e] \n", Xlla[0]*180.0/M_PI, Xlla[1]*180.0/M_PI, Xlla[2]);
//  printf("Xr   = [%e %e %e] \n", Xr[0], Xr[1], Xr[2]);

/* 
 *  assuming the receiver clock error and initial position not sufficiently
 *  good known, I make two passes through the processing steps
 */

  for ( n = 0; n < n_max; n++) 
    p_raw[n] = (*Trc - transmit_time[n]) * LIGHTVEL;

  for ( loop = 0; loop < 2; loop++) 
  {
    for ( n = 0; n < n_max; n++) 
    {
      prn = Chan[tr_ch[n]].prn;

      eph = &GPS_Eph[prn];
/*  
 * set all transit times to nominal value and calculate time of transmission
 */
      if ( loop == 0)
      {
        tau = 0.075;
        Cr  = 0.0;
      }
      else
//        tau = (Pcor[prn-1] + Cr) / LIGHTVEL;
        tau = (p_cor[n] + Cr) / LIGHTVEL;

      Ttr = *Trc - tau;

/* calculate SV position and correct for earth rotation */
//      satellite_position_old( prn, eph[prn-1], Ttr, &Trel, tmp3);

#if 0
      gotoxy( 1, 22+2*loop);
#endif

      tmp_pos = satellite_position( prn, Ttr, &Trel);
      
      alpha = tau * OMEGAE;

      xmit_pos[n].x =   tmp_pos.x * cos( alpha) + tmp_pos.y * sin( alpha);
      xmit_pos[n].y = - tmp_pos.x * sin( alpha) + tmp_pos.y * cos( alpha);
      xmit_pos[n].z =   tmp_pos.z;

/* calculate azimuth and elevation */
      sat_azimuth_elevation( xmit_pos[n], *rcvr_pos, &Az, &El);

#if 0
      if ( prn == SelPRN)
      {
        gotoxy( 1, 26+2*loop);
        printf("new: (%d) PRN=%d, Ttr:%e, xmit: [%.8e %.8e %.8e], rcvr: [%e %e %e]\n", 
	  loop, prn, Ttr, xmit_pos[n].x, xmit_pos[n].y, xmit_pos[n].z,
	  rcvr_pos->x, rcvr_pos->y, rcvr_pos->z);
      }
#endif

/* calculate pseudorange corrections and apply to pseudoranges */

/* clock correction */
      Tcor = Ttr - eph->toc;

//    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;

/* at end of week? */
      if ( Tcor > 302400.0)
        Tcor -= 604800.0;
      else if ( Tcor < -302400.0)
        Tcor += 604800.0;

      dTclck = eph->af0 + 
               eph->af1 * Tcor +
               eph->af2 * Tcor * Tcor + 
               Trel - eph->tgd;

/* ionospheric correction */
      rcvr_llh = ecef_to_llh( *rcvr_pos);

      dTiono = ionocorr( Iono, rcvr_llh.lat, rcvr_llh.lon, 
        Az, El, Ttr);

/* tropospheric correction using standard atmosphere values */
      dRtrop = 2.312 / sin( sqrt( El * El + 1.904e-3)) +
               0.084 / sin( sqrt( El * El + 0.6854e-3));

/* correct pseudorange */
      p_cor[n] = p_raw[n] + 
                 dTclck * LIGHTVEL - 
                 dTiono * LIGHTVEL - 
                 dRtrop + Cr;

#if 0
      if ( prn == SelPRN)
      {
        gotoxy( 1, 30 + loop*2);
        printf("new: (%d) pcor:%.8e, dTclck:%.8e, dTiono:%.8e, dRtrop:%.8e, El:%.8e\n", 
	  loop, p_cor[n], dTclck, dTiono, dRtrop, El);
      }
#endif

//      Pcor[prn-1] = Praw[prn-1] + dTclck * LIGHTVEL - 
//                    dTiono * LIGHTVEL - dRtrop + Cr;

//      printf("(1) dTiono=%.9e dTclck=%.9e dRtrop=%.9e Pcor=%.9e Praw=%.9e El=%.9e\n", 
//        dTiono, dTclck, dRtrop, Pcor[prn-1], Praw[prn-1], El);

    }  // --- for ( n = 0; n < n_max; n++) 

/* calculate receiver position */
    retcode = solve( n_max, rcvr_pos, xmit_pos, p_cor, &Cr);

    if ( retcode)
      return (retcode);

///*convert back to Lat, Lon, Alt */
//    xyz2lla( Xr, Xlla);

/* {correct receiver clock} */
    *Trc += Cr / LIGHTVEL;

  }  // --- for (loop ... ---

//  make clock correction known globally ...
//  Ctrl.clock_offset = Tcor;

//  ... and make sat positions known globally
  for ( n = 0; n < n_max; n++) 
  {
    prn = Chan[tr_ch[n]].prn;
//    memcpy( &Xmit_Info[prn].pos, &xmit_pos[n], sizeof( ECEF));
    Xmit_Info[prn].pos = xmit_pos[n];
  }

//  determine dilution of precision
  dops( n_max, *rcvr_pos, xmit_pos, Cr, 
        &Dops.hdop, &Dops.vdop, &Dops.tdop, &Dops.pdop, &Dops.gdop);

//  calc_error( Xlla[0] * 180.0 / M_PI, Xlla[1] * 180.0 / M_PI, Xlla[2]);

  return (retcode);
}



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

PARAMETERS None.

PURPOSE
  This function determines the pseudorange and doppler to each
  satellite and calls pos_vel_time to determine the position and
  velocity of the receiver

WRITTEN BY
  Clifford Kelley, G. Beyerle

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

#if !defined( USE_OPENSRCGPS_NAVFIX)
void nav_fix( void)
{
  INT16  ch, n = 0, tr_ch[NOFCHN];
  int    retcode;
  double Trc,                     // receiver
         transmit_time[NOFCHN];
  ECEF   rcvr_pos;
  LLH    rcvr_llh;

//  static double time[3];

  for ( ch=0; ch<NOFCHN; ch++)
  {

//  Use only satellites being tracked with valid ephemeris data
//  (valid subframe 1,2,3) and high enough raw SNR

    if ( Chan[ch].state == track && 
         GPS_Eph[Chan[ch].prn].valid == 1 &&
         GPS_Eph[Chan[ch].prn].health == 0 &&
         Chan[ch].tow_sync == 1)
    {

      tr_ch[n] = ch;

/*
 *  transmit_time[n] : time of signal transmission
 */
      transmit_time[n] =  NavData.tow_1s[ch] +
         (NavData.epoch[ch] >> 8) / 50.0 + 
         (NavData.epoch[ch] & 0x1f) / 1000.0 + 
         NavData.code_phase[ch] / 1000.0 / 2046.0 +
         NavData.code_dco_phase[ch] / 1000.0 / 2046.0 / 1024.0;

#if 0
      printf("[%2d] tow=%6ld, ms=%4d, cd_phs=%4d, cd_dco_phs=%4d Ttr=%16.9f s dT = %12.9f s \n", 
        ch, NavData.tow_1s[ch],  
        (NavData.epoch[ch] >> 8)*20 + (NavData.epoch[ch] & 0x1f), 
        NavData.code_phase[ch], NavData.code_dco_phase[ch], transmit_time[n], 
        NavData.tow_tic_count[ch]/10.0 - transmit_time[n]);
#endif

//      Chan[ch].epoch_nav & 0x1f : msec counter  0,...,19
//      Chan[ch].epoch_nav >> 8   : bit counter   0,...,49

      if ( FpDmp)
      {
        unsigned long phase_counter;
        double rec_time;

        phase_counter = ( NavData.carr_dco_phase[ch] * 2) & 0x7ff;
        phase_counter += (NavData.carr_cycle_lo[ch] << 11);
        phase_counter += (NavData.carr_cycle_hi[ch] << 27);

        rec_time = NavData.tow_tic_count[ch] / 10.0;

        write_msg_0x38( FpDmp, ch, (rec_time - transmit_time[n]) * LIGHTVEL, 
          phase_counter, rec_time);
      }
      n += 1;

#if 0
      gotoxy(1,22+ch);
      printf("okay!\n"); 
#endif

    }  // --- if () ... ---
#if 0
    else
    {
      if ( Chan[ch].state == track)
      {
        gotoxy(1,22+ch);
        printf("prn=%d, state = %d, valid = %d, health = %d, sync = %d\n", 
        Chan[ch].prn, Chan[ch].state, GPS_Eph[Chan[ch].prn].valid, 
        GPS_Eph[Chan[ch].prn].health, Chan[ch].tow_sync);
      }  
    }
#endif
  }  // for ( ch=0; ch<NOFCHN; ch++)

//  number of tracked satellites
  Ctrl.nof_trk = n;

//  need at least four
  if ( Ctrl.nof_trk < 4)
    return;
      
//  *** FIXME: try to find a 2-dim nav solution for nof_trk == 3 ***    

/*
 * 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;

  rcvr_pos = Rcvr_Info.pos;

//  calculate receiver location
  retcode = calc_rcvr_pos( Ctrl.nof_trk, tr_ch, transmit_time,
    &rcvr_pos, &Trc);

  rcvr_llh = ecef_to_llh( rcvr_pos);

//  gotoxy( 1, 24);
//  printf("X: %.8e %.8e %.8e lo=%.8e la=%.8e h=%.8e\n", 
//    rcvr_pos.x, rcvr_pos.y, rcvr_pos.z, 
//    rcvr_llh.lat*180.0/M_PI, rcvr_llh.lon*180.0/M_PI, rcvr_llh.hae);

  if ( retcode || rcvr_llh.hae > 10e3 || rcvr_llh.hae < -5e3)
  {
    Ctrl.status = mode_tracking;
    return;
  }  

  Ctrl.status = mode_navigate;

  Rcvr_Info.pos = rcvr_pos;

  Rcvr_Info.llh = ecef_to_llh( Rcvr_Info.pos);

  if ( FpDmpASCII)
    write_dump_file_ascii( Ctrl.nof_trk, tr_ch, Rcvr_Info.pos);


//  reduce carrier space search range, since we're know where we are
  RT_Ctrl->search_max_f = 5;

/*
 *  Translate velocity into North, East, Up coordinates
 */
//  velocity( rec_pos_llh);

  return;

#if 0

//  calculate receiver position/velocity from pseudoranges
  rpvt        = pos_vel_time( n_track);

//  determine dilution of precision
  dops( n_track);

  cbias       = rpvt.dt;
  clock_error = rpvt.df;
  time[1]     = transmit_time[1] + dtime[1] - cbias;
  rp_ecef.x   = rpvt.x;
  rp_ecef.y   = rpvt.y;
  rp_ecef.z   = rpvt.z;

  rp_llh      = ecef_to_llh( rp_ecef);

//  a quick reasonableness check
  if ( rp_llh.hae > -2000.0 && rp_llh.hae < 1.0e6) 
  {
    if ( fabs( clock_error) < 5.0)
      clock_offset = clock_error;

    if ( almanac_valid == 1)
      status = mode_navigate;

    rec_pos_llh.lon = rp_llh.lon;
    rec_pos_llh.lat = rp_llh.lat;
    rec_pos_llh.hae = rp_llh.hae;

    rec_pos_xyz.x   = rp_ecef.x;
    rec_pos_xyz.y   = rp_ecef.y;
    rec_pos_xyz.z   = rp_ecef.z;

//
//    Since we have a valid position/velocity narrow the doppler search window
//
    Ctrl->search_max_f = 5;

//
//  Translate velocity into North, East, Up coordinates
//
    velocity();

    time[0] = time[1];
  }
  else
  {
    rec_pos_llh.lon = 0.0;
    rec_pos_llh.lat = 0.0;
    rec_pos_llh.hae = 0.0;
    rec_pos_xyz.x   = 0.0;
    rec_pos_xyz.y   = 0.0;
    rec_pos_xyz.z   = 0.0;
  }
#endif

}

#endif

/* ------------------------------- end of file ---------------------------- */