/* 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);
}
}
/* timeget() */
void utest10(void)
{
char s1[64],s2[64];
gtime_t time1,time2;
int i,j;
time1=timeget();
for (i=0;i<2000000000;i++) j=1;
time2=timeget();
time2str(time1,s1,6);
time2str(time2,s2,6);
assert(timediff(time1,time2)<=0.0);
printf("%s utset10 : OK\n",__FILE__);
}
}
/* read antenna file ---------------------------------------------------------*/
static void readant(vt_t *vt, prcopt_t *opt, nav_t *nav)
{
const pcv_t pcv0={0};
pcvs_t pcvr={0},pcvs={0};
pcv_t *pcv;
gtime_t time=timeget();
int i;
trace(3,"readant:\n");
opt->pcvr[0]=opt->pcvr[1]=pcv0;
if (!*filopt.rcvantp) return;
if (readpcv(filopt.rcvantp,&pcvr)) {
for (i=0;i<2;i++) {
if (!*opt->anttype[i]) continue;
if (!(pcv=searchpcv(0,opt->anttype[i],time,&pcvr))) {
printvt(vt,"no antenna %s in %s",opt->anttype[i],filopt.rcvantp);
continue;
}
opt->pcvr[i]=*pcv;
}
}
else printvt(vt,"antenna file open error %s",filopt.rcvantp);
if (readpcv(filopt.satantp,&pcvs)) {
for (i=0;i<MAXSAT;i++) {
if (!(pcv=searchpcv(i+1,"",time,&pcvs))) continue;
nav->pcvs[i]=*pcv;
}
}
else printvt(vt,"antenna file open error %s",filopt.satantp);
static void RtkServer__readsatant(JNIEnv* env, jclass thiz, jstring file)
{
struct native_ctx_t *nctx;
pcvs_t pcvs={0};
pcv_t *pcv;
int i;
gtime_t now=timeget();
nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field);
if (nctx == NULL) {
LOGV("nctx is null");
return;
}
const char *filename = (*env)->GetStringUTFChars(env, file, 0);
rtksvrlock(&nctx->rtksvr);
if (readpcv(filename,&pcvs)) {
for (i=0;i<MAXSAT;i++) {
if (!(pcv=searchpcv(i+1,"",now,&pcvs))) continue;
nctx->rtksvr.nav.pcvs[i]=*pcv;
}
}
rtksvrunlock(&nctx->rtksvr);
(*env)->ReleaseStringUTFChars(env,file, filename);
}
/* 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;
}
//---------------------------------------------------------------------------
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;
}
/* 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);
}
/* 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);
}
void MClient::parseBuffer(){
m_buffer.remove(SEP + QString(_MTEMP_BOARD_OK));
switch (m_currentCommand) {
case Conf:
//per ora nulla;
break;
case TimeGet:
parseTimeGet();
break;
case TimeSet:
QThread::msleep(250);
m_currentCommand = NoCommand;
m_buffer.clear();
timeget();
return;
case RoomStat:
parseRoomStat();
break;
case RoomSet:
QThread::msleep(250);
m_currentCommand = NoCommand;
m_buffer.clear();
roomstat(m_currentRoom);
return;
case ProgGet:
parseProgGet();
break;
case ProgSet:
QThread::msleep(250);
m_currentCommand = NoCommand;
m_buffer.clear();
progget(m_currentRoom, m_currentDay);
return;
default:
break;
}
m_currentCommand = NoCommand;
m_buffer.clear();
}
/* convert pvt message -------------------------------------------------------*/
static void convpvt(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n)
{
gtime_t time,current;
static gtime_t firsttime={0,0};
double ep[6];
trace(3,"convpvt :\n");
if (!ofp[7]) return;
time=str->pvt->time;
if (!screent(time,opt->ts,opt->te,opt->tint)) return;
if(n[7]==0)
{
current=timeget();
if(timediff(time,current)>86400) return;
firsttime=time;
time2epoch(time,ep);
fprintf(ofp[7], "%04.0f %02.0f %02.0f %02.0f %02.0f %02.6f\n",
ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]);
}
fprintf(ofp[7], "%15.7f %14.3lf %14.3lf %14.3lf %11.3lf %11.3lf %11.3lf",
timediff(time,firsttime), str->pvt->pos[0], str->pvt->pos[1], str->pvt->pos[2],
str->pvt->vel[0], str->pvt->vel[1], str->pvt->vel[2]);
fprintf(ofp[7],"\n");
if (opt->tstart.time==0) opt->tstart=time;
opt->tend=time;
n[7]++;
}
/* execute processing session ------------------------------------------------*/
static int execses(gtime_t ts, gtime_t te, double ti, const prcopt_t *popt,
const solopt_t *sopt, const filopt_t *fopt, int flag,
char **infile, const int *index, int n, char *outfile)
{
FILE *fp;
prcopt_t popt_=*popt;
char tracefile[1024],statfile[1024];
trace(3,"execses : n=%d outfile=%s\n",n,outfile);
/* open debug trace */
if (flag&&sopt->trace>0) {
if (*outfile) {
strcpy(tracefile,outfile);
strcat(tracefile,".trace");
}
else {
strcpy(tracefile,fopt->trace);
}
traceclose();
traceopen(tracefile);
tracelevel(sopt->trace);
}
/* read obs and nav data */
if (!readobsnav(ts,te,ti,infile,index,n,&popt_,&obss,&navs,stas)) return 0;
/* set antenna paramters */
if (popt_.sateph==EPHOPT_PREC||popt_.sateph==EPHOPT_SSRCOM) {
setpcv(obss.n>0?obss.data[0].time:timeget(),&popt_,&navs,&pcvss,&pcvsr,
stas);
}
/* read ocean tide loading parameters */
if (popt_.mode>PMODE_SINGLE&&fopt->blq) {
readotl(&popt_,fopt->blq,stas);
}
/* rover/reference fixed position */
if (popt_.mode==PMODE_FIXED) {
if (!antpos(&popt_,1,&obss,&navs,stas,fopt->stapos)) {
freeobsnav(&obss,&navs);
return 0;
}
}
else if (PMODE_DGPS<=popt_.mode&&popt_.mode<=PMODE_STATIC) {
if (!antpos(&popt_,2,&obss,&navs,stas,fopt->stapos)) {
freeobsnav(&obss,&navs);
return 0;
}
}
/* open solution statistics */
if (flag&&sopt->sstat>0) {
strcpy(statfile,outfile);
strcat(statfile,".stat");
rtkclosestat();
rtkopenstat(statfile,sopt->sstat);
}
/* write header to output file */
if (flag&&!outhead(outfile,infile,n,&popt_,sopt)) {
freeobsnav(&obss,&navs);
return 0;
}
iobsu=iobsr=isbs=ilex=revs=aborts=0;
if (popt_.mode==PMODE_SINGLE||popt_.soltype==0) {
if ((fp=openfile(outfile))) {
procpos(fp,&popt_,sopt,0); /* forward */
fclose(fp);
}
}
else if (popt_.soltype==1) {
if ((fp=openfile(outfile))) {
revs=1; iobsu=iobsr=obss.n-1; isbs=sbss.n-1; ilex=lexs.n-1;
procpos(fp,&popt_,sopt,0); /* backward */
fclose(fp);
}
}
else { /* combined */
solf=(sol_t *)malloc(sizeof(sol_t)*nepoch);
solb=(sol_t *)malloc(sizeof(sol_t)*nepoch);
rbf=(double *)malloc(sizeof(double)*nepoch*3);
rbb=(double *)malloc(sizeof(double)*nepoch*3);
if (solf&&solb) {
isolf=isolb=0;
procpos(NULL,&popt_,sopt,1); /* forward */
revs=1; iobsu=iobsr=obss.n-1; isbs=sbss.n-1; ilex=lexs.n-1;
procpos(NULL,&popt_,sopt,1); /* backward */
/* combine forward/backward solutions */
if (!aborts&&(fp=openfile(outfile))) {
combres(fp,&popt_,sopt);
fclose(fp);
}
}
else showmsg("error : memory allocation");
free(solf);
free(solb);
free(rbf);
free(rbb);
}
/* free obs and nav data */
freeobsnav(&obss,&navs);
return aborts?1:0;
}
/* 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;
}
static jboolean RtkServer__rtksvrstart(JNIEnv* env, jclass thiz,
jint j_cycle,
jint j_buffsize,
jintArray j_strs,
jobjectArray j_paths,
jintArray j_format,
jint j_navsel,
jobjectArray j_cmds,
jobjectArray j_rcvopts,
jint j_nmea_cycle,
jint j_nmea_req,
jdoubleArray j_nmeapos,
jobject j_procopt,
jobject j_solopt1,
jobject j_solopt2
) {
struct native_ctx_t *nctx;
int i;
jobject obj;
jboolean res;
/* Input stream types */
int strs[8];
/* input stream paths */
const char *paths[8] = {NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL };
jstring paths_jstring[8] = {NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL };
/* input stream formats (STRFMT_???) */
int format[3];
/* input stream start commands */
const char *cmds[3] = {NULL, NULL, NULL};
jstring cmds_jstring[3] = {NULL, NULL, NULL};
/* receiver options */
const char *rcvopts[3] = {NULL, NULL, NULL};
char *rcvopts_jstring[3] = {NULL, NULL, NULL};
/* rtk processing options */
prcopt_t prcopt;
/* solution options */
solopt_t solopt[2];
/* nmea position */
double nmeapos[3];
res = JNI_FALSE;
for (i=0; i<sizeof(paths)/sizeof(paths[0]); ++i) {
paths_jstring[i] = (*env)->GetObjectArrayElement(env, j_paths, i);
if ((*env)->ExceptionOccurred(env))
goto rtksvrstart_end;
if (paths_jstring[i] != NULL) {
paths[i] = (*env)->GetStringUTFChars(env, paths_jstring[i], NULL);
if (paths[i] == NULL)
goto rtksvrstart_end;
}
}
for (i=0; i<sizeof(cmds)/sizeof(cmds[0]); ++i) {
cmds_jstring[i] = (*env)->GetObjectArrayElement(env, j_cmds, i);
if ((*env)->ExceptionOccurred(env))
goto rtksvrstart_end;
if (cmds_jstring[i] != NULL) {
cmds[i] = (*env)->GetStringUTFChars(env, cmds_jstring[i], NULL);
if (cmds[i] == NULL)
goto rtksvrstart_end;
}
}
for (i=0; i<sizeof(rcvopts)/sizeof(rcvopts[0]); ++i) {
rcvopts_jstring[i] = (*env)->GetObjectArrayElement(env, j_rcvopts, i);
if ((*env)->ExceptionOccurred(env))
goto rtksvrstart_end;
if (rcvopts_jstring[i] != NULL) {
rcvopts[i] = (*env)->GetStringUTFChars(env, rcvopts_jstring[i], NULL);
if (rcvopts[i] == NULL)
goto rtksvrstart_end;
}
}
(*env)->GetIntArrayRegion(env, j_strs, 0,
sizeof(strs)/sizeof(strs[0]), strs);
if ((*env)->ExceptionOccurred(env))
goto rtksvrstart_end;
(*env)->GetIntArrayRegion(env, j_format, 0,
sizeof(format)/sizeof(format[0]), format);
if ((*env)->ExceptionOccurred(env))
goto rtksvrstart_end;
(*env)->GetDoubleArrayRegion(env, j_nmeapos, 0,
sizeof(nmeapos)/sizeof(nmeapos[0]), nmeapos);
if ((*env)->ExceptionOccurred(env))
goto rtksvrstart_end;
solution_options2solopt_t(env, j_solopt1, &solopt[0]);
solution_options2solopt_t(env, j_solopt2, &solopt[1]);
nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field);
if (nctx == NULL) {
LOGV("nctx is null");
goto rtksvrstart_end;
}
{
/* Open trace / solution files */
gtime_t now;
now = timeget();
if (solopt[0].trace > 0)
open_trace_file(env, solopt[0].trace, now);
/* treat processing options only now for having trace */
processing_options2prcopt_t(env, j_procopt, &prcopt);
if (solopt[0].sstat > 0)
open_solution_status_file(env, solopt[0].sstat, now);
}
if ((*env)->ExceptionOccurred(env))
goto rtksvrstart_end;
char errmsg[2048]=""; // Modif Mathieu Peyréga : adapt to new 2.4.3b26 API
char *cmds_periodic[]={NULL,NULL,NULL}; // Modif Mathieu Peyréga : adapt to new 2.4.3b26 API
if (!rtksvrstart(
&nctx->rtksvr,
/* SvrCycle ms */ j_cycle,
/* SvrBuffSize */ j_buffsize,
/* stream types */ strs,
/* paths */ (char **)paths,
/* input stream format */ format,
/* NavSelect */ j_navsel,
/* input stream start commands */ (char **)cmds,
/* input stream periodic commands */ (char **)cmds_periodic, // Modif Mathieu Peyréga : adapt to new 2.4.3b26 API
/* receiver options */ (char **)rcvopts,
/* nmea request cycle ms */ j_nmea_cycle,
/* nmea request type */ j_nmea_req,
/* transmitted nmea position */ nmeapos,
/* rtk processing options */&prcopt,
/* solution options */ solopt,
/* monitor stream */ &nctx->monistr,
/* err */errmsg // Modif Mathieu Peyréga : adapt to new 2.4.3b26 API
)) {
}else {
res = JNI_TRUE;
}
rtksvrstart_end:
for (i=0; i<sizeof(paths)/sizeof(paths[0]); ++i) {
if (paths[i] != NULL)
(*env)->ReleaseStringUTFChars(env, paths_jstring[i], paths[i]);
}
for (i=0; i<sizeof(cmds)/sizeof(cmds[0]); ++i) {
if (cmds[i] != NULL)
(*env)->ReleaseStringUTFChars(env, cmds_jstring[i], cmds[i]);
}
for (i=0; i<sizeof(rcvopts)/sizeof(rcvopts[0]); ++i) {
if (rcvopts[i] != NULL)
(*env)->ReleaseStringUTFChars(env, rcvopts_jstring[i], rcvopts[i]);
}
if (!res) {
traceclose();
rtkclosestat();
}
return res;
}
/* 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;
}
/* 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
}
/* execute local file test -----------------------------------------------------
* execute local file test
* args : gtime_t ts,te I time start and end
* double tint I time interval (s)
* url_t *urls I download urls
* int nurl I number of urls
* char **stas I stations
* int nsta I number of stations
* char *dir I local directory
* int ncol I number of column
* int datefmt I date format (0:year-dow,1:year-dd/mm,2:week)
* FILE *fp IO log test result file pointer
* return : status (1:ok,0:error,-1:aborted)
*-----------------------------------------------------------------------------*/
extern void dl_test(gtime_t ts, gtime_t te, double ti, const url_t *urls,
int nurl, char **stas, int nsta, const char *dir,
int ncol, int datefmt, FILE *fp)
{
gtime_t time;
double tow;
char year[32],date[32],date_p[32];
int i,j,n,m,*nc,*nt,week,flag,abort=0;
if (ncol<1) ncol=1; else if (ncol>200) ncol=200;
fprintf(fp,"** LOCAL DATA AVAILABILITY (%s, %s) **\n\n",
time_str(timeget(),0),*dir?dir:"*");
for (i=n=0;i<nurl;i++) {
n+=strstr(urls[i].path,"%s")||strstr(urls[i].path,"%S")?nsta:1;
}
nc=imat(n,1);
nt=imat(n,1);
for (i=0;i<n;i++) nc[i]=nt[i]=0;
for (;timediff(ts,te)<1E-3&&!abort;ts=timeadd(ts,ti*ncol)) {
genpath(datefmt==0?" %Y-":"%Y/%m/","",ts,0,year);
if (datefmt<=1) fprintf(fp,"%s %s",datefmt==0?"DOY ":"DATE",year);
else fprintf(fp,"WEEK ");
*date_p='\0'; flag=0;
m=datefmt==2?1:2;
for (i=0;i<(ncol+m-1)/m;i++) {
time=timeadd(ts,ti*i*m);
if (timediff(time,te)>=1E-3) break;
if (datefmt<=1) {
genpath(datefmt==0?"%n":"%d","",time,0,date);
fprintf(fp,"%-4s",strcmp(date,date_p)?date:"");
}
else {
if (fabs(time2gpst(time,&week))<1.0) {
fprintf(fp,"%04d",week); flag=1;
}
else {
fprintf(fp,"%s",flag?"":" "); flag=0;
}
}
strcpy(date_p,date);
}
fprintf(fp,"\n");
for (i=j=0;i<nurl&&!abort;i++) {
time=timeadd(ts,ti*ncol-1.0);
if (timediff(time,te)>=0.0) time=te;
/* test local files */
abort=test_locals(ts,time,ti,urls+i,stas,nsta,dir,nc+j,nt+j,fp);
j+=strstr(urls[i].path,"%s")||strstr(urls[i].path,"%S")?nsta:1;
}
fprintf(fp,"\n");
}
fprintf(fp,"# COUNT : FILES/TOTAL\n");
for (i=j=0;i<nurl;i++) {
j+=print_total(urls+i,stas,nsta,nc+j,nt+j,fp);
}
free(nc); free(nt);
}
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);
}
