/**
Free SERVO_T struct
*/
void servo_free(SERVO_T *st){
if(!st) return;
dcellfree(st->mlead);
dcellfree(st->merrlast);
dcellfree(st->mpreint);
cellfree(st->merrhist);
cellfree(st->mint);
dfree(st->ap);
dfree(st->ep);
free(st);
}
static void pbuf_free(struct pbuf_t *pb)
{
#ifndef _FOR_VALGRIND_
cellfree(pbuf_cells, pb);
#else
free(pb);
#endif
if (debug > 1) printf("pbuf_free(%p)\n",pb);
}
static void heard_free_single(struct client_heard_t **list)
{
int i;
struct client_heard_t *n, *h;
for (i = 0; i < CLIENT_HEARD_BUCKETS; i++) {
h = list[i];
while (h) {
n = h->next;
#ifndef _FOR_VALGRIND_
cellfree(client_heard_cells, h);
#else
hfree(h);
#endif
h = n;
}
list[i] = NULL;
}
}
/**
Setup the least square reconstruct by directly inverting GA matrix.
The reconstructor is simply the pseudo inverse of GA matrix:
\f[\hat{x}=(G_a^TC_g^{-1}G_a)^{-1}G_a^TC_g^{-1}\f]
This is very close to RR except replacing GX with GA.
We use the tomograhy parameters for lsr, since lsr is simply "tomography" onto DM directly.
*/
void setup_recon_lsr(RECON_T *recon, const PARMS_T *parms){
const int ndm=parms->ndm;
const int nwfs=parms->nwfsr;
cell *GAlsr;
cell *GAM=parms->recon.modal?(cell*)recon->GM:(cell*)recon->GA;
if(parms->recon.split){ //high order wfs only in split mode.
GAlsr=parms->recon.modal?(cell*)recon->GMhi:(cell*)recon->GAhi;
}else{ //all wfs in integrated mode.
GAlsr=GAM;
}
int free_GAlsr=0;
if(GAlsr->p[0]->id!=M_DBL){
dsp *tmp=dsp_cast(GAlsr->p[0]);
if(tmp->nzmax>tmp->nx*tmp->ny*0.2){//not very sparse
dcell *tmp2=0;
free_GAlsr=1;
dcelladd(&tmp2, 1, (dspcell*)GAlsr, 1);
GAlsr=(cell*)tmp2;
}
}
info2("Building recon->LR\n");
recon->LR.M=dcellmm2(GAlsr, recon->saneai, "tn");
// Tip/tilt and diff focus removal low rand terms for LGS WFS.
if(recon->TTF){
dcellmm(&recon->LR.U, recon->LR.M, recon->TTF, "nn", 1);
recon->LR.V=dcelltrans(recon->PTTF);
}
info2("Building recon->LL\n");
recon->LL.M=dcellmm2(recon->LR.M, GAlsr, "nn");
if(free_GAlsr){
cellfree(GAlsr);
}
double maxeig=pow(recon->neamhi * recon->aloc->p[0]->dx, -2);
if(parms->recon.modal){
double strength=1;
for(int idm=0; idm<ndm; idm++){
strength*=dnorm(recon->amod->p[idm]);
}
strength=pow(strength, 2./ndm);
maxeig*=strength;
}
if(fabs(parms->lsr.tikcr)>EPS){
info2("Adding tikhonov constraint of %g to LLM\n", parms->lsr.tikcr);
info2("The maximum eigen value is estimated to be around %g\n", maxeig);
dcelladdI(recon->LL.M, parms->lsr.tikcr*maxeig);
}
dcell *NW=NULL;
if(!parms->recon.modal){
if(parms->lsr.alg!=2){
/* Not SVD, need low rank terms for piston/waffle mode constraint. */
NW=dcellnew(ndm,1);
int nmod=2;/*two modes. */
for(int idm=0; idm<ndm; idm++){
loc_create_map(recon->aloc->p[idm]);
const long nloc=recon->aloc->p[idm]->nloc;
NW->p[idm]=dnew(nloc, ndm*nmod);
double *p=NW->p[idm]->p+nmod*idm*nloc;
const double *cpl=recon->actcpl->p[idm]->p;
for(long iloc=0; iloc<nloc; iloc++){
if(cpl[iloc]>0.1){
p[iloc]=1;/*piston mode */
}
}
/*notice offset of 1 because map start count at 1 */
p=NW->p[idm]->p+(1+nmod*idm)*nloc-1;
map_t *map=recon->aloc->p[idm]->map;
for(long iy=0; iy<map->ny; iy++){
for(long ix=0; ix<map->nx; ix++){
if(IND(map,ix,iy)){
p[(long)IND(map,ix,iy)]=(double)2*((iy+ix)&1)-1;
}
}
}
}
/*scale it to match the magnitude of LL.M */
dcellscale(NW, sqrt(maxeig));
if(parms->save.setup){
writebin(NW, "lsrNW");
}
}
if(parms->lsr.actslave){
/*actuator slaving. important. change from 0.5 to 0.1 on 2011-07-14. */
dspcell *actslave=slaving(recon->aloc, recon->actcpl, NW,
recon->actstuck, recon->actfloat, parms->lsr.actthres, maxeig);
if(parms->save.setup){
if(NW){
writebin(NW, "lsrNW2");
}
writebin(actslave,"actslave");
}
dcelladd(&recon->LL.M, 1, actslave, 1);
cellfree(actslave);
}
}
/*Low rank terms for low order wfs. Only in Integrated tomography. */
dcell *ULo=dcellnew(ndm,nwfs);
dcell *VLo=dcellnew(ndm,nwfs);
dcell* pULo=ULo/*PDELL*/;
dcell* pVLo=VLo/*PDELL*/;
for(int iwfs=0; iwfs<nwfs; iwfs++){
int ipowfs=parms->wfsr[iwfs].powfs;
if(parms->powfs[ipowfs].skip || !parms->powfs[ipowfs].lo){
continue;
}
for(int idm=0; idm<ndm; idm++){
dspfull(PIND(pULo,idm,iwfs), (dsp*)IND(recon->LR.M, idm, iwfs),'n',-1);
dspfull(PIND(pVLo,idm,iwfs), (dsp*)IND(GAM, iwfs, idm),'t',1);
}
}
recon->LL.U=dcellcat(recon->LR.U, ULo, 2);
dcell *GPTTDF=NULL;
dcellmm(&GPTTDF, GAM, recon->LR.V, "tn", 1);
recon->LL.V=dcellcat(GPTTDF, VLo, 2);
dcellfree(GPTTDF);
dcellfree(ULo);
dcellfree(VLo);
if(!parms->recon.modal && NW){
info2("Create piston and check board modes that are in NULL space of GA.\n");
/*add to low rank terms. */
dcell *tmp=recon->LL.U;
recon->LL.U=dcellcat(tmp, NW, 2);
dcellfree(tmp);
dcellscale(NW, -1);
tmp=recon->LL.V;
recon->LL.V=dcellcat(tmp, NW, 2);
dcellfree(tmp);
dcellfree(NW);
}
if(parms->lsr.fnreg){
warning("Loading LSR regularization from file %s.\n", parms->lsr.fnreg);
dspcell *tmp=dspcellread("%s", parms->lsr.fnreg);
dcelladd(&recon->LL.M, 1, tmp, 1);
dspcellfree(tmp);
}
recon->LL.alg = parms->lsr.alg;
recon->LL.bgs = parms->lsr.bgs;
recon->LL.warm = parms->recon.warm_restart;
recon->LL.maxit = parms->lsr.maxit;
/*Remove empty cells. */
dcelldropempty(&recon->LR.U,2);
dcelldropempty(&recon->LR.V,2);
dcelldropempty(&recon->LL.U,2);
dcelldropempty(&recon->LL.V,2);
if(parms->save.recon){
writebin(recon->LR.M,"LRM");
writebin(recon->LR.U,"LRU");
writebin(recon->LR.V,"LRV");
writebin(recon->LL.M,"LLM.bin");/*disable compression */
writebin(recon->LL.U,"LLU");
writebin(recon->LL.V,"LLV");
}
if(parms->lsr.alg==0 || parms->lsr.alg==2){
if(!parms->lsr.bgs){
muv_direct_prep(&recon->LL, (parms->lsr.alg==2)*parms->lsr.svdthres);
if(parms->save.recon){
if(recon->LL.C)
chol_save(recon->LL.C, "LLC.bin");
else
writebin(recon->LL.MI, "LLMI.bin");
}
cellfree(recon->LL.M);
dcellfree(recon->LL.U);
dcellfree(recon->LL.V);
}else{
muv_direct_diag_prep(&(recon->LL), (parms->lsr.alg==2)*parms->lsr.svdthres);
if(parms->save.recon){
for(int ib=0; ib<recon->LL.nb; ib++){
if(recon->LL.CB)
chol_save(recon->LL.CB[ib],"LLCB_%d.bin", ib);
else
writebin(recon->LL.MI,"LLMIB_%d.bin", ib);
}
}
/*Don't free M, U, V */
}
}
}
static int heard_find(struct client_t *c, struct client_heard_t **list, int *entrycount, const char *callsign, int call_len, int storetime, int drop_if_found, char *which)
{
struct client_heard_t *h, *next;
uint32_t hash, idx;
int i;
hash = keyhashuc(callsign, call_len, 0);
idx = hash;
// "CLIENT_HEARD_BUCKETS" is 16..
idx ^= (idx >> 16);
idx ^= (idx >> 8);
idx ^= (idx >> 4);
i = idx % CLIENT_HEARD_BUCKETS;
//DLOG(LOG_DEBUG, "heard_find fd %d %s: checking for %.*s (hash %u i %d)", c->fd, which, call_len, callsign, hash, i);
int expire_below = tick - storetime;
next = NULL;
for (h = list[i]; (h); h = next) {
next = h->next; // we might free this one
// expire old entries
// TODO: move expiration to heard_list_update, too, it's run much more often.
if (h->last_heard < expire_below || h->last_heard > tick) {
DLOG(LOG_DEBUG, "heard_find fd %d %s: expiring %.*s (%ul below %ul)", c->fd, which, h->call_len, h->callsign, h->last_heard, expire_below);
*h->prevp = h->next;
if (h->next)
h->next->prevp = h->prevp;
#ifndef _FOR_VALGRIND_
cellfree(client_heard_cells, h);
#else
hfree(h);
#endif
*entrycount = *entrycount -1;
continue;
}
if (h->hash == hash && call_len == h->call_len
&& strncasecmp(callsign, h->callsign, h->call_len) == 0) {
/* OK, found it from the list. */
//DLOG(LOG_DEBUG, "heard_find fd %d %s: found %.*s%s", c->fd, which, call_len, callsign, (drop_if_found) ? " (dropping)" : "");
if (drop_if_found) {
DLOG(LOG_DEBUG, "heard_find fd %d %s: dropping %.*s%s", c->fd, which, call_len, callsign, (drop_if_found) ? " (dropping)" : "");
*h->prevp = h->next;
if (h->next)
h->next->prevp = h->prevp;
#ifndef _FOR_VALGRIND_
cellfree(client_heard_cells, h);
#else
hfree(h);
#endif
*entrycount = *entrycount -1;
}
return 1;
}
}
return 0;
}
static void test_accuracy(int argc, char **argv){
double displacex=0.01;
double displacey=0.05;
double scale=1.01;/*.414065; */
int wrap=0;
double D=30;
double D2=32;
int save=1;
if(argc>1){
scale=strtod(argv[1], 0);
}
if(argc>2){
displacex=strtod(argv[2], 0);
}
if(argc>3){
displacey=strtod(argv[3], 0);
}
if(argc>4){
wrap=strtol(argv[4], 0, 10);
}
if(argc>5){
D=strtod(argv[5], 0);
}
if(argc>6){
D2=strtod(argv[6], 0);
}
if(argc>7){
save=strtol(argv[7], 0, 10);
}
double dx=1/64.;
double dsa=0.5;
map_t *screen=mapnew(D2/dx, D2/dx, dx, dx, 0);
dset((dmat*)screen, 1);
for(long iy=0; iy<screen->ny; iy++){
for(long ix=0; ix<screen->nx; ix++){
screen->p[ix+iy*screen->nx]=sin((double)ix/screen->nx*2*M_PI)*sin((double)iy/screen->ny*2*M_PI);
}
}
/*loc for the map */
loc_t *locin=mksqloc(screen->nx,screen->ny,dx,dx,screen->ox,screen->oy);
//pts_t *pts=realloc(mkannloc(D, 0, 1./2.,0), sizeof(pts_t));
pts_t *pts=realloc(mksqloc_auto(D/dsa, D/dsa, dsa, dsa), sizeof(pts_t));
pts->dx=dx;
pts->dy=dx;
pts->nx=dsa/dx;
pts->ny=pts->nx;
loc_t *loc=pts2loc(pts);
loc_create_map(locin);
loc_create_stat(loc);
loc_create_map(loc);
locstat_t *locstat=loc->stat;
map_t *screen2=mapnew2(locin->map);
dset((dmat*)screen2, NAN);
loc_embed(screen2, locin, screen->p);
double *phi_pts, *phi_loc, *phi_stat;
double *phi_loc2loc, *phi_h, *phi_cub, *phi_cub2, *phi_cubh;
double cubic=0.3;
int ii;
info("displacex=%g, displacey=%g, scale=%g, wrap=%d\n",
displacex, displacey,scale,wrap);
double diff1, diff2,diff3,diff14,diff15;
diff1=0;
diff2=0;
diff3=0;
diff14=0;
diff15=0;
phi_pts=calloc(loc->nloc, sizeof(double));
phi_loc=calloc(loc->nloc, sizeof(double));
phi_stat=calloc(loc->nloc, sizeof(double));
phi_loc2loc=calloc(loc->nloc, sizeof(double));
map_t *map1=mapnew2(loc->map);
prop_grid_map(screen, map1, -2, displacex, displacey, scale, wrap, 0,0);
tic;
prop_grid_map(screen, map1, 1, displacex, displacey, scale, wrap, 0,0);
toc("map\t\t");
prop_grid_pts(screen, pts, phi_pts, -2, displacex, displacey, scale, wrap, 0,0);
tic;
prop_grid_pts(screen, pts, phi_pts, 1, displacex, displacey, scale, wrap, 0,0);
toc("pts\t\t");
prop_grid_stat(screen, locstat, phi_stat, -2,displacex, displacey, scale, wrap, 0,0);
tic;
prop_grid_stat(screen, locstat, phi_stat , 1, displacex, displacey, scale, wrap, 0,0);
toc("stat\t");tic;
prop_grid(screen, loc, phi_loc, -2,displacex, displacey, scale, wrap, 0,0);
tic;
prop_grid(screen, loc, phi_loc, 1,displacex, displacey, scale, wrap, 0,0);
toc("loc\t\t");
prop_nongrid(locin, screen->p,loc, phi_loc2loc, -2,displacex, displacey, scale,0,0);
toc("nongrid\t"); tic;
prop_nongrid(locin, screen->p,loc, phi_loc2loc, 1,displacex, displacey, scale,0,0);
toc("nongrid\t");
phi_h=calloc(loc->nloc,sizeof(double));
tic;
dsp *hfor=mkh(locin, loc, displacex, displacey, scale);
toc("mkh\t\t\t");
dspmulvec(phi_h,hfor, screen->p, 'n', -2);
tic;
dspmulvec(phi_h,hfor, screen->p, 'n', 1);
toc("mul h\t\t");
phi_cub=calloc(loc->nloc,sizeof(double));
phi_cub2=calloc(loc->nloc,sizeof(double));
double *phi_cub3=calloc(loc->nloc,sizeof(double));
double *phi_cub4=calloc(loc->nloc,sizeof(double));
phi_cubh=calloc(loc->nloc, sizeof(double));
prop_nongrid_cubic(locin,screen->p,loc,phi_cub,-2, displacex, displacey, scale, cubic,0,0);
tic;
prop_nongrid_cubic(locin,screen->p,loc,phi_cub,1, displacex, displacey, scale, cubic,0,0);
toc("nongrid, cubic\t");
prop_grid_cubic(screen, loc, phi_cub2, -2,displacex, displacey, scale, cubic, 0,0);
tic;
prop_grid_cubic(screen, loc, phi_cub2, 1,displacex, displacey, scale, cubic, 0,0);
toc("grid, cubic\t");
prop_grid_cubic(screen2, loc,phi_cub3, -2,displacex, displacey, scale, cubic, 0,0);
tic;
prop_grid_cubic(screen2, loc,phi_cub3, 1,displacex, displacey, scale, cubic, 0,0);
toc("grid2, cubic\t");
prop_grid_stat_cubic(screen, locstat,phi_cub4, -2,displacex, displacey, scale, cubic, 0,0);
tic;
prop_grid_stat_cubic(screen, locstat,phi_cub4, 1,displacex, displacey, scale, cubic, 0,0);
toc("grid 2stat, cubic\t");
dsp *hforcubic;
tic;
hforcubic=mkh_cubic(locin, loc, displacex, displacey, scale, cubic);
toc("mkh cubic \t\t");
dspmulvec(phi_cubh, hforcubic,screen->p,'n',-2);
tic;
dspmulvec(phi_cubh, hforcubic,screen->p,'n',1);
toc("cubic mul h\t\t");
double diffc12=0,diff45=0,diff46=0,diff47=0;
for(ii=0; ii<loc->nloc; ii++){
diff1+=fabs(phi_loc[ii]-phi_pts[ii]);
diff2+=fabs(phi_stat[ii]-phi_loc[ii]);
diff3+=fabs(phi_stat[ii]-phi_pts[ii]);
diff14+=fabs(phi_loc2loc[ii]-phi_pts[ii]);
diff15+=fabs(phi_h[ii]-phi_pts[ii]);
diff45+=fabs(phi_loc2loc[ii]-phi_h[ii]);
diffc12+=fabs(phi_cub[ii]-phi_cubh[ii]);
diff46+=fabs(phi_cub[ii]-phi_cub2[ii]);
diff47+=fabs(phi_cub[ii]-phi_cub3[ii]);
}
info2("(pts-loc)=\t%g\n(loc-stat)=\t%g\n(stat-pts)=\t%g\n"
"(loc2loc-pts)=\t%g\n(h-pts)=\t%g\n"
"(loc2loc-h)=\t%g\n"
"(cub:h-loc2loc)=\t%g\n"
"(cub:map2loc-loc2loc)=\t%g\n"
"(cub:locmap2loc-loc2loc=\t%g\n"
,diff1, diff2,diff3,diff14,diff15,diff45,diffc12,
diff46, diff47);
// exit(0);
if(!save) return;
mapwrite(screen2,"accphi_screen2");
mapwrite(screen,"accphi_screen");
locwrite((loc_t*)pts,"accphi_pts");
locwrite(loc,"accphi_loc");
locwrite(locin, "accphi_locin");
loc_create_map_npad(locin, 0, 0, 0);
mapwrite(locin->map, "accphi_locin_map");
mapwrite(loc->map, "accphi_loc_map");
mapwrite(map1, "accphi_map2map.bin");
writedbl(phi_pts,loc->nloc,1,"accphi_pts1.bin");
writedbl(phi_loc,loc->nloc,1,"accphi_loc0.bin");
writedbl(phi_stat,loc->nloc,1,"accphi_stat.bin");
writedbl(phi_loc2loc,loc->nloc,1,"accphi_loc2loc.bin");
writedbl(phi_h,loc->nloc,1,"accphi_loc2h.bin");
writedbl(phi_cub,loc->nloc,1,"accphi_cub_loc2loc.bin");
writedbl(phi_cub2,loc->nloc,1,"accphi_cub_map2loc.bin");
writedbl(phi_cub3,loc->nloc,1,"accphi_cub_locmap2loc.bin");
writedbl(phi_cub4,loc->nloc,1,"accphi_cub_locmap2stat.bin");
writedbl(phi_cubh,loc->nloc,1,"accphi_cub_loc2h.bin");
info("saved\n");
writedbl(phi_pts,loc->nloc,1,"accphi_pts.bin");
writedbl(phi_cub,loc->nloc,1,"accphi_cub.bin");
writebin(hfor, "accphi_hfor");
writebin(hforcubic, "accphi_cub_hfor");
dspfree(hfor);
dspfree(hforcubic);
free(phi_loc); free(phi_stat);
free(phi_loc2loc);
free(phi_pts);
free(phi_h);
free(phi_cub);
free(phi_cubh);
cellfree(screen);
locfree(locin);
}