/* process positioning -------------------------------------------------------*/
static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt,
int mode)
{
gtime_t time={0};
sol_t sol={{0}};
rtk_t rtk;
obsd_t obs[MAXOBS];
double rb[3]={0};
int i,nobs,n,solstatic,pri[]={0,1,2,3,4,5,1,6};
trace(3,"procpos : mode=%d\n",mode);
solstatic=sopt->solstatic&&
(popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC);
rtkinit(&rtk,popt);
rtcm_path[0]='\0';
while ((nobs=inputobs(obs,rtk.sol.stat,popt))>=0) {
/* exclude satellites */
for (i=n=0;i<nobs;i++) {
if ((satsys(obs[i].sat,NULL)&popt->navsys)&&
popt->exsats[obs[i].sat-1]!=1) obs[n++]=obs[i];
}
if (n<=0) continue;
if (!rtkpos(&rtk,obs,n,&navs)) continue;
if (mode==0) { /* forward/backward */
if (!solstatic) {
outsol(fp,&rtk.sol,rtk.rb,sopt);
}
else if (time.time==0||pri[rtk.sol.stat]<=pri[sol.stat]) {
sol=rtk.sol;
for (i=0;i<3;i++) rb[i]=rtk.rb[i];
if (time.time==0||timediff(rtk.sol.time,time)<0.0) {
time=rtk.sol.time;
}
}
}
else if (!revs) { /* combined-forward */
if (isolf>=nepoch) return;
solf[isolf]=rtk.sol;
for (i=0;i<3;i++) rbf[i+isolf*3]=rtk.rb[i];
isolf++;
}
else { /* combined-backward */
if (isolb>=nepoch) return;
solb[isolb]=rtk.sol;
for (i=0;i<3;i++) rbb[i+isolb*3]=rtk.rb[i];
isolb++;
}
}
if (mode==0&&solstatic&&time.time!=0.0) {
sol.time=time;
outsol(fp,&sol,rb,sopt);
}
rtkfree(&rtk);
}
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);
}
/* input obs data, navigation messages and sbas correction -------------------*/
static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt)
{
gtime_t time={0};
int i,nu,nr,n=0;
char str[32];
trace(3,"infunc : revs=%d iobsu=%d iobsr=%d isbs=%d\n",revs,iobsu,iobsr,isbs);
if (0<=iobsu&&iobsu<obss.n) {
settime((time=obss.data[iobsu].time));
if (checkbrk("processing : %s Q=%d",time_str(time,0),solq)) {
aborts=1; showmsg("aborted"); return -1;
}
}
if (!revs) { /* input forward data */
if ((nu=nextobsf(&obss,&iobsu,1))<=0) return -1;
if (popt->intpref) {
for (;(nr=nextobsf(&obss,&iobsr,2))>0;iobsr+=nr)
if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)>-DTTOL) break;
}
else {
for (i=iobsr;(nr=nextobsf(&obss,&i,2))>0;iobsr=i,i+=nr)
if (timediff(obss.data[i].time,obss.data[iobsu].time)>DTTOL) break;
}
nr=nextobsf(&obss,&iobsr,2);
for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu+i];
for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr+i];
iobsu+=nu;
/* update sbas corrections */
while (isbs<sbss.n) {
time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow);
if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */
sbsupdatecorr(sbss.msgs+isbs,&navs);
}
if (timediff(time,obs[0].time)>-1.0-DTTOL) break;
isbs++;
}
/* update lex corrections */
while (ilex<lexs.n) {
if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) {
if (timediff(time,obs[0].time)>-1.0-DTTOL) break;
}
ilex++;
}
/* update rtcm corrections */
if (fp_rtcm) {
if (rtcm.time.time==0) rtcm.time=obs[0].time;
#if 1
while (timediff(rtcm.time,obs[0].time)<-1.0) {
#else
while (timediff(rtcm.time,obs[0].time)<5.0) {
#endif
if (input_rtcm3f(&rtcm,fp_rtcm)==-2) break;
}
for (i=0;i<MAXSAT;i++) navs.ssr[i]=rtcm.ssr[i];
}
}
else { /* input backward data */
if ((nu=nextobsb(&obss,&iobsu,1))<=0) return -1;
if (popt->intpref) {
for (;(nr=nextobsb(&obss,&iobsr,2))>0;iobsr-=nr)
if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)<DTTOL) break;
}
else {
for (i=iobsr;(nr=nextobsb(&obss,&i,2))>0;iobsr=i,i-=nr)
if (timediff(obss.data[i].time,obss.data[iobsu].time)<-DTTOL) break;
}
nr=nextobsb(&obss,&iobsr,2);
for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu-nu+1+i];
for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr-nr+1+i];
iobsu-=nu;
/* update sbas corrections */
while (isbs>=0) {
time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow);
if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */
sbsupdatecorr(sbss.msgs+isbs,&navs);
}
if (timediff(time,obs[0].time)<1.0+DTTOL) break;
isbs--;
}
/* update lex corrections */
while (ilex>=0) {
if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) {
if (timediff(time,obs[0].time)<1.0+DTTOL) break;
}
ilex--;
}
}
return n;
}
/* process positioning -------------------------------------------------------*/
static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt,
int mode)
{
gtime_t time={0};
sol_t sol={{0}};
rtk_t rtk;
obsd_t obs[MAXOBS];
double rb[3]={0};
int i,nobs,n,solstatic,pri[]={0,1,2,3,4,5,1,6};
trace(3,"procpos : mode=%d\n",mode);
solstatic=sopt->solstatic&&
(popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC);
rtkinit(&rtk,popt);
while ((nobs=inputobs(obs,rtk.sol.stat,popt))>=0) {
/* exclude satellites */
for (i=n=0;i<nobs;i++) {
if ((satsys(obs[i].sat,NULL)&popt->navsys)&&
popt->exsats[obs[i].sat-1]!=1) obs[n++]=obs[i];
}
if (n<=0) continue;
if (!rtkpos(&rtk,obs,n,&navs)) continue;
if (mode==0) { /* forward/backward */
if (!solstatic) {
outsol(fp,&rtk.sol,rtk.rb,sopt);
}
else if (time.time==0||pri[rtk.sol.stat]<=pri[sol.stat]) {
sol=rtk.sol;
for (i=0;i<3;i++) rb[i]=rtk.rb[i];
if (time.time==0||timediff(rtk.sol.time,time)<0.0) {
time=rtk.sol.time;
}
}
}
else if (!revs) { /* combined-forward */
if (isolf>=nepoch) return;
solf[isolf]=rtk.sol;
for (i=0;i<3;i++) rbf[i+isolf*3]=rtk.rb[i];
isolf++;
}
else { /* combined-backward */
if (isolb>=nepoch) return;
solb[isolb]=rtk.sol;
for (i=0;i<3;i++) rbb[i+isolb*3]=rtk.rb[i];
isolb++;
}
}
if (mode==0&&solstatic&&time.time!=0.0) {
sol.time=time;
outsol(fp,&sol,rb,sopt);
}
rtkfree(&rtk);
}
int main()//OPTIMIZATION LEVEL = 0
{
HAL_Init();
SystemClockConfig();
ConfigLED();
ConfigTimer();
rtksvrstart(&svr);
ConfigUART(svr.format[0]);
fobs[0]=fobs[1]=0;
//svr.raw[1].time.time = 1429540822;//test SS2 data
//svr.raw[1].time.time = 1429539852;//test SS2 data
while (HAL_UART_Receive_DMA(&UartGPSHandle,svr.buff[0],MAX_RAW_LEN) != HAL_OK);
while (HAL_UART_Receive_DMA(&UartRFHandle,svr.buff[1],MAX_RAW_LEN) != HAL_OK);
HAL_Delay(3000);
sendRequest(svr.format[0]);
// test();
while(1)
{
#ifndef _TEST_RESULT
if (flagTimeout)
{
int index,temp;
flagTimeout=0;
//SendIntStr(UartGPSHandle.Instance->SR);
//SendIntStr(UartRFHandle.Instance->SR);
for (index=0;index<2;index++)
{
if (index==0)
temp = UartGPSHandle.hdmarx->Instance->NDTR & 0xffff;
else
temp = UartRFHandle.hdmarx->Instance->NDTR & 0xffff;
if (temp + svr.buffPtr[index] <= MAX_RAW_LEN)
svr.nb[index] = MAX_RAW_LEN - svr.buffPtr[index] - temp;
else
svr.nb[index] = 2*MAX_RAW_LEN - temp - svr.buffPtr[index];
fobs[index] = decode_raw(&svr,index);
svr.buffPtr[index] = MAX_RAW_LEN - temp;
}
// temp = UartGPSHandle.hdmarx->Instance->NDTR & 0xffff;
// if (temp + svr.buffPtr[0] <= MAX_RAW_LEN)
// svr.nb[0] = MAX_RAW_LEN - svr.buffPtr[0] - temp;
// else
// svr.nb[0] = 2*MAX_RAW_LEN - temp - svr.buffPtr[0];
// if (svr.buffPtr[0] + svr.nb[0] <= MAX_RAW_LEN)
// {
// for (i = svr.buff[0] + svr.buffPtr[0] ;
// i < svr.buff[0] + svr.buffPtr[0] + svr.nb[0]; i++)
// {
// HAL_UART_Transmit(&UartResultHandle,i,1,1);
// }
// }
// else
// {
// for (i = svr.buff[0] + svr.buffPtr[0] ;
// i < svr.buff[0] + MAX_RAW_LEN; i++)
// {
// HAL_UART_Transmit(&UartResultHandle,i,1,1);
// }
// for (i = svr.buff[0] ;
// i < svr.buff[0] + svr.nb[0] + svr.buffPtr[0] - MAX_RAW_LEN ; i++)
// {
// HAL_UART_Transmit(&UartResultHandle,i,1,1);
// }
// }
// svr.buffPtr[0] = MAX_RAW_LEN - temp;
//rtk positioning**********************************************************************
// if (0)
if (fobs[1])
{
fobs[1]=0;
LED4_TOGGLE;
}
if (fobs[0])
{
int i;
fobs[0]=0;
LED3_TOGGLE;
#ifdef TIME_MEASURE
start=HAL_GetTick();
#endif
temp=svr.obs[0].n;
for (i=0;i<temp;i++)
{
obsd[i]=svr.obs[0].data[i];
}
for (i=0;(i<svr.obs[1].n)&&(i+temp<MAX_OBS);i++)
{
obsd[i+temp]=svr.obs[1].data[i];
}
if (!rtkpos(&svr.rtk,obsd,i+temp,&svr.nav))
// if (1)
{
LED5_TOGGLE;
#ifdef TIME_MEASURE
t=HAL_GetTick()-start;
svr.rtk.sol.processTime = t;
#endif
if (svr.rtk.sol.stat==SOLQ_FIX)
LED6_TOGGLE;
outsol(&svr.rtk.sol,svr.rtk.rb);
SendStr(svr.rtk.sol.result);
}
else
{
HAL_UART_Transmit_DMA(&UartResultHandle,(unsigned char*)svr.rtk.errbuf,svr.rtk.errLen);
}
}
}
#else
if (flagTimeout)
{
static int i;
char* res = svr.rtk.sol.result;
flagTimeout = 0;
res+=sprintf(res,
"%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%06.3f %14.4f %14.4f %14.4f %3d %3d %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %6.2f %6.1f",
2015.0,10.0,12.0,3.0,45.0,18.0,//time yy/mm/dd hh:mm:ss.ssss
1.0,2.0,1.0,
1,1,
1.0,1.0,1.0,
1.0,1.0,1.0,
1.0,1.0);
res+=sprintf(res," %4d",i++);
res[0]='\n';
SendStr(svr.rtk.sol.result);
}
#endif
}
}
