ogr_solveosg.c

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

  {
/*
 *   Compute line of sight vectors
 */
    r = sqrt( pow( track_sat[i].x-x, 2.0) +
              pow( track_sat[i].y-y, 2.0)+
              pow( track_sat[i].z-z, 2.0));
    ms[1][i] = (track_sat[i].x-x) / r;
    ms[2][i] = (track_sat[i].y-y) / r;
    ms[3][i] = (track_sat[i].z-z) / r;
    ms[4][i] = 1.0;
  }

  for (i=1; i<=nsl; i++)
  {
    msx = ms[1][i]*north.x + ms[2][i]*north.y + ms[3][i]*north.z;
    msy = ms[1][i]*east.x  + ms[2][i]*east.y;
    msz = ms[1][i]*up.x    + ms[2][i]*up.y    + ms[3][i]*up.z;

    ms[1][i] = msx;
    ms[2][i] = msy;
    ms[3][i] = msz;
    ms[4][i] = 1.0;
  }

  for (k=1; k<=nsl; k++)
  {
    a1 += ms[1][k] * ms[1][k];
    b1 += ms[1][k] * ms[2][k];
    c1 += ms[1][k] * ms[3][k];
    d1 += ms[1][k] * ms[4][k];
//   e1+=ms[2][k]*ms[1][k];
    f1 += ms[2][k] * ms[2][k];
    g1 += ms[2][k] * ms[3][k];
    h1 += ms[2][k] * ms[4][k];
//   i1+=ms[3][k]*ms[1][k];
//   j1+=ms[3][k]*ms[2][k];
    k1 += ms[3][k] * ms[3][k];
    l1 += ms[3][k] * ms[4][k];
//   m1+=ms[1][k];
//   n1+=ms[2][k];
//   o1+=ms[3][k];
    p1 += ms[4][k];
  }

  o1 = l1;
  m1 = d1;
  n1 = h1;
  e1 = b1;
  i1 = c1;
  j1 = g1;

/*
 *     SOLVE FOR THE DIAGONALS OF THE MATRIX INVERSE
 */
  denom = (k1*p1-l1*o1) * (a1*f1-b1*e1) + 
          (l1*n1-j1*p1) * (a1*g1-c1*e1) + 
          (j1*o1-k1*n1) * (a1*h1-d1*e1) + 
          (l1*m1-i1*p1) * (c1*f1-b1*g1) + 
          (i1*o1-k1*m1) * (d1*f1-b1*h1) +
          (i1*n1-j1*m1) * (c1*h1-d1*g1);

  ddx = f1*(k1*p1-l1*o1) + g1*(l1*n1-j1*p1) + h1*(j1*o1-k1*n1);
  ddy = a1*(k1*p1-l1*o1) + c1*(l1*m1-i1*p1) + d1*(i1*o1-k1*m1);
  ddz = a1*(f1*p1-h1*n1) + b1*(h1*m1-e1*p1) + d1*(e1*n1-f1*m1);
  ddt = a1*(f1*k1-g1*j1) + b1*(g1*i1-e1*k1) + c1*(e1*j1-f1*i1);

  if ( denom <= 0.0 )
  {
    Dops.hdop = 1.0e6;      // something went wrong
    Dops.vdop = 1.0e6;      // set dops to a large number
    Dops.tdop = 1.0e6;
    Dops.pdop = 1.0e6;
    Dops.gdop = 1.0e6;
  }
  else
  {
    Dops.hdop = sqrt( (ddx+ddy) / denom);
    Dops.vdop = sqrt( ddz / denom);
    Dops.tdop = sqrt( ddt / denom);
    Dops.pdop = sqrt( Dops.vdop*Dops.vdop + Dops.hdop*Dops.hdop);
    Dops.gdop = sqrt( Dops.pdop*Dops.pdop + Dops.tdop*Dops.tdop);
  }
  return;
}

#endif

#ifdef USE_OPENSRCGPS_NAVFIX

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

PARAMETERS 
  az       - azimuth
  el       - elevation
  gps_time - time

PURPOSE
  This function corrects the pseudoranges with a tropospheric model
  and the broadcast ionospheric message corrections.

WRITTEN BY
  Clifford Kelley

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

static double tropo_iono( double az, double el, double gps_time)
{
  double d_Trop, d_Ion, 
         psi, phi, 
         lambdai, phim, per, 
         x, F, amp, t, alt_factor;

// troposphere model
  if ( current_loc.hae > 200000.0) 
    alt_factor = 0.0;
  else if ( current_loc.hae < 0.0) 
    alt_factor = 1.0;
  else 
    alt_factor = exp( -current_loc.hae * 1.33e-4);

#ifdef TROPO_CORR
  d_Trop = 2.47 / (sin( el) + 0.0121) * alt_factor / LIGHTVEL;
#else
  d_Trop = 0.0;
#endif

//  tropo[ch] = d_Trop;

//  if ( d_Trop < 0.0) 
//    printf( "el=%lf, hae=%lf", el, current_loc.hae);

// ionosphere model

#ifdef IONO_CORR
  psi = 0.0137 / (el / PI + 0.11) - 0.022;
  phi = current_loc.lat / PI + psi * cos(az);
  if ( phi > 0.416) 
    phi = 0.416;
  else if ( phi < -0.416 )
    phi = -0.416;

  lambdai = current_loc.lon / PI + psi * sin( az) / cos( phi * PI);

  t = 43200.0 * lambdai + gps_time - 
    (INT16)((43200.0 * lambdai + gps_time) / 86400.0)*86400.0;

  if ( t < 0.0) 
    t = t + 86400.0;

  phim = phi + 0.064 * cos( (lambdai-1.617) * PI);

  per = Iono.b0  + 
        Iono.b1 * phim  + 
        Iono.b2 * phim * phim  + 
        Iono.b3 * phim * phim * phim;
  amp = Iono.al0 + 
        Iono.al1 * phim + 
        Iono.al2 * phim * phim + 
        Iono.al3 * phim * phim * phim;

  if ( per < 72000.0 ) 
    per = 72000.0;

  x = 2.0 * PI * (t-50400.0) / per;
  F = 1.0 + 16.0 * pow( 0.53-el/PI, 3);

  if ( amp < 0.0 ) 
    amp = 0.0;

  if ( fabs(x) < 1.5707)  
    d_Ion=F*(5.0e-9+amp*(1.0-x*x/2.+x*x*x*x/24.0));
  else                   
    d_Ion=F*5.0e-9;
#else
  d_Ion = 0.0;
#endif

//  iono[ch] = d_Ion;

  return ( d_Trop + d_Ion);

#if 0
  d_Trop = 2.47/(sin(el)+.0121) * 
    exp(-current_loc.hae*1.33e-4) / LIGHTVEL;
//  If available from the nav message use its Ionosphere model
  if (Iono.b0 != 0.0 && Iono.al0 != 0.0)
  {
    psi=0.0137/(el/PI+0.11)-0.022;
    phi=current_loc.lat/PI+psi*cos(az);
    if (phi > 0.416)
      phi= 0.416;
    else if (phi <-0.416 )
      phi=-0.416;
    lambdai=current_loc.lon/PI+psi*sin(az)/cos(phi*PI);
    t=(INT16)(43200.*lambdai+gps_time)%86400L;
    if (t<0.0) t=t+86400.0;
    phim=phi+0.064*cos((lambdai-1.617)*PI);

    per = Iono.b0 + Iono.b1*phim + Iono.b2*phim*phim + 
          Iono.b3*phim*phim*phim;

    if ( per <72000.0 ) per=72000.0;
    x=2.*PI*(t-50400.)/per;
    F=1.0 + 16.0 * pow( 0.53-el/PI, 3.0);

    amp = Iono.al0 + Iono.al1*phim + Iono.al2*phim*phim + 
          Iono.al3 * phim * phim * phim;

    if ( amp < 0.0 ) 
      amp = 0.0;
    if ( fabs( x) < 1.5707)  
      d_Ion = F*(5.0e-9+amp*(1.0-x*x/2.+x*x*x*x/24.0));
    else                   
      d_Ion = F*5.0e-9;
  }
// else try this
  else
  {
    d_Ion = 16.33 / sin( sqrt( el*el+0.1255)) / LIGHTVEL;
  }

//  fprintf(stream,"d_trop=%le,d_ion=%le,az=%f,el=%f\n",d_Trop,d_Ion,az,el);

  return (d_Trop + d_Ion);
#endif
}

#endif

#if 0
/*******************************************************************************
FUNCTION fix_atan2(long y,long x)
RETURNS  long integer

PARAMETERS 
      x  in phase fixed point value
      y  quadrature fixed point value

PURPOSE
      This function computes the fixed point arctangent represented by
      x and y in the parameter list
      1 radian = 16384
      based on the power series  f-f^3*2/9

WRITTEN BY
  Clifford Kelley
  Fixed for y==x  added special code for x==0
*******************************************************************************/

#define SCALED_PI_ON_2  25736L
#define SCALED_PI       51472L

inline long fix_atan2(long y,long x)
{
  long res = 0, n, n3;

  if ((x==0) && (y==0))
    return(0); // invalid case

  if (x>0 &&  x>=labs(y))
  {
     n=(y<<14)/x;
     n3=((((n*n)>>14)*n)>>13)/9;
     res=n-n3;
  }
  else if (x<=0 && -x>=labs(y))
  {
     n=(y<<14)/x;
     n3=((((n*n)>>14)*n)>>13)/9;
     if      ( y>0) res=n-n3+SCALED_PI;
     else if (y<=0) res=n-n3-SCALED_PI;
  }
  else if (y>0 &&  y>labs(x))
  {
     n=(x<<14)/y;
     n3=((((n*n)>>14)*n)>>13)/9;
     res=SCALED_PI_ON_2-n+n3;
  }
  else if (y<0 && -y>labs(x))
  {
     n=(x<<14)/y;
     n3=((((n*n)>>14)*n)>>13)/9;
     res=-n+n3-SCALED_PI_ON_2;
  }
  return(res);
}
#endif

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

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

#ifdef USE_OPENSRCGPS_NAVFIX
void nav_fix( void)
{
  INT16  i, ch, n = 1, tr_ch[13];
  unsigned long phase_counter;
  float  clock_error;
  double transmit_time[13],
         t_cor[13], 
         dtime[13], 
         transmit_time_min,
         transmit_time_max;
  static double time[3];
  ECEF   rp_ecef, d_sat[13];
  ECEFT  dm_gps_sat[13], 
         dp_gps_sat[13];
  LLH    rp_llh;       

  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 && 
//         Chan[ch].CN0 > 30 &&
//         GPS_Eph[Chan[ch].prn].valid == 1 &&
//         GPS_Eph[Chan[ch].prn].health == 0 &&
//         Chan[ch].tow_sync == 1)

    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] : transmission time in sec
//  2nd to 5th term = time passed since arrival of TLM + n*sec
//  1st term makes sure that 1 s < transmit_time < 2 s

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

//      Chan[ch].epoch_nav & 0x1f;  // 0,...,49
//      Chan[ch].epoch_nav >> 8;    // 0,...,19


#if DBG_PRANGE
       if ( !fp_dbg_prange)
       {
         char tmpfile[64];
         sprintf( tmpfile, "debug/pseudorange-%03d-%06ld.dat", 
           gps_week, Ctrl->clock_tow);
         fp_dbg_prange = fopen( tmpfile, "w");
         if ( !fp_dbg_prange)
         {
            printf( "error opening debug file %s\n", tmpfile);
         }
       }

       if ( fp_dbg_prange)
         fprintf( fp_dbg_prange,
           "%d %d %d %d %d %d %d %ld %20.10lf\n",
            ch, Chan[ch].prn, Chan[ch].epoch, ms, chip, phase, n,
            Chan[ch].meas_bit_time,  transmit_time[n]);
#endif

#if 0
      phase_counter = ( Chan[ch].carr_dco_phase_nav * 2) & 0x7ff;
      phase_counter += (Chan[ch].carr_cycle_l_nav << 11);
      phase_counter += (Chan[ch].carr_cycle_h_nav << 27);
#endif


      if ( FpDmp)
      {
        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);
   
        write_msg_0x38( FpDmp, ch, transmit_time[n] * LIGHTVEL, phase_counter);

#if 0
        {
          gotoxy( 1, 22+ch);
          printf( "carr_dco  = %d,", NavData.carr_dco_phase[ch]);
          printf( "carr_cycl_lo = %d, ", NavData.carr_cycle_lo[ch]);
          printf( "carr_cycl_hi = %d, ", NavData.carr_cycle_hi[ch]);
          printf( "phasecounter = %ld, ", phase_counter);
          printf( "\n");
        }
#endif
      }

      n += 1;
    }  // --- if () ... ---    
  }

//  number of tracked satellites
  n_track = n - 1;

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

  transmit_time_max = -10.0;
  transmit_time_min =  10.0;
  for ( i=1; i<=n_track; i++)
  {
    if ( transmit_time[i] > transmit_time_max)
      transmit_time_max = transmit_time[i];
    if ( transmit_time[i] < transmit_time_min)
      transmit_time_min = transmit_time[i];
  }

// check if nav_tic hit a boundary region
  if ( transmit_time_max - transmit_time_min > 0.5)
  {
    for ( i=1; i<=n_track; i++)
    {
      if ( transmit_time[i] - transmit_time_min > 0.5)
        transmit_time[i] -= 1.0;
    }
  }

// default transmission time is 67 ms (30 000 km)
  transmit_time_max += 0.067;       

  for ( i=1; i<=n_track; i++)
  {

    track_sat[i]  = satpos_ephemeris( transmit_time[i], Chan[tr_ch[i]].prn);

    dm_gps_sat[i] = satpos_ephemeris( transmit_time[i]-0.5, Chan[tr_ch[i]].prn);
    dp_gps_sat[i] = satpos_ephemeris( transmit_time[i]+0.5, Chan[tr_ch[i]].prn);

//      if (out_debug)
//        fprintf( stream, "i=%d,az=%20.10lf,el=%20.10lf\n",
//          i, track_sat[i]. az, track_sat[i].el);

    t_cor[i] = track_sat[i].tb +
      tropo_iono( track_sat[i].az, track_sat[i].el, transmit_time[i]);

    dt[i] = transmit_time_max - (transmit_time[i] - t_cor[i]);

//       if (dt[i]>=1) printf("dt[%d]=%20.10lf\n",i,dt[i]);

//      if (out_debug)
//        fprintf( stream, "i=% d,tb= %20.10lf,tropo_iono= %20.10lf, dt= %20.10lf\n",
//          i,track_sat[i].tb,tropo_iono(track_sat[i].az,track_sat[i].el,transmit_time[i]),dt[i]);
//      if (out_debug)
//        fprintf( stream, "%20.10lf,%20.10lf,%20.10lf,%20.10lf\n",
//          track_sat[i].x,track_sat[i].y,track_sat[i].z,dt[i]);

      d_sat[i].x = dp_gps_sat[i].x - dm_gps_sat[i].x -
        track_sat[i].y*OMEGAE;

      d_sat[i].y = dp_gps_sat[i].y - dm_gps_sat[i].y +
        track_sat[i].x*OMEGAE;

      d_sat[i].z = dp_gps_sat[i].z - dm_gps_sat[i].z;

//      if ( out_kalman)
//        fprintf( out, "%20.10lf,%20.10lf,%20.10lf,%20.10lf\n",
//          track_sat[i].x,track_sat[i].y,track_sat[i].z,dt[i]);
//      if ( out_kalman)
//        fprintf( out,"%20.10lf,%20.10lf,%20.10lf,",
//          d_sat[i].x,d_sat[i].y,d_sat[i].z);

//      meas_dop[i] = (Chan[tr_ch[i]].doppler - 33010105l) * 42.574746268e-3;
    meas_dop[i] = (Chan[tr_ch[i]].car_frq - CARRIER_REF) * CARFRQRES;

//      if ( out_kalman)
//        fprintf( out, "%lf\n", meas_dop[i]);
  } // --- for (i=1; i<=n_track; i++) ---

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

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

//      if ( out_pos)
//        fprintf(stream,"lat= %lf,long= %lf,hae= %lf,gdop= %f,hdop= %f,vdop= %f\n",
//          rec_pos_llh.lat*RAD2DEG,rec_pos_llh.lon*RAD2DEG,rec_pos_llh.hae,gdop,hdop,vdop);

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

//      if ( out_kalman)
//        fprintf( out, "  %20.10lf,%20.10lf,%20.10lf,%20.10lf\n",
//          rp_ecef.x, rp_ecef.y, rp_ecef.z, cbias);
//      if ( out_kalman)
//        fprintf( out, "  %20.10lf,%20.10lf,%20.10lf,%10.10lf\n\n",
//          rpvt.xv,rpvt.yv,rpvt.zv,clock_error);

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

  return;
}
#endif

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