gpsnav.cpp

基于USB接口的GPS应用程序

  {
   ch[idx].state = ACQUISITION;
   ch[idx].mn_confirm =0;
   ch[idx].n_confirm =0;
  }
  else
  {
   ch[idx].state = PULLIN;
   ch[idx].mn_confirm =0;
   ch[idx].n_confirm =0;
   ch[idx].cd_phase0 = ch[idx].cd_phase1; // prepare for PULLIN

   ch[idx].sum_prompt_mag = 0.;
   ch[idx].sum_delta_theta = 0.;
   ch[idx].pullin_time_idx = 0;
   ch[idx].pull_time_count =0;
   ch[idx].ms_count = 0;
   ch[idx].ms_set = 0;
   ch[idx].old_theta = ch[idx].theta[ ch[idx].valid_data_num -1 ];
   ch[idx].old_prompt_q = ch[idx].prompt_iq[ch[idx].valid_data_num -1][1];
      
   for(int l=0; l< PULLIN_CHECK_TIME; l++)
   {
    ch[idx].delta_theta[l] = 0.;
    ch[idx].pullin_prompt_mag[l] =0.;
   }
  }
 } // end of if(ch[idx].n_confirm == 7)
}

void  GPS_Nav::ch_pullin(int idx)
{
 double cd_f, ca_f, ca_theta,ca_t, theta_err, tmp_theta;
 bool sign_changed, mag_bigenough;
 int t_idx;
 assert( idx < CH_NUM && idx >=0);

 // init total_theta to 0
 ch[idx].total_theta = 0;

 // calculate sum_theta & sum_prompt_mag for data 1 to valid_data_num
 for( int l = 0; l< ch[idx].valid_data_num; l++)
 {
  t_idx = ch[idx].pullin_time_idx;

  theta_err = fabs(ch[idx].theta[l] - sign(ch[idx].theta[l])*PI/2);

  ch[idx].sum_delta_theta +=(theta_err - ch[idx].delta_theta[t_idx] );
  ch[idx].delta_theta[ t_idx ] = theta_err;

  ch[idx].sum_prompt_mag += ( ch[idx].prompt_mag[l] -
                            ch[idx].pullin_prompt_mag[t_idx]);
  ch[idx].pullin_prompt_mag[t_idx] = ch[idx].prompt_mag[l];

  ch[idx].pullin_time_idx = (t_idx + 1)%PULLIN_CHECK_TIME;

  // calculate total_theta from 1 to valid_data_num
  if( l == 0)
   ca_t =  ch[idx].theta[l] - ch[idx].old_theta;
  else
   ca_t = ch[idx].theta[l] - ch[idx].theta[l-1];

  if( fabs(ca_t) > PI )
   ca_theta = ca_t > 0? ca_t - 2*PI : ca_t + 2*PI;
  else
   ca_theta = ca_t;
  ch[idx].total_theta += ca_theta;

  // check ms_count and ms_set
  ch[idx].ms_count = (++ch[idx].ms_count) % 20;

  sign_changed = mag_bigenough = false;
  if( l == 0)
  {
   sign_changed = (sign(ch[idx].prompt_iq[l][1]) == -sign(ch[idx].old_prompt_q));
   mag_bigenough= (fabs( ch[idx].prompt_iq[l][1]) > 800)
                &&(fabs( ch[idx].old_prompt_q) > 800);
   tmp_theta = ch[idx].old_theta;
  }
  else
  {
   sign_changed = (sign(ch[idx].prompt_iq[l][1]) == -sign(ch[idx].prompt_iq[l-1][1]));
   mag_bigenough= (fabs( ch[idx].prompt_iq[l][1]) > 800)
                &&(fabs( ch[idx].prompt_iq[l-1][1]) > 800);
   tmp_theta = ch[idx].theta[l-1];
  }

  if( sign_changed && mag_bigenough
     && fabs( fabs(ch[idx].theta[l]) - PI/2 ) < PI/8
     && fabs( fabs(tmp_theta) - PI/2 ) < PI/8 )
  {
   ch[idx].ms_count =0;
   ch[idx].ms_set = 1;
  }
 }   // end of for( int l = 0;

 ch[idx].old_prompt_q = ch[idx].prompt_iq[ch[idx].valid_data_num -1][1];

 // code pullin loop
 cd_f = (ch[idx].cd_phase1 - ch[idx].cd_phase0)* pull_code_d;
 ch[idx].code_freq = (int)(ch[idx].cd_phase1 + cd_f) + ch[idx].code_freq;
 ch_code( idx,ch[idx].code_freq);
 ch[idx].cd_phase0 = ch[idx].cd_phase1;

 // carrier pullin loop
 ca_f = ch[idx].total_theta * pull_carrier_d;
 ch[idx].carrier_freq = (int)(pull_carrier_k*ch[idx].theta[ ch[idx].valid_data_num -1 ] + ca_f) + ch[idx].carrier_freq;
 ch_carrier(idx,ch[idx].carrier_freq);
 ch[idx].old_theta = ch[idx].theta[ ch[idx].valid_data_num - 1 ];

 ch[idx].pull_time_count += ch[idx].valid_data_num;


 // after 2000ms check if we can jump to TRACK
 if( ch[idx].pull_time_count > 500)
 {
  if( ch[idx].sum_prompt_mag > 1e5 &&
      ch[idx].sum_delta_theta < 80 &&
      ch[idx].ms_set )
  {
   int valid_datanum = ch[idx].valid_data_num;
   double i_sum, q_sum;
   i_sum = ch[idx].prompt_iq[valid_datanum-1][0];
   q_sum = ch[idx].prompt_iq[valid_datanum-1][1];

   ch[idx].state = TRACKING;
   ch[idx].tlm_tst = 0x0;
   ch[idx].how_tst = 0x0;
   ch[idx].subframe_i = -1;
   ch[idx].TIC_mscount = 0;

   ch[idx].q_prompt_20 = 0.;
   ch[idx].i_prompt_20 = 0.;
   ch[idx].q_track_20 = 0.;
   ch[idx].i_track_20 = 0.;

   ch[idx].sn_mag_count = 0;
   ch[idx].sn_mag_avg =0.;
   ch[idx].SigNoise = 0.0;
   ch[idx].pseudorange = 0.0;
   ch[idx].ele = -1.;
   ch[idx].azi = 0.;
   ch[idx].tr_time_HOW = -1.;

   if( ch[idx].prompt_mag[valid_datanum-1] != 0)
    ch[idx].ca_old_phase = i_sum*sign(q_sum)/ch[idx].prompt_mag[valid_datanum-1];
   ch[idx].trk_cd_freq = ch[idx].cd_phase1;
  }
 }
 // after 10000ms if we are still in PULLIN, jump back to ACQUSITION
 if( ch[idx].pull_time_count > 10000)
 {
   ch[idx].state = ACQUISITION;
   ch[idx].code =0;
   ch[idx].n_freq -=1;  // slew back 200 Hz
   ch[idx].carrier_freq = carrier_ref+(int)(4698*ch[idx].n_freq);
 }

}
void  GPS_Nav::ch_track(int idx)
{
 double trk_ca_f,total_p, tr_time_frac;
 bool  tic_happen, tic_happen_how_rdy;
 int t_tr=0;
 int valid_datanum = ch[idx].valid_data_num;
 assert( idx < CH_NUM && idx >=0);

 ch[idx].tic_time = 0.;
 tic_happen = false;
 tic_happen_how_rdy = false;

 if( ch[idx].tr_time_HOW > 0. )  // this channel has got HOW
 {
                                // then it's time to calculate tr_time
  for( int j=0; j<4; j++)
  {
   bool new_data = (frm_4ms_6ch.all_ch[j].accum_status_a) & (ch_ready_mask[idx+CH_OFFSET]);
   if( new_data ) // new accum data for channel idx
    t_tr++;
   if( (frm_4ms_6ch.all_ch[j].accum_status_b) & 0x2000 ) // TIC happened here
    {
     tic_happen = true;
     if( new_data )
      ch[idx].tic_time = t_tr;
     else
      ch[idx].tic_time = t_tr + 0.5;
    }

  } // end of for( int j

  if( tic_happen )
  {
     if( ch[idx].tic_time == 0.5 )
     {
      int  ms_diff, tic_ms_mod20, compensate_ms;


      ms_diff = (frm_4ms_6ch.ch_cntl[idx].epoch) & 0xff;
      ms_diff -= ch[idx].epoch_ref;
      
      if(ms_diff < 0) ms_diff += 20;
      tic_ms_mod20 = ch[idx].TIC_mscount %20;
      if( fabs( ms_diff - tic_ms_mod20) < 10)
       compensate_ms = ms_diff - tic_ms_mod20;
      else
      {
       if( ms_diff > tic_ms_mod20)
        compensate_ms = -(tic_ms_mod20 - ms_diff + 20);
       else
        compensate_ms = (ms_diff - tic_ms_mod20 + 20);
      }

      ch[idx].tr_time = ch[idx].tr_time_HOW +(ch[idx].TIC_mscount+compensate_ms)*0.001+
                        (frm_4ms_6ch.ch_cntl[idx].code_phase)/2.046e6+
                        (frm_4ms_6ch.ch_cntl[idx].cd_dco_phase)/2.046e6/1024.;
                        
#if GPS_NAV_RAW_DATA == 1
     ch_rawdata_file[idx].precision(16);
     ch_rawdata_file[idx] <<dec<< ch[idx].TIC_mscount%20 << " "
                          << ch[idx].tr_time << " "
                          << ms_diff<< " "
                          << compensate_ms << " "
                          << int(ch[idx].epoch_ref)
                          << endl;
#endif
     }
     else
     {
      tr_time_frac = ch[idx].tic_time - 1.;  // possible value: 1,1.5,2,2.5,3,3.5,4
      tic_happen_how_rdy = true;
     }
    tic_happen = false;
  } // end of if( tic_happen )
 }



 total_p=0.;


 for(int j=0; j< valid_datanum; j++)
 {
  ch[idx].q_prompt_20 += ch[idx].prompt_iq[j][1];
  ch[idx].i_prompt_20 += ch[idx].prompt_iq[j][0];
  ch[idx].q_track_20 += ch[idx].track_iq[j][1];
  ch[idx].i_track_20 += ch[idx].track_iq[j][0];
  ch[idx].ms_count = (++ch[idx].ms_count)%20;
  ch[idx].TIC_mscount++;

  if( tic_happen_how_rdy )
  {
   if( floor(tr_time_frac) == j )
   {
    int  ms_diff, tic_ms_mod20, compensate_ms;

    ms_diff = (frm_4ms_6ch.ch_cntl[idx].epoch) & 0xff;
    ms_diff -= ch[idx].epoch_ref;  // minus epoch_check value
    if(ms_diff < 0) ms_diff += 20; // get the value of [0 -- 19]
    tic_ms_mod20 = ch[idx].TIC_mscount %20;
    if( fabs( ms_diff - tic_ms_mod20) < 10)
     compensate_ms = ms_diff - tic_ms_mod20;
    else  // ms_diff and tic_ms_mod20 should be very close,
     {
      if( ms_diff > tic_ms_mod20)
       compensate_ms = -(tic_ms_mod20 - ms_diff + 20);
      else
       compensate_ms = (ms_diff - tic_ms_mod20 + 20);
     }


    ch[idx].tr_time = ch[idx].tr_time_HOW+(ch[idx].TIC_mscount+compensate_ms)*0.001+
                     (frm_4ms_6ch.ch_cntl[idx].code_phase)/2.046e6 +
                     (frm_4ms_6ch.ch_cntl[idx].cd_dco_phase)/2.046e6L/1024.;
                     
#if GPS_NAV_RAW_DATA == 1
    ch_rawdata_file[idx].precision(16);
    ch_rawdata_file[idx] <<dec<< (ch[idx].TIC_mscount%20)<< " "
                         <<ch[idx].tr_time << " "
                         << ms_diff << " "
                         << compensate_ms << " "
                         << int(ch[idx].epoch_ref)
                         << endl;
#endif

    tic_happen_how_rdy = false;
   }
  }


  if( j>=1 ) // check and reset ms_count
  {
   if( ch[idx].prompt_iq[j][1]*ch[idx].prompt_iq[j-1][1] < -250000.
             && fabs( ch[idx].prompt_iq[j][1]) > 500.
             && ch[idx].prompt_iq[j-1][1] > 500.)
   {
    if( ch[idx].ms_count != 0)
     ch[idx].ms_count = 0;

    if( epoch_chk )
    {
     ch[idx].epoch_ref = ch[idx].epochchk[j] & 0xff;
    }
   }
  }
  if( ch[idx].ms_count == 19) // it's time to set code frequency
  {
   double prompt_mag, track_mag,cd_df;
   prompt_mag = sqrt(pow((float)(ch[idx].i_prompt_20),2)+pow((float)(ch[idx].q_prompt_20),2));
   track_mag = sqrt(pow((float)(ch[idx].i_track_20),2)+pow((float)(ch[idx].q_track_20),2));
   cd_df = (prompt_mag - track_mag)*200./(prompt_mag+track_mag)*trk_code_k;
   ch[idx].trk_cd_freq += cd_df;
   ch[idx].code_freq = (int)((ch[idx].trk_cd_freq/trk_code_d + cd_df)/256. -
                        (carrier_ref-ch[idx].carrier_freq)/cc_scale)
                        + code_ref;
   ch_code( idx,ch[idx].code_freq);

   ch[idx].sn_mag_avg += (prompt_mag + track_mag)/2.;

   // save TLM and HOW and try to find sync
   ch[idx].msg_count = (++ ch[idx].msg_count)%302;  // save msg to nav_msg
   ch[idx].nav_msg[ ch[idx].msg_count ] = (ch[idx].tlm_tst & 0x20000000);

   if( ch[idx].how_tst & 0x20000000 )  // check 30th bit of how_tst
    ch[idx].tlm_tst = ch[idx].tlm_tst*2 + 1;
   else
    ch[idx].tlm_tst = ch[idx].tlm_tst*2;
   // q_prompt_20 is the data bit , pipe into how

   ch[idx].how_tst = ch[idx].how_tst *2 + bit_sign(ch[idx].q_prompt_20);

   // try to find prem and calc parity
   search_pream( idx );

   if(++ch[idx].sn_mag_count == 15) // it's time to calculate SigNoise(every 15  data bits )
   {
    ch[idx].SigNoise = ch[idx].sn_mag_avg;
    if( ch[idx].SigNoise < 55000. ) // S/N less than 25dB
    {
     ch[idx].state = ACQUISITION;
     ch[idx].code =0;
     ch[idx].n_freq = (int)((ch[idx].carrier_freq - carrier_ref)/4698) -2 ;  // slew back 400 Hz
     ch[idx].carrier_freq = carrier_ref+(int)(4698*ch[idx].n_freq);
     ch[idx].subframe_i = -1;
     ch[idx].tlm_tst = 0x0;
     ch[idx].how_tst = 0x0;
     ch[idx].SigNoise = 0.0;
     ch[idx].pseudorange = 0.0;
     ch[idx].ele = 0.;
     ch[idx].azi = 0.;

    }

    ch[idx].sn_mag_count = 0;
    ch[idx].sn_mag_avg = 0.;
   }
   ch[idx].q_prompt_20 = 0.;
   ch[idx].i_prompt_20 = 0.;
   ch[idx].q_track_20 = 0.;
   ch[idx].i_track_20 = 0.;
  }

  total_p  += ch[idx].ca_phase[j];
 }

 // carrier tracking loop
 trk_ca_f = (ch[idx].ca_phase[valid_datanum-1] - ch[idx].ca_old_phase) * trk_carrier_d;
 ch[idx].carrier_freq = (int)(trk_carrier_k*total_p + trk_ca_f) + ch[idx].carrier_freq;
 ch_carrier(idx,ch[idx].carrier_freq);
 ch[idx].ca_old_phase = ch[idx].ca_phase[valid_datanum - 1];

}


