/* temporal update of tropospheric parameters --------------------------------*/
static void udtrop_ppp(rtk_t *rtk)
{
double pos[3],azel[]={0.0,PI/2.0},ztd,var;
int i=IT(&rtk->opt),j;
trace(3,"udtrop_ppp:\n");
if (rtk->x[i]==0.0) {
ecef2pos(rtk->sol.rr,pos);
ztd=sbstropcorr(rtk->sol.time,pos,azel,&var);
initx(rtk,ztd,var,i);
if (rtk->opt.tropopt>=TROPOPT_ESTG) {
for (j=0;j<2;j++) initx(rtk,1E-6,VAR_GRA,++i);
}
}
else {
rtk->P[i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2])*fabs(rtk->tt);
if (rtk->opt.tropopt>=TROPOPT_ESTG) {
for (j=0;j<2;j++) {
rtk->P[++i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2]*0.1)*fabs(rtk->tt);
}
}
}
}
/* temporal update of position/velocity/acceleration -------------------------*/
static void udpos(rtk_t *rtk, double tt)
{
// double *F,*FP,*xp,pos[3],Q[9]={0},Qv[9],var=0.0;
int i;
/* initialize position for first epoch */
if (sos3(rtk->x)<=0.0) {
for (i=0; i<3; i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);
}
/* kinmatic mode without dynamics */
for (i=0; i<3; i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);
}
void testrot()
{ for (n = 1; n <= 20; n++) {
printf(" testing n=%d\n", n);
for (rotdist = 0; rotdist <= n; rotdist++) {
/* printf(" testing rotdist=%d\n", rotdist); */
initx(); revrot(rotdist, n); checkrot();
initx(); jugglerot(rotdist, n); checkrot();
initx(); jugglerot2(rotdist, n); checkrot();
initx(); gcdrot(rotdist, n); checkrot();
}
}
}
int main(void)
{
xxfmt *xx;
kissfmt *kk;
xx = (xxfmt *) malloc(sizeof(xxfmt));
if (xx == NULL)
{
fprintf(stderr,"main: out of memory "
"allocating xx\n");
exit(1);
} /* out of mem */
initx(xx);
kk = (kissfmt *) ekissinit();
xx->runflg = 1;
while(xx->runflg)
{
int x;
int y;
x = (int) ekisspwr(kk,9); /* 0 to 511 */
y = (int) ekisspwr(kk,9); /* 0 to 511 */
if (ekisspwr(kk,1))
XSetForeground(xx->dpy, xx->gc, xx->whiteColor);
else
XSetForeground(xx->dpy, xx->gc, xx->blackColor);
XDrawPoint(xx->dpy,xx->w,xx->gc,x,y);
ifkey(xx);
} /* while runflg != 0 */
XFreeGC(xx->dpy,xx->gc);
XDestroyWindow(xx->dpy,xx->w);
XCloseDisplay(xx->dpy);
free(kk->state);
free(kk);
free(xx);
return(0);
} /* main */
/* initizlize bias parameter --------------------------------------------------*/
static void init_bias(const obsd_t *obs, double *x, double *P, int nx)
{
double bias;
if (obs->L[0]==0||obs->L[1]==0||obs->P[0]==0||obs->P[1]==0) return;
bias=(obs->L[0]-obs->L[1])-(obs->P[0]-obs->P[1]);
initx(x,P,nx,IB(obs->sat),bias,VAR_BIAS);
}
/* initizlize ionosphere parameter --------------------------------------------*/
static void init_iono(const obsd_t *obs, const double *azel, double *x,
double *P, int nx)
{
double map,iono;
if (obs->P[0]==0||obs->P[1]==0) return;
map=ionmapf(pos,azel);
iono=(obs->P[0]-obs->P[1])/map;
initx(x,P,nx,II(obs->sat),iono,VAR_IONO);
}
/* temporal update of position -----------------------------------------------*/
static void udpos_ppp(rtk_t *rtk)
{
int i;
trace(3,"udpos_ppp:\n");
/* fixed mode */
if (rtk->opt.mode==PMODE_PPP_FIXED) {
for (i=0;i<3;i++) initx(rtk,rtk->opt.ru[i],1E-8,i);
return;
}
/* initialize position for first epoch */
if (norm(rtk->x,3)<=0.0) {
for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);
}
/* static ppp mode */
if (rtk->opt.mode==PMODE_PPP_STATIC) return;
/* kinmatic mode without dynamics */
for (i=0;i<3;i++) {
initx(rtk,rtk->sol.rr[i],VAR_POS,i);
}
}
int main(void) {
initx();
xTimerHandle timer = xTimerCreate((const signed char *) "timer",
1000 / portTICK_RATE_MS, pdTRUE, NULL, toggleLedCallback);
send_string_uart("timer created\n");
queue = xQueueCreate(20, 1);
xTaskCreate(acceleration_task, (signed char*)"acceleration_task", 128, NULL,
tskIDLE_PRIORITY+1, NULL);
send_string_uart("queue created\n");
xTimerStart(timer, 0);
send_string_uart("timer started\n");
send_string_uart("INIT DONE, Starting\n");
vTaskStartScheduler();
return 0;
}
void main(int argc, char *argv[])
{
void encoder();
static void usage(char*);
if (argc != 3)
usage(argv[0]);
#ifdef __ADSP21000__
initx();
#else
ifile_name = argv[1];
xfile_name = argv[2];
#endif
ffase = -4;
encoder();
}
int main()
{
int *p,*q; /* pointer to random walk array */
int rslt; /* return code from walk() */
xxfmt *xx; /* declare X Windows structure */
/* allocate memory for X Windows structure */
xx = (xxfmt *) malloc(sizeof(xxfmt));
if (xx == NULL)
{
fprintf(stderr,"main: out of memory "
"allocating xx\n");
exit(1);
} /* out of mem */
initx(xx); /* initialize X Windows */
/* allocate memory for random walk array */
xx->newwave = (int *) malloc(xx->dpywdth + 100);
if (xx->newwave == NULL)
{
fprintf(stderr,"main: out of memory "
"allocating xx->newwavemtx\n");
exit(1);
} /* out of mem */
/* set y coordinate for the middle row line */
xx->middlerow = xx->dpyhght >> 1;
/*************************************/
/* paint the middle row line */
/*************************************/
p = (int *) xx->newwave;
q = (int *) xx->newwave + xx->dpywdth;
while (p < q) *p++ = xx->middlerow;
/*************************************/
xx->runflg = 1; /* set the run flag to run */
while (xx->runflg) /* random walk until quit or eof */
{
rslt = walk(xx); /* walk one pixel */
if (rslt == EOF) break; /* if eof, go to end of job */
ifkey(xx); /* if keyboard interrupt test for 'q' */
} /* for each pixel in random walk */
/* end of job: free memory */
XDestroyWindow(xx->dpy, xx->w); /* delete current window */
XCloseDisplay(xx->dpy); /* stop X Windows */
free(xx->newwave); /* free random walk array */
free(xx); /* free X Windows structure */
return(0); /* normal end of job */
} /* main */
/* temporal update of clock --------------------------------------------------*/
static void udclk_ppp(rtk_t *rtk)
{
double dtr;
int i;
trace(3,"udclk_ppp:\n");
/* initialize every epoch for clock (white noise) */
for (i=0;i<NSYS;i++) {
if (rtk->opt.sateph==EPHOPT_PREC) {
/* time of prec ephemeris is based gpst */
/* negelect receiver inter-system bias */
dtr=rtk->sol.dtr[0];
}
else {
dtr=i==0?rtk->sol.dtr[0]:rtk->sol.dtr[0]+rtk->sol.dtr[i];
}
initx(rtk,CLIGHT*dtr,VAR_CLK,IC(i,&rtk->opt));
}
}
void timedriver()
{ int i, algnum, numtests, start, clicks;
while (scanf("%d %d %d %d", &algnum, &numtests, &n, &rotdist) != EOF) {
initx();
start = clock();
for (i = 0; i < numtests; i++) {
if (algnum == 1)
revrot(rotdist, n);
else if (algnum == 2)
jugglerot(rotdist, n);
else if (algnum == 22)
jugglerot2(rotdist, n);
else if (algnum == 3)
gcdrot(rotdist, n);
else if (algnum == 4)
slide(rotdist, n);
}
clicks = clock() - start;
printf("%d\t%d\t%d\t%d\t%d\t%g\n",
algnum, numtests, n, rotdist, clicks,
1e9*clicks/((float) CLOCKS_PER_SEC*n*numtests));
}
}
static void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
const int *iu, const int *ir, int ns, const nav_t *nav,char **msg)
{
double cp,pr,*bias,offset;
int i,j,slip,reset;
for (i=0; i<ns; i++)
{
/* detect cycle slip by LLI */
rtk->ssat[sat[i]-1].slip&=0xFC;
detslp_ll(rtk,obs,iu[i],1,msg);
detslp_ll(rtk,obs,ir[i],2,msg);
}
/* reset phase-bias if expire obs outage counter */
for (i=1; i<=MAX_SAT; i++) {
reset=++rtk->ssat[i-1].outc>(unsigned int)rtk->opt.maxout;
if (reset&&rtk->x[IB(i,0,&rtk->opt)]!=0.0) {
initx(rtk,0.0,0.0,IB(i,0,&rtk->opt));
}
if (reset) {
rtk->ssat[i-1].lock=-rtk->opt.minlock;
}
}
/* reset phase-bias if instantaneous AR or expire obs outage counter */
for (i=1; i<=MAX_SAT; i++) {
reset=++rtk->ssat[i-1].outc>(unsigned int)rtk->opt.maxout;
if (reset&&rtk->x[IB(i,0,&rtk->opt)]!=0.0) {
initx(rtk,0.0,0.0,IB(i,0,&rtk->opt));
}
if (rtk->opt.modear!=ARMODE_INST&&reset) {
rtk->ssat[i-1].lock=-rtk->opt.minlock;
}
}
/* reset phase-bias if detecting cycle slip */
for (i=0; i<ns; i++) {
j=IB(sat[i],0,&rtk->opt);
rtk->P[j+j*rtk->nx]+=rtk->opt.prn[0]*rtk->opt.prn[0]*tt;
slip=rtk->ssat[sat[i]-1].slip;
if (!(slip&1)) continue;
rtk->x[j]=0.0;
rtk->ssat[sat[i]-1].lock=-rtk->opt.minlock;
}
bias=zeros(ns,1);
/* estimate approximate phase-bias by phase - code */
for (i=j=0,offset=0.0; i<ns; i++)
{
cp=sdobs(obs,iu[i],ir[i],0); /* cycle */
pr=sdobs(obs,iu[i],ir[i],1);
if (cp==0.0||pr==0.0) continue;
bias[i]=cp-pr*FREQ1/CLIGHT;
if (rtk->x[IB(sat[i],0,&rtk->opt)]!=0.0) {
offset+=bias[i]-rtk->x[IB(sat[i],0,&rtk->opt)];
j++;
}
}
/* correct phase-bias offset to enssure phase-code coherency */
if (j>0)
{
for (i=1; i<=MAX_SAT; i++)
{
if (rtk->x[IB(i,0,&rtk->opt)]!=0.0)
rtk->x[IB(i,0,&rtk->opt)]+=offset/j;
}
}
/* set initial states of phase-bias */
for (i=0; i<ns; i++)
{
if (bias[i]==0.0||rtk->x[IB(sat[i],0,&rtk->opt)]!=0.0) continue;
initx(rtk,bias[i],SQR(rtk->opt.std[0]),IB(sat[i],0,&rtk->opt));
}
free(bias);
}
/* temporal update of phase biases -------------------------------------------*/
static void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
double meas[2],var[2],bias[MAXOBS]={0},offset=0.0,pos[3]={0};
int i,j,k,sat,brk=0;
trace(3,"udbias : n=%d\n",n);
for (i=0;i<MAXSAT;i++) for (j=0;j<rtk->opt.nf;j++) {
rtk->ssat[i].slip[j]=0;
}
/* detect cycle slip by LLI */
detslp_ll(rtk,obs,n);
/* detect cycle slip by geometry-free phase jump */
detslp_gf(rtk,obs,n,nav);
/* reset phase-bias if expire obs outage counter */
for (i=0;i<MAXSAT;i++) {
if (++rtk->ssat[i].outc[0]>(unsigned int)rtk->opt.maxout) {
initx(rtk,0.0,0.0,IB(i+1,&rtk->opt));
}
}
ecef2pos(rtk->sol.rr,pos);
for (i=k=0;i<n&&i<MAXOBS;i++) {
sat=obs[i].sat;
j=IB(sat,&rtk->opt);
if (!corrmeas(obs+i,nav,pos,rtk->ssat[sat-1].azel,&rtk->opt,NULL,NULL,
0.0,meas,var,&brk)) continue;
if (brk) {
rtk->ssat[sat-1].slip[0]=1;
trace(2,"%s: sat=%2d correction break\n",time_str(obs[i].time,0),sat);
}
bias[i]=meas[0]-meas[1];
if (rtk->x[j]==0.0||
rtk->ssat[sat-1].slip[0]||rtk->ssat[sat-1].slip[1]) continue;
offset+=bias[i]-rtk->x[j];
k++;
}
/* correct phase-code jump to enssure phase-code coherency */
if (k>=2&&fabs(offset/k)>0.0005*CLIGHT) {
for (i=0;i<MAXSAT;i++) {
j=IB(i+1,&rtk->opt);
if (rtk->x[j]!=0.0) rtk->x[j]+=offset/k;
}
trace(2,"phase-code jump corrected: %s n=%2d dt=%12.9fs\n",
time_str(rtk->sol.time,0),k,offset/k/CLIGHT);
}
for (i=0;i<n&&i<MAXOBS;i++) {
sat=obs[i].sat;
j=IB(sat,&rtk->opt);
rtk->P[j+j*rtk->nx]+=SQR(rtk->opt.prn[0])*fabs(rtk->tt);
if (rtk->x[j]!=0.0&&
!rtk->ssat[sat-1].slip[0]&&!rtk->ssat[sat-1].slip[1]) continue;
if (bias[i]==0.0) continue;
/* reinitialize phase-bias if detecting cycle slip */
initx(rtk,bias[i],VAR_BIAS,IB(sat,&rtk->opt));
trace(5,"udbias_ppp: sat=%2d bias=%.3f\n",sat,meas[0]-meas[1]);
}
}