/* 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);
}
}
/* open output/log stream ------------------------------------------------------
* open output/log stream
* args : rtksvr_t *svr IO rtk server
* int index I output/log stream index
* (3:solution 1,4:solution 2,5:log rover,
* 6:log base station,7:log correction)
* int str I output/log stream types (STR_???)
* char *path I output/log stream path
* solopt_t *solopt I solution options
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
extern int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
const solopt_t *solopt)
{
tracet(3,"rtksvropenstr: index=%d str=%d path=%s\n",index,str,path);
if (index<3||index>7||!svr->state) return 0;
rtksvrlock(svr);
if (svr->stream[index].state>0) {
rtksvrunlock(svr);
return 0;
}
if (!stropen(svr->stream+index,str,STR_MODE_W,path)) {
tracet(2,"stream open error: index=%d\n",index);
rtksvrunlock(svr);
return 0;
}
if (index<=4) {
svr->solopt[index-3]=*solopt;
/* write solution header to solution stream */
writesolhead(svr->stream+index,svr->solopt+index-3);
}
rtksvrunlock(svr);
return 1;
}
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);
}
static void RtkServer__readsp3(JNIEnv* env, jclass thiz, jstring file)
{
struct native_ctx_t *nctx;
nav_t nav={0};
rtksvr_t *svr;
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);
readsp3(filename,&nav,0);
/* test if there is some ephemeris */
if (nav.ne<=0) {
tracet(1,"sp3 file read error: %s\n",file);
return;
}
//OK so update to server
svr = &nctx->rtksvr;
rtksvrlock(svr);
if (svr->nav.peph) free(svr->nav.peph);
svr->nav.ne=svr->nav.nemax=nav.ne;
svr->nav.peph=nav.peph;
rtksvrunlock(svr);
(*env)->ReleaseStringUTFChars(env,file, filename);
}
/* 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);
}
}
/* get observation data status -------------------------------------------------
* get current observation data status
* args : rtksvr_t *svr I rtk server
* int rcv I receiver (0:rover,1:base,2:ephem)
* gtime_t *time O time of observation data
* int *sat O satellite prn numbers
* double *az O satellite azimuth angles (rad)
* double *el O satellite elevation angles (rad)
* int **snr O satellite snr for each freq (dBHz)
* snr[i][j] = sat i freq j snr
* int *vsat O valid satellite flag
* return : number of satellites
*-----------------------------------------------------------------------------*/
extern int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
double *az, double *el, int **snr, int *vsat)
{
int i,j,ns;
tracet(4,"rtksvrostat: rcv=%d\n",rcv);
if (!svr->state) return 0;
rtksvrlock(svr);
ns=svr->obs[rcv][0].n;
if (ns>0) {
*time=svr->obs[rcv][0].data[0].time;
}
for (i=0;i<ns;i++) {
sat [i]=svr->obs[rcv][0].data[i].sat;
az [i]=svr->rtk.ssat[sat[i]-1].azel[0];
el [i]=svr->rtk.ssat[sat[i]-1].azel[1];
for (j=0;j<NFREQ;j++) {
snr[i][j]=(int)(svr->obs[rcv][0].data[i].SNR[j]*0.25);
}
if (svr->rtk.sol.stat==SOLQ_NONE||svr->rtk.sol.stat==SOLQ_SINGLE) {
vsat[i]=svr->rtk.ssat[sat[i]-1].vs;
}
else {
vsat[i]=svr->rtk.ssat[sat[i]-1].vsat[0];
}
}
rtksvrunlock(svr);
return ns;
}
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);
}
}
/* save output buffer --------------------------------------------------------*/
static void saveoutbuf(rtksvr_t *svr, unsigned char *buff, int n, int index)
{
rtksvrlock(svr);
n=n<svr->buffsize-svr->nsb[index]?n:svr->buffsize-svr->nsb[index];
memcpy(svr->sbuf[index]+svr->nsb[index],buff,n);
svr->nsb[index]+=n;
rtksvrunlock(svr);
}
}
char get_solution(double &lat, double &lon, double &height)
{
rtksvrlock(&svr);
const sol_t *sol = &svr.solbuf[svr.nsol-1];
const double *rb = svr.rtk.rb;
double pos[3]={0},Qr[9],Qe[9]={0},dms1[3]={0},dms2[3]={0},bl[3]={0};
double enu[3]={0},pitch=0.0,yaw=0.0,len;
int i;
int status;
if (sol->time.time == 0 || !sol->stat) {
rtksvrunlock(&svr);
return -1; // no fix
}
if (1 <= sol->stat && sol->stat <= 2) status = 0; // fix
if (sol->stat == 3) status = 1; // sbas fix
else status = 2; // differential fix
if (norm(sol->rr,3) > 0.0 && norm(rb,3) > 0.0) {
for (i=0;i<3;i++) bl[i]=sol->rr[i]-rb[i];
}
len=norm(bl,3);
Qr[0]=sol->qr[0];
Qr[4]=sol->qr[1];
Qr[8]=sol->qr[2];
Qr[1]=Qr[3]=sol->qr[3];
Qr[5]=Qr[7]=sol->qr[4];
Qr[2]=Qr[6]=sol->qr[5];
if (norm(sol->rr,3) > 0.0) {
ecef2pos(sol->rr,pos);
covenu(pos,Qr,Qe);
lat = pos[0]*R2D;
lon = pos[1]*R2D;
if (solopt[0].height == 1) pos[2]-=geoidh(pos); /* geodetic */
height = pos[2];
}
svr.nsol=0;
rtksvrunlock(&svr);
/* close output/log stream -----------------------------------------------------
* close output/log stream
* args : rtksvr_t *svr IO rtk server
* int index I output/log stream index
* (3:solution 1,4:solution 2,5:log rover,
* 6:log base station,7:log correction)
* return : none
*-----------------------------------------------------------------------------*/
extern void rtksvrclosestr(rtksvr_t *svr, int index)
{
tracet(3,"rtksvrclosestr: index=%d\n",index);
if (index<3||index>7||!svr->state) return;
rtksvrlock(svr);
strclose(svr->stream+index);
rtksvrunlock(svr);
}
/* decode receiver raw/rtcm data ---------------------------------------------*/
static int decoderaw(rtksvr_t *svr, int index)
{
obs_t *obs;
nav_t *nav;
sbsmsg_t *sbsmsg=NULL;
int i,ret,sat,fobs=0;
tracet(4,"decoderaw: index=%d\n",index);
rtksvrlock(svr);
for (i=0;i<svr->nb[index];i++) {
/* input rtcm/receiver raw data from stream */
if (svr->format[index]==STRFMT_RTCM2) {
ret=input_rtcm2(svr->rtcm+index,svr->buff[index][i]);
obs=&svr->rtcm[index].obs;
nav=&svr->rtcm[index].nav;
sat=svr->rtcm[index].ephsat;
}
else if (svr->format[index]==STRFMT_RTCM3) {
ret=input_rtcm3(svr->rtcm+index,svr->buff[index][i]);
obs=&svr->rtcm[index].obs;
nav=&svr->rtcm[index].nav;
sat=svr->rtcm[index].ephsat;
}
else {
ret=input_raw(svr->raw+index,svr->format[index],svr->buff[index][i]);
obs=&svr->raw[index].obs;
nav=&svr->raw[index].nav;
sat=svr->raw[index].ephsat;
sbsmsg=&svr->raw[index].sbsmsg;
}
#if 0 /* record for receiving tick */
if (ret==1) {
trace(0,"%d %10d T=%s NS=%2d\n",index,tickget(),
time_str(obs->data[0].time,0),obs->n);
}
#endif
/* update rtk server */
if (ret>0) updatesvr(svr,ret,obs,nav,sat,sbsmsg,index,fobs);
/* observation data received */
if (ret==1) {
if (fobs<MAXOBSBUF) fobs++; else svr->prcout++;
}
}
svr->nb[index]=0;
rtksvrunlock(svr);
return fobs;
}
static void RtkServer__get_rtk_status(JNIEnv* env, jclass thiz, jobject j_rtk_control_result)
{
struct native_ctx_t *nctx;
nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field);
if (nctx == NULL) {
LOGV("nctx is null");
return;
}
rtksvrlock(&nctx->rtksvr);
set_rtk_status(env, j_rtk_control_result, &nctx->rtksvr.rtk);
rtksvrunlock(&nctx->rtksvr);
}
static void RtkServer__read_solution_buffer(JNIEnv* env, jclass thiz,
jobject j_solbuf)
{
struct native_ctx_t *nctx;
nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field);
if (nctx == NULL) {
LOGV("nctx is null");
return;
}
rtksvrlock(&nctx->rtksvr);
set_solution_buffer(env, j_solbuf, nctx->rtksvr.solbuf, nctx->rtksvr.nsol);
nctx->rtksvr.nsol=0;
rtksvrunlock(&nctx->rtksvr);
}
/* get stream status -----------------------------------------------------------
* get current stream status
* args : rtksvr_t *svr I rtk server
* int *sstat O status of streams
* char *msg O status messages
* return : none
*-----------------------------------------------------------------------------*/
extern void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg)
{
int i;
char s[MAXSTRMSG],*p=msg;
INIT_ZERO(s);
tracet(4,"rtksvrsstat:\n");
rtksvrlock(svr);
for (i=0;i<MAXSTRRTK;i++) {
sstat[i]=strstat(svr->stream+i,s);
if (*s) p+=sprintf(p,"(%d) %s ",i+1,s);
}
rtksvrunlock(svr);
}
static void RtkServer__write_commands(JNIEnv* env, jclass thiz,
jobjectArray j_cmds)
{
unsigned i;
struct native_ctx_t *nctx;
const char *cmds[3] = {NULL, NULL, NULL};
jstring cmds_jstring[3] = {NULL, NULL, NULL};
LOGV("RtkServer__write_start_commands() ");
nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field);
if (nctx == NULL) {
LOGV("nctx is null");
return;
}
for (i=0; i<sizeof(cmds)/sizeof(cmds[0]); ++i) {
cmds_jstring[i] = (*env)->GetObjectArrayElement(env, j_cmds, i);
if ((*env)->ExceptionOccurred(env))
goto write_commands_end;
if (cmds_jstring[i] != NULL) {
cmds[i] = (*env)->GetStringUTFChars(env, cmds_jstring[i], NULL);
if (cmds[i] == NULL)
goto write_commands_end;
}
}
rtksvrlock(&nctx->rtksvr);
if (cmds[0]) strsendcmd(&nctx->rtksvr.stream[0], cmds[0]);
if (cmds[1]) strsendcmd(&nctx->rtksvr.stream[1], cmds[1]);
if (cmds[2]) strsendcmd(&nctx->rtksvr.stream[2], cmds[2]);
rtksvrunlock(&nctx->rtksvr);
write_commands_end:
for (i=0; i<sizeof(cmds)/sizeof(cmds[0]); ++i) {
if (cmds[i] != NULL)
(*env)->ReleaseStringUTFChars(env, cmds_jstring[i], cmds[i]);
}
}
int main(int argc,char **argv)
{
ros::init(argc, argv, "rtk_robot");
ROS_INFO("RTKlib for ROS Robot Edition");
ros::NodeHandle nn;
ros::NodeHandle pn("~");
ros::Subscriber ecef_sub;
if(pn.getParam("base_position/x", ecef_base_station.position.x) && pn.getParam("base_position/y", ecef_base_station.position.y) && pn.getParam("base_position/z", ecef_base_station.position.z))
{
ROS_INFO("RTK -- Loading base station parameters from the parameter server...");
XmlRpc::XmlRpcValue position_covariance;
if( pn.getParam("base_position/covariance", position_covariance) )
{
ROS_ASSERT(position_covariance.getType() == XmlRpc::XmlRpcValue::TypeArray);
if(position_covariance.size() != 9)
{
ROS_WARN("RTK -- The base station covariances are not complete! Using default values...");
}
else
{
for(int i=0 ; i<position_covariance.size() ; ++i)
{
ROS_ASSERT(position_covariance[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
ecef_base_station.position_covariance[i] = static_cast<double>(position_covariance[i]);
}
}
}
}
else
{
ROS_INFO("RTK -- Subscribing to the base station for online parameters...");
ecef_sub = nn.subscribe("base_station/gps/ecef", 50, ecefCallback);
}
double rate;
pn.param("rate", rate, 2.0);
std::string gps_frame_id;
pn.param<std::string>("gps_frame_id", gps_frame_id, "gps");
std::string port;
pn.param<std::string>("port", port, "ttyACM0");
int baudrate;
pn.param("baudrate", baudrate, 115200);
ros::Publisher gps_pub = nn.advertise<sensor_msgs::NavSatFix>("gps/fix", 50);
ros::Publisher status_pub = nn.advertise<rtk_msgs::Status>("gps/status", 50);
ros::Subscriber gps_sub = nn.subscribe("base_station/gps/raw_data", 50, baseStationCallback);
int n;
//********************* rtklib stuff *********************
rtksvrinit(&server);
if(server.state)
{
ROS_FATAL("RTK -- Failed to initialize rtklib server!");
ROS_BREAK();
}
gtime_t time, time0 = {0};
int format[] = {STRFMT_UBX, STRFMT_UBX, STRFMT_RTCM2};
prcopt_t options = prcopt_default;
options.mode = 2;
options.nf = 1;
options.navsys = SYS_GPS | SYS_SBS;
options.modear = 3;
options.glomodear = 0;
options.minfix = 3;
options.ionoopt = IONOOPT_BRDC;
options.tropopt = TROPOPT_SAAS;
options.rb[0] = ecef_base_station.position.x;
options.rb[1] = ecef_base_station.position.y;
options.rb[2] = ecef_base_station.position.z;
strinitcom();
server.cycle = 10;
server.nmeacycle = 1000;
server.nmeareq = 0;
for(int i=0 ; i<3 ; i++) server.nmeapos[i] = 0;
server.buffsize = BUFFSIZE;
for(int i=0 ; i<3 ; i++) server.format[i] = format[i];
server.navsel = 0;
server.nsbs = 0;
server.nsol = 0;
server.prcout = 0;
rtkfree(&server.rtk);
rtkinit(&server.rtk, &options);
for(int i=0 ; i<3 ; i++)
{
server.nb[i] = server.npb[i] = 0;
if(!(server.buff[i]=(unsigned char *)malloc(BUFFSIZE)) || !(server.pbuf[i]=(unsigned char *)malloc(BUFFSIZE)))
{
ROS_FATAL("RTK -- Failed to initialize rtklib server - malloc error!");
ROS_BREAK();
}
for(int j=0 ; j<10 ; j++) server.nmsg[i][j] = 0;
for(int j=0 ; j<MAXOBSBUF ; j++) server.obs[i][j].n = 0;
/* initialize receiver raw and rtcm control */
init_raw(server.raw + i);
init_rtcm(server.rtcm + i);
/* set receiver option */
strcpy(server.raw[i].opt, "");
strcpy(server.rtcm[i].opt, "");
/* connect dgps corrections */
server.rtcm[i].dgps = server.nav.dgps;
}
/* output peek buffer */
for(int i=0 ; i<2 ; i++)
{
if (!(server.sbuf[i]=(unsigned char *)malloc(BUFFSIZE)))
{
ROS_FATAL("RTK -- Failed to initialize rtklib server - malloc error!");
ROS_BREAK();
}
}
/* set solution options */
solopt_t sol_options[2];
sol_options[0] = solopt_default;
sol_options[1] = solopt_default;
for(int i=0 ; i<2 ; i++) server.solopt[i] = sol_options[i];
/* set base station position */
for(int i=0 ; i<6 ; i++) server.rtk.rb[i] = i < 3 ? options.rb[i] : 0.0;
/* update navigation data */
for(int i=0 ; i<MAXSAT*2 ; i++) server.nav.eph[i].ttr = time0;
for(int i=0 ; i<NSATGLO*2 ; i++) server.nav.geph[i].tof = time0;
for(int i=0 ; i<NSATSBS*2 ; i++) server.nav.seph[i].tof = time0;
updatenav(&server.nav);
/* set monitor stream */
server.moni = NULL;
/* open input streams */
int stream_type[8] = {STR_SERIAL, 0, 0, 0, 0, 0, 0, 0};
char gps_path[64];
sprintf(gps_path, "%s:%d:8:n:1:off", port.c_str(), baudrate);
char * paths[] = {gps_path, "localhost:27015", "", "", "", "", "", ""};
char * cmds[] = {"", "", ""};
int rw;
for(int i=0 ; i<8 ; i++)
{
rw = i < 3 ? STR_MODE_R : STR_MODE_W;
if(stream_type[i] != STR_FILE) rw |= STR_MODE_W;
if(!stropen(server.stream+i, stream_type[i], rw, paths[i]))
{
ROS_ERROR("RTK -- Failed to open stream %s", paths[i]);
for(i-- ; i>=0 ; i--) strclose(server.stream+i);
ROS_FATAL("RTK -- Failed to initialize rtklib server - failed to open all streams!");
ROS_BREAK();
}
/* set initial time for rtcm and raw */
if(i<3)
{
time = utc2gpst(timeget());
server.raw[i].time = stream_type[i] == STR_FILE ? strgettime(server.stream+i) : time;
server.rtcm[i].time = stream_type[i] == STR_FILE ? strgettime(server.stream+i) : time;
}
}
/* sync input streams */
strsync(server.stream, server.stream+1);
strsync(server.stream, server.stream+2);
/* write start commands to input streams */
for(int i=0 ; i<3 ; i++)
{
if(cmds[i]) strsendcmd(server.stream+i, cmds[i]);
}
/* write solution header to solution streams */
for(int i=3 ; i<5 ; i++)
{
unsigned char buff[1024];
int n;
n = outsolheads(buff, server.solopt+i-3);
strwrite(server.stream+i, buff, n);
}
//********************************************************
obs_t obs;
obsd_t data[MAXOBS*2];
server.state=1;
obs.data=data;
double tt;
unsigned int tick;
int fobs[3] = {0};
server.tick = tickget();
ROS_DEBUG("RTK -- Initialization complete.");
ros::Rate r(rate);
while(ros::ok())
{
tick = tickget();
unsigned char *p = server.buff[RTK_ROBOT]+server.nb[RTK_ROBOT];
unsigned char *q = server.buff[RTK_ROBOT]+server.buffsize;
ROS_DEBUG("RTK -- Getting data from GPS...");
/* read receiver raw/rtcm data from input stream */
n = strread(server.stream, p, q-p);
/* write receiver raw/rtcm data to log stream */
strwrite(server.stream+5, p, n);
server.nb[RTK_ROBOT] += n;
/* save peek buffer */
rtksvrlock(&server);
n = n < server.buffsize - server.npb[RTK_ROBOT] ? n : server.buffsize - server.npb[RTK_ROBOT];
memcpy(server.pbuf[RTK_ROBOT] + server.npb[RTK_ROBOT], p, n);
server.npb[RTK_ROBOT] += n;
rtksvrunlock(&server);
ROS_DEBUG("RTK -- Decoding GPS data...");
/* decode data */
fobs[RTK_ROBOT] = decoderaw(&server, RTK_ROBOT);
fobs[RTK_BASE_STATION] = decoderaw(&server, RTK_BASE_STATION);
ROS_DEBUG("RTK -- Got %d observations.", fobs[RTK_ROBOT]);
/* for each rover observation data */
for(int i=0 ; i<fobs[RTK_ROBOT] ; i++)
{
obs.n = 0;
for(int j=0 ; j<server.obs[RTK_ROBOT][i].n && obs.n<MAXOBS*2 ; j++)
{
obs.data[obs.n++] = server.obs[RTK_ROBOT][i].data[j];
}
for(int j=0 ; j<server.obs[1][0].n && obs.n<MAXOBS*2 ; j++)
{
obs.data[obs.n++] = server.obs[1][0].data[j];
}
ROS_DEBUG("RTK -- Calculating RTK positioning...");
/* rtk positioning */
rtksvrlock(&server);
rtkpos(&server.rtk, obs.data, obs.n, &server.nav);
rtksvrunlock(&server);
sensor_msgs::NavSatFix gps_msg;
gps_msg.header.stamp = ros::Time::now();
gps_msg.header.frame_id = gps_frame_id;
rtk_msgs::Status status_msg;
status_msg.stamp = gps_msg.header.stamp;
if(server.rtk.sol.stat != SOLQ_NONE)
{
/* adjust current time */
tt = (int)(tickget()-tick)/1000.0+DTTOL;
timeset(gpst2utc(timeadd(server.rtk.sol.time,tt)));
/* write solution */
unsigned char buff[1024];
n = outsols(buff, &server.rtk.sol, server.rtk.rb, server.solopt);
if(n==141 && buff[0]>'0' && buff[0]<'9')
{
int ano,mes,dia,horas,minutos,Q,nsat;
double segundos,lat,longi,alt,sde,sdn,sdu,sdne,sdeu,sdun;
sscanf((const char *)(buff),"%d/%d/%d %d:%d:%lf %lf %lf %lf %d %d %lf %lf %lf %lf %lf %lf", &ano, &mes, &dia, &horas, &minutos, &segundos, &lat, &longi, &alt, &Q, &nsat, &sdn, &sde, &sdu, &sdne, &sdeu, &sdun);
gps_msg.latitude = lat;
gps_msg.longitude = longi;
gps_msg.altitude = alt;
gps_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
gps_msg.position_covariance[0] = sde + ecef_base_station.position_covariance[0];
gps_msg.position_covariance[1] = sdne + ecef_base_station.position_covariance[1];
gps_msg.position_covariance[2] = sdeu + ecef_base_station.position_covariance[2];
gps_msg.position_covariance[3] = sdne + ecef_base_station.position_covariance[3];
gps_msg.position_covariance[4] = sdn + ecef_base_station.position_covariance[4];
gps_msg.position_covariance[5] = sdun + ecef_base_station.position_covariance[5];
gps_msg.position_covariance[6] = sdeu + ecef_base_station.position_covariance[6];
gps_msg.position_covariance[7] = sdun + ecef_base_station.position_covariance[7];
gps_msg.position_covariance[8] = sdu + ecef_base_station.position_covariance[8];
gps_msg.status.status = Q==5 ? sensor_msgs::NavSatStatus::STATUS_FIX : sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
status_msg.fix_quality = Q;
status_msg.number_of_satellites = nsat;
}
}
else
{
gps_msg.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
}
ROS_DEBUG("RTK -- Publishing ROS msg...");
gps_pub.publish(gps_msg);
status_pub.publish(status_msg);
}
ros::spinOnce();
r.sleep();
}
return(0);
}
/* start rtk server ------------------------------------------------------------
* start rtk server thread
* args : rtksvr_t *svr IO rtk server
* int cycle I server cycle (ms)
* int buffsize I input buffer size (bytes)
* int *strs I stream types (STR_???)
* types[0]=input stream rover
* types[1]=input stream base station
* types[2]=input stream correction
* types[3]=output stream solution 1
* types[4]=output stream solution 2
* types[5]=log stream rover
* types[6]=log stream base station
* types[7]=log stream correction
* char *paths I input stream paths
* int *format I input stream formats (STRFMT_???)
* format[0]=input stream rover
* format[1]=input stream base station
* format[2]=input stream correction
* int navsel I navigation message select
* (0:rover,1:base,2:ephem,3:all)
* char **cmds I input stream start commands
* cmds[0]=input stream rover (NULL: no command)
* cmds[1]=input stream base (NULL: no command)
* cmds[2]=input stream corr (NULL: no command)
* char **rcvopts I receiver options
* rcvopt[0]=receiver option rover
* rcvopt[1]=receiver option base
* rcvopt[2]=receiver option corr
* int nmeacycle I nmea request cycle (ms) (0:no request)
* int nmeareq I nmea request type (0:no,1:base pos,2:single sol)
* double *nmeapos I transmitted nmea position (ecef) (m)
* prcopt_t *prcopt I rtk processing options
* solopt_t *solopt I solution options
* solopt[0]=solution 1 options
* solopt[1]=solution 2 options
* stream_t *moni I monitor stream (NULL: not used)
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
extern int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
char **paths, int *formats, int navsel, char **cmds,
char **rcvopts, int nmeacycle, int nmeareq,
const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni)
{
gtime_t time,time0;
int i,j,rw;
INIT_ZERO(time);
INIT_ZERO(time0);
tracet(3,"rtksvrstart: cycle=%d buffsize=%d navsel=%d nmeacycle=%d nmeareq=%d\n",
cycle,buffsize,navsel,nmeacycle,nmeareq);
if (svr->state) return 0;
strinitcom();
svr->cycle=cycle>1?cycle:1;
svr->nmeacycle=nmeacycle>1000?nmeacycle:1000;
svr->nmeareq=nmeareq;
for (i=0;i<3;i++) svr->nmeapos[i]=nmeapos[i];
svr->buffsize=buffsize>4096?buffsize:4096;
for (i=0;i<3;i++) svr->format[i]=formats[i];
svr->navsel=navsel;
svr->nsbs=0;
svr->nsol=0;
svr->prcout=0;
rtkfree(&svr->rtk);
rtkinit(&svr->rtk,prcopt);
for (i=0;i<3;i++) { /* input/log streams */
svr->nb[i]=svr->npb[i]=0;
if (!(svr->buff[i]=(unsigned char *)malloc(buffsize))||
!(svr->pbuf[i]=(unsigned char *)malloc(buffsize))) {
tracet(1,"rtksvrstart: malloc error\n");
return 0;
}
for (j=0;j<10;j++) svr->nmsg[i][j]=0;
for (j=0;j<MAXOBSBUF;j++) svr->obs[i][j].n=0;
/* initialize receiver raw and rtcm control */
init_raw (svr->raw +i);
init_rtcm(svr->rtcm+i);
/* set receiver and rtcm option */
strcpy(svr->raw [i].opt,rcvopts[i]);
strcpy(svr->rtcm[i].opt,rcvopts[i]);
/* connect dgps corrections */
svr->rtcm[i].dgps=svr->nav.dgps;
}
for (i=0;i<2;i++) { /* output peek buffer */
if (!(svr->sbuf[i]=(unsigned char *)malloc(buffsize))) {
tracet(1,"rtksvrstart: malloc error\n");
return 0;
}
}
/* set solution options */
for (i=0;i<2;i++) {
svr->solopt[i]=solopt[i];
}
/* set base station position */
for (i=0;i<6;i++) {
svr->rtk.rb[i]=i<3?prcopt->rb[i]:0.0;
}
/* update navigation data */
for (i=0;i<MAXSAT *2;i++) svr->nav.eph [i].ttr=time0;
for (i=0;i<NSATGLO*2;i++) svr->nav.geph[i].tof=time0;
for (i=0;i<NSATSBS*2;i++) svr->nav.seph[i].tof=time0;
updatenav(&svr->nav);
/* set monitor stream */
svr->moni=moni;
/* open input streams */
for (i=0;i<8;i++) {
rw=i<3?STR_MODE_R:STR_MODE_W;
if (strs[i]!=STR_FILE) rw|=STR_MODE_W;
if (!stropen(svr->stream+i,strs[i],rw,paths[i])) {
for (i--;i>=0;i--) strclose(svr->stream+i);
return 0;
}
/* set initial time for rtcm and raw */
if (i<3) {
time=utc2gpst(timeget());
svr->raw [i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time;
svr->rtcm[i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time;
}
}
/* sync input streams */
strsync(svr->stream,svr->stream+1);
strsync(svr->stream,svr->stream+2);
/* write start commands to input streams */
for (i=0;i<3;i++) {
if (cmds[i]) strsendcmd(svr->stream+i,cmds[i]);
}
/* write solution header to solution streams */
for (i=3;i<5;i++) {
writesolhead(svr->stream+i,svr->solopt+i-3);
}
/* create rtk server thread */
#ifdef WIN32
if (!(svr->thread=CreateThread(NULL,0,rtksvrthread,svr,0,NULL))) {
#else
if (pthread_create(&svr->thread,NULL,rtksvrthread,svr)) {
#endif
for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i);
return 0;
}
return 1;
}
/* stop rtk server -------------------------------------------------------------
* start rtk server thread
* args : rtksvr_t *svr IO rtk server
* char **cmds I input stream stop commands
* cmds[0]=input stream rover (NULL: no command)
* cmds[1]=input stream base (NULL: no command)
* cmds[2]=input stream ephem (NULL: no command)
* return : none
*-----------------------------------------------------------------------------*/
extern void rtksvrstop(rtksvr_t *svr, char **cmds)
{
int i;
tracet(3,"rtksvrstop:\n");
/* write stop commands to input streams */
rtksvrlock(svr);
for (i=0;i<3;i++) {
if (cmds[i]) strsendcmd(svr->stream+i,cmds[i]);
}
rtksvrunlock(svr);
/* stop rtk server */
svr->state=0;
/* free rtk server thread */
#ifdef WIN32
WaitForSingleObject(svr->thread,10000);
CloseHandle(svr->thread);
#else
pthread_join(svr->thread,NULL);
#endif
}
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;
}