int do_save_desktop (Desktop * d, char *filename) { int f; if ((f = creat (filename, 0644)) >= 0) { int i; Desktop save; memcpy (&save, d, sizeof (Desktop)); save.cal_points = 0; save.cal_file = 0; save.temp_dir = save.image_dir = 0; for (i = 0; i < save.num_views; i++) { save.view[i].filename = 0; memset (&save.view[i].pic.main_image, 0, (unsigned long) \ (&save.view[i].pic.last_pointer) - (unsigned long) \ (&save.view[i].pic.main_image)); } strwrite (f, "stereo\n - saved desktop\n\n"); write (f, &save, sizeof (Desktop)); write (f, d->cal_points, d->num_cal_points * sizeof (Vec)); strwrite (f, d->cal_file); strwrite (f, d->temp_dir); strwrite (f, d->image_dir); if (d->num_views) for (i = 0; i < d->num_views; i++) strwrite (f, d->view[i].filename); close (f); return 0; } else { Cerrordialogue (CMain, 20, 20, " Save Desktop ", \ get_sys_error (" Error trying to save file. ")); } return 1; }
/* ARGSUSED */ static int cnwrite(dev_t dev, struct uio *uio, struct cred *cred) { if (rconsvp == NULL) { uio->uio_resid = 0; return (0); } /* * Output to virtual console for logging if enabled. */ if (vsconsvp != NULL && vsconsvp->v_stream != NULL) { struiod_t uiod; /* * strwrite modifies uio so need to make copy. */ (void) uiodup(uio, &uiod.d_uio, uiod.d_iov, sizeof (uiod.d_iov) / sizeof (*uiod.d_iov)); (void) strwrite(vsconsvp, &uiod.d_uio, cred); } if (rconsvp->v_stream != NULL) return (strwrite(rconsvp, uio, cred)); else return (cdev_write(rconsdev, uio, cred)); }
/* write solution to output stream -------------------------------------------*/ static void writesol(rtksvr_t *svr, int index) { solopt_t solopt=solopt_default; unsigned char buff[1024]; int i,n; tracet(4,"writesol: index=%d\n",index); for (i=0;i<2;i++) { /* output solution */ n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,svr->solopt+i); strwrite(svr->stream+i+3,buff,n); /* save output buffer */ saveoutbuf(svr,buff,n,i); /* output extended solution */ n=outsolexs(buff,&svr->rtk.sol,svr->rtk.ssat,svr->solopt+i); strwrite(svr->stream+i+3,buff,n); /* save output buffer */ saveoutbuf(svr,buff,n,i); } /* output solution to monitor port */ if (svr->moni) { n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,&solopt); strwrite(svr->moni,buff,n); } /* save solution buffer */ if (svr->nsol<MAXSOLBUF) { rtksvrlock(svr); svr->solbuf[svr->nsol++]=svr->rtk.sol; rtksvrunlock(svr); } }
void tcpmux (int s, struct servtab *sep) { char service[MAX_SERV_LEN + 1]; int len; /* Get requested service name */ if ((len = fd_getline (s, service, MAX_SERV_LEN)) < 0) { strwrite (s, "-Error reading service name\r\n"); _exit (1); } service[len] = '\0'; if (debug) fprintf (stderr, "tcpmux: someone wants %s\n", service); /* * Help is a required command, and lists available services, * one per line. */ if (!strcasecmp (service, "help")) { for (sep = servtab; sep; sep = sep->se_next) { if (!ISMUX (sep)) continue; write (s, sep->se_service, strlen (sep->se_service)); strwrite (s, "\r\n"); } _exit (1); } /* Try matching a service in inetd.conf with the request */ for (sep = servtab; sep; sep = sep->se_next) { if (ISMUX (sep) && !strcasecmp (service, sep->se_service)) { if (ISMUXPLUS (sep)) { strwrite (s, "+Go\r\n"); } run_service (s, sep); return; } } strwrite (s, "-Service not available\r\n"); exit (1); }
void baseStationCallback(const std_msgs::ByteMultiArray::ConstPtr& msg) { int n = msg->data.size(); unsigned char *p = server.buff[RTK_BASE_STATION]+server.nb[RTK_BASE_STATION]; unsigned char *q = server.buff[RTK_BASE_STATION]+server.buffsize; if(server.nb[RTK_BASE_STATION] + n < server.buffsize) { for(int i=0 ; i<n ; i++) { *(server.buff[RTK_BASE_STATION] + server.nb[RTK_BASE_STATION] + i) = msg->data[i]; } server.nb[RTK_BASE_STATION] += n; /* write receiver raw/rtcm data to log stream */ strwrite(server.stream+6, p, n); server.nb[RTK_BASE_STATION] += n; /* save peek buffer */ rtksvrlock(&server); n = n < server.buffsize - server.npb[RTK_BASE_STATION] ? n : server.buffsize - server.npb[RTK_BASE_STATION]; memcpy(server.pbuf[RTK_BASE_STATION] + server.npb[RTK_BASE_STATION], p, n); server.npb[RTK_BASE_STATION] += n; rtksvrunlock(&server); } }
// disconnect from external sources ----------------------------------------- void __fastcall TPlot::Disconnect(void) { AnsiString s; char *cmd; int i; trace(3,"Disconnect\n"); if (!ConnectState) return; ConnectState=0; for (i=0;i<2;i++) { if (StrCmdEna[i][1]) { cmd=StrCmds[i][1].c_str(); strwrite(Stream+i,(unsigned char *)cmd,strlen(cmd)); } strclose(Stream+i); } if (strstr(Caption.c_str(),"CONNECT")) { Caption=s.sprintf("DISCONNECT%s",Caption.c_str()+7); } UpdateTime(); UpdatePlot(); }
/* write solution header to output stream ------------------------------------*/ static void writesolhead(stream_t *stream, const solopt_t *solopt) { unsigned char buff[1024]; int n; n=outsolheads(buff,solopt); strwrite(stream,buff,n); }
// connect to external sources ---------------------------------------------- void __fastcall TPlot::Connect(void) { AnsiString s; char *cmd,*path,buff[MAXSTRPATH],*name[2]={"",""},*p; int i,mode=STR_MODE_R; trace(3,"Connect\n"); if (ConnectState) return; for (i=0;i<2;i++) { if (RtStream[i]==STR_NONE ) continue; else if (RtStream[i]==STR_SERIAL ) path=StrPaths[i][0].c_str(); else if (RtStream[i]==STR_FILE ) path=StrPaths[i][2].c_str(); else if (RtStream[i]<=STR_NTRIPCLI) path=StrPaths[i][1].c_str(); else continue; if (RtStream[i]==STR_FILE||!SolData[i].cyclic||SolData[i].nmax!=RtBuffSize+1) { Clear(); initsolbuf(SolData+i,1,RtBuffSize+1); } if (RtStream[i]==STR_SERIAL) mode|=STR_MODE_W; strcpy(buff,path); if ((p=strstr(buff,"::"))) *p='\0'; if ((p=strstr(buff,"/:"))) *p='\0'; if ((p=strstr(buff,"@"))) name[i]=p+1; else name[i]=buff; if (!stropen(Stream+i,RtStream[i],mode,path)) { ShowMsg(s.sprintf("connect error: %s",name)); ShowLegend(NULL); trace(1,"stream open error: ch=%d type=%d path=%s\n",i+1,RtStream[i],path); continue; } strsettimeout(Stream+i,RtTimeOutTime,RtReConnTime); if (StrCmdEna[i][0]) { cmd=StrCmds[i][0].c_str(); strwrite(Stream+i,(unsigned char *)cmd,strlen(cmd)); } ConnectState=1; } if (!ConnectState) return; if (Title!="") Caption=Title; else Caption=s.sprintf("CONNECT %s %s",name[0],name[1]); BtnConnect->Down=true; BtnSol1 ->Down=*name[0]; BtnSol2 ->Down=*name[1]; BtnSol12 ->Down=false; BtnShowTrack->Down=true; BtnFixHoriz->Down=true; UpdateEnable(); UpdateTime(); UpdatePlot(); }
} /* thread to send keep alive for monitor port --------------------------------*/ static void *sendkeepalive(void *arg) { trace(3,"sendkeepalive: start\n"); while (keepalive) { strwrite(&moni,(unsigned char *)"\r",1); sleepms(INTKEEPALIVE); } trace(3,"sendkeepalive: stop\n");
} /* close monitor port --------------------------------------------------------*/ static void closemoni(void) { trace(3,"closemoni:\n"); keepalive=0; /* send disconnect message */ strwrite(&moni,(unsigned char *)MSG_DISCONN,strlen(MSG_DISCONN)); /* wait fin from clients */ sleepms(1000);
/* 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); }
/* send receiver command ------------------------------------------------------- * send receiver commands to stream * args : stream_t *stream I stream * char *cmd I receiver command strings * return : none *-----------------------------------------------------------------------------*/ extern void strsendcmd(stream_t *str, const char *cmd) { unsigned char buff[1024]; const char *p=cmd,*q; char msg[1024],cmdend[]="\r\n"; int n,m,ms; tracet(3,"strsendcmd: cmd=%s\n",cmd); for (;;) { for (q=p;;q++) if (*q=='\r'||*q=='\n'||*q=='\0') break; n=(int)(q-p); strncpy(msg,p,n); msg[n]='\0'; if (!*msg||*msg=='#') { /* null or comment */ ; } else if (*msg=='!') { /* binary escape */ if (!strncmp(msg+1,"WAIT",4)) { /* wait */ if (sscanf(msg+5,"%d",&ms)<1) ms=100; if (ms>3000) ms=3000; /* max 3 s */ sleepms(ms); } else if (!strncmp(msg+1,"UBX",3)) { /* ublox */ if ((m=gen_ubx(msg+4,buff))>0) strwrite(str,buff,m); } else if (!strncmp(msg+1,"STQ",3)) { /* skytraq */ if ((m=gen_stq(msg+4,buff))>0) strwrite(str,buff,m); } else if (!strncmp(msg+1,"NVS",3)) { /* nvs */ if ((m=gen_nvs(msg+4,buff))>0) strwrite(str,buff,m); } else if (!strncmp(msg+1,"LEXR",3)) { /* lex receiver */ if ((m=gen_lexr(msg+5,buff))>0) strwrite(str,buff,m); } } else { strwrite(str,(unsigned char *)msg,n); strwrite(str,(unsigned char *)cmdend,2); } if (*q=='\0') break; else p=q+1; } }
static void *rtksvrthread(void *arg) #endif { rtksvr_t *svr=(rtksvr_t *)arg; obs_t obs; obsd_t data[MAXOBS*2]; double tt; unsigned int tick,ticknmea; unsigned char *p,*q; int i,j,n,fobs[3],cycle,cputime; INIT_ZERO(fobs); tracet(3,"rtksvrthread:\n"); svr->state=1; obs.data=data; svr->tick=tickget(); ticknmea=svr->tick-1000; for (cycle=0;svr->state;cycle++) { tick=tickget(); for (i=0;i<3;i++) { p=svr->buff[i]+svr->nb[i]; q=svr->buff[i]+svr->buffsize; /* read receiver raw/rtcm data from input stream */ if ((n=strread(svr->stream+i,p,q-p))<=0) { continue; } /* write receiver raw/rtcm data to log stream */ strwrite(svr->stream+i+5,p,n); svr->nb[i]+=n; /* save peek buffer */ rtksvrlock(svr); n=n<svr->buffsize-svr->npb[i]?n:svr->buffsize-svr->npb[i]; memcpy(svr->pbuf[i]+svr->npb[i],p,n); svr->npb[i]+=n; rtksvrunlock(svr); } for (i=0;i<3;i++) { if (svr->format[i]==STRFMT_SP3||svr->format[i]==STRFMT_RNXCLK) { /* decode download file */ decodefile(svr,i); } else { /* decode receiver raw/rtcm data */ fobs[i]=decoderaw(svr,i); } } for (i=0;i<fobs[0];i++) { /* for each rover observation data */ obs.n=0; for (j=0;j<svr->obs[0][i].n&&obs.n<MAXOBS*2;j++) { obs.data[obs.n++]=svr->obs[0][i].data[j]; } for (j=0;j<svr->obs[1][0].n&&obs.n<MAXOBS*2;j++) { obs.data[obs.n++]=svr->obs[1][0].data[j]; } /* rtk positioning */ rtksvrlock(svr); rtkpos(&svr->rtk,obs.data,obs.n,&svr->nav); rtksvrunlock(svr); if (svr->rtk.sol.stat!=SOLQ_NONE) { /* adjust current time */ tt=(int)(tickget()-tick)/1000.0+DTTOL; timeset(gpst2utc(timeadd(svr->rtk.sol.time,tt))); /* write solution */ writesol(svr,i); } /* if cpu overload, inclement obs outage counter and break */ if ((int)(tickget()-tick)>=svr->cycle) { svr->prcout+=fobs[0]-i-1; #if 0 /* omitted v.2.4.1 */ break; #endif } } /* send null solution if no solution (1hz) */ if (svr->rtk.sol.stat==SOLQ_NONE&&cycle%(1000/svr->cycle)==0) { writesol(svr,0); } /* send nmea request to base/nrtk input stream */ if (svr->nmeacycle>0&&(int)(tick-ticknmea)>=svr->nmeacycle) { if (svr->stream[1].state==1) { if (svr->nmeareq==1) { strsendnmea(svr->stream+1,svr->nmeapos); } else if (svr->nmeareq==2&&norm(svr->rtk.sol.rr,3)>0.0) { strsendnmea(svr->stream+1,svr->rtk.sol.rr); } } ticknmea=tick; } if ((cputime=(int)(tickget()-tick))>0) svr->cputime=cputime; /* sleep until next cycle */ sleepms(svr->cycle-cputime); } for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i); for (i=0;i<3;i++) { svr->nb[i]=svr->npb[i]=0; free(svr->buff[i]); svr->buff[i]=NULL; free(svr->pbuf[i]); svr->pbuf[i]=NULL; free_raw (svr->raw +i); free_rtcm(svr->rtcm+i); } for (i=0;i<2;i++) { svr->nsb[i]=0; free(svr->sbuf[i]); svr->sbuf[i]=NULL; } 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); }
int main(int argc, char **argv){ if (nva_init()) { fprintf (stderr, "PCI init failure!\n"); return 1; } int c; while ((c = getopt (argc, argv, "c:")) != -1) switch (c) { case 'c': sscanf(optarg, "%d", &cnum); break; } if (cnum >= nva_cardsnum) { if (nva_cardsnum) fprintf (stderr, "No such card.\n"); else fprintf (stderr, "No cards found.\n"); return 1; } nv50_graph_reset(); nva_wr32(cnum, 0x1700, 0x400); nva_wr32(cnum, 0x700020, 0x190000); nva_wr32(cnum, 0x700024, 0x4000000 + 0x80000); nva_wr32(cnum, 0x700028, 0x4000000 + 0x10000); nva_wr32(cnum, 0x70002c, 0); nva_wr32(cnum, 0x700030, 0); nva_wr32(cnum, 0x700034, 0x10000); nva_wr32(cnum, 0x700200, 0x190000); nva_wr32(cnum, 0x700204, 0x4000000 + 0x80000); nva_wr32(cnum, 0x700208, 0x4000000 + 0x10000); nva_wr32(cnum, 0x70020c, 0); nva_wr32(cnum, 0x700210, 0); nva_wr32(cnum, 0x700214, 0x10000); int j; int k; for (j = 0; j < 16; j++) { printf ("Strand %d!\n", j); uint32_t tab[0x3000][6]; kill(j); for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0xdeafbeef); strread(j, 0); for (k = 4*(j&7); k < 0x3000*32; k += 4*8) { tab[k/32][0] = nva_rd32(cnum, 0x710000 + k); } for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strwrite(j, 0); for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strread(j, 0); for (k = 4*(j&7); k < 0x3000*32; k += 4*8) { tab[k/32][1] = nva_rd32(cnum, 0x710000 + k); } for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, -1); strwrite(j, 0); for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strread(j, 0); for (k = 4*(j&7); k < 0x3000*32; k += 4*8) { tab[k/32][2] = nva_rd32(cnum, 0x710000 + k); } for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strwrite(j, 8); for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strread(j, 8); for (k = 4*(j&7); k < 0x3000*32; k += 4*8) { tab[k/32][3] = nva_rd32(cnum, 0x710000 + k); } for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, -1); strwrite(j, 8); for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strread(j, 8); for (k = 4*(j&7); k < 0x3000*32; k += 4*8) { tab[k/32][4] = nva_rd32(cnum, 0x710000 + k); tab[k/32][5] = 0; } int z; for (z = 0; z < 32; z++) { for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, -1); strwrite(j, 8); nva_wr32(cnum, 0x400040, 1 << z); nva_wr32(cnum, 0x400040, 0); strwait(); for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strread(j, 8); for (k = 4*(j&7); k < 0x3000*32; k += 4*8) { if (tab[k/32][4] != nva_rd32(cnum, 0x710000 + k)) tab[k/32][5] |= 1 << z; } } for (k = 0; k < 0x3000; k++) if (tab[k][0] == 0xdeafbeef && !tab[k][4]) { slen[j] = k; break; } if (slen[j]) { printf ("Initial length: %04x\n", slen[j]); while (slen[j]) { for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); nva_wr32(cnum, 0x710000 + (j&7)*4 + (slen[j] - 1) * 32, 0xffffffff); strwrite(j, 8); for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strread(j, 8); if (nva_rd32(cnum, 0x710000 + (j&7)*4 + (slen[j] - 1) * 32)) { break; } else { slen[j]--; } } } printf ("Length: %04x\n", slen[j]); int i; for (i = 0; i < 0x400; i++) { int pos = -1; int l; int ctr = 0; // int try; for (l = 1; l < 0x14; l++) { // try = 0; //respin: for (k = 0; k < 0x70000; k += 4) nva_wr32(cnum, 0x710000 + k, 0); strwrite(j, 8); for (k = 0; k < l; k++) nva_wr32(cnum, 0x400420 + k * 4, 0xffffffff); nva_wr32(cnum, 0x400408, i << 16); nva_wr32(cnum, 0x400404, 0x30000 | (j & 7) << 8 | (j&8) << (12-3) | l); strwait(); strread(j, 8); ctr = 0; pos = -1; for (k = 0; k < 0x70000; k += 4) if (nva_rd32(cnum, 0x710000 + k) && (k & 0x1c) == (j & 7) << 2) { if (pos == -1) pos = k / 32; ctr++; // printf ("%04x: %08x\n", k/32, nva_rd32(cnum, 0x710000 + k)); } if (ctr <= l && ctr) break; // else // if (try++ < 4) // goto respin; } if (pos == -1) { seekcnt[j] = i; break; } assert(ctr == l); printf ("SEEK: %04x [%d/%d]...\n", pos, ctr, l); seekstart[j][i] = pos; seeksize[j][i] = l; } i = 0; for (k = 0; k < slen[j]; k++) { if (i < seekcnt[j] && k == seekstart[j][i]) { printf ("\nSEEK %03x [unit %x]\n", i, seeksize[j][i]); i++; } else if (i && seeksize[j][i-1] != 1 && !((k - seekstart[j][i-1])%seeksize[j][i-1])) printf ("\n"); printf ("%04x: %08x %08x %08x %08x %08x %08x\n", k, tab[k][0], tab[k][1], tab[k][2], tab[k][3], tab[k][4], tab[k][5]); } } return 0; }