ogr_navsolve2.c

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

//          noftrk += 1;
        }
        else
          tau = (Pcor[prn-1] + Cr) / LIGHTVEL;

        Ttr = *Trc - tau;

/* calculate SV position and correct for earth rotation */

        gotoxy( 1, 21+2*loop);
        satellite_position_old( prn, eph[prn-1], Ttr, &Trel, tmp3);

//        if ( prn == 13 && loop == 0)
//        {
//          gotoxy( 1, 23);
//          printf("old: PRN = %d, pos: [%e %e %e] \n", 
//	    prn, tmp3[0], tmp3[1], tmp3[2]);
//        }

        alpha = tau * OMEGAE;

        Xs[prn-1][0] =   tmp3[0] * cos(alpha) + tmp3[1] * sin(alpha);
        Xs[prn-1][1] = - tmp3[0] * sin(alpha) + tmp3[1] * cos(alpha) ;
        Xs[prn-1][2] =   tmp3[2];

/* calculate azimuth and elevation */
        calcAzEl( Xs[prn-1], Xr, &Az, &El, &status);

        if ( prn == SelPRN)
        {
          gotoxy( 1, 25+2*loop);
          printf("old: (%d) PRN=%d, Ttr:%e, xmit: [%.8e %.8e %.8e], rcvr: [%e %e %e]\n", 
            loop, prn, Ttr, Xs[prn-1][0], Xs[prn-1][1], Xs[prn-1][2],
	    Xr[0], Xr[1], Xr[2]);
        }


        if ( !status) 
          return (-1);

/* calculate pseudorange corrections and apply to pseudoranges */

/* clock correction */
        Tcor = Ttr - clk[prn-1][1];

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

        dTclck = clk[prn-1][4] + clk[prn-1][3] * Tcor +
                 clk[prn-1][2] * Tcor * Tcor + 
                 Trel - clk[prn-1][0];

//      printf(" dTclck = %.9e Ttr = %.9e Trel = %.9e Tcor = %.9e\n", dTclck, Ttr, Trel, Tcor);
//      printf(" clck[0]=%.6e clck[1]=%.6e clck[2]=%.6e clck[3]=%.6e clck[4]=%.6e \n", 
//         clk[prn-1][0], clk[prn-1][1], clk[prn-1][2], clk[prn-1][3], clk[prn-1][4]);

/* iono correction */
        Lat = Xlla[0];
        Lon = Xlla[1];

        ionocorr_old( ion, Lat, Lon, Az, El, Ttr, &dTiono);

/* tropo 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 */
        Pcor[prn-1] = Praw[prn-1] + dTclck * LIGHTVEL - 
                      dTiono * LIGHTVEL - dRtrop + Cr;

        if ( prn == SelPRN)
        {
          gotoxy( 1, 29 + loop*2);
          printf("old: (%d) Pcor:%.8e, dTclck:%.8e, dTiono:%.8e, dRtrop:%.8e, El:%.8e\n", 
	    loop, Pcor[prn-1], dTclck, dTiono, dRtrop, El);
        }

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

      }  // --- if (SV[prn-1])  ---
    }  // --- for (prn = 1; prn <= 32; prn++) ---

/* calculate receiver position */
    solve_old( Xs, SV, Pcor, Xr, &Cr, &status);

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

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

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

//  determine dilution of precision
  dops_old( SV, Xs, Xr, 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 (0);
}
#endif

/*
 *
 */
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 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
 */
#if 0
      transmit_time[n] = 2.0 - 
        ((NavData.epoch[ch] >> 8) / 50.0 + 
         (NavData.epoch[ch] & 0x1f) / 1000.0 + 
         NavData.code_phase[ch] / 2.046e6 + 
         NavData.code_dco_phase[ch] / 2.046e6 / 1024.0);

      transmit_time[n] =  1.0 - 
        ((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);
#endif

      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 () ... ---
  }  // 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 ***    

#if 0
//  fill variables eph, clk, etc. from global data
  stdalone_input_phase( Xlla, &Trc, SV, eph, clk, ion, Praw, 
    tr_ch, Ctrl.nof_trk, transmit_time);
#endif

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

#if 0
//  do the calculation
  ret = calc_rcvr_pos_old( Xlla, Xr, &Trc, SV, eph, clk, ion, Praw);
#endif

  rcvr_pos = Rcvr_Info.pos;

//  new version
  retcode = calc_rcvr_pos( Ctrl.nof_trk, tr_ch, transmit_time,
    &rcvr_pos, &Trc);

//  printf("X: old = %e, new = %e\n", Rcvr_Info.pos.x, rcvr_pos.x);
//  printf("Y: old = %e, new = %e\n", Rcvr_Info.pos.y, rcvr_pos.y);
//  printf("Z: old = %e, new = %e\n", Rcvr_Info.pos.z, rcvr_pos.z);
//  gotoxy( 1, 34);
//  printf("X: old = %e, new = %e\n", Xr[0], rcvr_pos.x);
//  printf("Y: old = %e, new = %e\n", Xr[1], rcvr_pos.y);
//  printf("Z: old = %e, new = %e\n", Xr[2], rcvr_pos.z);
//  printf("Trc: old = %e, new = %e\n", Trc, Trc_new);

  rcvr_llh = ecef_to_llh( rcvr_pos);

//  gotoxy( 1, 34);
//  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)
    return;

  Rcvr_Info.pos = rcvr_pos;

  Rcvr_Info.llh = ecef_to_llh( Rcvr_Info.pos);

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

  if ( Ctrl.almanac_valid == 1)
    Ctrl.status = mode_navigate;

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