📄 gpsnav.cpp
字号:
if (parity0 ==(TLM & 0x3f)) // is parity of TLM ok?
{
parity1=(xors(HOW & pb1)*32)+(xors(HOW & pb2)*16)+
(xors(HOW & pb3)*8)+(xors(HOW & pb4)*4)+
(xors(HOW & pb5)*2)+(xors(HOW & pb6));
if (parity1==(HOW & 0x3f)) // is parity of HOW ok?
{
// Get the correct sync , TLM & HOW
frm = int((HOW & 0x700)>>8);
if( frm > 0 && frm < 6) // valid sub frame #?
{
#if GPS_NAV_RAW_DATA == 2
ch_rawdata_file[idx]<<dec << TIC_count << "* "
<< ch[idx].TIC_mscount << "* "
<< ch[idx].subframe_i << " "
<< endl;
#endif
if( ch[idx].TIC_mscount % 6000 == 0) // this is a correct pream ?
{
ch[idx].subframe_i = frm;
ch[idx].TOW =(HOW & 0x3fffffffL)>>13;
ch[idx].TLM =(TLM>>8) & 0x3fff;
ch[idx].tr_time_HOW = (ch[idx].TOW-1)*6. + 1.2;
if( !newfrmflag ) // is it the first frame?
{
newfrmcount = 0; // reset newfrmcount to zero
newfrmflag = true; // set newfrmflag to true
}
// then copy msg
allch_navmsg[idx].newdata = true;
allch_navmsg[idx].prn = ch[idx].prn;
allch_navmsg[idx].index = ch[idx].msg_count;
memcpy(&allch_navmsg[idx].msg[0], &ch[idx].nav_msg[0], 302*sizeof( unsigned int ));
}
ch[idx].TIC_mscount = 0; //
}
}
}
}
}
// allocate prn to channel 1-6
// pn is the pointer pointed to prn numbers
void GPS_Nav::SetPrn(int* pn)
{
for(int i =0; i<CH_NUM; i++)
ch[i].prn = pn[i];
}
void GPS_Nav::init_gp2021(void)
{
int prn_idx;
// first package to initialize ,
// setup io_config, system_setup, enable 8 channels
// then set PRN code for these 8 channels
clear_all_reg(); // every time to write one packet, need to clear all values
reset_cntl(0x0); // 0x7f
io_config(0x0301); // 0xf0
test_control(0x0); // 0x7c
system_setup(0x0); // 0x7e
reset_cntl(ENABLE_6CH); // 0x7f
test_data(0xccdd); // for debug purpose
IoHandle->GetCntlData(0,(unsigned char*) &gp2021_wr_data); // buffer0
IoHandle->rd_wr_gp2021_all(0,false,NULL); // send to GP2021, buffer0, no returen
Sleep(50);
// second package to initialize
// setup the carrier and code DCO frequency
clear_all_reg(); // every time to write one packet, need to clear all values
for(char i=0; i<CH_NUM; i++){
ch_code( i,code_ref);
ch_carrier(i,carrier_ref+(long)(4698*ch[i].n_freq));
ch[i].carrier_freq = carrier_ref+(int)(4698*ch[i].n_freq);
prn_idx = ch[i].prn;
ch_cntl(i, prn_code[prn_idx] | 0xa000);
}
IoHandle->GetCntlData(0,(unsigned char*) &gp2021_wr_data); // buffer0
IoHandle->rd_wr_gp2021_all(0,false,NULL); // send to GP2021, buffer0, no returen
Sleep(50);
clear_all_reg();
}
/* end of GP2021 related function **/
/***************************************************************************/
//---------------------------------------------------------------------------
/* Routine Description: */
/* Init proc for class; */
/* Return non-zero if something wrong, otherwise return 0 */
/* Error code is defined in gbl_var.h */
/***************************************************************************/
int GPS_Nav::InitProc()
{
#if GPS_NAV_RAW_DATA > 0
char tmpbuf[80];
// open raw data file for channels
for(int i=0; i<CH_NUM; i++)
{
sprintf(tmpbuf, ".\\log\\ch%d_raw.dat", i);
ch_rawdata_file[i].open(tmpbuf, ios::out);
}
// now open accum_int and accum_mis file object
sprintf(tmpbuf, ".\\log\\ch_accum_int.dat");
sprintf(tmpbuf, ".\\log\\ch_accum_mis.dat");
ch_accum_mis_file.open(tmpbuf,ios::out);
#endif
for(int i=0; i<CH_NUM; i++){
ch[i].code = 0;
ch[i].n_confirm =0;
ch[i].state = ACQUISITION;
ch[i].n_freq = SEARCH_MIN_F;
ch[i].del_freq = 1;
ch[i].rawdata.ch_num = i;
ch[i].rawdata.prn_num = ch[i].prn;
ch[i].code_freq = code_ref;
ch[i].cd_phase0=ch[i].cd_phase1= 0.0;
for(int j=0; j<4; j++)
ch[i].theta[j] = 0.0;
data_count[i] = 0;
ch[i].msg_count = 0;
memset( ch[i].nav_msg, 0, sizeof( ch[i].nav_msg ));
}
#ifdef GPS_NAV_DBG
debugfile.open(".\\log\\gps_nav.log",ios::out);
if(!debugfile)
return(GPS_NAV_FILEOPENERR);
#endif
return (GPS_NAV_SUCCESS);
}
//---------------------------------------------------------------------------
/* Routine Description: */
/* print debug info for class; */
/***************************************************************************/
void GPS_Nav::PrintDbgInfo(char* info)
{
#ifdef GPS_NAV_DBG
if(debugfile)
debugfile << info << endl;
#endif
}
//---------------------------------------------------------------------------
/* Routine Description: */
/* get data from sampling thread and save to local data buffer */
/***************************************************************************/
void GPS_Nav::Get_oneframedata()
{
count ++;
IoHandle->CopyData((unsigned char*)&frm_4ms_6ch);
}
//---------------------------------------------------------------------------
/* Routine Description: */
/* process data for current frame */
/***************************************************************************/
int GPS_Nav::ProcessData()
{
char buf[80];
int tic;
tic = 0;
count1 = (count1+1)%16; // for debugging
if(frm_4ms_6ch.timestamp[0] != frm_4ms_6ch.timestamp[3])
{
sprintf(buf, "%4x * %4x * %4x *%4x @ %6d ",
(int)frm_4ms_6ch.timestamp[0],
(int)frm_4ms_6ch.timestamp[1],
(int)frm_4ms_6ch.timestamp[2],
(int)frm_4ms_6ch.timestamp[3],
count);
PrintDbgInfo(buf);
}
#if GPS_NAV_RAW_DATA > 0
/* here write into accum_mis_file ACCUM_STATUS_B every frame
ch_accum_mis_file.width(4);
ch_accum_mis_file << hex
<< frm_4ms_8ch.all_ch[0].accum_status_b << " "
<< frm_4ms_8ch.all_ch[1].accum_status_b << " "
<< frm_4ms_8ch.all_ch[2].accum_status_b << " "
<< frm_4ms_8ch.all_ch[3].accum_status_b << " "
<< (int)((frm_4ms_6ch.dummy[0] & 0xf0)>>4) <<" "
<< (int)count1<< " "
<< (int)(frm_4ms_6ch.dummy[1]) << " "
<< (int)(frm_4ms_6ch.dummy[2])
<< endl;
// here dummy[0] indicate the result of which command
// count1 indicate current command timestamp
// dummy[1] indicate the frm count of this result
// dummy[2] indicate the frm subcount of this result frame
// all these data are used for debugging
*/
/* here write into accum_mis_file ACCUM_STATUS_B only when timeout happens
*/
for(int j=0; j<4; j++)
if( frm_4ms_6ch.all_ch[j].accum_status_b & 0x3f )
{
ch_accum_mis_file.width(4);
ch_accum_mis_file << hex
<< frm_4ms_6ch.all_ch[0].accum_status_b << " "
<< frm_4ms_6ch.all_ch[1].accum_status_b << " "
<< frm_4ms_6ch.all_ch[2].accum_status_b << " "
<< frm_4ms_6ch.all_ch[3].accum_status_b << " "
<< endl;
break;
}
#endif
clear_all_reg();
// first get new accum data and calculate norm, phase,etc
get_newaccumdata();
// then process different channel according to new accum data
for(int ch_i=0; ch_i< CH_NUM; ch_i++)
process_channel(ch_i);
#if GPS_NAV_RAW_DATA == 3
for(int i=0; i< CH_NUM; i++)
{
// ch_rawdata_file[i]<< (int)ch[i].valid_data_num << " ";
if( ch[i].state >= PULLIN )
for( int k=0; k < ch[i].valid_data_num; k++)
{
ch_rawdata_file[i] << ch[i].prompt_iq[ k ][0] << " "
<< ch[i].prompt_iq[ k ][1] << " "
//<< ch[i].track_iq[ k ][0] << " "
//<< ch[i].track_iq[ k ][1] << " "
<< ch[i].theta[k]<< " "
//<< ch[i].sum_prompt_mag << " "
//<< ch[i].sum_delta_theta << " "
//<< ch[i].total_theta << " "//ca_phase[k] << " "
<< (ch[i].code_freq - code_ref)*0.08514949<< " "
<< (ch[i].carrier_freq - carrier_ref)* 0.04257475 <<" "
<< ch[i].total_theta<<" "
<< ch[i].ms_count << " "
<< endl;
}
}
#endif
for(int j=0; j<4; j++)
{
if( (frm_4ms_6ch.all_ch[j].accum_status_b) & 0x2000 ) // TIC happened here
{
allchtrtime.ch_count = 0;
if( (++newfrmcount % NAV_UPDATE_INT) == 2) // every 2sec to trigger gpsFunc thrd
{
if( newfrmflag ) // if new frame data is get, no matter how many channels
{
newmsg_rdy = true; // GpsFunc class should process msg & calculate PVT
newfrmflag = false; // reset newfrmflag to not valid
}
else
newmsg_rdy = false; // GpsFunc class only need to calculate PVT
int ch_cnt ;
for( int i=0; i<CH_NUM; i++)
{
if( ch[i].state == TRACKING && ch[i].SigNoise > 111500.
&& ch[i].tr_time_HOW > 0 && eph_valid[ ch[i].prn ] )
{
ch_cnt = allchtrtime.ch_count++;
allchtrtime.ch_transtime[ch_cnt].prn = ch[i].prn;
allchtrtime.ch_transtime[ch_cnt].tr_time =ch[i].tr_time;
allchtrtime.ch_transtime[ch_cnt].dopp_f = (ch[i].carrier_freq - carrier_ref)* 0.04257475;
}
}
SetEvent(gpsEvent); // inform GpsFunc class to process msg
}// end of if( (++newfrmcount % NAV_UPDATE_INT) == 2)
for(int i=0; i<CH_NUM; i++)
{
ch[i].rawdata.code_slew = frm_4ms_6ch.ch_cntl[i].code_slew;
ch[i].rawdata.code_phase = frm_4ms_6ch.ch_cntl[i].code_phase;
ch[i].rawdata.carrier_cycle_high = frm_4ms_6ch.ch_cntl[i].ca_cycle_high;
ch[i].rawdata.carrier_cycle_low = frm_4ms_6ch.ch_cntl[i].ca_cycle_low;
ch[i].rawdata.carrier_dco_phase = frm_4ms_6ch.ch_cntl[i].ca_dco_phase;
ch[i].rawdata.code_dco_phase = frm_4ms_6ch.ch_cntl[i].cd_dco_phase;
ch[i].rawdata.epoch = frm_4ms_6ch.ch_cntl[i].epoch;
ch[i].rawdata.i_track = frm_4ms_6ch.all_ch[j].ch_accm[i].I_track;
ch[i].rawdata.q_track = frm_4ms_6ch.all_ch[j].ch_accm[i].Q_track;
ch[i].rawdata.i_prompt= frm_4ms_6ch.all_ch[j].ch_accm[i].I_prompt;
ch[i].rawdata.q_prompt= frm_4ms_6ch.all_ch[j].ch_accm[i].Q_prompt;
ch[i].rawdata.freq_off = ch[i].n_freq*200;
tic = 1;
}
TIC_count++;
EnterCriticalSection(&GpsCriticalSection);
for( int i=0; i< allchtrtime.ch_count; i++)
{
if( allch_navresult[i].valid )
{
for( int k=0; k<CH_NUM; k++)
if( ch[k].prn == allch_navresult[i].prn)
{
ch[k].pseudorange = allch_navresult[i].pseudorange;
ch[k].ele = allch_navresult[i].ele;
ch[k].azi = allch_navresult[i].azi;
}
allch_navresult[i].valid = false;
} //end of if( allch_navresult[i].valid )
} // end of for( int i=0; i< allchtr
LeaveCriticalSection(&GpsCriticalSection);
}// end of if( (frm_4ms_6ch.all_ch[j].accum_status_b) & 0x2000 )
}
// copy control data to interface 's data buffer
gp2021_wr_data.flag = count1 << 4;
IoHandle->GetCntlData(IoHandle->get_free_pp_flag(),
(unsigned char*) &gp2021_wr_data);
return (tic);
}
//---------------------------------------------------------------------------
/* Routine Description: */
/* thread handling funtcions */
/***************************************************************************/
void GPS_Nav::CreateNavThrd()
{
clear_all_reg();
IoHandle->CreatSampleThrd();
navthrd = new NavThread(true, this);
thrdtermed = false;
navthrd->Resume();
}
// terminate sampling thread
void GPS_Nav::TerminateThrd()
{
if(navthrd)
navthrd->Terminate();
}
void GPS_Nav::ResumeThrd()
{
if(navthrd)
navthrd->Resume();
}
void GPS_Nav::SuspendThrd()
{
if(navthrd)
navthrd->Suspend();
}
// return the terminate status of sampling thread
bool GPS_Nav::NavThrdIsTermed()
{
return (thrdtermed);
}
// function to set the terminate status of sampling thread
void GPS_Nav::SetThrdStatus(bool stat)
{
thrdtermed = stat;
}
// to copy raw data for display use
void GPS_Nav::get_rawdata( PT_allch_rawdata_disp rawdata_pt )
{
for( int i=0; i<CH_NUM; i++)
rawdata_pt->allch_rawdata[i] = ch[i].rawdata;
}
void GPS_Nav::get_trkloop(PT_channel_disp res_pt)
{
for(int i=0; i<CH_NUM; i++)
{
res_pt[i].prn_num = ch[i].prn;
res_pt[i].state_info = ch[i].state;
res_pt[i].freq_off = (ch[i].carrier_freq - carrier_ref)* 0.04257475;
res_pt[i].SigNoise = ch[i].SigNoise;
res_pt[i].pseudorange = ch[i].pseudorange;
res_pt[i].frm_idx = ch[i].subframe_i;
res_pt[i].tow = ch[i].TOW;
res_pt[i].tlm = ch[i].TLM;
res_pt[i].ele = ch[i].ele;
res_pt[i].azi = ch[i].azi;
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -