📄 gpstest_cpp.mht
字号:
}
else
{
chan[ch].n_freq=3Dsearch_min_f; // keep looping
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
chan[ch].codes=3D0;
}
}
}
void ch_confirm(char ch)
{
long prompt_mag,track_mag;
prompt_mag=3Drss(chan[ch].i_prompt,chan[ch].q_prompt);
track_mag =3Drss(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=3D=3Dconfirm_m)
{
if (chan[ch].n_thresh >=3D n_of_m_thresh)
{
chan[ch].state=3Dpull_in;
chan[ch].ch_time=3D0;
chan[ch].sum=3D0;
chan[ch].th_rms=3D0;
}
else chan[ch].state=3Dacquisition;
}
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 <=3D pull_in_time-1 )
{
prompt_mag=3Drss(chan[ch].i_prompt,chan[ch].q_prompt);
track_mag =3Drss(chan[ch].i_track,chan[ch].q_track);
chan[ch].sum+=3Dprompt_mag+track_mag;
// code tracking loop
if ( prompt_mag !=3D 0 || track_mag !=3D 0)
{
=
chan[ch].dfreq=3D((prompt_mag-track_mag)<<14)*pull_code_k/(prompt_m=
ag+track_mag);
ddf =3D(chan[ch].dfreq-chan[ch].dfreq1)*pull_code_d;
if ( chan[ch].ch_time > 2 )
{
chan[ch].code_freq =
=3D((chan[ch].dfreq+ddf)>>14)+chan[ch].code_freq;
ch_code(ch,chan[ch].code_freq);
}
}
chan[ch].dfreq1=3Dchan[ch].dfreq;
q_sum=3Dchan[ch].q_track+chan[ch].q_prompt;
i_sum=3Dchan[ch].i_track+chan[ch].i_prompt;
if (i_sum !=3D0 || q_sum !=3D0) theta=3Dfix_atan2(q_sum,-i_sum);
else theta=3Dchan[ch].old_theta;
theta_dot=3Dtheta-chan[ch].old_theta;
chan[ch].ms_count++;
chan[ch].old_theta=3Dtheta;
if ( theta> 0 ) theta_e=3Dtheta-25736;
else if ( theta<=3D 0) theta_e=3Dtheta+25736;
if (chan[ch].ch_time>pull_in_time-phase_test) =
chan[ch].th_rms+=3D(theta_e*theta_e)>>14;
if ( labs(theta_dot) < 32768L )
{
if (q_sum !=3D 0 || i_sum !=3D0)
{
wdot_gain=3Dchan[ch].ch_time/499;
wdot_gain*=3Dwdot_gain;
wdot_gain*=3Dwdot_gain;
chan[ch].dcarr=3Dpull_carr_k*(theta_dot*5/(1+wdot_gain)+theta_e);
ddcar=3D(chan[ch].dcarr-chan[ch].dcarr1)*pull_carr_d;
if ( chan[ch].ch_time > 5 )
{
=
chan[ch].carrier_freq=3D((chan[ch].dcarr+ddcar)>>14)+chan[ch].carri=
er_freq;
ch_carrier(ch,chan[ch].carrier_freq);
}
}
}
chan[ch].dcarr1=3Dchan[ch].dcarr;
chan[ch].old_q_sum=3Dq_sum;
chan[ch].ch_time++;
}
if (chan[ch].ch_time=3D=3Dpull_in_time)
{
chan[ch].ms_count=3Dchan[ch].ms_count%20;
chan[ch].avg=3Dchan[ch].sum/pull_in_time/2;
chan[ch].th_rms=3Dfix_sqrt(chan[ch].th_rms/phase_test);
if ( chan[ch].avg>14*rms/10 && chan[ch].th_rms<9830)
{
chan[ch].avg=3Dchan[ch].avg*20;
chan[ch].state=3Dtrack;
chan[ch].t_count=3D0;
chan[ch].sum=3D0;
chan[ch].q_track_20=3D0;
chan[ch].q_prompt_20=3D0;
chan[ch].i_track_20=3D0;
chan[ch].i_prompt_20=3D0;
}
else
{
chan[ch].state=3Dacquisition;
chan[ch].codes=3D0;
chan[ch].code_freq=3Dcode_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=3D(++chan[ch].ms_count)%20;
if (chan[ch].ms_count<=3D19)
{
chan[ch].q_track_20+=3Dchan[ch].q_track;
chan[ch].q_prompt_20+=3Dchan[ch].q_prompt;
chan[ch].i_track_20+=3Dchan[ch].i_track;
chan[ch].i_prompt_20+=3Dchan[ch].i_prompt;
// now the carrier loop
q_sum=3Dchan[ch].q_track+chan[ch].q_prompt;
i_sum=3Dchan[ch].i_track+chan[ch].i_prompt;
if ( q_sum !=3D 0 || i_sum !=3D 0)
{
=
chan[ch].dcarr=3D(i_sum<<14)*trk_carr_k*sign(q_sum)/rss(q_sum,i_sum=
);//check here
ddcar=3D(chan[ch].dcarr-chan[ch].dcarr1)*trk_carr_d;
=
chan[ch].carrier_freq=3D((chan[ch].dcarr+ddcar)>>14)+chan[ch].carri=
er_freq;
ch_carrier(ch,chan[ch].carrier_freq);
if ((a_missed & test[ch])=3D=3D0 && =
chan[ch].ms_count<10 &&
sign(q_sum)=3D=3D-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=3D0;
chan[ch].q_track_20=3Dchan[ch].q_track;
chan[ch].q_prompt_20=3Dchan[ch].q_prompt;
chan[ch].i_track_20=3Dchan[ch].i_track;
chan[ch].i_prompt_20=3Dchan[ch].i_prompt;
}
if ((a_missed & test[ch])=3D=3D0 && =
chan[ch].ms_count>=3D10 &&
sign(q_sum)=3D=3D-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=3D20;
}
}
chan[ch].old_q_sum=3Dq_sum;
chan[ch].dcarr1=3Dchan[ch].dcarr;
}
if (chan[ch].ms_count>=3D19)
{
chan[ch].tr_bit_time++;
chan[ch].prompt_mag=3Drss(chan[ch].i_prompt_20,chan[ch].q_prompt_20);
chan[ch].track_mag =3Drss(chan[ch].i_track_20,chan[ch].q_track_20);
chan[ch].sum+=3Dchan[ch].prompt_mag+chan[ch].track_mag;
// code tracking loop
if ( chan[ch].prompt_mag !=3D 0 || chan[ch].track_mag !=3D 0)
{
chan[ch].dfreq=3Dtrk_code_k*(chan[ch].prompt_mag-chan[ch].track_mag);
ddf =3D(chan[ch].dfreq-chan[ch].dfreq1)*trk_code_d;
chan[ch].code_freq =3D(chan[ch].dfreq+ddf)/trk_div+chan[ch].code_freq; =
// here was >>12
ch_code(ch,chan[ch].code_freq);
}
chan[ch].dfreq1=3Dchan[ch].dfreq;
// chan[ch].bit=3Dbsign(chan[ch].q_prompt_20+chan[ch].q_track_20);
// chan[ch].message[chan[ch].t_count]=3Dchan[ch].bit;
chan[ch].t_count++;
if (chan[ch].t_count%5=3D=3D0)
{
chan[ch].avg=3Dchan[ch].sum/10;
chan[ch].sum=3D0;
}
chan[ch].q_track_20=3D0;
chan[ch].q_prompt_20=3D0;
chan[ch].i_track_20=3D0;
chan[ch].i_prompt_20=3D0;
}
if (chan[ch].t_count=3D=3D1500)
{
chan[ch].n_frame++;
chan[ch].t_count=3D0;
}
}
//int xors(long pattern)
//{
// int count,i;
// count=3D0;
// pattern=3Dpattern>>6;
// for (i=3D0;i<=3D25;i++)
// {
// count+=3Dpattern & 0x1;
// pattern=3Dpattern>>1;
// }
// count=3Dcount%2;
// return(count);
//}
inline int sign(long data)
{
int result;
if ( data > 0 ) result=3D 1;
else if ( data =3D=3D 0 ) result=3D 0;
else if ( data < 0 ) result=3D-1;
return(result);
}
//inline int bsign(long data)
//{
// int result;
// if ( data > 0 ) result=3D 1;
// else result=3D 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=3Dinput+0.5;
else result=3Dinput-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=3Dlabs(a);
d=3Dlabs(b);
if (c=3D=3D0 && d=3D=3D0) result=3D0;
else
{
if (c>d) result=3D(d>>1)+c;
else result=3D(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=3D0;
xt=3Dx;
do
{
xt=3Dxt>>1;
i++;
} while (xt>0);
i=3D(i>>1)+1;
xt=3Dx>>i;
do
{
scr=3Dxt*xt;
scr=3Dx-scr;
scr=3Dscr>>1;
scr=3Dscr/xt;
xt=3Dscr+xt;
} while (scr!=3D0);
xt=3Dxt<<7;
return(xt);
}
/************************************************************************=
*******
FUNCTION fix_atan2(long y,long x)
RETURNS long integer
PARAMETERS=20
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 =3D 16384
based on the power series f-f^3*2/9
WRITTEN BY
Clifford Kelley
Fixed for y=3D=3Dx added special code for x=3D=3D0 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=3D=3D0) && (y=3D=3D0))
return(0); // invalid case
if (x>0 && x>=3Dlabs(y))
{
n=3D(y<<14)/x;
n3=3D((((n*n)>>14)*n)>>13)/9;
result=3Dn-n3;
}
else if (x<=3D0 && -x>=3Dlabs(y))
{
n=3D(y<<14)/x;
n3=3D((((n*n)>>14)*n)>>13)/9;
if ( y>0) result=3Dn-n3+SCALED_PI;
else if (y<=3D0) result=3Dn-n3-SCALED_PI;
}
else if (y>0 && y>labs(x))
{
n=3D(x<<14)/y;
n3=3D((((n*n)>>14)*n)>>13)/9;
result=3DSCALED_PI_ON_2-n+n3;
}
else if (y<0 && -y>labs(x))
{
n=3D(x<<14)/y;
n3=3D((((n*n)>>14)*n)>>13)/9;
result=3D-n+n3-SCALED_PI_ON_2;
}
return(result);
}
</PRE></BODY></HTML>
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -