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=0x1f7b1b9L,code_ref=0x016ea4a8L;
int a_missed,test_cnt;
int prn_code[37]={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]={0x2c4,0x10a,0x3e3,0x0f8,0x25f,0x1e7,0x2b5,0x10e};
unsigned test[16]=
{0x0001,0x0002,0x0004,0x0008,0x0010,0x0020,0x0040,0x0080,
0x0100,0x0200,0x0400,0x0800,0x1000,0x2000,0x4000,0x8000};
float clock_offset=0.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=0,cold_prn=1;
long rms=312,acq_thresh=575,code_corr,time_on=0;
long pull_code_k=111,pull_code_d=7,pull_carr_k=-12,pull_carr_d=28;
long trk_code_k=55, trk_code_d=3, trk_carr_k=-9, trk_carr_d=21;
double speed,heading,nav_up=1.0;
int pull_in_time=1500,phase_test=500;
long d_freq=2349,d_tow,trk_div=19643;
int confirm_m=5,n_of_m_thresh=5,key,tic_count=0,hms_count=0;
int nav_count,min_flag,nav_flag,sec_flag,n_track,test_type,counter=0;
unsigned int interr_int=512;
void main()
{
// int i,j;//,indatam,imask;
char ch;
self_test();
io_config(0x301);
test_cnt=0x10;
test_control(test_cnt); // set up ch 0 as a test pattern generator
system_setup(0x00);
reset_cntl(0x1fff); // release reset
ch_status=0x1;
nav_tic=nav_up*10;
interr_int=512;
code_corr=0.0;
for (ch=0;ch<=11;ch++) chan[ch].state=off;
clrscr();
test_allocate();
Interrupt_Install();
//
do
{
if (kbhit()) key = getch();
else key = '\0';
if (sec_flag==1)
{
// key='x';
sec_flag=0;
for (ch=0;ch<=11;ch++)
{
if (chan[ch].state==track)
{
chan[ch].CNo=10.*log10(chan[ch].avg/1395.*chan[ch].avg/1395.*25.*1.7777+1.0);
if (chan[ch].CNo<25.0)
{
chan[ch].carrier_corr=0.0;
// calculate code correction
code_corr=0.0;
chan[ch].code_freq=code_ref+code_corr;
ch_code(ch,chan[ch].code_freq); // 1.023 MHz chipping rate
chan[ch].state=acquisition;
chan[ch].t_count=0;
chan[ch].n_frame=0;
chan[ch].codes=0;
chan[ch].n_freq=search_min_f;
chan[ch].del_freq=1;
chan[ch].carrier_freq=carrier_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 != 'x' && key != '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=0;ch<=11;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=interr_int>>8;
i_low=interr_int&0xff;
Old_Interrupt = getvect(8 + IRQLEVEL);
disable();
setvect(8 + IRQLEVEL,GPS_Interrupt);
int_mask = inportb(0x21); // get hardware interrupt mask
int_mask = 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 = inportb(0x21); // get hardware interrupt mask
int_mask = 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=20;
reset_cntl(0x1fff); // turn all the channels on
for (ch=0;ch<=11;ch++)
{
chan[ch].code_freq=code_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=1;
chan[ch].state=acquisition;
chan[ch].codes=0;
chan[ch].n_freq=search_min_f;
chan[ch].del_freq=1;
chan[ch].carrier_freq=carrier_ref; // set carrier
if (ch>0) chan[ch].carrier_freq+=15000;
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=from_gps(0x83);
astat=from_gps(0x82);
for (ch=0;ch<=11;ch++)
{
if (astat & test[ch])
{
add=(ch<<2)+0x84;
itemp=from_gps(add);
chan[ch].i_track=itemp;
add++;
itemp=from_gps(add);
chan[ch].q_track=itemp;
add++;
itemp=from_gps(add);
chan[ch].i_prompt=itemp;
add++;
itemp=from_gps(add);
chan[ch].q_prompt=itemp;
if (a_missed & test[ch])
{
chan[ch].missed++;
ch_accum_reset(ch);
}
}
}
mstat=from_gps(0x81)& 0x2000;
if (mstat)
{
tic_count=(++tic_count)%10;
if (tic_count==0) sec_flag=1;
hms_count=(++hms_count)%600;
if (hms_count==0) min_flag=1;
nav_count=(nav_count+1)%nav_tic;
if (nav_count==0) nav_flag=1;
test_cnt=test_cnt ^ 0x20;
test_control(test_cnt);
}
for (ch=0;ch<=11;ch++)
{
if (nav_count==0)
{
epoch=ch_epoch(ch);
chan[ch].epoch_ms=epoch&0x1f;
chan[ch].epoch_bit=epoch>>8;
chan[ch].epoch_code_phase=ch_code_phase(ch);
chan[ch].epoch_code_DCO_phase=ch_code_DCO_phase(ch);
chan[ch].meas_bit_time=chan[ch].tr_bit_time;
chan[ch].epoch_carrier_DCO_phase=ch_carrier_DCO_phase(ch);
chan[ch].epoch_carrier_cycle=ch_carrier_cycle(ch);
chan[ch].doppler=chan[ch].carrier_freq;
}
}
for (ch=0;ch<=11;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)<=search_max_f)// search frequencies
{
prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
track_mag =rss(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=confirm; // confirm the signal
chan[ch].i_confirm=0;
chan[ch].n_thresh=0;
}
else
{
ch_code_slew(ch,2);
chan[ch].codes+=2;
}
if (chan[ch].codes==2044)
{
chan[ch].n_freq+=chan[ch].del_freq;
chan[ch].del_freq=-(chan[ch].del_freq+sign(chan[ch].del_freq));
chan[ch].carrier_freq=carrier_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=0;
}
}
else
{
chan[ch].n_freq=search_min_f; // keep looping
chan[ch].del_freq=1;
chan[ch].carrier_freq=carrier_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=0;
}
}
}
void ch_confirm(char ch)
{
long prompt_mag,track_mag;
prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
track_mag =rss(chan[ch].i_track,chan[ch].q_track);
if ((prompt_mag > acq_thresh) || (track_mag > acq_thresh)) chan[ch].n_thresh++;
if (chan[ch].i_confirm==confirm_m)
{
if (chan[ch].n_thresh >= n_of_m_thresh)
{
chan[ch].state=pull_in;
chan[ch].ch_time=0;
chan[ch].sum=0;
chan[ch].th_rms=0;
}
else chan[ch].state=acquisition;
}
chan[ch].i_confirm++;
}
void ch_pull_in(char ch)
{
long ddf,ddcar,theta_e,wdot_gain;
long q_sum,i_sum,theta,theta_dot;
long prompt_mag,track_mag;
if ( chan[ch].ch_time <= pull_in_time-1 )
{
prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
track_mag =rss(chan[ch].i_track,chan[ch].q_track);
chan[ch].sum+=prompt_mag+track_mag;
// code tracking loop
if ( prompt_mag != 0 || track_mag != 0)
{
chan[ch].dfreq=((prompt_mag-track_mag)<<14)*pull_code_k/(prompt_mag+track_mag);
ddf =(chan[ch].dfreq-chan[ch].dfreq1)*pull_code_d;
if ( chan[ch].ch_time > 2 )
{
chan[ch].code_freq =((chan[ch].dfreq+ddf)>>14)+chan[ch].code_freq;
ch_code(ch,chan[ch].code_freq);
}
}
chan[ch].dfreq1=chan[ch].dfreq;
q_sum=chan[ch].q_track+chan[ch].q_prompt;
i_sum=chan[ch].i_track+chan[ch].i_prompt;
if (i_sum !=0 || q_sum !=0) theta=fix_atan2(q_sum,-i_sum);
else theta=chan[ch].old_theta;
theta_dot=theta-chan[ch].old_theta;
chan[ch].ms_count++;
chan[ch].old_theta=theta;
if ( theta> 0 ) theta_e=theta-25736;
else if ( theta<= 0) theta_e=theta+25736;
if (chan[ch].ch_time>pull_in_time-phase_test) chan[ch].th_rms+=(theta_e*theta_e)>>14;
if ( labs(theta_dot) < 32768L )
{
if (q_sum != 0 || i_sum !=0)
{
wdot_gain=chan[ch].ch_time/499;
wdot_gain*=wdot_gain;
wdot_gain*=wdot_gain;
chan[ch].dcarr=pull_carr_k*(theta_dot*5/(1+wdot_gain)+theta_e);
ddcar=(chan[ch].dcarr-chan[ch].dcarr1)*pull_carr_d;
if ( chan[ch].ch_time > 5 )
{
chan[ch].carrier_freq=((chan[ch].dcarr+ddcar)>>14)+chan[ch].carrier_freq;
ch_carrier(ch,chan[ch].carrier_freq);
}
}
}
chan[ch].dcarr1=chan[ch].dcarr;
chan[ch].old_q_sum=q_sum;
chan[ch].ch_time++;
}
if (chan[ch].ch_time==pull_in_time)
{
chan[ch].ms_count=chan[ch].ms_count%20;
chan[ch].avg=chan[ch].sum/pull_in_time/2;
chan[ch].th_rms=fix_sqrt(chan[ch].th_rms/phase_test);
if ( chan[ch].avg>14*rms/10 && chan[ch].th_rms<9830)
{
chan[ch].avg=chan[ch].avg*20;
chan[ch].state=track;
chan[ch].t_count=0;
chan[ch].sum=0;
chan[ch].q_track_20=0;
chan[ch].q_prompt_20=0;
chan[ch].i_track_20=0;
chan[ch].i_prompt_20=0;
}
else
{
chan[ch].state=acquisition;
chan[ch].codes=0;
chan[ch].code_freq=code_ref+code_corr;
ch_code(ch,chan[ch].code_freq); // 1.023 MHz chipping rate
}
}
}
void ch_track(char ch)
{
long ddf,ddcar,q_sum,i_sum;
//
// 50 Hz tracking loop
//
chan[ch].ms_count=(++chan[ch].ms_count)%20;
if (chan[ch].ms_count<=19)
{
chan[ch].q_track_20+=chan[ch].q_track;
chan[ch].q_prompt_20+=chan[ch].q_prompt;
chan[ch].i_track_20+=chan[ch].i_track;
chan[ch].i_prompt_20+=chan[ch].i_prompt;
// now the carrier loop
q_sum=chan[ch].q_track+chan[ch].q_prompt;
i_sum=chan[ch].i_track+chan[ch].i_prompt;
if ( q_sum != 0 || i_sum != 0)
{
chan[ch].dcarr=(i_sum<<14)*trk_carr_k*sign(q_sum)/rss(q_sum,i_sum);//check here
ddcar=(chan[ch].dcarr-chan[ch].dcarr1)*trk_carr_d;
chan[ch].carrier_freq=((chan[ch].dcarr+ddcar)>>14)+chan[ch].carrier_freq;
ch_carrier(ch,chan[ch].carrier_freq);
if ((a_missed & test[ch])==0 && chan[ch].ms_count<10 &&
sign(q_sum)==-sign(chan[ch].old_q_sum) &&
labs(chan[ch].dcarr1)<-4096*trk_carr_k &&
labs( chan[ch].dcarr)<-4096*trk_carr_k)
{
chan[ch].ms_count=0;
chan[ch].q_track_20=chan[ch].q_track;
chan[ch].q_prompt_20=chan[ch].q_prompt;
chan[ch].i_track_20=chan[ch].i_track;
chan[ch].i_prompt_20=chan[ch].i_prompt;
}
if ((a_missed & test[ch])==0 && chan[ch].ms_count>=10 &&
sign(q_sum)==-sign(chan[ch].old_q_sum) &&
labs(chan[ch].dcarr1)<-4096*trk_carr_k &&
labs( chan[ch].dcarr)<-4096*trk_carr_k)
{
chan[ch].ms_count=20;
}
}
chan[ch].old_q_sum=q_sum;
chan[ch].dcarr1=chan[ch].dcarr;
}
if (chan[ch].ms_count>=19)
{
chan[ch].tr_bit_time++;
chan[ch].prompt_mag=rss(chan[ch].i_prompt_20,chan[ch].q_prompt_20);
chan[ch].track_mag =rss(chan[ch].i_track_20,chan[ch].q_track_20);
chan[ch].sum+=chan[ch].prompt_mag+chan[ch].track_mag;
// code tracking loop
if ( chan[ch].prompt_mag != 0 || chan[ch].track_mag != 0)
{
chan[ch].dfreq=trk_code_k*(chan[ch].prompt_mag-chan[ch].track_mag);
ddf =(chan[ch].dfreq-chan[ch].dfreq1)*trk_code_d;
chan[ch].code_freq =(chan[ch].dfreq+ddf)/trk_div+chan[ch].code_freq; // here was >>12
ch_code(ch,chan[ch].code_freq);
}
chan[ch].dfreq1=chan[ch].dfreq;
// chan[ch].bit=bsign(chan[ch].q_prompt_20+chan[ch].q_track_20);
// chan[ch].message[chan[ch].t_count]=chan[ch].bit;
chan[ch].t_count++;
if (chan[ch].t_count%5==0)
{
chan[ch].avg=chan[ch].sum/10;
chan[ch].sum=0;
}
chan[ch].q_track_20=0;
chan[ch].q_prompt_20=0;
chan[ch].i_track_20=0;
chan[ch].i_prompt_20=0;
}
if (chan[ch].t_count==1500)
{
chan[ch].n_frame++;
chan[ch].t_count=0;
}
}
//int xors(long pattern)
//{
// int count,i;
// count=0;
// pattern=pattern>>6;
// for (i=0;i<=25;i++)
// {
// count+=pattern & 0x1;
// pattern=pattern>>1;
// }
// count=count%2;
// return(count);
//}
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);
}
//inline int bsign(long data)
//{
// int result;
// if ( data > 0 ) result= 1;
// else result= 0;
// return(result);
//}
//inline int bit_test(int data,char bit_n)
//{
// return(data & test[bit_n]);
//}
long near_int(double input)
{
long result;
if (input >0.0 )result=input+0.5;
else result=input-0.5;
return(result);
}
/*******************************************************************************
FUNCTION rss(long a, long b)
RETURNS long integer
PARAMETERS
a long integer
b long integer
PURPOSE
This function finds the fixed point magnitude of a 2 dimensional vector
WRITTEN BY
Clifford Kelley
*******************************************************************************/
inline long rss(long a,long b )
{
long result,c,d;
c=labs(a);
d=labs(b);
if (c==0 && d==0) result=0;
else
{
if (c>d) result=(d>>1)+c;
else result=(c>>1)+d;
}
return (result);
}
/*******************************************************************************
FUNCTION fix_sqrt(long x)
RETURNS long integer
PARAMETERS
x long integer
PURPOSE
This function finds the fixed point square root of a long integer
WRITTEN BY
Clifford Kelley
*******************************************************************************/
long fix_sqrt(long x)
{
long xt,scr;
int i;
i=0;
xt=x;
do
{
xt=xt>>1;
i++;
} while (xt>0);
i=(i>>1)+1;
xt=x>>i;
do
{
scr=xt*xt;
scr=x-scr;
scr=scr>>1;
scr=scr/xt;
xt=scr+xt;
} while (scr!=0);
xt=xt<<7;
return(xt);
}
/*******************************************************************************
FUNCTION fix_atan2(long y,long x)
RETURNS long integer
PARAMETERS
x long in-phase fixed point value
y long 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 suggested by Joel Barnes, UNSW
*******************************************************************************/
#define SCALED_PI_ON_2 25736L
#define SCALED_PI 51472L
inline long fix_atan2(long y,long x)
{
long result,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;
result=n-n3;
}
else if (x<=0 && -x>=labs(y))
{
n=(y<<14)/x;
n3=((((n*n)>>14)*n)>>13)/9;
if ( y>0) result=n-n3+SCALED_PI;
else if (y<=0) result=n-n3-SCALED_PI;
}
else if (y>0 && y>labs(x))
{
n=(x<<14)/y;
n3=((((n*n)>>14)*n)>>13)/9;
result=SCALED_PI_ON_2-n+n3;
}
else if (y<0 && -y>labs(x))
{
n=(x<<14)/y;
n3=((((n*n)>>14)*n)>>13)/9;
result=-n+n3-SCALED_PI_ON_2;
}
return(result);
}