📄 gpstest_cpp.mht
字号:
From: =?gb2312?B?08kgV2luZG93cyBJbnRlcm5ldCBFeHBsb3JlciA3ILGjtOY=?=
Subject: GPSTEST.CPP
Date: Fri, 24 Aug 2007 18:57:40 +0800
MIME-Version: 1.0
Content-Type: text/html;
charset="gb2312"
Content-Transfer-Encoding: quoted-printable
Content-Location: http://home.earthlink.net/~cwkelley/gpstest.htm
X-MimeOLE: Produced By Microsoft MimeOLE V6.0.6000.16480
<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0 Transitional//EN">
<HTML><HEAD><TITLE>GPSTEST.CPP</TITLE>
<META http-equiv=3DContent-Type content=3D"text/html; charset=3Dgb2312">
<META content=3D"MSHTML 6.00.6000.16527" name=3DGENERATOR></HEAD>
<BODY><PRE>struct channel
{
int state;
long code_freq,carrier_freq,doppler,carrier_corr;
char message[1500],sat_use,prn,bit,frame_ready;
int offset,codes,n_freq,del_freq,snr;
int bit_no,t_count,ms_count,i_confirm;
int ms_epoch,n_frame,ch_time,i_count;
int con_thresh,n_thresh,sfid,missed,page5;
int i_track,q_track,i_prompt,q_prompt;
long sum,avg,old_theta,old_q_sum,th_rms;
long dfreq,dfreq1,dcarr1,dcarr;
long q_track_20,q_prompt_20,i_track_20,i_prompt_20;
long prompt_mag,track_mag;
long tr_bit_time,meas_bit_time,TOW;
unsigned int epoch_ms,epoch_bit,epoch_code_phase,epoch_code_DCO_phase;
unsigned int epoch_carrier_DCO_phase;
long epoch_carrier_cycle;
float CNo;
};
enum {off,acquisition,confirm,pull_in,track};
// 0 1 2 3 4
channel chan[12];
long carrier_ref=3D0x1f7b1b9L,code_ref=3D0x016ea4a8L;
int a_missed,test_cnt;
int =
prn_code[37]=3D{0,0x3f6,0x3ec,0x3d8,0x3b0,0x04b,0x096,0x2cb,0x196,0x32c,
0x3ba,0x374,0x1d0,0x3a0,0x340,0x280,0x100,0x113,0x226,
0x04c,0x098,0x130,0x260,0x267,0x338,0x270,0x0e0,0x1c0,
0x380,0x22b,0x056,0x0ac,0x158,0x058,0x18b,0x316,0x058};
//int GIC[8]=3D{0x2c4,0x10a,0x3e3,0x0f8,0x25f,0x1e7,0x2b5,0x10e};
unsigned test[16]=3D
{0x0001,0x0002,0x0004,0x0008,0x0010,0x0020,0x0040,0x0080,
0x0100,0x0200,0x0400,0x0800,0x1000,0x2000,0x4000,0x8000};
float clock_offset=3D0.0;
//int iii,out_debug;
#include "gp2021.cpp"
//#include "gpsfuncs.cpp"
inline long rss(long a,long b );
inline long fix_atan2(long y,long x);
long fix_sqrt(long x);
#define IRQLEVEL 0 // IRQ Line
void interrupt (*Old_Interrupt)(...); // Old IRQ0 interrupt handler
void interrupt GPS_Interrupt(...); // New IRQ0 interrupt handler
void Interrupt_Install(void);
void Interrupt_Remove(void);
void display(void);
long near_int(double);
void test_allocate(void);
//inline int bit_test(int,char);
void ch_acq(char);
void ch_confirm(char);
void ch_pull_in(char);
void ch_track(char);
inline int sign(long);
//inline int bsign(long);
//int xors(long);
int nav_tic,search_max_f,search_min_f=3D0,cold_prn=3D1;
long rms=3D312,acq_thresh=3D575,code_corr,time_on=3D0;
long =
pull_code_k=3D111,pull_code_d=3D7,pull_carr_k=3D-12,pull_carr_d=3D28;
long trk_code_k=3D55, trk_code_d=3D3, trk_carr_k=3D-9, trk_carr_d=3D21;
double speed,heading,nav_up=3D1.0;
int pull_in_time=3D1500,phase_test=3D500;
long d_freq=3D2349,d_tow,trk_div=3D19643;
int confirm_m=3D5,n_of_m_thresh=3D5,key,tic_count=3D0,hms_count=3D0;
int =
nav_count,min_flag,nav_flag,sec_flag,n_track,test_type,counter=3D0;
unsigned int interr_int=3D512;
void main()
{
// int i,j;//,indatam,imask;
char ch;
self_test();
io_config(0x301);
test_cnt=3D0x10;
test_control(test_cnt); // set up ch 0 as a test pattern generator
system_setup(0x00);
reset_cntl(0x1fff); // release reset
ch_status=3D0x1;
nav_tic=3Dnav_up*10;
interr_int=3D512;
code_corr=3D0.0;
for (ch=3D0;ch<=3D11;ch++) chan[ch].state=3Doff;
clrscr();
test_allocate();
Interrupt_Install();
//
do
{
if (kbhit()) key =3D getch();
else key =3D '\0';
if (sec_flag=3D=3D1)
{
// key=3D'x';
sec_flag=3D0;
for (ch=3D0;ch<=3D11;ch++)
{
if (chan[ch].state=3D=3Dtrack)
{
=
chan[ch].CNo=3D10.*log10(chan[ch].avg/1395.*chan[ch].avg/1395.*25.*1.7777=
+1.0);
if (chan[ch].CNo<25.0)
{
chan[ch].carrier_corr=3D0.0;
// calculate code correction
code_corr=3D0.0;
chan[ch].code_freq=3Dcode_ref+code_corr;
ch_code(ch,chan[ch].code_freq); // 1.023 MHz chipping =
rate
chan[ch].state=3Dacquisition;
chan[ch].t_count=3D0;
chan[ch].n_frame=3D0;
chan[ch].codes=3D0;
chan[ch].n_freq=3Dsearch_min_f;
chan[ch].del_freq=3D1;
=
chan[ch].carrier_freq=3Dcarrier_ref+chan[ch].carrier_corr+d_freq*chan[ch]=
.n_freq; // set carrier
ch_carrier(ch,chan[ch].carrier_freq); // select carrier
}
}
}
}
display();
} while (key !=3D 'x' && key !=3D 'X'); /* Stay in loop until =
'X' key is pressed. */
Interrupt_Remove();
}
void display(void)
{
char ch;
gotoxy(1,1);
printf(" \n");
printf( " ch prn state code carr bit ms cd_ph cd_dco car_dco =
carcycle\n");
for (ch=3D0;ch<=3D11;ch++)
{
printf(" %2d %2d %2d %6lx %6lx %4d %4d %4d %5d %5d %6ld\n",
ch,chan[ch].prn,chan[ch].state,chan[ch].code_freq,
chan[ch].carrier_freq,chan[ch].epoch_bit,
chan[ch].epoch_ms,chan[ch].epoch_code_phase,
chan[ch].epoch_code_DCO_phase,chan[ch].epoch_carrier_DCO_phase,
chan[ch].epoch_carrier_cycle);
}
}
/*
*************************************************************************=
*****
FUNCTION Interrupt_Install()
RETURNS None.
PARAMETERS None.
PURPOSE
This function replaces the current IRQ0 Interrupt service routine with
our own custom function. The old vector is stored in a global variable
and will be reinstalled at the end of program execution. IRQ0 is
enabled by altering the interrupt mask stored by the 8259 interrupt
handler.
*************************************************************************=
*****
*/
void Interrupt_Install()
{
unsigned char int_mask,i_high,i_low;
i_high=3Dinterr_int>>8;
i_low=3Dinterr_int&0xff;
Old_Interrupt =3D getvect(8 + IRQLEVEL);
disable();
setvect(8 + IRQLEVEL,GPS_Interrupt);
int_mask =3D inportb(0x21); // get hardware interrupt mask
int_mask =3D int_mask & ~(1 << IRQLEVEL);
outportb(0x21,int_mask); // send new mask to 8259
enable();
// modify the timer to divide by interr_int
outportb(0x43,0x34);
outportb(0x40,i_low);
outportb(0x40,i_high);
outportb(0x20,0x20); // Clear PIC
}
/*
*************************************************************************=
*****
FUNCTION Interrupt_Remove()
RETURNS None.
PARAMETERS None.
PURPOSE
This function removes the custom interrupt vector from the vector
table and restores the previous vector.
*************************************************************************=
*****
*/
void Interrupt_Remove()
{
unsigned char int_mask;
outportb(0x20,0x20); // clear interrupt and allow next one
int_mask =3D inportb(0x21); // get hardware interrupt mask
int_mask =3D int_mask | (1 << IRQLEVEL);
disable();
//outportb(0x21,int_mask); // send new mask to 8259
setvect(8 + IRQLEVEL,Old_Interrupt);
enable(); // allow hardware interrupts
outportb(0x20,0x20); // clear interrupt and allow next one
outportb(0x43,0x34); // reset clock
outportb(0x40,0xff);
outportb(0x40,0xff);
}
void test_allocate()
{
char ch;
search_max_f=3D20;
reset_cntl(0x1fff); // turn all the channels on
for (ch=3D0;ch<=3D11;ch++)
{
chan[ch].code_freq=3Dcode_ref;
ch_code(ch,chan[ch].code_freq); // 1.023 MHz chipping rate
ch_cntl(ch,prn_code[1]|0xa000); // select satellite track late
chan[ch].prn=3D1;
chan[ch].state=3Dacquisition;
chan[ch].codes=3D0;
chan[ch].n_freq=3Dsearch_min_f;
chan[ch].del_freq=3D1;
chan[ch].carrier_freq=3Dcarrier_ref; // set carrier
if (ch>0) chan[ch].carrier_freq+=3D15000;
ch_carrier(ch,chan[ch].carrier_freq); // select carrier
}
}
void interrupt GPS_Interrupt(...)
{
int astat,mstat,itemp;
unsigned int epoch,add;
char ch;
to_gps(0x80,0);
a_missed=3Dfrom_gps(0x83);
astat=3Dfrom_gps(0x82);
for (ch=3D0;ch<=3D11;ch++)
{
if (astat & test[ch])
{
add=3D(ch<<2)+0x84;
itemp=3Dfrom_gps(add);
chan[ch].i_track=3Ditemp;
add++;
itemp=3Dfrom_gps(add);
chan[ch].q_track=3Ditemp;
add++;
itemp=3Dfrom_gps(add);
chan[ch].i_prompt=3Ditemp;
add++;
itemp=3Dfrom_gps(add);
chan[ch].q_prompt=3Ditemp;
if (a_missed & test[ch])
{
chan[ch].missed++;
ch_accum_reset(ch);
}
}
}
mstat=3Dfrom_gps(0x81)& 0x2000;
if (mstat)
{
tic_count=3D(++tic_count)%10;
if (tic_count=3D=3D0) sec_flag=3D1;
hms_count=3D(++hms_count)%600;
if (hms_count=3D=3D0) min_flag=3D1;
nav_count=3D(nav_count+1)%nav_tic;
if (nav_count=3D=3D0) nav_flag=3D1;
test_cnt=3Dtest_cnt ^ 0x20;
test_control(test_cnt);
}
for (ch=3D0;ch<=3D11;ch++)
{
if (nav_count=3D=3D0)
{
epoch=3Dch_epoch(ch);
chan[ch].epoch_ms=3Depoch&0x1f;
chan[ch].epoch_bit=3Depoch>>8;
chan[ch].epoch_code_phase=3Dch_code_phase(ch);
chan[ch].epoch_code_DCO_phase=3Dch_code_DCO_phase(ch);
chan[ch].meas_bit_time=3Dchan[ch].tr_bit_time;
chan[ch].epoch_carrier_DCO_phase=3Dch_carrier_DCO_phase(ch);
chan[ch].epoch_carrier_cycle=3Dch_carrier_cycle(ch);
chan[ch].doppler=3Dchan[ch].carrier_freq;
}
}
for (ch=3D0;ch<=3D11;ch++)
{
if (astat & test[ch])
{
switch(chan[ch].state)
{
case acquisition:
ch_acq(ch);
break;
case confirm :
ch_confirm(ch);
break;
case pull_in :
ch_pull_in(ch);
break;
case track :
ch_track(ch);
break;
}
}
}
// reset the interrupt
outportb(0x20,0x20);
}
void ch_acq(char ch)
{
long prompt_mag,track_mag;
if (ch>0)
{
if (abs(chan[ch].n_freq)<=3Dsearch_max_f)// search frequencies
{
prompt_mag=3Drss(chan[ch].i_prompt,chan[ch].q_prompt);
track_mag =3Drss(chan[ch].i_track,chan[ch].q_track);
if ((track_mag > acq_thresh) && (prompt_mag > =
acq_thresh))
{
ch_code_slew(ch,2044); // slew back 1 chip
chan[ch].state=3Dconfirm; // confirm the signal
chan[ch].i_confirm=3D0;
chan[ch].n_thresh=3D0;
}
else
{
ch_code_slew(ch,2);
chan[ch].codes+=3D2;
}
if (chan[ch].codes=3D=3D2044)
{
chan[ch].n_freq+=3Dchan[ch].del_freq;
chan[ch].del_freq=3D-(chan[ch].del_freq+sign(chan[ch].del_freq));
=
chan[ch].carrier_freq=3Dcarrier_ref+chan[ch].carrier_corr+d_freq*chan[ch]=
.n_freq; // set carrier
ch_carrier(ch,chan[ch].carrier_freq); // select carrier
chan[ch].codes=3D0;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -