/* decode download file ------------------------------------------------------*/
static void decodefile(rtksvr_t *svr, int index)
{
nav_t nav;
char file[1024];
int nb;
INIT_ZERO(nav);
tracet(4,"decodefile: index=%d\n",index);
rtksvrlock(svr);
/* check file path completed */
if ((nb=svr->nb[index])<=2||
svr->buff[index][nb-2]!='\r'||svr->buff[index][nb-1]!='\n') {
rtksvrunlock(svr);
return;
}
strncpy(file,(char *)svr->buff[index],nb-2); file[nb-2]='\0';
svr->nb[index]=0;
rtksvrunlock(svr);
if (svr->format[index]==STRFMT_SP3) { /* precise ephemeris */
/* read sp3 precise ephemeris */
readsp3(file,&nav,0);
if (nav.ne<=0) {
tracet(1,"sp3 file read error: %s\n",file);
return;
}
/* update precise ephemeris */
rtksvrlock(svr);
if (svr->nav.peph) free(svr->nav.peph);
svr->nav.ne=svr->nav.nemax=nav.ne;
svr->nav.peph=nav.peph;
svr->ftime[index]=utc2gpst(timeget());
strcpy(svr->files[index],file);
rtksvrunlock(svr);
}
else if (svr->format[index]==STRFMT_RNXCLK) { /* precise clock */
/* read rinex clock */
if (readrnxc(file,&nav)<=0) {
tracet(1,"rinex clock file read error: %s\n",file);
return;
}
/* update precise clock */
rtksvrlock(svr);
if (svr->nav.pclk) free(svr->nav.pclk);
svr->nav.nc=svr->nav.ncmax=nav.nc;
svr->nav.pclk=nav.pclk;
svr->ftime[index]=utc2gpst(timeget());
strcpy(svr->files[index],file);
rtksvrunlock(svr);
}
}
/* decode binex mesaage 0x01-02: decoded glonass ephmemeris ------------------*/
static int decode_bnx_01_02(raw_t *raw, unsigned char *buff, int len)
{
geph_t geph={0};
unsigned char *p=buff;
double tod,tof,tau_gps;
int prn,day,leap;
trace(4,"binex 0x01-02: len=%d\n",len);
if (len>=119) {
prn =U1(p)+1; p+=1;
day =U2(p); p+=2;
tod =U4(p); p+=4;
geph.taun =-R8(p); p+=8;
geph.gamn =R8(p); p+=8;
tof =U4(p); p+=4;
geph.pos[0]=R8(p)*1E3; p+=8;
geph.vel[0]=R8(p)*1E3; p+=8;
geph.acc[0]=R8(p)*1E3; p+=8;
geph.pos[1]=R8(p)*1E3; p+=8;
geph.vel[1]=R8(p)*1E3; p+=8;
geph.acc[1]=R8(p)*1E3; p+=8;
geph.pos[2]=R8(p)*1E3; p+=8;
geph.vel[2]=R8(p)*1E3; p+=8;
geph.acc[2]=R8(p)*1E3; p+=8;
geph.svh =U1(p)&0x1; p+=1;
geph.frq =I1(p); p+=1;
geph.age =U1(p); p+=1;
leap =U1(p); p+=1;
tau_gps =R8(p); p+=8;
geph.dtaun =R8(p);
}
else {
trace(2,"binex 0x01-02: length error len=%d\n",len);
return -1;
}
if (!(geph.sat=satno(SYS_GLO,prn))) {
trace(2,"binex 0x01-02: satellite error prn=%d\n",prn);
return -1;
}
if (raw->time.time==0) return 0;
geph.toe=utc2gpst(adjday(raw->time,tod-10800.0));
geph.tof=utc2gpst(adjday(raw->time,tof-10800.0));
geph.iode=(int)(fmod(tod+10800.0,86400.0)/900.0+0.5);
if (!strstr(raw->opt,"-EPHALL")) {
if (fabs(timediff(geph.toe,raw->nav.geph[prn-MINPRNGLO].toe))<1.0&&
geph.svh==raw->nav.geph[prn-MINPRNGLO].svh) return 0; /* unchanged */
}
raw->nav.geph[prn-1]=geph;
raw->ephsat=geph.sat;
return 2;
}
/* decode solution time ------------------------------------------------------*/
static char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time)
{
double v[MAXFIELD];
char *p,*q,s[64]=" ";
int n,len;
trace(4,"decode_soltime:\n");
if (!strcmp(opt->sep,"\\t")) strcpy(s,"\t");
else if (*opt->sep) strcpy(s,opt->sep);
len=(int)strlen(s);
/* yyyy/mm/dd hh:mm:ss or yyyy mm dd hh:mm:ss */
if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf",v,v+1,v+2,v+3,v+4,v+5)>=6) {
if (v[0]<100.0) {
v[0]+=v[0]<80.0?2000.0:1900.0;
}
*time=epoch2time(v);
if (opt->times==TIMES_UTC) {
*time=utc2gpst(*time);
}
else if (opt->times==TIMES_JST) {
*time=utc2gpst(timeadd(*time,-9*3600.0));
}
if (!(p=strchr(buff,':'))||!(p=strchr(p+1,':'))) return NULL;
for (p++;isdigit((int)*p)||*p=='.';) p++;
return p+len;
}
if (opt->posf==SOLF_GSIF) {
if (sscanf(buff,"%lf %lf %lf %lf:%lf:%lf",v,v+1,v+2,v+3,v+4,v+5)<6) {
return NULL;
}
*time=timeadd(epoch2time(v),-12.0*3600.0);
if (!(p=strchr(buff,':'))||!(p=strchr(p+1,':'))) return NULL;
for (p++;isdigit((int)*p)||*p=='.';) p++;
return p+len;
}
/* wwww ssss */
for (p=buff,n=0;n<2;p=q+len) {
if ((q=strstr(p,s))) *q='\0';
if (*p) v[n++]=atof(p);
if (!q) break;
}
if (n>=2&&0.0<=v[0]&&v[0]<=3000.0&&0.0<=v[1]&&v[1]<604800.0) {
*time=gpst2time((int)v[0],v[1]);
return p;
}
return NULL;
}
/* decode solution -----------------------------------------------------------*/
static int decode_gw10sol(raw_t *raw)
{
gtime_t time;
double ep[6]={0},sec;
unsigned char *p=raw->buff+6;
trace(4,"decode_gw10sol : len=%d\n",raw->len);
if (U2(p+42)&0xC00) { /* time valid? */
trace(2,"gw10 sol time/day invalid\n");
return 0;
}
sec=U4(p+27)/16384.0;
sec=floor(sec*1000.0+0.5)/1000.0;
ep[2]=bcd2num(p[31]);
ep[1]=bcd2num(p[32]);
ep[0]=bcd2num(p[33])*100+bcd2num(p[34]);
time=utc2gpst(timeadd(epoch2time(ep),sec));
/* set time if no time available */
if (raw->time.time==0) {
raw->time=time;
}
return 0;
}
/* tle_pos() -----------------------------------------------------------------*/
static void utest2(void)
{
const char *file2="../data/tle/tle_sgp4.txt";
const double ep0[6]={1980,1,1};
tle_t tle={0};
gtime_t epoch;
double min,rs[6];
int i,stat;
epoch=utc2gpst(timeadd(epoch2time(ep0),274.98708465*86400.0));
stat=tle_read(file2,&tle);
assert(stat);
stat=tle_pos(epoch,"TEST_ERR","","",&tle,NULL,rs);
assert(!stat);
for (i=0;i<5;i++) {
min=360.0*i;
stat=tle_pos(timeadd(epoch,min*60.0),"TEST_SAT","","",&tle,NULL,rs);
assert(stat);
fprintf(OUT,"%4.0f: %14.8f %14.8f %14.8f %11.8f %11.8f %11.8f\n",min,
rs[0]/1e3,rs[1]/1e3,rs[2]/1e3,rs[3]/1e3,rs[4]/1e3,rs[5]/1e3);
}
fprintf(OUT,"%s utest2 : OK\n",__FILE__);
}
/* convert glonass time to time -----------------------------------------------
* convert glonass time in ephemeris to gtime_t struct
* args : int nt I current day
* int n4 I 4 year interval
* int h I hour
* int m I minute
* int s I second
* return : gtime_t struct
* note : Reference: GLONASS ICD A.3.1.3
*-----------------------------------------------------------------------------*/
gtime_t glot2time(int nt, int n4, int h, int m, int s)
{
double ep[6];
int j=0,year,mon,day=0,doy=0;
int doys[]= {1,32,60,91,121,152,182,213,244,274,305,335}; /* day of year */
int doysl[]={1,32,61,92,122,153,183,214,245,275,306,336}; /* leap year */
ep[3]=h; ep[4]=m; ep[5]=s;
if (nt<= 366) {j=1; doy=nt;}
else if(nt<= 731) {j=2; doy=nt-366+1;}
else if(nt<=1096) {j=3; doy=nt-731+1;}
else if(nt<=1461) {j=4; doy=nt-1096+1;}
/* current year */
year=1996+4*(n4-1)+(j-1);
/* compute current month and day */
for (mon=1;mon<12;mon++) {
if (j==1) {
if (doy<doysl[mon]) {
day=doy-doysl[mon-1];
break;
}
} else {
if (doy<doys[ mon]) {
day=doy-doys[mon-1];
break;
}
}
}
ep[0]=year; ep[1]=mon; ep[2]=day;
return utc2gpst(epoch2time(ep)); /* convert to gtime_t */
}
/* main ----------------------------------------------------------------------*/
int main(int argc, char **argv)
{
gtime_t time;
double ep[6]= {0};
int i,page=1;
time2epoch(utc2gpst(timeget()),ep);
ep[4]=ep[5]=0.0;
for (i=1; i<argc; i++) {
if (!strcmp(argv[i],"-t")&&i+2<argc) {
sscanf(argv[++i],"%lf/%lf/%lf",ep ,ep+1,ep+2);
sscanf(argv[++i],"%lf:%lf:%lf",ep+3,ep+4,ep+5);
}
else if (!strcmp(argv[i],"-p")&&i+1<argc) page=atoi(argv[++i]);
}
time=epoch2time(ep);
switch (page) {
case 1:
gen_page1(time,TEMP1,PAGE1);
break;
case 2:
gen_page2(time,TEMP2,PAGE2);
break;
case 3:
gen_page3(time,TEMP3,PAGE3);
break;
}
return 0;
}
/* decode nmea gpgga: fix information ----------------------------------------*/
static int decode_nmeagga(char **val, int n, sol_t *sol)
{
gtime_t time;
double tod=0.0,lat=0.0,lon=0.0,hdop=0.0,alt=0.0,msl=0.0,ep[6],tt;
double pos[3]={0};
char ns='N',ew='E',ua=' ',um=' ';
int i,solq=0,nrcv=0;
trace(4,"decode_nmeagga: n=%d\n",n);
for (i=0;i<n;i++) {
switch (i) {
case 0: tod =atof(val[i]); break; /* time in utc (hhmmss) */
case 1: lat =atof(val[i]); break; /* latitude (ddmm.mmm) */
case 2: ns =*val[i]; break; /* N=north,S=south */
case 3: lon =atof(val[i]); break; /* longitude (dddmm.mmm) */
case 4: ew =*val[i]; break; /* E=east,W=west */
case 5: solq=atoi(val[i]); break; /* fix quality */
case 6: nrcv=atoi(val[i]); break; /* # of satellite tracked */
case 7: hdop=atof(val[i]); break; /* hdop */
case 8: alt =atof(val[i]); break; /* altitude in msl */
case 9: ua =*val[i]; break; /* unit (M) */
case 10: msl =atof(val[i]); break; /* height of geoid */
case 11: um =*val[i]; break; /* unit (M) */
}
}
if ((ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) {
trace(2,"invalid nmea gpgga format\n");
return 0;
}
if (sol->time.time==0.0) {
trace(2,"no date info for nmea gpgga\n");
return 0;
}
pos[0]=(ns=='N'?1.0:-1.0)*dmm2deg(lat)*D2R;
pos[1]=(ew=='E'?1.0:-1.0)*dmm2deg(lon)*D2R;
pos[2]=alt+msl;
time2epoch(sol->time,ep);
septime(tod,ep+3,ep+4,ep+5);
time=utc2gpst(epoch2time(ep));
tt=timediff(time,sol->time);
if (tt<-43200.0) sol->time=timeadd(time, 86400.0);
else if (tt> 43200.0) sol->time=timeadd(time,-86400.0);
else sol->time=time;
pos2ecef(pos,sol->rr);
sol->stat=0<=solq&&solq<=8?solq_nmea[solq]:SOLQ_NONE;
sol->ns=nrcv;
sol->type=0; /* postion type = xyz */
trace(5,"decode_nmeagga: %s rr=%.3f %.3f %.3f stat=%d ns=%d hdop=%.1f ua=%c um=%c\n",
time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns,
hdop,ua,um);
return 1;
}
//---------------------------------------------------------------------------
void __fastcall TStartDialog::FormShow(TObject *Sender)
{
char tstr[64];
if (Time.time==0) {
Time=utc2gpst(timeget());
}
time2str(Time,tstr,0);
tstr[10]='\0';
TimeY1->Text=tstr;
TimeH1->Text=tstr+11;
}
/* decode type 18: rtk uncorrected carrier-phase -----------------------------*/
static int decode_type18(rtcm_t *rtcm)
{
gtime_t time;
double usec,cp,tt;
int i=48,index,freq,sync=1,code,sys,prn,sat,loss;
rtklib_trace(4,"decode_type18: len=%d\n",rtcm->len);
if (i+24<=rtcm->len*8) {
freq=getbitu(rtcm->buff,i, 2); i+= 2+2;
usec=getbitu(rtcm->buff,i,20); i+=20;
}
else {
rtklib_trace(2,"rtcm2 18 length error: len=%d\n",rtcm->len);
return -1;
}
if (freq&0x1) {
rtklib_trace(2,"rtcm2 18 not supported frequency: freq=%d\n",freq);
return -1;
}
freq>>=1;
while (i+48<=rtcm->len*8&&rtcm->obs.n<MAXOBS) {
sync=getbitu(rtcm->buff,i, 1); i+= 1;
code=getbitu(rtcm->buff,i, 1); i+= 1;
sys =getbitu(rtcm->buff,i, 1); i+= 1;
prn =getbitu(rtcm->buff,i, 5); i+= 5+3;
loss=getbitu(rtcm->buff,i, 5); i+= 5;
cp =getbits(rtcm->buff,i,32); i+=32;
if (prn==0) prn=32;
if (!(sat=satno(sys?SYS_GLO:SYS_GPS,prn))) {
rtklib_trace(2,"rtcm2 18 satellite number error: sys=%d prn=%d\n",sys,prn);
continue;
}
time=timeadd(rtcm->time,usec*1E-6);
if (sys) time=utc2gpst(time); /* convert glonass time to gpst */
tt=timediff(rtcm->obs.data[0].time,time);
if (rtcm->obsflag||fabs(tt)>1E-9) {
rtcm->obs.n=rtcm->obsflag=0;
}
if ((index=obsindex(&rtcm->obs,time,sat))>=0) {
rtcm->obs.data[index].L[freq]=-cp/256.0;
rtcm->obs.data[index].LLI[freq]=rtcm->loss[sat-1][freq]!=loss;
rtcm->obs.data[index].code[freq]=
!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
rtcm->loss[sat-1][freq]=loss;
}
}
rtcm->obsflag=!sync;
return sync?0:1;
}
/* merge ephmeris data ---------------------------------------------------------
* merge separated ephmeris data to ephemeris struct
* args : sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void merge_g1(sdreph_t *eph)
{
double ep[6];
eph->geph.tof=glot2time(eph->nt,eph->n4,eph->tk[0],eph->tk[1],eph->tk[2]);
/* compute current tow (tk is time of word 1) */
eph->tow_gpst=time2gpst(eph->geph.tof,&eph->eph.week)+eph->s1cnt*2.0;
eph->week_gpst=eph->eph.week;
ep[3]=0;ep[4]=eph->geph.iode-60*3;ep[5]=0; /* 3 hour bias in UTC-GPST */
eph->geph.toe=utc2gpst(epoch2time(ep)); /* time of ephemeris */
}
/* send nmea request -----------------------------------------------------------
* send nmea gpgga message to stream
* args : stream_t *stream I stream
* double *pos I position {x,y,z} (ecef) (m)
* return : none
*-----------------------------------------------------------------------------*/
extern void strsendnmea(stream_t *stream, const double *pos)
{
sol_t sol={{0}};
unsigned char buff[1024];
int i,n;
tracet(3,"strsendnmea: pos=%.3f %.3f %.3f\n",pos[0],pos[1],pos[2]);
sol.stat=SOLQ_SINGLE;
sol.time=utc2gpst(timeget());
for (i=0;i<3;i++) sol.rr[i]=pos[i];
n=outnmea_gga(buff,&sol);
strwrite(stream,buff,n);
}
/* adjust hourly rollover of rtcm 2 time -------------------------------------*/
static void adjhour(rtcm_t *rtcm, double zcnt)
{
double tow,hour,sec;
int week;
/* if no time, get cpu time */
if (rtcm->time.time==0) rtcm->time=utc2gpst(timeget());
tow=time2gpst(rtcm->time,&week);
hour=floor(tow/3600.0);
sec=tow-hour*3600.0;
if (zcnt<sec-1800.0) zcnt+=3600.0;
else if (zcnt>sec+1800.0) zcnt-=3600.0;
rtcm->time=gpst2time(week,hour*3600+zcnt);
}
/* utc2gpst(), gpst2utc() */
void utest8(void)
{
double ep0[]={1980, 1, 6, 0, 0, 0.000000};
double ep1[]={2010,12,31,23,59,59.999999};
gtime_t t0,t1,t2,t3;
t0=epoch2time(ep0);
t1=epoch2time(ep1);
while (t0.time<t1.time) {
t2=utc2gpst(t0); t3=gpst2utc(t2); assert(t0.time==t3.time&&t0.sec==t3.sec);
t0.time+=86400.0;
}
printf("%s utset8 : OK\n",__FILE__);
}
// update information for current-cursor position ---------------------------
void __fastcall TPlot::UpdatePoint(int x, int y)
{
gtime_t time;
TPoint p(x,y);
double enu[3]={0},rr[3],pos[3],xx,yy,r,xl[2],yl[2],q[2],az,el,snr;
int i;
char tstr[64];
AnsiString msg;
trace(4,"UpdatePoint: x=%d y=%d\n",x,y);
if (PlotType==PLOT_TRK) { // track-plot
if (norm(OPos,3)>0.0) {
GraphT->ToPos(p,enu[0],enu[1]);
ecef2pos(OPos,pos);
enu2ecef(pos,enu,rr);
for (i=0;i<3;i++) rr[i]+=OPos[i];
ecef2pos(rr,pos);
msg=LatLonStr(pos,8);
}
}
else if (PlotType==PLOT_SKY||PlotType==PLOT_MPS) { // sky-plot
GraphS->GetLim(xl,yl);
GraphS->ToPos(p,q[0],q[1]);
r=(xl[1]-xl[0]<yl[1]-yl[0]?xl[1]-xl[0]:yl[1]-yl[0])*0.45;
if ((el=90.0-90.0*norm(q,2)/r)>0.0) {
az=el>=90.0?0.0:ATAN2(q[0],q[1])*R2D;
if (az<0.0) az+=360.0;
msg.sprintf("AZ=%5.1f" CHARDEG " EL=%4.1f" CHARDEG,az,el);
}
}
else if (PlotType==PLOT_SNRE) { // snr-el-plot
GraphE[0]->ToPos(p,q[0],q[1]);
msg.sprintf("EL=%4.1f " CHARDEG,q[0]);
}
else {
GraphG[0]->ToPos(p,xx,yy);
time=gpst2time(Week,xx);
if (TimeLabel==2) time=utc2gpst(time); // UTC
else if (TimeLabel==3) time=timeadd(gpst2utc(time),-9*3600.0); // JST
TimeStr(time,0,1,tstr);
msg=tstr;
}
Panel22->Visible=true;
Message2->Caption=A2U(msg);
}
/* decode nmea gprmc: recommended minumum data for gps -----------------------*/
static int decode_nmearmc(char **val, int n, sol_t *sol)
{
double tod=0.0,lat=0.0,lon=0.0,vel=0.0,dir=0.0,date=0.0,ang=0.0,ep[6];
double pos[3]={0};
char act=' ',ns='N',ew='E',mew='E',mode='A';
int i;
trace(4,"decode_nmearmc: n=%d\n",n);
for (i=0;i<n;i++) {
switch (i) {
case 0: tod =atof(val[i]); break; /* time in utc (hhmmss) */
case 1: act =*val[i]; break; /* A=active,V=void */
case 2: lat =atof(val[i]); break; /* latitude (ddmm.mmm) */
case 3: ns =*val[i]; break; /* N=north,S=south */
case 4: lon =atof(val[i]); break; /* longitude (dddmm.mmm) */
case 5: ew =*val[i]; break; /* E=east,W=west */
case 6: vel =atof(val[i]); break; /* speed (knots) */
case 7: dir =atof(val[i]); break; /* track angle (deg) */
case 8: date=atof(val[i]); break; /* date (ddmmyy) */
case 9: ang =atof(val[i]); break; /* magnetic variation */
case 10: mew =*val[i]; break; /* E=east,W=west */
case 11: mode=*val[i]; break; /* mode indicator (>nmea 2) */
/* A=autonomous,D=differential */
/* E=estimated,N=not valid,S=simulator */
}
}
if ((act!='A'&&act!='V')||(ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) {
trace(2,"invalid nmea gprmc format\n");
return 0;
}
pos[0]=(ns=='S'?-1.0:1.0)*dmm2deg(lat)*D2R;
pos[1]=(ew=='W'?-1.0:1.0)*dmm2deg(lon)*D2R;
septime(date,ep+2,ep+1,ep);
septime(tod,ep+3,ep+4,ep+5);
ep[0]+=ep[0]<80.0?2000.0:1900.0;
sol->time=utc2gpst(epoch2time(ep));
pos2ecef(pos,sol->rr);
sol->stat=mode=='D'?SOLQ_DGPS:SOLQ_SINGLE;
sol->ns=0;
sol->type=0; /* postion type = xyz */
trace(5,"decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d vel=%.2f dir=%.0f ang=%.0f mew=%c mode=%c\n",
time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns,
vel,dir,ang,mew,mode);
return 1;
}
/* decode id#20 navigation data (user) ---------------------------------------*/
static int decode_ss2llh(raw_t *raw)
{
double ep[6];
unsigned char *p=raw->buff+4;
trace(4,"decode_ss2llh: len=%d\n",raw->len);
if (raw->len!=77) {
trace(2,"ss2 id#20 length error: len=%d\n",raw->len);
return -1;
}
ep[3]=U1(p ); ep[4]=U1(p+ 1); ep[5]=R8(p+ 2);
ep[2]=U1(p+10); ep[1]=U1(p+11); ep[0]=U2(p+12);
raw->time=utc2gpst(epoch2time(ep));
return 0;
}
static jboolean open_solution_status_file(JNIEnv* env, int level, gtime_t timestamp)
{
double ep[6];
char filename[1024];
if (level <= 0)
return JNI_FALSE;
time2epoch(utc2gpst(timestamp),ep);
sprintf(filename,"rtkgps_%04.0f%02.0f%02.0f%02.0f%02.0f%02.0f.stat",
ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]);
if (!get_path_in_storage_dir(env, filename, sizeof(filename)))
return JNI_FALSE;
LOGV("open_solution_status_file() %s", filename);
rtkopenstat(filename, level);
return JNI_TRUE;
}
/* str2str -------------------------------------------------------------------*/
int main(int argc, char **argv)
{
static char cmd[MAXRCVCMD]="";
const char ss[]={'E','-','W','C','C'};
strconv_t *conv[MAXSTR]={NULL};
double pos[3],stapos[3]={0};
char *paths[MAXSTR],s[MAXSTR][MAXSTRPATH]={{0}},*cmdfile="";
char *local="",*proxy="",*msg="1004,1019",*opt="",buff[256],*p;
char strmsg[MAXSTRMSG]="";
int i,n=0,dispint=5000,trlevel=0,opts[]={10000,10000,2000,32768,10,0,30};
int types[MAXSTR]={0},stat[MAXSTR]={0},byte[MAXSTR]={0},bps[MAXSTR]={0};
int fmts[MAXSTR],sta=0;
for (i=0;i<MAXSTR;i++) paths[i]=s[i];
for (i=1;i<argc;i++) {
if (!strcmp(argv[i],"-in")&&i+1<argc) {
if (!decodepath(argv[++i],types,paths[0],fmts)) return -1;
}
else if (!strcmp(argv[i],"-out")&&i+1<argc&&n<MAXSTR-1) {
if (!decodepath(argv[++i],types+n+1,paths[n+1],fmts+n+1)) return -1;
n++;
}
else if (!strcmp(argv[i],"-p")&&i+3<argc) {
pos[0]=atof(argv[++i])*D2R;
pos[1]=atof(argv[++i])*D2R;
pos[2]=atof(argv[++i]);
pos2ecef(pos,stapos);
}
else if (!strcmp(argv[i],"-msg")&&i+1<argc) msg=argv[++i];
else if (!strcmp(argv[i],"-opt")&&i+1<argc) opt=argv[++i];
else if (!strcmp(argv[i],"-sta")&&i+1<argc) sta=atoi(argv[++i]);
else if (!strcmp(argv[i],"-d" )&&i+1<argc) dispint=atoi(argv[++i]);
else if (!strcmp(argv[i],"-s" )&&i+1<argc) opts[0]=atoi(argv[++i]);
else if (!strcmp(argv[i],"-r" )&&i+1<argc) opts[1]=atoi(argv[++i]);
else if (!strcmp(argv[i],"-n" )&&i+1<argc) opts[5]=atoi(argv[++i]);
else if (!strcmp(argv[i],"-f" )&&i+1<argc) opts[6]=atoi(argv[++i]);
else if (!strcmp(argv[i],"-c" )&&i+1<argc) cmdfile=argv[++i];
else if (!strcmp(argv[i],"-l" )&&i+1<argc) local=argv[++i];
else if (!strcmp(argv[i],"-x" )&&i+1<argc) proxy=argv[++i];
else if (!strcmp(argv[i],"-t" )&&i+1<argc) trlevel=atoi(argv[++i]);
else if (*argv[i]=='-') printhelp();
}
if (!*paths[0]) {
fprintf(stderr,"specify input stream\n");
return -1;
}
if (n<=0) {
fprintf(stderr,"specify output stream(s)\n");
return -1;
}
for (i=0;i<n;i++) {
if (fmts[i+1]<0) continue;
if (fmts[i+1]!=STRFMT_RTCM3) {
fprintf(stderr,"unsupported output format\n");
return -1;
}
if (fmts[0]<0) {
fprintf(stderr,"specify input format\n");
return -1;
}
if (!(conv[i]=strconvnew(fmts[0],fmts[i+1],msg,sta,sta!=0,opt))) {
fprintf(stderr,"stream conversion error\n");
return -1;
}
}
signal(SIGTERM,sigfunc);
signal(SIGINT ,sigfunc);
signal(SIGHUP ,SIG_IGN);
signal(SIGPIPE,SIG_IGN);
strsvrinit(&strsvr,n+1);
if (trlevel>0) {
traceopen(TRFILE);
tracelevel(trlevel);
}
fprintf(stderr,"stream server start\n");
strsetdir(local);
strsetproxy(proxy);
if (*cmdfile) readcmd(cmdfile,cmd,0);
/* start stream server */
if (!strsvrstart(&strsvr,opts,types,paths,conv,*cmd?cmd:NULL,stapos)) {
fprintf(stderr,"stream server start error\n");
return -1;
}
for (intrflg=0;!intrflg;) {
/* get stream server status */
strsvrstat(&strsvr,stat,byte,bps,strmsg);
/* show stream server status */
for (i=0,p=buff;i<MAXSTR;i++) p+=sprintf(p,"%c",ss[stat[i]+1]);
fprintf(stderr,"%s [%s] %10d B %7d bps %s\n",
time_str(utc2gpst(timeget()),0),buff,byte[0],bps[0],strmsg);
sleepms(dispint);
}
if (*cmdfile) readcmd(cmdfile,cmd,1);
/* stop stream server */
strsvrstop(&strsvr,*cmd?cmd:NULL);
for (i=0;i<n;i++) {
strconvfree(conv[i]);
}
if (trlevel>0) {
traceclose();
}
fprintf(stderr,"stream server stop\n");
return 0;
}
//---------------------------------------------------------------------------
void __fastcall TOptDialog::SaveOpt(AnsiString file)
{
AnsiString ProxyAddrE_Text=ProxyAddrE->Text;
AnsiString ExSatsE_Text=ExSatsE->Text;
AnsiString FieldSep_Text=FieldSep->Text;
AnsiString RovAnt_Text=RovAnt->Text,RefAnt_Text=RefAnt->Text;
AnsiString SatPcvFile_Text=SatPcvFile->Text;
AnsiString AntPcvFile_Text=AntPcvFile->Text;
AnsiString StaPosFile_Text=StaPosFile->Text;
AnsiString GeoidDataFile_Text=GeoidDataFile->Text;
AnsiString DCBFile_Text=DCBFile->Text;
AnsiString LocalDir_Text=LocalDir->Text;
int itype[]={STR_SERIAL,STR_TCPCLI,STR_TCPSVR,STR_NTRIPCLI,STR_FILE,STR_FTP,STR_HTTP};
int otype[]={STR_SERIAL,STR_TCPCLI,STR_TCPSVR,STR_NTRIPSVR,STR_FILE};
TEdit *editu[]={RovPos1,RovPos2,RovPos3};
TEdit *editr[]={RefPos1,RefPos2,RefPos3};
char buff[1024],*p,id[32],comment[256],s[64];
int sat,ex;
prcopt_t prcopt=prcopt_default;
solopt_t solopt=solopt_default;
filopt_t filopt={""};
for (int i=0;i<8;i++) {
strtype[i]=i<3?itype[MainForm->Stream[i]]:otype[MainForm->Stream[i]];
strfmt[i]=MainForm->Format[i];
if (!MainForm->StreamC[i]) {
strtype[i]=STR_NONE;
strcpy(strpath[i],"");
}
else if (strtype[i]==STR_SERIAL) {
strcpy(strpath[i],MainForm->Paths[i][0].c_str());
}
else if (strtype[i]==STR_FILE) {
strcpy(strpath[i],MainForm->Paths[i][2].c_str());
}
else if (strtype[i]<=STR_NTRIPCLI) {
strcpy(strpath[i],MainForm->Paths[i][1].c_str());
}
else if (strtype[i]<=STR_HTTP) {
strcpy(strpath[i],MainForm->Paths[i][3].c_str());
}
}
nmeareq =MainForm->NmeaReq;
nmeapos[0]=MainForm->NmeaPos[0];
nmeapos[1]=MainForm->NmeaPos[1];
svrcycle =SvrCycleE ->Text.ToInt();
timeout =TimeoutTimeE->Text.ToInt();
reconnect =ReconTimeE ->Text.ToInt();
nmeacycle =NmeaCycleE ->Text.ToInt();
buffsize =SvrBuffSizeE->Text.ToInt();
navmsgsel =NavSelectS ->ItemIndex;
strcpy(proxyaddr,ProxyAddrE_Text.c_str());
fswapmargin =FileSwapMarginE->Text.ToInt();
prcopt.sbassatsel=SbasSatE->Text.ToInt();
prcopt.mode =PosMode ->ItemIndex;
prcopt.nf =Freq ->ItemIndex+1;
prcopt.soltype =Solution ->ItemIndex;
prcopt.elmin =str2dbl(ElMask ->Text)*D2R;
prcopt.dynamics =DynamicModel->ItemIndex;
prcopt.tidecorr =TideCorr ->ItemIndex;
prcopt.ionoopt =IonoOpt ->ItemIndex;
prcopt.tropopt =TropOpt ->ItemIndex;
prcopt.sateph =SatEphem ->ItemIndex;
if (ExSatsE->Text!="") {
strcpy(buff,ExSatsE_Text.c_str());
for (p=strtok(buff," ");p;p=strtok(NULL," ")) {
if (*p=='+') {ex=2; p++;} else ex=1;
if (!(sat=satid2no(p))) continue;
prcopt.exsats[sat-1]=ex;
}
}
prcopt.navsys = (NavSys1->Checked?SYS_GPS:0)|
(NavSys2->Checked?SYS_GLO:0)|
(NavSys3->Checked?SYS_GAL:0)|
(NavSys4->Checked?SYS_QZS:0)|
(NavSys5->Checked?SYS_SBS:0)|
(NavSys6->Checked?SYS_CMP:0);
prcopt.posopt[0]=PosOpt1->Checked;
prcopt.posopt[1]=PosOpt2->Checked;
prcopt.posopt[2]=PosOpt3->Checked;
prcopt.posopt[3]=PosOpt4->Checked;
prcopt.posopt[4]=PosOpt5->Checked;
prcopt.modear =AmbRes ->ItemIndex;
prcopt.glomodear=GloAmbRes ->ItemIndex;
prcopt.thresar[0]=str2dbl(ValidThresAR->Text);
prcopt.maxout =str2dbl(OutCntResetAmb->Text);
prcopt.minfix =str2dbl(FixCntHoldAmb->Text);
prcopt.minlock =str2dbl(LockCntFixAmb->Text);
prcopt.elmaskar =str2dbl(ElMaskAR ->Text)*D2R;
prcopt.elmaskhold=str2dbl(ElMaskHold->Text)*D2R;
prcopt.maxtdiff =str2dbl(MaxAgeDiff ->Text);
prcopt.maxgdop =str2dbl(RejectGdop ->Text);
prcopt.maxinno =str2dbl(RejectThres->Text);
prcopt.thresslip=str2dbl(SlipThres ->Text);
prcopt.niter =str2dbl(NumIter ->Text);
prcopt.syncsol =SyncSol->ItemIndex;
if (prcopt.mode==PMODE_MOVEB&&BaselineConst->Checked) {
prcopt.baseline[0]=str2dbl(BaselineLen->Text);
prcopt.baseline[1]=str2dbl(BaselineSig->Text);
}
solopt.posf =SolFormat ->ItemIndex;
solopt.timef =TimeFormat ->ItemIndex==0?0:1;
solopt.times =TimeFormat ->ItemIndex==0?0:TimeFormat->ItemIndex-1;
solopt.timeu =str2dbl(TimeDecimal ->Text);
solopt.degf =LatLonFormat->ItemIndex;
strcpy(solopt.sep,FieldSep_Text.c_str());
solopt.outhead =OutputHead ->ItemIndex;
solopt.outopt =OutputOpt ->ItemIndex;
solopt.datum =OutputDatum ->ItemIndex;
solopt.height =OutputHeight->ItemIndex;
solopt.geoid =OutputGeoid ->ItemIndex;
solopt.nmeaintv[0]=str2dbl(NmeaIntv1->Text);
solopt.nmeaintv[1]=str2dbl(NmeaIntv2->Text);
solopt.trace =DebugTrace ->ItemIndex;
solopt.sstat =DebugStatus ->ItemIndex;
prcopt.eratio[0]=str2dbl(MeasErrR1->Text);
prcopt.eratio[1]=str2dbl(MeasErrR2->Text);
prcopt.err[1] =str2dbl(MeasErr2->Text);
prcopt.err[2] =str2dbl(MeasErr3->Text);
prcopt.err[3] =str2dbl(MeasErr4->Text);
prcopt.err[4] =str2dbl(MeasErr5->Text);
prcopt.sclkstab =str2dbl(SatClkStab->Text);
prcopt.prn[0] =str2dbl(PrNoise1->Text);
prcopt.prn[1] =str2dbl(PrNoise2->Text);
prcopt.prn[2] =str2dbl(PrNoise3->Text);
prcopt.prn[3] =str2dbl(PrNoise4->Text);
prcopt.prn[4] =str2dbl(PrNoise5->Text);
if (RovAntPcv->Checked) strcpy(prcopt.anttype[0],RovAnt_Text.c_str());
if (RefAntPcv->Checked) strcpy(prcopt.anttype[1],RefAnt_Text.c_str());
prcopt.antdel[0][0]=str2dbl(RovAntE->Text);
prcopt.antdel[0][1]=str2dbl(RovAntN->Text);
prcopt.antdel[0][2]=str2dbl(RovAntU->Text);
prcopt.antdel[1][0]=str2dbl(RefAntE->Text);
prcopt.antdel[1][1]=str2dbl(RefAntN->Text);
prcopt.antdel[1][2]=str2dbl(RefAntU->Text);
prcopt.rovpos=RovPosTypeP->ItemIndex<3?0:4;
prcopt.refpos=RefPosTypeP->ItemIndex<3?0:4;
if (prcopt.rovpos==0) GetPos(RovPosTypeP->ItemIndex,editu,prcopt.ru);
if (prcopt.refpos==0) GetPos(RefPosTypeP->ItemIndex,editr,prcopt.rb);
strcpy(filopt.satantp,SatPcvFile_Text.c_str());
strcpy(filopt.rcvantp,AntPcvFile_Text.c_str());
strcpy(filopt.stapos, StaPosFile_Text.c_str());
strcpy(filopt.geoid, GeoidDataFile_Text.c_str());
strcpy(filopt.dcb, DCBFile_Text.c_str());
strcpy(filopt.tempdir,LocalDir_Text.c_str());
time2str(utc2gpst(timeget()),s,0);
sprintf(comment,"RTKNAVI options (%s, v.%s)",s,VER_RTKLIB);
setsysopts(&prcopt,&solopt,&filopt);
if (!saveopts(file.c_str(),"w",comment,sysopts)||
!saveopts(file.c_str(),"a","",rcvopts)) return;
}
/* str2str -------------------------------------------------------------------*/
int main(int argc, char **argv)
{
static char cmd[MAXRCVCMD]="";
const char ss[]={'E','-','W','C','C'};
strconv_t *conv[MAXSTR]={NULL};
double pos[3],stapos[3]={0},off[3]={0};
char *paths[MAXSTR],s[MAXSTR][MAXSTRPATH]={{0}},*cmdfile="";
char *local="",*proxy="",*msg="1004,1019",*opt="",buff[256],*p;
char strmsg[MAXSTRMSG]="",*antinfo="",*rcvinfo="";
char *ant[]={"","",""},*rcv[]={"","",""};
int i,j,n=0,dispint=5000,trlevel=0,opts[]={10000,10000,2000,32768,10,0,30};
int types[MAXSTR]={STR_FILE,STR_FILE},stat[MAXSTR]={0},byte[MAXSTR]={0};
int bps[MAXSTR]={0},fmts[MAXSTR]={0},sta=0;
int ctl_port=2097;
char ctl_buffer[256];
char *nodefile;
int sockfd;
char *rip;
struct sockaddr_in serv_addr;
rip=ctl_buffer;
fd_set sockset;
roverMsg rmsg;
for (i=0;i<MAXSTR;i++) paths[i]=s[i];
for (i=1;i<argc;i++) {
if (!strcmp(argv[i],"-in")&&i+1<argc) {
if (!decodepath(argv[++i],types,paths[0],fmts)) return -1;
}
else if (!strcmp(argv[i],"-out")&&i+1<argc&&n<MAXSTR-1) {
if (!decodepath(argv[++i],types+n+1,paths[n+1],fmts+n+1)) return -1;
n++;
}
else if (!strcmp(argv[i],"-p")&&i+3<argc) {
pos[0]=atof(argv[++i])*D2R;
pos[1]=atof(argv[++i])*D2R;
pos[2]=atof(argv[++i]);
pos2ecef(pos,stapos);
}
else if (!strcmp(argv[i],"-o")&&i+3<argc) {
off[0]=atof(argv[++i]);
off[1]=atof(argv[++i]);
off[2]=atof(argv[++i]);
}
else if (!strcmp(argv[i],"-msg")&&i+1<argc) msg=argv[++i];
else if (!strcmp(argv[i],"-opt")&&i+1<argc) opt=argv[++i];
else if (!strcmp(argv[i],"-sta")&&i+1<argc) sta=atoi(argv[++i]);
else if (!strcmp(argv[i],"-d" )&&i+1<argc) dispint=atoi(argv[++i]);
else if (!strcmp(argv[i],"-s" )&&i+1<argc) opts[0]=atoi(argv[++i]);
else if (!strcmp(argv[i],"-r" )&&i+1<argc) opts[1]=atoi(argv[++i]);
else if (!strcmp(argv[i],"-n" )&&i+1<argc) opts[5]=atoi(argv[++i]);
else if (!strcmp(argv[i],"-f" )&&i+1<argc) opts[6]=atoi(argv[++i]);
else if (!strcmp(argv[i],"-c" )&&i+1<argc) cmdfile=argv[++i];
else if (!strcmp(argv[i],"-a" )&&i+1<argc) antinfo=argv[++i];
else if (!strcmp(argv[i],"-i" )&&i+1<argc) rcvinfo=argv[++i];
else if (!strcmp(argv[i],"-l" )&&i+1<argc) local=argv[++i];
else if (!strcmp(argv[i],"-x" )&&i+1<argc) proxy=argv[++i];
else if (!strcmp(argv[i],"-t" )&&i+1<argc) trlevel=atoi(argv[++i]);
else if (!strcmp(argv[i],"-port")&&i+1<argc) ctl_port=atoi(argv[++i]);
else if (!strcmp(argv[i], "-nodefile")&& i+1<argc) nodefile=argv[++i];
else if (!strcmp(argv[i], "-rip")&&i+1<argc) rip=argv[++i];
else if (*argv[i]=='-') printhelp();
}
if (n<=0) n=1; /* stdout */
fprintf(stderr,"argv parsed\n");
if(rip==ctl_buffer)
{
fprintf(stderr, "No rover ip address\n");
return(-1);
}
serv_addr.sin_family=AF_INET;
inet_aton(rip, &serv_addr.sin_addr);
serv_addr.sin_port=htons(ctl_port);
sockfd=socket(AF_INET,SOCK_STREAM, 0);
if (sockfd < 0)
fprintf(stderr,"ERROR opening socket\n");
FD_ZERO(&sockset);
FD_SET(sockfd, &sockset);
for (i=0;i<n;i++) {
if (fmts[i+1]<=0) continue;
if (fmts[i+1]!=STRFMT_RTCM3) {
fprintf(stderr,"unsupported output format\n");
return -1;
}
if (fmts[0]<0) {
fprintf(stderr,"specify input format\n");
return -1;
}
if (!(conv[i]=strconvnew(fmts[0],fmts[i+1],msg,sta,sta!=0,opt))) {
fprintf(stderr,"stream conversion error\n");
return -1;
}
strcpy(buff,antinfo);
for (p=strtok(buff,","),j=0;p&&j<3;p=strtok(NULL,",")) ant[j++]=p;
strcpy(conv[i]->out.sta.antdes,ant[0]);
strcpy(conv[i]->out.sta.antsno,ant[1]);
conv[i]->out.sta.antsetup=atoi(ant[2]);
strcpy(buff,rcvinfo);
for (p=strtok(buff,","),j=0;p&&j<3;p=strtok(NULL,",")) rcv[j++]=p;
strcpy(conv[i]->out.sta.rectype,rcv[0]);
strcpy(conv[i]->out.sta.recver ,rcv[1]);
strcpy(conv[i]->out.sta.recsno ,rcv[2]);
matcpy(conv[i]->out.sta.pos,pos,3,1);
matcpy(conv[i]->out.sta.del,off,3,1);
}
signal(SIGTERM,sigfunc);
signal(SIGINT ,sigfunc);
signal(SIGHUP ,SIG_IGN);
signal(SIGPIPE,SIG_IGN);
strsvrinit(&strsvr,n+1);
if (trlevel>0) {
traceopen(TRFILE);
tracelevel(trlevel);
}
fprintf(stderr,"stream server start\n");
strsetdir(local);
strsetproxy(proxy);
if (*cmdfile) readcmd(cmdfile,cmd,0);
/* start stream server */
if (!strsvrstart(&strsvr,opts,types,paths,conv,*cmd?cmd:NULL,stapos)) {
fprintf(stderr,"stream server start error\n");
return -1;
}
// Socket server start
if(connect(sockfd,(struct sockaddr*)&serv_addr,sizeof(serv_addr))<0)
fprintf(stderr,"error connecting\n");
if (bind(sockfd, (struct sockaddr *) &serv_addr,
sizeof(serv_addr)) < 0)
fprintf(stderr,"ERROR on binding");
for (intrflg=0;!intrflg;) {
/* get stream server status */
strsvrstat(&strsvr,stat,byte,bps,strmsg);
/* show stream server status */
for (i=0,p=buff;i<MAXSTR;i++) p+=sprintf(p,"%c",ss[stat[i]+1]);
fprintf(stderr,"%s [%s] %10d B %7d bps %s\n",
time_str(utc2gpst(timeget()),0),buff,byte[0],bps[0],strmsg);
sleepms(dispint);
}
if (*cmdfile) readcmd(cmdfile,cmd,1);
/* stop stream server */
strsvrstop(&strsvr,*cmd?cmd:NULL);
for (i=0;i<n;i++) {
strconvfree(conv[i]);
}
if (trlevel>0) {
traceclose();
}
fprintf(stderr,"stream server stop\n");
return 0;
}
int main(int argc,char **argv)
{
ros::init(argc, argv, "rtk_robot");
ROS_INFO("RTKlib for ROS Robot Edition");
ros::NodeHandle nn;
ros::NodeHandle pn("~");
ros::Subscriber ecef_sub;
if(pn.getParam("base_position/x", ecef_base_station.position.x) && pn.getParam("base_position/y", ecef_base_station.position.y) && pn.getParam("base_position/z", ecef_base_station.position.z))
{
ROS_INFO("RTK -- Loading base station parameters from the parameter server...");
XmlRpc::XmlRpcValue position_covariance;
if( pn.getParam("base_position/covariance", position_covariance) )
{
ROS_ASSERT(position_covariance.getType() == XmlRpc::XmlRpcValue::TypeArray);
if(position_covariance.size() != 9)
{
ROS_WARN("RTK -- The base station covariances are not complete! Using default values...");
}
else
{
for(int i=0 ; i<position_covariance.size() ; ++i)
{
ROS_ASSERT(position_covariance[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
ecef_base_station.position_covariance[i] = static_cast<double>(position_covariance[i]);
}
}
}
}
else
{
ROS_INFO("RTK -- Subscribing to the base station for online parameters...");
ecef_sub = nn.subscribe("base_station/gps/ecef", 50, ecefCallback);
}
double rate;
pn.param("rate", rate, 2.0);
std::string gps_frame_id;
pn.param<std::string>("gps_frame_id", gps_frame_id, "gps");
std::string port;
pn.param<std::string>("port", port, "ttyACM0");
int baudrate;
pn.param("baudrate", baudrate, 115200);
ros::Publisher gps_pub = nn.advertise<sensor_msgs::NavSatFix>("gps/fix", 50);
ros::Publisher status_pub = nn.advertise<rtk_msgs::Status>("gps/status", 50);
ros::Subscriber gps_sub = nn.subscribe("base_station/gps/raw_data", 50, baseStationCallback);
int n;
//********************* rtklib stuff *********************
rtksvrinit(&server);
if(server.state)
{
ROS_FATAL("RTK -- Failed to initialize rtklib server!");
ROS_BREAK();
}
gtime_t time, time0 = {0};
int format[] = {STRFMT_UBX, STRFMT_UBX, STRFMT_RTCM2};
prcopt_t options = prcopt_default;
options.mode = 2;
options.nf = 1;
options.navsys = SYS_GPS | SYS_SBS;
options.modear = 3;
options.glomodear = 0;
options.minfix = 3;
options.ionoopt = IONOOPT_BRDC;
options.tropopt = TROPOPT_SAAS;
options.rb[0] = ecef_base_station.position.x;
options.rb[1] = ecef_base_station.position.y;
options.rb[2] = ecef_base_station.position.z;
strinitcom();
server.cycle = 10;
server.nmeacycle = 1000;
server.nmeareq = 0;
for(int i=0 ; i<3 ; i++) server.nmeapos[i] = 0;
server.buffsize = BUFFSIZE;
for(int i=0 ; i<3 ; i++) server.format[i] = format[i];
server.navsel = 0;
server.nsbs = 0;
server.nsol = 0;
server.prcout = 0;
rtkfree(&server.rtk);
rtkinit(&server.rtk, &options);
for(int i=0 ; i<3 ; i++)
{
server.nb[i] = server.npb[i] = 0;
if(!(server.buff[i]=(unsigned char *)malloc(BUFFSIZE)) || !(server.pbuf[i]=(unsigned char *)malloc(BUFFSIZE)))
{
ROS_FATAL("RTK -- Failed to initialize rtklib server - malloc error!");
ROS_BREAK();
}
for(int j=0 ; j<10 ; j++) server.nmsg[i][j] = 0;
for(int j=0 ; j<MAXOBSBUF ; j++) server.obs[i][j].n = 0;
/* initialize receiver raw and rtcm control */
init_raw(server.raw + i);
init_rtcm(server.rtcm + i);
/* set receiver option */
strcpy(server.raw[i].opt, "");
strcpy(server.rtcm[i].opt, "");
/* connect dgps corrections */
server.rtcm[i].dgps = server.nav.dgps;
}
/* output peek buffer */
for(int i=0 ; i<2 ; i++)
{
if (!(server.sbuf[i]=(unsigned char *)malloc(BUFFSIZE)))
{
ROS_FATAL("RTK -- Failed to initialize rtklib server - malloc error!");
ROS_BREAK();
}
}
/* set solution options */
solopt_t sol_options[2];
sol_options[0] = solopt_default;
sol_options[1] = solopt_default;
for(int i=0 ; i<2 ; i++) server.solopt[i] = sol_options[i];
/* set base station position */
for(int i=0 ; i<6 ; i++) server.rtk.rb[i] = i < 3 ? options.rb[i] : 0.0;
/* update navigation data */
for(int i=0 ; i<MAXSAT*2 ; i++) server.nav.eph[i].ttr = time0;
for(int i=0 ; i<NSATGLO*2 ; i++) server.nav.geph[i].tof = time0;
for(int i=0 ; i<NSATSBS*2 ; i++) server.nav.seph[i].tof = time0;
updatenav(&server.nav);
/* set monitor stream */
server.moni = NULL;
/* open input streams */
int stream_type[8] = {STR_SERIAL, 0, 0, 0, 0, 0, 0, 0};
char gps_path[64];
sprintf(gps_path, "%s:%d:8:n:1:off", port.c_str(), baudrate);
char * paths[] = {gps_path, "localhost:27015", "", "", "", "", "", ""};
char * cmds[] = {"", "", ""};
int rw;
for(int i=0 ; i<8 ; i++)
{
rw = i < 3 ? STR_MODE_R : STR_MODE_W;
if(stream_type[i] != STR_FILE) rw |= STR_MODE_W;
if(!stropen(server.stream+i, stream_type[i], rw, paths[i]))
{
ROS_ERROR("RTK -- Failed to open stream %s", paths[i]);
for(i-- ; i>=0 ; i--) strclose(server.stream+i);
ROS_FATAL("RTK -- Failed to initialize rtklib server - failed to open all streams!");
ROS_BREAK();
}
/* set initial time for rtcm and raw */
if(i<3)
{
time = utc2gpst(timeget());
server.raw[i].time = stream_type[i] == STR_FILE ? strgettime(server.stream+i) : time;
server.rtcm[i].time = stream_type[i] == STR_FILE ? strgettime(server.stream+i) : time;
}
}
/* sync input streams */
strsync(server.stream, server.stream+1);
strsync(server.stream, server.stream+2);
/* write start commands to input streams */
for(int i=0 ; i<3 ; i++)
{
if(cmds[i]) strsendcmd(server.stream+i, cmds[i]);
}
/* write solution header to solution streams */
for(int i=3 ; i<5 ; i++)
{
unsigned char buff[1024];
int n;
n = outsolheads(buff, server.solopt+i-3);
strwrite(server.stream+i, buff, n);
}
//********************************************************
obs_t obs;
obsd_t data[MAXOBS*2];
server.state=1;
obs.data=data;
double tt;
unsigned int tick;
int fobs[3] = {0};
server.tick = tickget();
ROS_DEBUG("RTK -- Initialization complete.");
ros::Rate r(rate);
while(ros::ok())
{
tick = tickget();
unsigned char *p = server.buff[RTK_ROBOT]+server.nb[RTK_ROBOT];
unsigned char *q = server.buff[RTK_ROBOT]+server.buffsize;
ROS_DEBUG("RTK -- Getting data from GPS...");
/* read receiver raw/rtcm data from input stream */
n = strread(server.stream, p, q-p);
/* write receiver raw/rtcm data to log stream */
strwrite(server.stream+5, p, n);
server.nb[RTK_ROBOT] += n;
/* save peek buffer */
rtksvrlock(&server);
n = n < server.buffsize - server.npb[RTK_ROBOT] ? n : server.buffsize - server.npb[RTK_ROBOT];
memcpy(server.pbuf[RTK_ROBOT] + server.npb[RTK_ROBOT], p, n);
server.npb[RTK_ROBOT] += n;
rtksvrunlock(&server);
ROS_DEBUG("RTK -- Decoding GPS data...");
/* decode data */
fobs[RTK_ROBOT] = decoderaw(&server, RTK_ROBOT);
fobs[RTK_BASE_STATION] = decoderaw(&server, RTK_BASE_STATION);
ROS_DEBUG("RTK -- Got %d observations.", fobs[RTK_ROBOT]);
/* for each rover observation data */
for(int i=0 ; i<fobs[RTK_ROBOT] ; i++)
{
obs.n = 0;
for(int j=0 ; j<server.obs[RTK_ROBOT][i].n && obs.n<MAXOBS*2 ; j++)
{
obs.data[obs.n++] = server.obs[RTK_ROBOT][i].data[j];
}
for(int j=0 ; j<server.obs[1][0].n && obs.n<MAXOBS*2 ; j++)
{
obs.data[obs.n++] = server.obs[1][0].data[j];
}
ROS_DEBUG("RTK -- Calculating RTK positioning...");
/* rtk positioning */
rtksvrlock(&server);
rtkpos(&server.rtk, obs.data, obs.n, &server.nav);
rtksvrunlock(&server);
sensor_msgs::NavSatFix gps_msg;
gps_msg.header.stamp = ros::Time::now();
gps_msg.header.frame_id = gps_frame_id;
rtk_msgs::Status status_msg;
status_msg.stamp = gps_msg.header.stamp;
if(server.rtk.sol.stat != SOLQ_NONE)
{
/* adjust current time */
tt = (int)(tickget()-tick)/1000.0+DTTOL;
timeset(gpst2utc(timeadd(server.rtk.sol.time,tt)));
/* write solution */
unsigned char buff[1024];
n = outsols(buff, &server.rtk.sol, server.rtk.rb, server.solopt);
if(n==141 && buff[0]>'0' && buff[0]<'9')
{
int ano,mes,dia,horas,minutos,Q,nsat;
double segundos,lat,longi,alt,sde,sdn,sdu,sdne,sdeu,sdun;
sscanf((const char *)(buff),"%d/%d/%d %d:%d:%lf %lf %lf %lf %d %d %lf %lf %lf %lf %lf %lf", &ano, &mes, &dia, &horas, &minutos, &segundos, &lat, &longi, &alt, &Q, &nsat, &sdn, &sde, &sdu, &sdne, &sdeu, &sdun);
gps_msg.latitude = lat;
gps_msg.longitude = longi;
gps_msg.altitude = alt;
gps_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
gps_msg.position_covariance[0] = sde + ecef_base_station.position_covariance[0];
gps_msg.position_covariance[1] = sdne + ecef_base_station.position_covariance[1];
gps_msg.position_covariance[2] = sdeu + ecef_base_station.position_covariance[2];
gps_msg.position_covariance[3] = sdne + ecef_base_station.position_covariance[3];
gps_msg.position_covariance[4] = sdn + ecef_base_station.position_covariance[4];
gps_msg.position_covariance[5] = sdun + ecef_base_station.position_covariance[5];
gps_msg.position_covariance[6] = sdeu + ecef_base_station.position_covariance[6];
gps_msg.position_covariance[7] = sdun + ecef_base_station.position_covariance[7];
gps_msg.position_covariance[8] = sdu + ecef_base_station.position_covariance[8];
gps_msg.status.status = Q==5 ? sensor_msgs::NavSatStatus::STATUS_FIX : sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
status_msg.fix_quality = Q;
status_msg.number_of_satellites = nsat;
}
}
else
{
gps_msg.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
}
ROS_DEBUG("RTK -- Publishing ROS msg...");
gps_pub.publish(gps_msg);
status_pub.publish(status_msg);
}
ros::spinOnce();
r.sleep();
}
return(0);
}
/* start rtk server ------------------------------------------------------------
* start rtk server thread
* args : rtksvr_t *svr IO rtk server
* int cycle I server cycle (ms)
* int buffsize I input buffer size (bytes)
* int *strs I stream types (STR_???)
* types[0]=input stream rover
* types[1]=input stream base station
* types[2]=input stream correction
* types[3]=output stream solution 1
* types[4]=output stream solution 2
* types[5]=log stream rover
* types[6]=log stream base station
* types[7]=log stream correction
* char *paths I input stream paths
* int *format I input stream formats (STRFMT_???)
* format[0]=input stream rover
* format[1]=input stream base station
* format[2]=input stream correction
* int navsel I navigation message select
* (0:rover,1:base,2:ephem,3:all)
* char **cmds I input stream start commands
* cmds[0]=input stream rover (NULL: no command)
* cmds[1]=input stream base (NULL: no command)
* cmds[2]=input stream corr (NULL: no command)
* char **rcvopts I receiver options
* rcvopt[0]=receiver option rover
* rcvopt[1]=receiver option base
* rcvopt[2]=receiver option corr
* int nmeacycle I nmea request cycle (ms) (0:no request)
* int nmeareq I nmea request type (0:no,1:base pos,2:single sol)
* double *nmeapos I transmitted nmea position (ecef) (m)
* prcopt_t *prcopt I rtk processing options
* solopt_t *solopt I solution options
* solopt[0]=solution 1 options
* solopt[1]=solution 2 options
* stream_t *moni I monitor stream (NULL: not used)
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
extern int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
char **paths, int *formats, int navsel, char **cmds,
char **rcvopts, int nmeacycle, int nmeareq,
const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni)
{
gtime_t time,time0;
int i,j,rw;
INIT_ZERO(time);
INIT_ZERO(time0);
tracet(3,"rtksvrstart: cycle=%d buffsize=%d navsel=%d nmeacycle=%d nmeareq=%d\n",
cycle,buffsize,navsel,nmeacycle,nmeareq);
if (svr->state) return 0;
strinitcom();
svr->cycle=cycle>1?cycle:1;
svr->nmeacycle=nmeacycle>1000?nmeacycle:1000;
svr->nmeareq=nmeareq;
for (i=0;i<3;i++) svr->nmeapos[i]=nmeapos[i];
svr->buffsize=buffsize>4096?buffsize:4096;
for (i=0;i<3;i++) svr->format[i]=formats[i];
svr->navsel=navsel;
svr->nsbs=0;
svr->nsol=0;
svr->prcout=0;
rtkfree(&svr->rtk);
rtkinit(&svr->rtk,prcopt);
for (i=0;i<3;i++) { /* input/log streams */
svr->nb[i]=svr->npb[i]=0;
if (!(svr->buff[i]=(unsigned char *)malloc(buffsize))||
!(svr->pbuf[i]=(unsigned char *)malloc(buffsize))) {
tracet(1,"rtksvrstart: malloc error\n");
return 0;
}
for (j=0;j<10;j++) svr->nmsg[i][j]=0;
for (j=0;j<MAXOBSBUF;j++) svr->obs[i][j].n=0;
/* initialize receiver raw and rtcm control */
init_raw (svr->raw +i);
init_rtcm(svr->rtcm+i);
/* set receiver and rtcm option */
strcpy(svr->raw [i].opt,rcvopts[i]);
strcpy(svr->rtcm[i].opt,rcvopts[i]);
/* connect dgps corrections */
svr->rtcm[i].dgps=svr->nav.dgps;
}
for (i=0;i<2;i++) { /* output peek buffer */
if (!(svr->sbuf[i]=(unsigned char *)malloc(buffsize))) {
tracet(1,"rtksvrstart: malloc error\n");
return 0;
}
}
/* set solution options */
for (i=0;i<2;i++) {
svr->solopt[i]=solopt[i];
}
/* set base station position */
for (i=0;i<6;i++) {
svr->rtk.rb[i]=i<3?prcopt->rb[i]:0.0;
}
/* update navigation data */
for (i=0;i<MAXSAT *2;i++) svr->nav.eph [i].ttr=time0;
for (i=0;i<NSATGLO*2;i++) svr->nav.geph[i].tof=time0;
for (i=0;i<NSATSBS*2;i++) svr->nav.seph[i].tof=time0;
updatenav(&svr->nav);
/* set monitor stream */
svr->moni=moni;
/* open input streams */
for (i=0;i<8;i++) {
rw=i<3?STR_MODE_R:STR_MODE_W;
if (strs[i]!=STR_FILE) rw|=STR_MODE_W;
if (!stropen(svr->stream+i,strs[i],rw,paths[i])) {
for (i--;i>=0;i--) strclose(svr->stream+i);
return 0;
}
/* set initial time for rtcm and raw */
if (i<3) {
time=utc2gpst(timeget());
svr->raw [i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time;
svr->rtcm[i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time;
}
}
/* sync input streams */
strsync(svr->stream,svr->stream+1);
strsync(svr->stream,svr->stream+2);
/* write start commands to input streams */
for (i=0;i<3;i++) {
if (cmds[i]) strsendcmd(svr->stream+i,cmds[i]);
}
/* write solution header to solution streams */
for (i=3;i<5;i++) {
writesolhead(svr->stream+i,svr->solopt+i-3);
}
/* create rtk server thread */
#ifdef WIN32
if (!(svr->thread=CreateThread(NULL,0,rtksvrthread,svr,0,NULL))) {
#else
if (pthread_create(&svr->thread,NULL,rtksvrthread,svr)) {
#endif
for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i);
return 0;
}
return 1;
}
/* stop rtk server -------------------------------------------------------------
* start rtk server thread
* args : rtksvr_t *svr IO rtk server
* char **cmds I input stream stop commands
* cmds[0]=input stream rover (NULL: no command)
* cmds[1]=input stream base (NULL: no command)
* cmds[2]=input stream ephem (NULL: no command)
* return : none
*-----------------------------------------------------------------------------*/
extern void rtksvrstop(rtksvr_t *svr, char **cmds)
{
int i;
tracet(3,"rtksvrstop:\n");
/* write stop commands to input streams */
rtksvrlock(svr);
for (i=0;i<3;i++) {
if (cmds[i]) strsendcmd(svr->stream+i,cmds[i]);
}
rtksvrunlock(svr);
/* stop rtk server */
svr->state=0;
/* free rtk server thread */
#ifdef WIN32
WaitForSingleObject(svr->thread,10000);
CloseHandle(svr->thread);
#else
pthread_join(svr->thread,NULL);
#endif
}
/* read sp3 body -------------------------------------------------------------*/
static void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact,
char *tsys, int index, int opt, nav_t *nav)
{
peph_t peph;
gtime_t time;
double val,std,base;
int i,j,sat,sys,prn,n=ns*(type=='P'?1:2),pred_o,pred_c;
char buff[1024];
trace(3,"readsp3b: type=%c ns=%d index=%d opt=%d\n",type,ns,index,opt);
while (fgets(buff,sizeof(buff),fp)) {
if (!strncmp(buff,"EOF",3)) break;
if (buff[0]!='*'||str2time(buff,3,28,&time)) {
trace(2,"sp3 invalid epoch %31.31s\n",buff);
continue;
}
if (!strcmp(tsys,"UTC")) time=utc2gpst(time); /* utc->gpst */
peph.time =time;
peph.index=index;
for (i=0;i<MAXSAT;i++) for (j=0;j<4;j++) {
peph.pos[i][j]=0.0;
peph.std[i][j]=0.0f;
}
for (i=0;i<n&&fgets(buff,sizeof(buff),fp);i++) {
if (strlen(buff)<4||buff[0]!='P') continue;
sys=buff[1]==' '?SYS_GPS:code2sys(buff[1]);
prn=(int)str2num(buff,2,2);
if (sys==SYS_SBS) prn+=100;
else if (sys==SYS_QZS) prn+=192; /* extension to sp3-c */
if (!(sat=satno(sys,prn))) continue;
pred_c=strlen(buff)>=76&&buff[75]=='P';
pred_o=strlen(buff)>=80&&buff[79]=='P';
for (j=0;j<4;j++) {
/* read option for predicted value */
if (j< 3&&opt==1&& pred_o) continue;
if (j< 3&&opt==2&&!pred_o) continue;
if (j==3&&opt==1&& pred_c) continue;
if (j==3&&opt==2&&!pred_c) continue;
val=str2num(buff, 4+j*14,14);
std=str2num(buff,61+j* 3,j<3?2:3);
if (val!=0.0&&fabs(val-999999.999999)>=1E-6) {
peph.pos[sat-1][j]=val*(j<3?1000.0:1E-6);
}
if ((base=bfact[j<3?0:1])>0.0) {
peph.std[sat-1][j]=(float)(pow(base,std)*(j<3?1E-3:1E-12));
}
}
}
if (!addpeph(nav,&peph)) return;
}
}
/* decode gloephem -----------------------------------------------------------*/
static int decode_gloephem(int sat, raw_t *raw)
{
geph_t geph={0};
unsigned char *p=(raw->buff)+2;
int prn,tk,tb;
if (raw->len>=93) {
prn =I1(p+ 1);
geph.frq =I1(p+ 2);
geph.pos[0]=R8(p+ 3);
geph.pos[1]=R8(p+11);
geph.pos[2]=R8(p+19);
geph.vel[0]=R8(p+27) * 1e+3;
geph.vel[1]=R8(p+35) * 1e+3;
geph.vel[2]=R8(p+43) * 1e+3;
geph.acc[0]=R8(p+51) * 1e+6;
geph.acc[1]=R8(p+59) * 1e+6;
geph.acc[2]=R8(p+67) * 1e+6;
tb = R8(p+75) * 1e-3;
tk = tb;
geph.gamn =R4(p+83);
geph.taun =R4(p+87) * 1e-3;
geph.age =I2(p+91);
}
else {
trace(2,"nvs NE length error: len=%d\n",raw->len);
return -1;
}
if (!(geph.sat=satno(SYS_GLO,prn))) {
trace(2,"nvs NE satellite error: prn=%d\n",prn);
return -1;
}
if (raw->time.time==0) return 0;
geph.iode=(tb/900)&0x3F;
geph.toe=utc2gpst(adjday(raw->time,tb-10800.0));
geph.tof=utc2gpst(adjday(raw->time,tk-10800.0));
#if 0
/* check illegal ephemeris by toe */
tt=timediff(raw->time,geph.toe);
if (fabs(tt)>3600.0) {
trace(3,"nvs NE illegal toe: prn=%2d tt=%6.0f\n",prn,tt);
return 0;
}
#endif
#if 0
/* check illegal ephemeris by frequency number consistency */
if (raw->nav.geph[prn-MINPRNGLO].toe.time&&
geph.frq!=raw->nav.geph[prn-MINPRNGLO].frq) {
trace(2,"nvs NE illegal freq change: prn=%2d frq=%2d->%2d\n",prn,
raw->nav.geph[prn-MINPRNGLO].frq,geph.frq);
return -1;
}
if (!strstr(raw->opt,"-EPHALL")) {
if (fabs(timediff(geph.toe,raw->nav.geph[prn-MINPRNGLO].toe))<1.0&&
geph.svh==raw->nav.geph[prn-MINPRNGLO].svh) return 0;
}
#endif
raw->nav.geph[prn-1]=geph;
raw->ephsat=geph.sat;
return 2;
}
/* decode glonass ephemeris strings --------------------------------------------
* decode glonass ephemeris string (ref [2])
* args : unsigned char *buff I glonass navigation data string bits in frames
* (without hamming and time mark)
* buff[ 0- 9]: string #1 (77 bits)
* buff[10-19]: string #2
* buff[20-29]: string #3
* buff[30-39]: string #4
* geph_t *geph IO glonass ephemeris message
* return : status (1:ok,0:error)
* notes : geph->tof should be set to frame time witin 1/2 day before calling
* geph->frq is set to 0
*-----------------------------------------------------------------------------*/
extern int decode_glostr(const unsigned char *buff, geph_t *geph)
{
double tow,tod,tof,toe;
int P,P1,P2,P3,P4,tk_h,tk_m,tk_s,tb,ln,NT,slot,M,week;
int i=1,frn1,frn2,frn3,frn4;
trace(3,"decode_glostr:\n");
/* frame 1 */
frn1 =getbitu(buff,i, 4); i+= 4+2;
P1 =getbitu(buff,i, 2); i+= 2;
tk_h =getbitu(buff,i, 5); i+= 5;
tk_m =getbitu(buff,i, 6); i+= 6;
tk_s =getbitu(buff,i, 1)*30; i+= 1;
geph->vel[0]=getbitg(buff,i,24)*P2_20*1E3; i+=24;
geph->acc[0]=getbitg(buff,i, 5)*P2_30*1E3; i+= 5;
geph->pos[0]=getbitg(buff,i,27)*P2_11*1E3; i+=27+4;
/* frame 2 */
frn2 =getbitu(buff,i, 4); i+= 4;
geph->svh =getbitu(buff,i, 3); i+= 3;
P2 =getbitu(buff,i, 1); i+= 1;
tb =getbitu(buff,i, 7); i+= 7+5;
geph->vel[1]=getbitg(buff,i,24)*P2_20*1E3; i+=24;
geph->acc[1]=getbitg(buff,i, 5)*P2_30*1E3; i+= 5;
geph->pos[1]=getbitg(buff,i,27)*P2_11*1E3; i+=27+4;
/* frame 3 */
frn3 =getbitu(buff,i, 4); i+= 4;
P3 =getbitu(buff,i, 1); i+= 1;
geph->gamn =getbitg(buff,i,11)*P2_40; i+=11+1;
P =getbitu(buff,i, 2); i+= 2;
ln =getbitu(buff,i, 1); i+= 1;
geph->vel[2]=getbitg(buff,i,24)*P2_20*1E3; i+=24;
geph->acc[2]=getbitg(buff,i, 5)*P2_30*1E3; i+= 5;
geph->pos[2]=getbitg(buff,i,27)*P2_11*1E3; i+=27+4;
/* frame 4 */
frn4 =getbitu(buff,i, 4); i+= 4;
geph->taun =getbitg(buff,i,22)*P2_30; i+=22;
geph->dtaun =getbitg(buff,i, 5)*P2_30; i+= 5;
geph->age =getbitu(buff,i, 5); i+= 5+14;
P4 =getbitu(buff,i, 1); i+= 1;
geph->sva =getbitu(buff,i, 4); i+= 4+3;
NT =getbitu(buff,i,11); i+=11;
slot =getbitu(buff,i, 5); i+= 5;
M =getbitu(buff,i, 2);
if (frn1!=1||frn2!=2||frn3!=3||frn4!=4) {
trace(3,"decode_glostr error: frn=%d %d %d %d %d\n",frn1,frn2,frn3,frn4);
return 0;
}
if (!(geph->sat=satno(SYS_GLO,slot))) {
trace(2,"decode_glostr error: slot=%d\n",slot);
return 0;
}
geph->frq=0;
geph->iode=tb;
tow=time2gpst(gpst2utc(geph->tof),&week);
tod=fmod(tow,86400.0); tow-=tod;
tof=tk_h*3600.0+tk_m*60.0+tk_s-10800.0; /* lt->utc */
if (tof<tod-43200.0) tof+=86400.0;
else if (tof>tod+43200.0) tof-=86400.0;
geph->tof=utc2gpst(gpst2time(week,tow+tof));
toe=tb*900.0-10800.0; /* lt->utc */
if (toe<tod-43200.0) toe+=86400.0;
else if (toe>tod+43200.0) toe-=86400.0;
geph->toe=utc2gpst(gpst2time(week,tow+toe)); /* utc->gpst */
return 1;
}
