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;
}
