static int decode0(char *filename){
raw_t raw; int i;
FILE* file;
if(!(file = fopen(filename,"r"))) return -1;
if(!(init_raw(&raw))) return -1;
while(fgets(raw.buff,MAXRAWLEN,file)){
decode_gpgsa(&raw);
#if 0
printf("\nprn:");
for(i = 0;i < PRNNUMBER;i++) printf("%d ",raw.gpgsa.prn[i]);
#endif
#if 0
printf("\npdop:%f hdop:%f vdop:%f",raw.gpgsa.pdop,raw.gpgsa.hdop,
raw.gpgsa.vdop);
#endif
#if 0
fputs(raw.buff,stdout);
#endif
}
fclose(file);
free_raw(&raw);
return 0;
}
/* preconditions: memory for raw has been allocated and will not be freed later elsewhere, maxsize>0 */
void push_raw_to_channel_history(CHANNEL *chan, RAW *raw, apeconfig *config) {
if (chan == NULL || chan->history == NULL) return;
if (chan->history->filter != NULL) {
char *type = strstr(raw->data, "\"raw\":\"");
ITERATE_CONFIG_VAL(chan->history->filter, " ", {
if (type == NULL || strncmp(type+7, token, strlen(token)) != 0) {
free_raw(raw);
return;
}
});
/* free stream file ----------------------------------------------------------*/
static void free_strfile(strfile_t *str)
{
trace(3,"free_strfile:\n");
if (str->format==STRFMT_RTCM2||str->format==STRFMT_RTCM3) {
free_rtcm(&str->rtcm);
}
else if (str->format<=MAXRCVFMT) {
free_raw(&str->raw);
}
else if (str->format==STRFMT_RINEX) {
free_rnxctr(&str->rnx);
}
free(str);
}
void destroy_raw_pool(struct _raw_pool *ptr)
{
struct _raw_pool *pool = ptr, *tpool = NULL;
while (pool != NULL) {
if (pool->raw != NULL) {
free_raw(pool->raw);
}
if (pool->start) {
if (tpool != NULL) {
free(tpool);
}
tpool = pool;
}
pool = pool->next;
}
if (tpool != NULL) {
free(tpool);
}
}
static int decode1(char *filename){
raw_t raw;int i,sum,data,inputsum;
FILE * file;
if(!(file = fopen(filename,"rb"))) return -1;
if(!(init_raw(&raw))) return -1;
while((inputsum = input_oem4f(&raw,file)) > 0) ;
#if 0
while(1){
if((data=fgetc(file)) != EOF){
if((sum = fread(raw.buff,sizeof(char),3,file)) == 3){
if(sync_oem4(raw.buff,()))
}
}
}
#endif
fclose(file); free_raw(&raw);
return 0;
}
int main(int argc, char *argv[])
{
/* local variables */
raw_t *raw;
socket_t sock;
unsigned char buff[4096];
int recvnum, i;
/* initialise raw */
raw = (raw_t *)malloc(sizeof(raw_t));
if (0 == init_raw(raw))
{
trace(0, "%s\n\n", "ERROR: memory allocation error!");
return 0;
}
strcpy(raw->opt, "-LE"); /* the file type is set to LITTLE_ENDIAN */
raw->outtype = 1; /* set to output message type id */
/* initialize socket */
sock = creat_client_socket("192.168.3.108", 50000);
if(sock < 0) {
printf("sock error\n");
exit(0);
}
/* main loop */
while(1) {
memset(buff, 0x00, sizeof(buff));
recvnum = recv(sock, (char*)buff, 4000, 0);
#ifdef WIN32_
Sleep(1000);
#endif
decode_stream(raw, buff, recvnum);
}
/* clear */
free_raw(raw);
close_client_socket(sock);
}
/*
Send queue to socket
*/
int send_raws(subuser *user, acetables *g_ape)
{
int finish = 1, state = 0;
struct _raw_pool *pool;
struct _transport_properties *properties;
if (user->raw_pools.nraw == 0) {
return 1;
}
PACK_TCP(user->client->fd); /* Activate TCP_CORK */
properties = transport_get_properties(user->user->transport, g_ape);
if (!user->headers.sent) {
user->headers.sent = 1;
switch(user->user->transport) {
case TRANSPORT_XHRSTREAMING:
finish &= http_send_headers(user->headers.content, HEADER_XHR, HEADER_XHR_LEN, user->client, g_ape);
break;
case TRANSPORT_SSE_LONGPOLLING:
finish &= http_send_headers(user->headers.content, HEADER_SSE, HEADER_SSE_LEN, user->client, g_ape);
break;
case TRANSPORT_WEBSOCKET:
case TRANSPORT_WEBSOCKET_IETF:
break;
default:
finish &= http_send_headers(user->headers.content, HEADER_DEFAULT, HEADER_DEFAULT_LEN, user->client, g_ape);
break;
}
}
if (properties != NULL && properties->padding.left.val != NULL) {
finish &= sendbin(user->client->fd, properties->padding.left.val, properties->padding.left.len, 0, g_ape);
}
if (user->raw_pools.high.nraw) {
pool = user->raw_pools.high.rawfoot->prev;
} else {
pool = user->raw_pools.low.rawhead;
state = 1;
}
if (user->user->transport == TRANSPORT_WEBSOCKET_IETF) {
char payload_head[32] = { 0x84 };
int payload_size = raws_size(user); /* TODO: fragmentation? */
int payload_length = 0;
if (payload_size <= 125) {
payload_head[1] = (unsigned char)payload_size & 0x7F;
payload_length = 2;
} else if (payload_size <= 65535) {
unsigned short int s = htons(payload_size);
payload_head[1] = 126;
memcpy(&payload_head[2], &s, 2);
payload_length = 4;
} else if (payload_size <= 0xFFFFFFFF) {
unsigned int s = htonl(payload_size);
payload_head[1] = 127;
payload_head[2] = 0;
payload_head[3] = 0;
payload_head[4] = 0;
payload_head[5] = 0;
memcpy(&payload_head[6], &s, 4);
payload_length = 10;
}
finish &= sendbin(user->client->fd, payload_head, payload_length, 0, g_ape);
}
finish &= sendbin(user->client->fd, "[", 1, 0, g_ape);
while (pool->raw != NULL) {
struct _raw_pool *pool_next = (state ? pool->next : pool->prev);
finish &= sendbin(user->client->fd, pool->raw->data, pool->raw->len, 0, g_ape);
if ((pool_next != NULL && pool_next->raw != NULL) || (!state && user->raw_pools.low.nraw)) {
finish &= sendbin(user->client->fd, ",", 1, 0, g_ape);
} else {
finish &= sendbin(user->client->fd, "]", 1, 0, g_ape);
if (properties != NULL && properties->padding.right.val != NULL) {
finish &= sendbin(user->client->fd, properties->padding.right.val, properties->padding.right.len, 0, g_ape);
}
}
free_raw(pool->raw);
pool->raw = NULL;
pool = pool_next;
if ((pool == NULL || pool->raw == NULL) && !state) {
pool = user->raw_pools.low.rawhead;
state = 1;
}
}
user->raw_pools.high.nraw = 0;
user->raw_pools.low.nraw = 0;
user->raw_pools.nraw = 0;
user->raw_pools.high.rawfoot = user->raw_pools.high.rawhead;
user->raw_pools.low.rawfoot = user->raw_pools.low.rawhead;
FLUSH_TCP(user->client->fd);
return finish;
}
int send_raw_inline(ape_socket *client, transport_t transport, RAW *raw, acetables *g_ape)
{
struct _transport_properties *properties;
int finish = 1;
properties = transport_get_properties(transport, g_ape);
switch(transport) {
case TRANSPORT_XHRSTREAMING:
finish &= http_send_headers(NULL, HEADER_XHR, HEADER_XHR_LEN, client, g_ape);
break;
case TRANSPORT_SSE_LONGPOLLING:
finish &= http_send_headers(NULL, HEADER_SSE, HEADER_SSE_LEN, client, g_ape);
break;
case TRANSPORT_WEBSOCKET:
case TRANSPORT_WEBSOCKET_IETF:
break;
default:
finish &= http_send_headers(NULL, HEADER_DEFAULT, HEADER_DEFAULT_LEN, client, g_ape);
break;
}
if (properties != NULL && properties->padding.left.val != NULL) {
finish &= sendbin(client->fd, properties->padding.left.val, properties->padding.left.len, 0, g_ape);
}
if (transport == TRANSPORT_WEBSOCKET_IETF) {
char payload_head[32] = { 0x84 };
int payload_size = raw->len+2; /* TODO: fragmentation? */
int payload_length = 0;
if (payload_size <= 125) {
payload_head[1] = (unsigned char)payload_size & 0x7F;
payload_length = 2;
} else if (payload_size <= 65535) {
unsigned short int s = htons(payload_size);
payload_head[1] = 126;
memcpy(&payload_head[2], &s, 2);
payload_length = 4;
} else if (payload_size <= 0xFFFFFFFF) {
unsigned int s = htonl(payload_size);
payload_head[1] = 127;
payload_head[2] = 0;
payload_head[3] = 0;
payload_head[4] = 0;
payload_head[5] = 0;
memcpy(&payload_head[6], &s, 4);
payload_length = 10;
}
finish &= sendbin(client->fd, payload_head, payload_length, 0, g_ape);
}
finish &= sendbin(client->fd, "[", 1, 0, g_ape);
finish &= sendbin(client->fd, raw->data, raw->len, 0, g_ape);
finish &= sendbin(client->fd, "]", 1, 0, g_ape);
if (properties != NULL && properties->padding.right.val != NULL) {
finish &= sendbin(client->fd, properties->padding.right.val, properties->padding.right.len, 0, g_ape);
}
free_raw(raw);
return finish;
}
/* initialize receiver raw data control ----------------------------------------
* initialize receiver raw data control struct and reallocate obsevation and
* epheris buffer
* args : raw_t *raw IO receiver raw data control struct
* return : status (1:ok,0:memory allocation error)
*-----------------------------------------------------------------------------*/
extern int init_raw(raw_t *raw)
{
const double lam_glo[NFREQ]= {CLIGHT/FREQ1_GLO,CLIGHT/FREQ2_GLO};
gtime_t time0= {0};
obsd_t data0= {{0}};
eph_t eph0 = {0,-1,-1};
alm_t alm0 = {0,-1};
geph_t geph0= {0,-1};
seph_t seph0= {0};
sbsmsg_t sbsmsg0= {0};
lexmsg_t lexmsg0= {0};
int i,j,sys;
trace(3,"init_raw:\n");
raw->time=raw->tobs=time0;
raw->ephsat=0;
raw->sbsmsg=sbsmsg0;
raw->msgtype[0]='\0';
for (i=0; i<MAXSAT; i++) {
for (j=0; j<150 ; j++) raw->subfrm[i][j]=0;
for (j=0; j<NFREQ; j++) raw->lockt[i][j]=0.0;
for (j=0; j<NFREQ; j++) raw->halfc[i][j]=0;
raw->icpp[i]=raw->off[i]=raw->prCA[i]=raw->dpCA[i]=0.0;
}
for (i=0; i<MAXOBS; i++) raw->freqn[i]=0;
raw->lexmsg=lexmsg0;
raw->icpc=0.0;
raw->nbyte=raw->len=0;
raw->iod=raw->flag=raw->tbase=raw->outtype=0;
raw->tod=-1;
for (i=0; i<MAXRAWLEN; i++) raw->buff[i]=0;
raw->opt[0]='\0';
free_raw(raw);
if (!(raw->obs.data =(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))||
!(raw->obuf.data=(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))||
!(raw->nav.eph =(eph_t *)malloc(sizeof(eph_t )*MAXSAT))||
!(raw->nav.alm =(alm_t *)malloc(sizeof(alm_t )*MAXSAT))||
!(raw->nav.geph =(geph_t *)malloc(sizeof(geph_t)*NSATGLO))||
!(raw->nav.seph =(seph_t *)malloc(sizeof(seph_t)*NSATSBS*2))) {
free_raw(raw);
return 0;
}
raw->obs.n =0;
raw->obuf.n=0;
raw->nav.n =MAXSAT;
raw->nav.na=MAXSAT;
raw->nav.ng=NSATGLO;
raw->nav.ns=NSATSBS*2;
for (i=0; i<MAXOBS ; i++) raw->obs.data [i]=data0;
for (i=0; i<MAXOBS ; i++) raw->obuf.data[i]=data0;
for (i=0; i<MAXSAT ; i++) raw->nav.eph [i]=eph0;
for (i=0; i<MAXSAT ; i++) raw->nav.alm [i]=alm0;
for (i=0; i<NSATGLO ; i++) raw->nav.geph [i]=geph0;
for (i=0; i<NSATSBS*2; i++) raw->nav.seph [i]=seph0;
for (i=0; i<MAXSAT; i++) for (j=0; j<NFREQ; j++) {
if (!(sys=satsys(i+1,NULL))) continue;
raw->nav.lam[i][j]=sys==SYS_GLO?lam_glo[j]:lam[j];
}
return 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;
}
void history_free_node(CHANNEL_HISTORY_NODE *node) {
free_raw(node->value);
free(node);
}
void history_push_element(CHANNEL_HISTORY_DEQUE *deque, RAW *raw) {
free_raw(deque->head->value);
deque->head->value = raw;
deque->tail = deque->head;
deque->head = deque->head->next;
}
