As with all files, rename the file, adding a preceding l (the letter after k and before m)
#include "lincludes.h"
/****************************************************************************
* Function: void *TDisplay(void *arg)
*
* Revision: 6.05
*
* Last Update: 05/08/97
*
* This is the Task for sending data out through
the designated GP2021 DUART
* port for display on the PC monitor. All
output targeted for the PC monitor
* should be sent through this task to avoid
corruption of the communications
* buffers and pointers.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void *TDisplay(void *arg)
{
unsigned long start_tic;
/* Enable floating point operations */
pthread_setfp_np (pthread_self(), 1);
while(1)
{
pthread_wait_np();
CurrentTIC(&start_tic);
/* The Data output mode can now be configured
depending on the state of the
DISCIP1 input on the GP2021 correlator (on
pin 80). The standard
GPSArchitect platform does NOT hold the DISCIP1
input to a set logic level
and so the following code, which sets the
mode depending on the logic level
of DISCIP1, has been commented out, and the
default mode set to WINMON. */
if(SendData){
ContinuousData((int)arg);
Sentence_F03((int)arg);
Sentence_F04((int)arg);
Sentence_F05();
Sentence_F08();
}
// Suspend(10,start_tic); /* suspend for
1 second */
}
}
/****************************************************************************
* Function: void ContinuousData(int arg)
*
* Revision: 6.03
*
* Last Update: 20/03/97
*
* Transmit the 'Continuous Data' F00 sentence.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void ContinuousData(int arg)
{
// char output_buffer[1920];
int time_type,track_type,system_status;
gpstimestruc g;
timestruc t;
double gcdist,gcbrg,horizerr,verterr;
navstatestruc N;
filterednavstatestruc FN;
clockmodelstruc CLK;
typedef struct
{
//float deltat;
double Latitude;
double Longitude;
double Height;
double Speed;
double Heading;
double RateOfClimb;
double GDop;
double PDop;
double VDop;
int NoOfSVsTracked;
int NavMode;
int TrackMode;
double HorizontalError;
double VerticalError;
double DopplerOffset;
int Day;
int Month;
int Year;
int Hour;
int Minute;
int Second;
int TimeType;
int SystemStatus;
char ConfigCode[6];
} F00Sentence;
F00Sentence cd;
rec_mutex_lock ((int)arg);
N = CurNavState; /* Get a local copy of the
current navigation state */
FN = FilteredCurNavState;
rec_mutex_unlock ((int)arg);
rec_mutex_lock ((int)arg);
CLK = CurClkModel; /* Get a local copy of
the current clock model */
rec_mutex_unlock ((int)arg);
if(CLK.RCOtic==0)
system_status = 0;
else if(N.navmode==FIX_NO)
system_status = 1;
else if(N.navmode==FIX_2D)
system_status = 2;
else
system_status = 3;
GPSTime(&g,(int)arg);
GpsTimeToUTCDate(&g,&t,(int)arg);
GreatCircle((double)reflat,(double)reflon,FN.llhpos.lat,FN.llhpos.lon,
&gcdist,&gcbrg);
horizerr = gcdist*REARTH;
verterr = FN.llhpos.hgt - refhgt;
if(CLK.RCOtic==0)
time_type = 0; /* CLK */
else if(ionoutc.vflg==VALID)
time_type = 1; /* UTC */
else
time_type = 2; /* GPS */
if(TrackMode==COLD_START)
track_type = 0;
else if(N.diff)
track_type = 2;
else
track_type = 1;
/* Stop sending invalid heights to WINMON
in Cold Start. */
if(fabs(N.llhpos.hgt)>99999.99)
N.llhpos.hgt = 0;
/* sprintf(output_buffer,"F00%-+10.6lf%-+11.6lf%-+9.2lf"
"%-+7.2lf%-5.1lf%-+7.2lf"
"%-5.1lf%-5.1lf%-5.1lf"
"%-2.2d%-1.1d%-1.1d"
"%-+11.1lf%-+10.1lf%-+10.1lf"
"%-2.2d%-2.2d%-2.2d%-2.2d%-2.2d%-2.2d%-1.1d"
"%-1.1d%-s",
FN.llhpos.lat*R2D,FN.llhpos.lon*R2D,FN.llhpos.hgt,
FN.speed,FN.hdg,FN.roc,
N.dop.gdop,N.dop.pdop,N.dop.vdop,
N.nsats,N.navmode,track_type,
horizerr,verterr,CLK.DoppFromClk,
t.d,t.m,t.y%100,t.hh,t.mm,t.ss,time_type,
system_status,CONFIG_CODE); */
//cd.deltat = deltat;
cd.Latitude=FN.llhpos.lat*R2D;
cd.Longitude=FN.llhpos.lon*R2D;
cd.Height=FN.llhpos.hgt;
cd.Speed=FN.speed;
cd.Heading=FN.hdg;
cd.RateOfClimb=FN.roc;
cd.GDop=N.dop.gdop;
cd.PDop=N.dop.pdop;
cd.VDop=N.dop.vdop;
cd.NoOfSVsTracked=N.nsats;
cd.NavMode=N.navmode;
cd.TrackMode=track_type;
cd.HorizontalError=horizerr;
cd.VerticalError=verterr;
cd.DopplerOffset=CLK.DoppFromClk;
cd.Day=t.d;
cd.Month=t.m;
cd.Year=t.y%100;
cd.Hour=t.hh;
cd.Minute=t.mm;
cd.Second=t.ss;
cd.TimeType=time_type;
cd.SystemStatus=system_status;
cd.ConfigCode[0]=CONFIG_CODE[0];
cd.ConfigCode[1]=CONFIG_CODE[1];
cd.ConfigCode[2]=CONFIG_CODE[2];
cd.ConfigCode[3]=CONFIG_CODE[3];
cd.ConfigCode[4]=CONFIG_CODE[4];
cd.ConfigCode[5]=CONFIG_CODE[5];
write(fd_fifo[1],&cd,sizeof(cd));
}
/****************************************************************************
* Function: void Sentence_F03(int arg)
*
* Revision: 6.04
*
* Last Update: 27/05/98
*
* Transmit the F03 sentence.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Sentence_F03(int arg)
{
/* char statflag,Ererrstr[12],Errerrstr[12],DCstr[12],
output_buffer[1920],buff[1920]; */
char statflag;
int corlk,carfrlk,bitlk,framelk,chan,ncohz,dispsv,sf=0,ura=0;
unsigned long channel_status;
long ltemp;
unsigned long carrdco;
double dtemp,snr;
navstatestruc N;
typedef struct
{
char ElevationIndicator[12];
int SatelliteAllocated[12];
int SatelliteElevation[12];
int SatelliteAzimuth[12];
int SatelliteDoppler[12];
int NCOFrequency[12];
int Uere[12];
int Subframe[12];
double PRError[12];
double PRRError[12];
double ICPError[12];
double DiffCorrection[12];
char Locks[61];
double Snr[12];
} F03Sentence;
F03Sentence cd;
/* Translation table URA code -> predicted
accuracy in meters */
double uratbl[] = {2.0,2.8,4.0,5.7,8.0,11.3,16.0,32.0,64.0,128.0,
256.0,512.0,1024.0,2048.0,4096.0,9999.0};
*/
rec_mutex_lock ((int)arg);
N = CurNavState; /* Get a local copy of the
current navigation state */
rec_mutex_unlock ((int)arg);
for(chan=0; chan<MAXCHANNELS; ++chan)
{
statflag = ' ';
dispsv = CH[chan].SV;
if( dispsv)
{
/* Check for unhealthy according to either
the almanac
or subframe 1 health codes. */
if(ielvd[dispsv-1] < ElvMask)
statflag = 'E';
& ephemeris entries */
rec_mutex_lock ((int)arg);
if( ((ephs[dispsv-1].vflg==VALID) &&
(ephs[dispsv-1].s1hlth))
|| ((alms[dispsv-1].vflg==VALID) &&
(alms[dispsv-1].almhlth)) )
statflag = 'U';
rec_mutex_unlock ((int)arg);
if(Deselect[dispsv-1])
statflag = 'D';
}
// disable(); /* Must protect against the
correlator ISR */
rtl_stop_interrupts();
ltemp = CH[chan].CdLI;
carrdco = CH[chan].CARRDCO;
channel_status = CH[chan].Status;
// enable();
rtl_allow_interrupts();
corlk = (int)( channel_status&CODE_LOCK);
carfrlk = (int)( channel_status&CARRIER_LOCK);
bitlk = (int)( channel_status&BIT_LOCK);
framelk = (int)( channel_status&FRAME_SYNC);
dtemp = ltemp;
/* The post-detection SNR of a satellite's
signal is the ratio of
its average I**2 + Q**2, given by CH[].CdLI,
to the expectation
value of I**2+ Q**2 for noise only. */
if(( dtemp>(NOISE_FLOOR_FLOAT)/10.0) &&
( dtemp<(NOISE_FLOOR_FLOAT)*10000.0))
{
/* Display the power ratio in dB for SNR
values between
-10 and +40 dB */
snr = 10.0*log10( dtemp/(NOISE_FLOOR_FLOAT));
}
else if( dtemp<(NOISE_FLOOR_FLOAT)/10.0)
{
/* Display -- for SNR values below -10 dB
*/
snr = 0.0;
}
else
{
/* Display >40 for SNR values in excess
of +40 dB */
snr = 40.0;
}
dtemp = carrdco*CARRIER_DCO_RESOLUTION/CARRIER_DCO_SCALE;
ncohz = -1*(int)( dtemp
- CARR_DCO_ZERO_DOPPLER*CARRIER_DCO_RESOLUTION/CARRIER_DCO_SCALE);
if( dispsv && (chan<ActiveChannels))
{
& ephemeris entries */
rec_mutex_lock ((int)arg);
if(ephs[dispsv-1].vflg==VALID &&
ephs[dispsv-1].s1hlth==0)
ura = (int)uratbl[ephs[dispsv-1].ura];
else
ura = 0;
rec_mutex_unlock ((int)arg);
if(lastsf[dispsv-1])
sf = lastsf[dispsv-1];
else
sf = 0;
if(N.dispdata.Erngerr[chan]==0.0)
cd.PRError[chan] = 0.0;
else
cd.PRError[chan] = N.dispdata.Erngerr[chan];
if(N.dispdata.Erraterr[chan]==0.0)
cd.PRRError[chan]= 0.0;
else
cd.PRRError[chan]=N.dispdata.Erraterr[chan];
if(fabs(N.dispdata.EPRcorr[chan])<1000.0)
cd.DiffCorrection[chan]=N.dispdata.EPRcorr[chan];
else
cd.DiffCorrection[chan]= 0.0;
/* sprintf(buff,"%c%-2d",statflag,
dispsv);
sprintf(buff,"%s%-+3d",buff,ielvd[dispsv-1]);
sprintf(buff,"%s%-3d",buff,iazid[dispsv-1]);
sprintf(buff,"%s%-+5d",buff,idopp[dispsv-1]);
sprintf(buff,"%s%-+6d",buff,ncohz);
sprintf(buff,"%s%-2d%-1d",buff,ura,sf),
sprintf(buff,"%s%8s%8s 0.0%6s",buff,Ererrstr,Errerrstr,DCstr);
sprintf(buff,"%s%c",buff,corlk?'C':'
');
sprintf(buff,"%s%c",buff,carfrlk?'C':'
');
sprintf(buff,"%s%c",buff,bitlk?'B':'
');
sprintf(buff,"%s%c",buff,framelk?'F':'
');
sprintf(buff,"%s%c",buff,' ');
sprintf(buff,"%s%-+5.1lf",buff,snr);
strcat(output_buffer,buff); */
cd.ElevationIndicator[chan]=statflag;
cd.SatelliteAllocated[chan]=dispsv;
cd.SatelliteElevation[chan]=ielvd[dispsv-1];
cd.SatelliteAzimuth[chan]=iazid[dispsv-1];
cd.SatelliteDoppler[chan]=idopp[dispsv-1];
cd.NCOFrequency[chan]=ncohz;
cd.Uere[chan]=ura;
cd.Subframe[chan]=sf;
cd.ICPError[chan]=0;
cd.Locks[chan*5]=corlk?'C':' ';
cd.Locks[chan*5+1]=carfrlk?'C':' ';
cd.Locks[chan*5+2]=bitlk?'B':' ';
cd.Locks[chan*5+3]=framelk?'F':' ';
cd.Locks[chan*5+4]=' ';
cd.Snr[chan]=snr;
}
else
{
cd.ElevationIndicator[chan]=' ';
cd.SatelliteAllocated[chan]=0;
cd.SatelliteElevation[chan]=0;
cd.SatelliteAzimuth[chan]=0;
cd.SatelliteDoppler[chan]=0;
cd.NCOFrequency[chan]=0;
cd.Uere[chan]=0;
cd.Subframe[chan]=0;
cd.ICPError[chan]=0;
cd.Locks[chan*5]=' ';
cd.Locks[chan*5+1]=' ';
cd.Locks[chan*5+2]=' ';
cd.Locks[chan*5+3]=' ';
cd.Locks[chan*5+4]=' ';
cd.Snr[chan]=0.0;
cd.PRError[chan] = 0.0;
cd.PRRError[chan]= 0.0;
cd.DiffCorrection[chan]= 0.0;
}
}
// SendString(output_buffer);
write(fd_fifo[2],&cd,sizeof(cd));
}
/****************************************************************************
* Function: void Sentence_F04(int arg)
*
* Revision: 6.00
*
* Last Update: 17/06/96
*
* Transmit the F04 sentence.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Sentence_F04(int arg)
{
int dispsv;
char hlthflag,deselflag;
typedef struct
{
char AlmanacHealth[32];
char Deselect[32];
int AlmanacStatus[32];
int AlmanacElevation[32];
int AlmanacAzimuth[32];
int AlmanacDoppler[32];
int AlmanacUpdate;
int IonoUtcFlag;
} F04Sentence;
F04Sentence cd;
// strcpy(output_buffer,"F04");
for(dispsv=1; dispsv<33; ++dispsv)
{
if(alms[dispsv-1].vflg==VALID)
{
/* We have an almanac for this SV */
& ephemeris entries */
rec_mutex_lock ((int)arg);
if( ((ephs[dispsv-1].vflg==VALID) &&
(ephs[dispsv-1].s1hlth))
|| ((alms[dispsv-1].vflg==VALID) &&
(alms[dispsv-1].almhlth)) )
hlthflag = 'U';
else
hlthflag = ' ';
rec_mutex_unlock ((int)arg);
if(Deselect[dispsv-1])
deselflag = 'D';
else
deselflag = ' ';
/* sprintf(buff,"%c%c0%-+3d%-3d%-+5d",hlthflag,deselflag,
ielvd[dispsv-1],iazid[dispsv-1],idopp[dispsv-1]);
strcat(output_buffer,buff); */
cd.AlmanacHealth[dispsv-1] = hlthflag;
cd.Deselect[dispsv-1] = deselflag;
cd.AlmanacStatus[dispsv-1] = 0;
cd.AlmanacElevation[dispsv-1] = ielvd[dispsv-1];
cd.AlmanacAzimuth[dispsv-1] = iazid[dispsv-1];
cd.AlmanacDoppler[dispsv-1] = idopp[dispsv-1];
}
/* else if(alms[dispsv-1].vflg==NOT_LOGGED)
{ */
/* The SV probably does not exist (no almanac
subframe for
this SV has been found where it was expected
in the subframe
stream). */
/* strcat(output_buffer," 1+00000+0000");
*/ /* NoSuch */
/* }
else
{ */
/* We have no information about this SV */
/* strcat(output_buffer," 2+00000+0000");
*//* NoInfo */
/* } */
}
cd.AlmanacUpdate = LastAlm;
cd.IonoUtcFlag = ionoutc.vflg;
/* sprintf(buff,"%-2d%-1d",LastAlm,ionoutc.vflg);
strcat(output_buffer,buff);
SendString(output_buffer); */
write(fd_fifo[3],&cd,sizeof(cd));
}
/****************************************************************************
* Function: void Sentence_F05(void)
*
* Revision: 6.01
*
* Last Update: 08/01/97
*
* Transmit the F05 sentence.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Sentence_F05(void)
{
// char output_buffer[1920];
typedef struct
{
int AccumPending;
int AccumMissed;
int MeasMissed;
int ObserPending;
int ObserMissed;
int SubfrPending;
int SubfrMissed;
int PLLLossCount;
int ChansAlloc;
int ChansInUse;
} F05Sentence;
F05Sentence cd;
cd.AccumPending = 0;
cd.AccumMissed = missa;
cd.MeasMissed = missm;
cd.ObserPending = 0;
cd.ObserMissed = 0;
cd.SubfrPending = MaxSfPending;
cd.SubfrMissed = misssf;
cd. PLLLossCount = 0;
cd.ChansAlloc = MAXCHANNELS;
cd.ChansInUse = ActiveChannels;
/* Max accum pending always zero - need to
remove parameter from F05
and WINMON. */
/* sprintf(output_buffer,"F050000%-4d%-4d%-4d%-4d"
"%-2d%-2d0000%-2d%-2d",
missa,missm,0,0,MaxSfPending,misssf,
MAXCHANNELS,ActiveChannels);
SendString(output_buffer); */
write(fd_fifo[4],&cd,sizeof(cd));
}
/****************************************************************************
* Function: void Sentence_F08(void)
*
* Revision: 6.04
*
* Last Update: 20/03/97
*
* Transmit the F08 sentence.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Sentence_F08(void)
{
// char output_buffer[1920];
typedef struct F08Sentence
{
int TimeZone;
double ElevationMask;
double PDOPMask;
int SearchMode;
int AltitudeAiding;
int DontMove;
int PositionFilterCoeff;
int VelocityFilterCoeff;
} F08Sentence;
F08Sentence cd;
cd.TimeZone = 0;
cd.ElevationMask = ElvMask;
cd.PDOPMask = PdopMask;
cd.SearchMode = TrackMode;
cd.AltitudeAiding = AltitudeAided;
cd.DontMove = DontMove;
cd.PositionFilterCoeff = (int)PositionFilterCoeff;
cd.VelocityFilterCoeff = (int)VelocityFilterCoeff;
/* sprintf(output_buffer,"F08%-+3d%-+5.1lf%-5.1lf%-1d",
0,ElvMask,PdopMask,TrackMode);
sprintf(output_buffer,"%s%-1d",output_buffer,((AltitudeAided==FALSE)?0:1));
sprintf(output_buffer,"%s%-1d",output_buffer,((DontMove==FALSE)?0:1));
sprintf(output_buffer,"%s%-2d%-2d",output_buffer,
(int)PositionFilterCoeff,
(int)VelocityFilterCoeff);
SendString(output_buffer); */
write(fd_fifo[5],&cd,sizeof(cd));
}