// ************** debug *******************//  if ( ch==0)//  {//    printf( "trk: (%d) Ip=%4d, Qp=%4d, Id=%4d, Qd=%4d, r/lk:%.2f, d/lk:%.2f\n", //      chan[ch].t_count, Ip, Qp, Id, Qd, chan[ch].car_lock_det, chan[ch].cod_lock_det);//    getchar();  //  }  // ************** debug *******************  if ( chan[ch].ms_count >= 19)  {    chan[ch].tr_bit_time++;    chan[ch].prompt_mag = rss( chan[ch].i_prmt_sum, chan[ch].q_prmt_sum);    chan[ch].dith_mag   = rss( chan[ch].i_dith_sum, chan[ch].q_dith_sum);    chan[ch].sum        += chan[ch].prompt_mag + chan[ch].dith_mag;//    chan[ch].bit = ( chan[ch].i_prmt_sum + chan[ch].i_dith_sum > 0) ? 1 : 0;    chan[ch].bit = ( chan[ch].bitsign > 0) ? 1 : 0;// see if we can find the preamble    pream( ch, chan[ch].bit);       chan[ch].message[chan[ch].t_count] = chan[ch].bit;   // message[] : array of 0 & 1    chan[ch].t_count++;    write_to_file_navbit( ch);//    if ( chan[ch].t_count % 5 == 0)//    {//      chan[ch].avg = chan[ch].sum / 10;//      chan[ch].sum = 0;//    }////    chan[ch].q_dith_20 = 0;//    chan[ch].q_prmt_20 = 0;//    chan[ch].i_dith_20 = 0;//    chan[ch].i_prmt_20 = 0;  } // --- if ( chan[ch].ms_count >= 19) ---//  one data frame (1500 bits) completed  if ( chan[ch].t_count == 1500)  {    chan[ch].n_frame++;    chan[ch].t_count = 0;  }//  printf("chan[%d].t_count = %d\n", ch, chan[ch].t_count);//  getchar();  chan[ch].old_i_sum = i_sum;  chan[ch].ms_count = (++chan[ch].ms_count) % NOFMSECPERDATABIT;  return;}/*******************************************************************************FUNCTION pream()RETURNS  None.PARAMETERS  ch   channel number (0-11)  bit  the latest bit from the satellitePURPOSE  This function finds the preamble in the navigation messageWRITTEN BY  Clifford Kelley*******************************************************************************/void pream( char ch, char bit){  unsigned int parity, parity1;     // int replaced by unsigned int (GB-020217)  unsigned long TOWs, sum, pinv, dinv;  static unsigned long pream = 0x22c00000L,    // 0x8b << 22                       fifo0[13],                        fifo1[13];  static unsigned long pb1 = 0xbb1f3480L,                        pb2 = 0x5d8f9a40L,                        pb3 = 0xaec7cd00L,                       pb4 = 0x5763e680L,                        pb5 = 0x2bb1f340L,                        pb6 = 0xcb7a89c0L;////  fifo0 = 0x1,0x3,0x7,...,0x1fffffff,//  if ( fifo1[ch] & 0x20000000L)      fifo0[ch] = (fifo0[ch] << 1) + 0x1;  else     fifo0[ch] = fifo0[ch] << 1;      fifo1[ch] = (fifo1[ch] << 1) + bit;  sum = (pream^fifo0[ch]) & 0x3fc00000L;  if ( sum == 0 || sum == 0x3fc00000L)  {////  preamble bit pattern found://  bit 22-29 are 0x8b or 0x74 (= 0x8b^0xff)//    dinv = 0;    if ( sum == 0x3fc00000UL)       dinv = 0x3;    parity = (xors( fifo0[ch] & pb1) << 5) +              (xors( fifo0[ch] & pb2) << 4) +             (xors( fifo0[ch] & pb3) << 3) +              (xors( fifo0[ch] & pb4) << 2) +             (xors( fifo0[ch] & pb5) << 1) +                xors( fifo0[ch] & pb6);    pinv = 0;    if ( fifo1[ch] & 0x40000000L)       pinv = 0xffffffffL;    if ( parity == (fifo0[ch] & 0x3f))    {      parity1 = (xors( fifo1[ch] & pb1) << 5) +                 (xors( fifo1[ch] & pb2) << 4) +                (xors( fifo1[ch] & pb3) << 3) +                 (xors( fifo1[ch] & pb4) << 2) +                (xors( fifo1[ch] & pb5) << 1) +                  xors( fifo1[ch] & pb6);      if ( parity1 == (fifo1[ch] & 0x3f) && ((fifo1[ch] & 0x3) == dinv ))      {      //        printf("preamble found!\n");//        getchar();              chan[ch].sfid = (int)(((fifo1[ch]^pinv) & 0x700) >> 8);        if ( chan[ch].sfid == 1)     // synchronize on subframe 0 if TOW matches        {                            // clock within 300 seconds          TOWs = ((fifo1[ch]^pinv) & 0x3fffffffL) >> 13;          d_tow = clock_tow - TOWs * 6 + 5;          if ( labs( d_tow) < 300)          {            chan[ch].offset = chan[ch].t_count - 59;            ch_epoch_load( ch, (0x1f & ch_epoch_chk( ch)) | 0xa00);  ///// cwk            if ( chan[ch].offset < 0)               chan[ch].offset += 1500;            chan[ch].tr_bit_time = TOWs * 300 - 240;            chan[ch].TOW         = TOWs;            chan[ch].tow_sync    = 1;            thetime              = thetime - d_tow;            clock_tow            = TOWs * 6 - 5;          }        }// allow resync on other subframes if TOW matches clock to 3 seconds// this should improve the re-acquisition time        if ( chan[ch].sfid > 1 && chan[ch].sfid < 6)        {          TOWs = ((fifo1[ch]^pinv) & 0x3fffffffL) >> 13;          d_tow = clock_tow - TOWs * 6 + 5;          if ( labs( d_tow) < 3)          {            chan[ch].offset = chan[ch].t_count - 59 - (chan[ch].sfid - 1) * 300;            ch_epoch_load( ch, (0x1f & ch_epoch_chk( ch)) | 0xa00);  ///// cwk            if ( chan[ch].offset < 0)               chan[ch].offset += 1500;            chan[ch].tr_bit_time = TOWs * 300 - 240;            chan[ch].tow_sync    = 1;          }        }      }    }  }  // --- if ( sum == 0 || sum == 0x3fc00000L) ---//  a 1500 bit frame of data is ready to be read  if (( chan[ch].t_count-chan[ch].offset) % 1500 == 0)     chan[ch].frame_ready = 1;  return;  }/*******************************************************************************FUNCTION xors()RETURNS  IntegerPARAMETERS        pattern  a 32 bit dataPURPOSE        check the patternWRITTEN BY  Clifford Kelley*******************************************************************************/int xors( long pattern){  int count = 0, i;    pattern = pattern >> 6;  for ( i=0; i<=25; i++)  {    count += (int)( pattern & 0x1);    pattern = pattern >> 1;  }  count = count % 2;  return (count);}/*******************************************************************************FUNCTION sign()RETURNS  IntegerPARAMETERS LongPURPOSE       This function returns +1 when parameter is positive            0 when zero           -1 when negativeWRITTEN BY  Clifford Kelley*******************************************************************************/inline int sign( long data){  int result;    if      ( data > 0 )     result = 1;  else if ( data == 0 )     result = 0;  else if ( data < 0 )     result = -1;  return(result);}/*******************************************************************************FUNCTION bsign()RETURNS  IntegerPARAMETERS long integerPURPOSEWRITTEN BY  Clifford Kelley*******************************************************************************/inline int bsign( long data){  int result;    if ( data > 0 )     result = 1;  else      result = 0;  return (result);}/*******************************************************************************FUNCTION bit_test()RETURNS  None.PARAMETERS None.PURPOSEWRITTEN BY  Clifford Kelley*******************************************************************************/inline int  bit_test( int data, char bit_n){  return (data & test[bit_n]);}/*******************************************************************************FUNCTION near_int()RETURNS  longPARAMETERS doublePURPOSE  This function finds the nearest integer of a doubleWRITTEN BY  Clifford Kelley*******************************************************************************/long near_int( double input){  long result;    if ( input > 0.0 )    result = (long) (input + 0.5);  else     result = (long) (input - 0.5);  return result;}/*******************************************************************************FUNCTION velocity()RETURNS  None.PARAMETERS None.PURPOSE  To convert velocity from ecef to local level axesWRITTEN BY  Clifford Kelley*******************************************************************************/void velocity( void){  double  pt,pb,dxn,dyn,dzn,dn_mag,dxe,dye,de_mag,dxu,dyu,dzu;  double  north,east,up;  pt = (a*a-b*b) * sin( rp_llh.lat)*cos( rp_llh.lat);//  printf("vor sqrt() *** 3 ***\n");  pb = sqrt( a*a * cos( rp_llh.lat) * cos( rp_llh.lat) +              b*b * sin( rp_llh.lat) * sin( rp_llh.lat));//  printf("nach sqrt() *** 3 ***\n");  dxn =  a * a * pt / pow( pb, 3) * cos( rp_llh.lat) * cos( rp_llh.lon) -         (a * a / pb + rp_llh.hae) * sin( rp_llh.lat) * cos( rp_llh.lon);  dyn =  a * a * pt / pow( pb, 3) * cos( rp_llh.lat) * sin( rp_llh.lon) -         (a * a / pb + rp_llh.hae) * sin( rp_llh.lat) * sin( rp_llh.lon);  dzn =  b * b * pt / pow( pb, 3) * sin( rp_llh.lat) +         (b * b / pb + rp_llh.hae) * cos( rp_llh.lat);//  printf("vor sqrt() *** 4 ***\n");  dn_mag = sqrt( dxn * dxn + dyn * dyn + dzn * dzn);  // north magnitude//  printf("nach sqrt() *** 4 ***\n");  if ( dn_mag == 0.0)     north = 0.0;  else     north = (rpvt.xv*dxn+rpvt.yv*dyn+rpvt.zv*dzn)/dn_mag;  dxe = -(a*a/pb + rp_llh.hae) * cos( rp_llh.lat) * sin( rp_llh.lon);  dye =  (a*a/pb + rp_llh.hae) * cos( rp_llh.lat) * cos( rp_llh.lon);//  printf("vor sqrt() *** 5 ***\n");  de_mag = sqrt(dxe*dxe+dye*dye);             // east magnitude//  printf("nach sqrt() *** 5 ***\n");  if ( de_mag == 0.0)     east = 0.0;  else     east = (rpvt.xv * dxe + rpvt.yv * dye) / de_mag;  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//  printf("vor sqrt() *** 6 ***\n");  speed   = sqrt( north*north + east*east);//  printf("nach sqrt() *** 6 ***\n");  heading = atan2( east, north);  if ( out_vel && stream)     fprintf( stream, "vel north=%lf,east=%lf,up=%lf\n", north, east, up);  return;}/* ------------------------------------------------------------------FUNCTION calc_track_par()RETURNS  None.PARAMETERS None.  float BandWidth   : natural loop frequency  float DampRatio   : damping ratio  float Gain        : loop gain  float *par1   float *par2PURPOSE  Calculate tracking loop parameters from bandwidth and gain------------------------------------------------------------------ */static void calc_track_par( float BandWidth, float DampRatio, float Gain,  float *par1, float *par2){// typical values// loop gain//   CodGain  = 50e-3;    50 if chips are used//   CarGain  = 4*pi*100;  // band width//   CodBW    =  1;  //   CarBW    = 20; // damping ratio//   DampRatio = 0.707 : PLL is critically damped  float wT, c1, c2;  float tupd = UPDATETIME;// wT is natural frequency times update time  wT = 2.0 * BandWidth / (DampRatio + 1.0 / (4.0*DampRatio)) * tupd;  c1 = 1.0 / Gain * 8.0 * DampRatio*wT / (4.0 + 4.0 * DampRatio * wT + wT*wT);  c2 = 1.0 / Gain * 4.0 * wT*wT        / (4.0 + 4.0 * DampRatio * wT + wT*wT);  *par1 = (c1 + c2) / tupd;  *par2 = c1 / tupd;//  printf("calc_track_par: Gain = %e \n", Gain);//  printf("calc_track_par: wT = %e, BandWidth = %e \n", wT, BandWidth);//  printf("calc_track_par: c1 = %e, c2 = %e, tupd = %e\n", c1, c2, tupd);//  printf("calc_track_par: par1 = %e, par2 = %e, tupd = %e\n", *par1, *par2, tupd);//  getchar();  return;}static void init_tracking_par( void){//  tracking loops in tracking state  calc_track_par( bw_car_track, damp_car_track, gain_car_track,     &car_PLL_track_p1, &car_PLL_track_p2);  calc_track_par( bw_cod_track, damp_cod_track, gain_cod_track,     &cod_PLL_track_p1, &cod_PLL_track_p2);//  tracking loops in pul