/* convert lex type 12 to rtcm ssr message -----------------------------------*/
static int lex2rtcm(const unsigned char *msg, int i, unsigned char *buff)
{
unsigned int crc;
int j,ns,type,n=0;
if (i+12>=LEXFRMLEN-LEXRSLEN) return 0;
switch ((type=getbitu(msg,i,12))) {
case 1057: ns=getbitu(msg,i+62,6); n=68+ns*135; break; /* gps */
case 1058: ns=getbitu(msg,i+61,6); n=67+ns* 76; break;
case 1059: ns=getbitu(msg,i+61,6); n=67+ns* 11;
for (j=0;j<ns;j++) n+=getbitu(msg,i+73+j*11,5)*19; break;
case 1060: ns=getbitu(msg,i+62,6); n=68+ns*205; break;
case 1061: ns=getbitu(msg,i+61,6); n=67+ns* 12; break;
case 1062: ns=getbitu(msg,i+61,6); n=67+ns* 28; break;
case 1063: ns=getbitu(msg,i+59,6); n=65+ns*134; break; /* glonass */
case 1064: ns=getbitu(msg,i+58,6); n=64+ns* 75; break;
case 1065: ns=getbitu(msg,i+58,6); n=64+ns* 10;
for (j=0;j<ns;j++) n+=getbitu(msg,i+69+j*10,5)*19; break;
case 1066: ns=getbitu(msg,i+59,6); n=65+ns*204; break;
case 1067: ns=getbitu(msg,i+58,6); n=64+ns* 11; break;
case 1068: ns=getbitu(msg,i+58,6); n=64+ns* 27; break;
case 1240: ns=getbitu(msg,i+62,6); n=68+ns*135; break; /* galileo */
case 1241: ns=getbitu(msg,i+61,6); n=67+ns* 76; break;
case 1242: ns=getbitu(msg,i+61,6); n=67+ns* 11;
for (j=0;j<ns;j++) n+=getbitu(msg,i+73+j*11,5)*19; break;
case 1243: ns=getbitu(msg,i+62,6); n=68+ns*205; break;
case 1244: ns=getbitu(msg,i+61,6); n=67+ns* 12; break;
case 1245: ns=getbitu(msg,i+61,6); n=67+ns* 28; break;
case 1246: ns=getbitu(msg,i+62,4); n=66+ns*133; break; /* qzss */
case 1247: ns=getbitu(msg,i+61,4); n=65+ns* 74; break;
case 1248: ns=getbitu(msg,i+61,4); n=65+ns* 9;
for (j=0;j<ns;j++) n+=getbitu(msg,i+69+j*9,5)*19; break;
case 1249: ns=getbitu(msg,i+62,4); n=66+ns*203; break;
case 1250: ns=getbitu(msg,i+61,4); n=65+ns* 10; break;
case 1251: ns=getbitu(msg,i+61,4); n=65+ns* 26; break;
default:
if (type) trace(2,"lex 12: unsupported type=%4d\n",type);
return 0;
}
n=(n+7)/8; /* message length (bytes) */
if (i+n*8>LEXFRMLEN-LEXRSLEN) {
trace(2,"lex 12: invalid ssr size: len=%4d\n",n);
return 0;
}
/* save rtcm message to buffer */
setbitu(buff, 0, 8,RTCM3PREAMB);
setbitu(buff, 8, 6,0);
setbitu(buff,14,10,n);
for (j=0;j<n;j++) {
buff[j+3]=getbitu(msg,i+j*8,8);
}
crc=crc24q(buff,3+n);
setbitu(buff,24+n*8,24,crc);
return n;
}
void setbits(u8 *buff, u32 pos, u32 len, s32 data)
{
if (data < 0)
data |= 1 << (len - 1);
else
data &= ~(1 << (len - 1)); /* set sign bit */
setbitu(buff, pos, len, (u32)data);
}
/* getbitu(),getbits(),setbitu(),setbits() */
void utest4(void)
{
unsigned char buff[1024]={0};
unsigned int vu;
int vs;
setbitu(buff, 0, 8, 1); vu=getbitu(buff, 0, 8); assert(vu== 1);
setbitu(buff, 4, 8, 255); vu=getbitu(buff, 4, 8); assert(vu== 255);
setbitu(buff, 13, 8, 1); vu=getbitu(buff, 13, 8); assert(vu== 1);
setbitu(buff, 29, 8, 255); vu=getbitu(buff, 29, 8); assert(vu== 255);
setbitu(buff, 99,10, 1023); vu=getbitu(buff, 99,10); assert(vu== 1023);
setbitu(buff,666,31,123456); vu=getbitu(buff,666,31); assert(vu==123456);
setbitu(buff,777,32,789012); vu=getbitu(buff,777,32); assert(vu==789012);
setbits(buff,100, 8, 1); vs=getbitu(buff,100, 8); assert(vs== 1);
setbits(buff,104, 8, 127); vs=getbitu(buff,104, 8); assert(vs== 127);
setbits(buff,113, 8, 1); vs=getbitu(buff,113, 8); assert(vs== 1);
setbits(buff,129, 8, 127); vs=getbitu(buff,129, 8); assert(vs== 127);
setbits(buff,199,10, 511); vs=getbitu(buff,199,10); assert(vs== 511);
setbits(buff,766,31,123456); vs=getbitu(buff,766,31); assert(vs==123456);
setbits(buff,877,32,789012); vs=getbitu(buff,877,32); assert(vs==789012);
setbits(buff,200, 8, -1); vs=getbits(buff,200, 8); assert(vs== -1);
setbits(buff,204, 8, -127); vs=getbits(buff,204, 8); assert(vs== -127);
setbits(buff,213, 8, -3); vu=getbits(buff,213, 8); assert(vu== -3);
setbits(buff,229, 8, -126); vu=getbits(buff,229, 8); assert(vu== -126);
setbits(buff,299,24,-99999); vu=getbits(buff,299,24); assert(vu==-99999);
setbits(buff,866,31,-12345); vs=getbits(buff,866,31); assert(vs==-12345);
setbits(buff,977,32,-67890); vs=getbits(buff,977,32); assert(vs==-67890);
printf("%s utset4 : OK\n",__FILE__);
}
/** Write RTCM frame header and CRC into a buffer.
*
* The buffer should already contain the data message starting at the
* 4th byte of the buffer, i.e.
*
* data_message = &buff[3]
*
* The buffer must have 3 bytes free at the start to contain the header and
* must leave 3 bytes past the end free to contain the CRC. The total length of
* the buffer should be `len+6` bytes.
*
* `len` should be in the range 0-1023 as per the RTCM v3 standard.
*
* \param len The length of the data message contained in the buffer.
* \param buff A pointer to the RTCM message buffer.
* \return Zero on success, -1 if `len` is too large.
*/
s8 rtcm3_write_frame(u16 len, u8 *buff)
{
if (len > 1023)
return -1;
/* Set preamble, reserved and length bits. */
setbitu(buff, 0, 8, RTCM3_PREAMBLE);
setbitu(buff, 8, 6, 0);
setbitu(buff, 14, 10, len);
/* Calculate CRC of frame header and data message. */
u32 crc = crc24q(buff, len + 3, 0);
/* Write CRC to end of frame. */
setbitu(buff, (len+3)*8, 24, crc);
return 0;
}
/* encode type 1002: extended L1-only gps rtk observables --------------------*/
static int encode_type1002(rtcm_t *rtcm, int sync)
{
int i, j;
int code1, pr1, ppr1, lock1, amb, cnr1;
/* encode header */
i = encode_head(1002, rtcm, sync, rtcm->n);
for (j = 0; j < rtcm->n; j++) {
/* generate obs field data gps */
gen_obs_gps(rtcm, &(rtcm->obs[j]), &code1, &pr1, &ppr1, &lock1, &amb,
&cnr1);
setbitu(rtcm->buff, i, 6, rtcm->obs[j].prn+1 ); i += 6;
setbitu(rtcm->buff, i, 1, code1); i += 1;
setbitu(rtcm->buff, i, 24, pr1 ); i += 24;
setbits(rtcm->buff, i, 20, ppr1 ); i += 20;
setbitu(rtcm->buff, i, 7, lock1); i += 7;
setbitu(rtcm->buff, i, 8, amb ); i += 8;
setbitu(rtcm->buff, i, 8, cnr1 ); i += 8;
}
rtcm->nbit = i;
return 1;
}
/** Encode an RTCMv3 message type 1002 (Extended L1-Only GPS RTK Observables)
* Message type 1002 has length `64 + n_sat*74` bits. Returned message length
* is rounded up to the nearest whole byte.
*
* \param buff A pointer to the RTCM data message buffer.
* \param id Reference station ID (DF003).
* \param t GPS time of epoch (DF004).
* \param n_sat Number of GPS satellites included in the message (DF006).
* \param nm Struct containing the observation.
* \param sync Synchronous GNSS Flag (DF005).
* \return The message length in bytes.
*/
u16 rtcm3_encode_1002(u8 *buff, u16 id, gps_time_t t, u8 n_sat,
navigation_measurement_t *nm, u8 sync)
{
rtcm3_write_header(buff, 1002, id, t, sync, n_sat, 0, 0);
u16 bit = 64; /* Start at end of header. */
u32 pr;
s32 ppr;
u8 amb, lock, cnr;
for (u8 i=0; i<n_sat; i++) {
gen_obs_gps(&nm[i], &amb, &pr, &ppr, &lock, &cnr);
setbitu(buff, bit, 6, nm[i].sid.sat); bit += 6;
/* TODO: set GPS code indicator if we ever support P(Y) code measurements. */
setbitu(buff, bit, 1, 0); bit += 1;
setbitu(buff, bit, 24, pr); bit += 24;
setbits(buff, bit, 20, ppr); bit += 20;
setbitu(buff, bit, 7, lock); bit += 7;
setbitu(buff, bit, 8, amb); bit += 8;
setbitu(buff, bit, 8, cnr); bit += 8;
}
/* Round number of bits up to nearest whole byte. */
return (bit + 7) / 8;
}
/** Write RTCM header for observation message types 1001..1004.
*
* The data message header will be written starting from byte zero of the
* buffer. If the buffer also contains a frame header then be sure to pass a
* pointer to the start of the data message rather than a pointer to the start
* of the frame buffer. The RTCM observation header is 8 bytes (64 bits) long.
*
* If the Synchronous GNSS Message Flag is set to `0`, it means that no further
* GNSS observables referenced to the same Epoch Time will be transmitted. This
* enables the receiver to begin processing the data immediately after decoding
* the message. If it is set to `1`, it means that the next message will
* contain observables of another GNSS source referenced to the same Epoch
* Time.
*
* Divergence-free Smoothing Indicator values:
*
* Indicator | Meaning
* --------- | ----------------------------------
* 0 | Divergence-free smoothing not used
* 1 | Divergence-free smoothing used
*
* GPS Smoothing Interval indicator values are listed in RTCM 10403.1 Table
* 3.4-4, reproduced here:
*
* Indicator | Smoothing Interval
* --------- | ------------------
* 000 (0) | No smoothing
* 001 (1) | < 30 s
* 010 (2) | 30-60 s
* 011 (3) | 1-2 min
* 100 (4) | 2-4 min
* 101 (5) | 4-8 min
* 110 (6) | >8 min
* 111 (7) | Unlimited
*
* \param buff A pointer to the RTCM data message buffer.
* \param type Message type number, i.e. 1001..1004 (DF002).
* \param id Reference station ID (DF003).
* \param t GPS time of epoch (DF004).
* \param sync Synchronous GNSS Flag (DF005).
* \param n_sat Number of GPS satellites included in the message (DF006).
* \param div_free GPS Divergence-free Smoothing Indicator (DF007).
* \param smooth GPS Smoothing Interval indicator (DF008).
*/
void rtcm3_write_header(u8 *buff, u16 type, u16 id, gps_time_t t,
u8 sync, u8 n_sat, u8 div_free, u8 smooth)
{
setbitu(buff, 0, 12, type);
setbitu(buff, 12, 12, id);
setbitu(buff, 24, 30, round(t.tow*1e3));
setbitu(buff, 54, 1, sync);
setbitu(buff, 55, 5, n_sat);
setbitu(buff, 60, 1, div_free);
setbitu(buff, 61, 3, smooth);
}
END_TEST
START_TEST(test_setbitu)
{
u8 test_data[10];
u32 ret;
setbitu(test_data, 10, 13, 0x11A2);
ret = getbitu(test_data, 10, 13);
fail_unless(ret == 0x11A2,
"test case 1 expected 0x11A2, got 0x%04X", ret);
/* TODO: Check that setbitu doesn't write to bits other than those in the bit
* field. */
}
/* encode rtcm header --------------------------------------------------------*/
static int encode_head(int type, rtcm_t *rtcm, int sync, int nsat)
{
int i = 24, epoch;
setbitu(rtcm->buff, i, 12, type); i += 12; /* message no */
setbitu(rtcm->buff, i, 12, 0); i += 12; /* ref station id */
epoch = ROUND(rtcm->time.tow / 0.001);
setbitu(rtcm->buff, i, 30, epoch); i += 30; /* gps epoch time */
setbitu(rtcm->buff, i, 1, sync); i += 1; /* synchronous gnss flag */
setbitu(rtcm->buff, i, 5, nsat); i += 5; /* no of satellites */
setbitu(rtcm->buff, i, 1, 0); i += 1; /* smoothing indicator */
setbitu(rtcm->buff, i, 3, 0); i += 3; /* smoothing interval */
return i;
}
/* generate rtcm 3 message -----------------------------------------------------
* generate rtcm 3 message
* args : rtcm_t *rtcm IO rtcm control struct
* int type I message type
* int sync I sync flag (1:another message follows)
* return : status (1:ok,0:error)
*-----------------------------------------------------------------------------*/
int gen_rtcm3(rtcm_t *rtcm, int type, int sync)
{
unsigned int crc;
int i = 0;
rtcm->nbit = rtcm->len = rtcm->nbyte = 0;
/* set preamble and reserved */
setbitu(rtcm->buff, i, 8, RTCM3PREAMB); i += 8;
setbitu(rtcm->buff, i, 6, 0 ); i += 6;
setbitu(rtcm->buff, i, 10, 0 ); i += 10;
/* encode rtcm 3 message body */
if (!encode_rtcm3(rtcm, type, sync)) return 0;
/* padding to align 8 bit boundary */
for (i = rtcm->nbit; i % 8; i++)
setbitu(rtcm->buff, i, 1, 0);
/* message length (header+data) (bytes) */
if ((rtcm->len = i / 8) >= 3 + 1024) {
/*trace(2,"generate rtcm 3 message length error len=%d\n",rtcm->len-3);*/
rtcm->nbit = rtcm->len = 0;
return 0;
}
/* message length without header and parity */
setbitu(rtcm->buff, 14, 10, rtcm->len - 3);
/* crc-24q */
crc = crc24q(rtcm->buff, rtcm->len, 0);
setbitu(rtcm->buff, i, 24, crc);
/* length total (bytes) */
rtcm->nbyte = rtcm->len + 3;
return 1;
}
void *lexthread(void * arg)
#endif
{
sdrch_t *sdr=(sdrch_t*)arg;
int cnt=0,lexch=0,state,i,nerr,errloc[LENLEXRS],time=0,dt,mid;
uint64_t buffloc;
double dfreq,cn0;
char *data;
uint8_t corri,sendbuf[LENLEXRCV],rsmsg[LENLEXRS];
uint8_t lexpre[LENLEXPRE]={0x1A,0xCF,0xFC,0x1D}; /* preamble */
sdrlex_t sdrlex={{0}};
sdrout_t out={0};
unsigned long tick=0;
FILE *fplexlog=NULL,*fplexbin=NULL;
short *rcode;
cpx_t *xcode;
if (sdrini.log) {
fplexlog=fopen("LEXLOG.csv","w");
fplexbin=fopen("LEXBIN.bin","wb");
fprintf(fplexlog,"Tow,CN0,Time(ms),Error\n");
}
/* start tcp server (lex) */
if (sdrini.lex) {
out.soc_lex.port=sdrini.lexport;
tcpsvrstart(&out.soc_lex);
}
data=(char*)sdrmalloc(sizeof(char)*sdr->nsamp*sdr->dtype);
/* lex channel is last */
lexch=sdrini.nch;
/* FFT code generation */
if (!(rcode=(short *)sdrmalloc(sizeof(short)*sdr->nsamp)) ||
!(xcode=cpxmalloc(sdr->nsamp))) {
SDRPRINTF("error: initsdrch memory alocation\n");
return THRETVAL;
}
rescode(sdr->code,sdr->clen,0,0,sdr->ci,sdr->nsamp,rcode); /* resampled code */
cpxcpx(rcode,NULL,1.0,sdr->nsamp,xcode); /* FFT code */
cpxfft(NULL,xcode,sdr->nsamp);
sdrfree(rcode);
sleepms(3000*sdrini.nch);
SDRPRINTF("**** LEX sdr thread start! ****\n");
do {
/* wait event */
mlock(hlexmtx);
waitevent(hlexeve,hlexmtx);
unmlock(hlexmtx);
/* assist from L1CA */
buffloc=sdrch[lexch].trk.codei[1]+sdrch[lexch].currnsamp+DSAMPLEX;
dfreq=-sdrch[lexch].trk.D[1]*(FREQ6/FREQ1);
/* get current data */
rcvgetbuff(&sdrini,buffloc,sdr->nsamp,sdr->ftype,sdr->dtype,data);
tick=tickgetus();
/* LEX correlation */
corri=lexcorr_fft(sdr,data,sdr->dtype,sdr->ti,sdr->nsamp,dfreq,sdr->crate,
sdr->nsamp,xcode,&cn0);
dt=tickgetus()-tick;
time+=dt;
if (dt>4000)
SDRPRINTF("error: dt=%.1fms(must be < 4ms)\n",(double)dt/1000);
/* check computation time */
if (cnt%250==0) {
//SDRPRINTF("time=%.2fms doppler=%.1f\n",(double)time/250000,dfreq);
time=0;
}
shiftdata(&sdrlex.msg[0],&sdrlex.msg[1],1,LENLEXMSG-1); /* shift to left */
sdrlex.msg[LENLEXMSG-1]=corri; /* add last */
/* preamble search */
state=0;
for (i=0;i<LENLEXPRE;i++) state+=abs(sdrlex.msg[i]-lexpre[i]);
if (state==0) {
/* reed solomon */
memset(rsmsg,0,LENLEXRS);
memcpy(&rsmsg[9],&sdrlex.msg[LENLEXPRE],LENLEXRS-9);
/* RS decode */
nerr=decode_rs_ccsds(rsmsg,errloc,0,0);
if (nerr!=0)
SDRPRINTF("RS correct %d symbols!\n",nerr);
if (sdrini.log) {
fprintf(fplexlog,"%f,%f,%d,%d\n",
sdrch[lexch].trk.tow[0],cn0,time,nerr);
fwrite(sdrlex.msg,1,LENLEXMSG,fplexbin);
}
if (nerr<0) { cnt++; continue; } /* <0 means failed to RS decode */
/* correct lex message */
memcpy(&sdrlex.msg[LENLEXPRE],&rsmsg[9],LENLEXRSK-9);
mid=getbitu(sdrlex.msg,5*8,8);
SDRPRINTF("LEX Message Type ID=%d\n",mid);
/* generate send buffer */
sendbuf[0]=0xAA; /* sync code1 (see rcvlex.c) */
sendbuf[1]=0x55; /* sync code2 (see rcvlex.c) */
/* set tow (LEX message does not contain tow information...) */
setbitu(sendbuf,2*8,4*8,ROUND(sdrch[lexch].trk.tow[0]*1000));
/* set week ()*/
setbitu(sendbuf,6*8,2*8,sdrch[lexch].nav.sdreph.eph.week);
memcpy(&sendbuf[8],sdrlex.msg,LENLEXMSG-LENLEXRSP); /* LEX message */
/* send LEX message */
if (sdrini.lex&&out.soc_lex.flag)
send(out.soc_lex.c_soc,(char*)sendbuf,LENLEXRCV,0);
}
cnt++;
} while (!sdrstat.stopflag);
if (sdrini.log) {
fclose(fplexlog);
fclose(fplexbin);
}
if (out.soc_lex.flag) tcpsvrclose(&out.soc_lex);
cpxfree(xcode);
return THRETVAL;
}
/* encode type 1019: gps ephemerides -----------------------------------------*/
static int encode_type1019(rtcm_t *rtcm, int sync)
{
ephemeris_t *swift_eph;
unsigned int sqrtA, e;
int i = 24, week, toe, toc, i0, OMG0, omg, M0, deln, idot, OMGd, crs,
crc;
int cus, cuc, cis, cic, af0, af1, af2, tgd;
(void)sync;
swift_eph = rtcm->eph;
week = swift_eph->toe.wn % 1024;
toe = ROUND(swift_eph->toe.tow / 16.0);
toc = ROUND(swift_eph->toc.tow / 16.0);
sqrtA = ROUND_U(swift_eph->sqrta / P2_19);
e = ROUND_U(swift_eph->ecc / P2_33);
i0 = ROUND(swift_eph->inc / P2_31 / SC2RAD);
OMG0 = ROUND(swift_eph->omega0 / P2_31 / SC2RAD);
omg = ROUND(swift_eph->w / P2_31 / SC2RAD);
M0 = ROUND(swift_eph->m0 / P2_31 / SC2RAD);
deln = ROUND(swift_eph->dn / P2_43 / SC2RAD);
idot = ROUND(swift_eph->inc_dot / P2_43 / SC2RAD);
OMGd = ROUND(swift_eph->omegadot / P2_43 / SC2RAD);
crs = ROUND(swift_eph->crs / P2_5 );
crc = ROUND(swift_eph->crc / P2_5 );
cus = ROUND(swift_eph->cus / P2_29);
cuc = ROUND(swift_eph->cuc / P2_29);
cis = ROUND(swift_eph->cis / P2_29);
cic = ROUND(swift_eph->cic / P2_29);
af0 = ROUND(swift_eph->af0 / P2_31);
af1 = ROUND(swift_eph->af1 / P2_43);
af2 = ROUND(swift_eph->af2 / P2_55);
tgd = ROUND(swift_eph->tgd / P2_31);
/* TODO: Lots of fields missing from ephemeris!! Just hacked in reasonable
values here. */
setbitu(rtcm->buff, i, 12, 1019); i += 12;
setbitu(rtcm->buff, i, 6, rtcm->prn+1); i += 6;
setbitu(rtcm->buff, i, 10, week); i += 10;
setbitu(rtcm->buff, i, 4, 1); i += 4;
setbitu(rtcm->buff, i, 2, 0); i += 2;
setbits(rtcm->buff, i, 14, idot); i += 14;
setbitu(rtcm->buff, i, 8, 1); i += 8;
setbitu(rtcm->buff, i, 16, toc); i += 16;
setbits(rtcm->buff, i, 8, af2); i += 8;
setbits(rtcm->buff, i, 16, af1); i += 16;
setbits(rtcm->buff, i, 22, af0); i += 22;
setbitu(rtcm->buff, i, 10, 1); i += 10;
setbits(rtcm->buff, i, 16, crs); i += 16;
setbits(rtcm->buff, i, 16, deln); i += 16;
setbits(rtcm->buff, i, 32, M0); i += 32;
setbits(rtcm->buff, i, 16, cuc); i += 16;
setbitu(rtcm->buff, i, 32, e); i += 32;
setbits(rtcm->buff, i, 16, cus); i += 16;
setbitu(rtcm->buff, i, 32, sqrtA); i += 32;
setbitu(rtcm->buff, i, 16, toe); i += 16;
setbits(rtcm->buff, i, 16, cic); i += 16;
setbits(rtcm->buff, i, 32, OMG0); i += 32;
setbits(rtcm->buff, i, 16, cis); i += 16;
setbits(rtcm->buff, i, 32, i0); i += 32;
setbits(rtcm->buff, i, 16, crc); i += 16;
setbits(rtcm->buff, i, 32, omg); i += 32;
setbits(rtcm->buff, i, 24, OMGd); i += 24;
setbits(rtcm->buff, i, 8, tgd); i += 8;
setbitu(rtcm->buff, i, 6, (swift_eph->healthy) ? 0 : 1 ); i += 6;
setbitu(rtcm->buff, i, 1, 0); i += 1;
setbitu(rtcm->buff, i, 1, 1); i += 1;
rtcm->nbit = i;
return 1;
}