// check if we get the sync
void GPS_Nav::search_pream( int idx)
{
 unsigned int TOWs,HOW,TLM,TLMs,parity0,parity1,frm;
 char d29,d30;
 d29 = (ch[idx].tlm_tst&0x80000000)>>31;
 d30 = (ch[idx].tlm_tst&0x40000000)>>30;

//#if GPS_NAV_RAW_DATA == 2
// ch_rawdata_file[idx]<< hex << ch[idx].tlm_tst << " "
//                            << ch[idx].how_tst << " "
//                            << endl;
//#endif


  if (ch[idx].tlm_tst & 0x40000000)  //
  {                                  //
   TLM = ch[idx].tlm_tst^0x3fffffc0; // then data bit reverse
  }
  else
  {
   TLM = ch[idx].tlm_tst;            // Otherwise, data bit unchanged
  }

  if (ch[idx].how_tst & 0x40000000)  //
  {                                  //  then data bit reverse
   HOW = ch[idx].how_tst^0x3fffffc0;
  }
  else
  {
                                    // Otherwise, data bit unchanged
   HOW = ch[idx].how_tst;
  }
                                           // then check parity

  if (((NAG_PREAM^TLM)& 0x3fc00000) == 0x0) // preamble pattern found?
  {
            parity0=(xors(TLM & pb1)*32)+(xors(TLM & pb2)*16)+
			  (xors(TLM & pb3)*8)+(xors(TLM & pb4)*4)+
			  (xors(TLM & pb5)*2)+(xors(TLM & pb6));