/**
Add low order NGS modes to DM actuator commands for AHST and MVST
*/
void addlow2dm(dcell **dmval, const SIM_T *simu,
const dcell *low_val, double gain){
switch(simu->parms->recon.split){
case 0:
break;/*nothing to do. */
case 1:
dcellmm(dmval, simu->recon->ngsmod->Modes, low_val, "nn", gain);
break;
case 2:
dcellmm(dmval, simu->recon->MVModes, low_val, "nn", gain);
break;
default:
error("Not implemented\n");
}
}
/**
filter DM commands in open loop mode by simply copy the output
*/
static void filter_ol(SIM_T *simu){
assert(!simu->parms->sim.closeloop);
if(simu->dmerr){
dcellcp(&simu->dmcmd, simu->dmerr);
}else{
dcellzero(simu->dmcmd);
}
if(simu->Merr_lo){
addlow2dm(&simu->dmcmd, simu, simu->Merr_lo,1);
}
extern int DM_NCPA;
if(DM_NCPA && simu->recon->dm_ncpa){
warning_once("Add NCPA after integrator\n");
dcelladd(&simu->dmcmd, 1, simu->recon->dm_ncpa, 1);
}
//Extrapolate to edge actuators
if(simu->recon->actinterp && !simu->parms->recon.modal){
dcellcp(&simu->dmcmd0, simu->dmcmd);
dcellzero(simu->dmcmd);
dcellmm(&simu->dmcmd, simu->recon->actinterp, simu->dmcmd0, "nn", 1);
}
if(simu->ttmreal){
ttsplit_do(simu->recon, simu->dmcmd, simu->ttmreal, simu->parms->sim.lpttm);
}
/*hysterisis. */
if(simu->hyst){
hyst_dcell(simu->hyst, simu->dmreal, simu->dmcmd);
}
/*moao DM is already taken care of automatically.*/
}
void MUV(dcell **xout, const void *A,
const dcell *xin, const double alpha){
/**
Apply the sparse plug low rand compuation to xin
without scaling of alpha:
xout=(muv.M-muv.U*muv.V')*xin*alpha; U,V are low
rank.
*/
const MUV_T *muv=(const MUV_T *)A;
dspcellmulmat(xout, muv->M, xin, alpha);
dcell *tmp=NULL;
dcellmm(&tmp,muv->V, xin, "tn", -1.);
dcellmm(xout,muv->U, tmp, "nn", alpha);
dcellfree(tmp);
if(muv->invpsd){
apply_invpsd(xout, muv->xembed, muv->invpsd, muv->fftxopd, xin, alpha);
}
}
void save_recon(SIM_T *simu) {
const PARMS_T *parms=simu->parms;
const RECON_T *recon=simu->recon;
if(parms->plot.run) {
if(parms->recon.alg==0) {
for(int i=0; simu->opdr && i<simu->opdr->nx; i++) {
if(simu->opdr->p[i]) {
drawopd("opdr", recon->xloc->p[i], simu->opdr->p[i]->p, NULL,
"Reconstructed Atmosphere","x (m)","y (m)","opdr %d",i);
}
}
for(int i=0; simu->dmfit && i<simu->dmfit->nx; i++) {
if(simu->dmfit->p[i]) {
drawopd("DM", recon->aloc->p[i], simu->dmfit->p[i]->p,NULL,
"DM Fitting Output","x (m)", "y (m)","Fit %d",i);
}
}
}
if(!parms->recon.modal) {
for(int idm=0; simu->dmerr && idm<parms->ndm; idm++) {
if(simu->dmerr->p[idm]) {
drawopd("DM",recon->aloc->p[idm], simu->dmerr->p[idm]->p,NULL,
"DM Error Signal (Hi)","x (m)","y (m)",
"Err Hi %d",idm);
}
}
}
for(int idm=0; simu->dmerr && idm<parms->ndm; idm++) {
if(simu->dmint->mint->p[0]->p[idm]) {
drawopd("DM",recon->aloc->p[idm], simu->dmint->mint->p[0]->p[idm]->p,NULL,
"DM Integrator (Hi)","x (m)","y (m)",
"Int Hi %d",idm);
}
}
if(simu->dm_wfs) {
for(int iwfs=0; iwfs<parms->nwfs; iwfs++) {
int ipowfs=parms->wfs[iwfs].powfs;
int imoao=parms->powfs[ipowfs].moao;
if(imoao<0) continue;
drawopd("moao", recon->moao[imoao].aloc->p[0], simu->dm_wfs->p[iwfs]->p, NULL,
"MOAO", "x(m)", "y(m)", "WFS %d", iwfs);
}
}
if(simu->dm_evl) {
int imoao=parms->evl.moao;
for(int ievl=0; ievl<parms->evl.nevl && imoao>=0; ievl++) {
drawopd("moao", recon->moao[imoao].aloc->p[0], simu->dm_evl->p[ievl]->p, NULL,
"MOAO", "x(m)", "y(m)", "Evl %d", ievl);
}
}
}
if(parms->plot.run && simu->Merr_lo) {
dcell *dmlo=NULL;
switch(simu->parms->recon.split) {
case 1:
ngsmod2dm(&dmlo, recon, simu->Merr_lo, 1);
break;
case 2:
dcellmm(&dmlo, simu->recon->MVModes, simu->Merr_lo, "nn", 1);
break;
}
for(int idm=0; dmlo && idm<parms->ndm; idm++) {
drawopd("DM",recon->aloc->p[idm], dmlo->p[idm]->p,NULL,
"DM Error Signal (Lo)","x (m)","y (m)",
"Err Lo %d",idm);
}
dcellfree(dmlo);
}
if(parms->plot.run && simu->Mint_lo && simu->Mint_lo->mint->p[0]) {
dcell *dmlo=NULL;
switch(simu->parms->recon.split) {
case 1:
ngsmod2dm(&dmlo, recon, simu->Mint_lo->mint->p[0], 1);
break;
case 2:
dcellmm(&dmlo, simu->recon->MVModes, simu->Mint_lo->mint->p[0], "nn", 1);
break;
}
for(int idm=0; dmlo && idm<parms->ndm; idm++) {
drawopd("DM",recon->aloc->p[idm], dmlo->p[idm]->p,NULL,
"DM Integrator (Lo)","x (m)","y (m)",
"Int Lo %d",idm);
}
dcellfree(dmlo);
}
if(parms->recon.alg==0) { /*minimum variance tomo/fit reconstructor */
if(parms->save.opdr) {
cellarr_dcell(simu->save->opdr, simu->reconisim, simu->opdr);
}
if(parms->save.opdx || parms->plot.opdx) {
dcell *opdx=simu->opdx;
if(!opdx) {
atm2xloc(&opdx, simu);
}
if(parms->save.opdx) {
cellarr_dcell(simu->save->opdx, simu->isim, opdx);
}
if(parms->plot.opdx) { /*draw opdx */
for(int i=0; i<opdx->nx; i++) {
if(opdx->p[i]) {
drawopd("opdx", recon->xloc->p[i], opdx->p[i]->p, NULL,
"Atmosphere Projected to XLOC","x (m)","y (m)","opdx %d",i);
}
}
}
if(!parms->sim.idealfit) {
dcellfree(opdx);
}
}
}
if(parms->save.dm && (!parms->sim.closeloop || simu->isim>0)) {
if(simu->dmfit) {
cellarr_dcell(simu->save->dmfit, simu->reconisim, simu->dmfit);
}
if(simu->dmerr) {
cellarr_dcell(simu->save->dmerr, simu->reconisim, simu->dmerr);
}
if(simu->dmint->mint->p[0]) {
cellarr_dcell(simu->save->dmint, simu->reconisim, simu->dmint->mint->p[0]);
}
if(simu->Merr_lo) {
cellarr_dcell(simu->save->Merr_lo, simu->reconisim, simu->Merr_lo);
if(!parms->sim.fuseint && simu->Mint_lo->mint->p[0]) {
cellarr_dcell(simu->save->Mint_lo, simu->reconisim, simu->Mint_lo->mint->p[0]);
}
}
}
const int seed=simu->seed;
if(parms->save.ngcov>0 && CHECK_SAVE(parms->sim.start, parms->sim.end-(parms->sim.closeloop?1:0), simu->reconisim, parms->save.gcovp)) {
double scale=1./(double)(simu->reconisim-parms->sim.start+1);
dcellscale(simu->gcov, scale);
for(int igcov=0; igcov<parms->save.ngcov; igcov++) {
writebin(simu->gcov->p[igcov], "gcov_%d_wfs%ld_%ld_%d.bin", seed,
parms->save.gcov->p[igcov*2], parms->save.gcov->p[igcov*2+1],
simu->reconisim+1);
}
dcellscale(simu->gcov, 1./scale);
}
if(parms->sim.psfr && CHECK_SAVE(parms->evl.psfisim, parms->sim.end-(parms->sim.closeloop?1:0), simu->reconisim, parms->sim.psfr)) {
info2("Output PSF Recon Telemetry\n");
long nstep=simu->reconisim+1-parms->evl.psfisim;
double scale=1./nstep;
dcellscale(simu->ecov, scale);
if(!parms->dbg.useopdr || parms->sim.idealfit) {
writebin(simu->ecov, "ecov_%d_%ld", seed, nstep);
} else { /*deprecated */
char strht[24];
for(int ievl=0; ievl<parms->evl.nevl; ievl++) {
if(!simu->ecov->p[ievl]) continue;
if(isfinite(parms->evl.hs->p[ievl])) {
snprintf(strht, 24, "_%g", parms->evl.hs->p[ievl]);
} else {
strht[0]='\0';
}
writebin(simu->ecov->p[ievl], "ecov_%d_x%g_y%g%s_%ld.bin", seed,
parms->evl.thetax->p[ievl]*206265,
parms->evl.thetay->p[ievl]*206265, strht, nstep);
}
}
dcellscale(simu->ecov, 1./scale);//scale back to continuous accumulation
}
}
/**
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 */
}
}
}
/**
Update DM command for next cycle using info from last cycle (two cycle delay)
in closed loop mode */
static void filter_cl(SIM_T *simu){
/*
2009-11-02: Moved to the end of isim loop to update
for next step. only need to cache a single dmerrlast
now.
2009-12-23: Updated low fs to do lead filter/type II
2010-01-07: Create an option to merge the last
integrator in the hi/lo loop to simulation the actual
block diagram. removed dmreal_hi, Mreal_lo;
2010-01-08: Changed the filtering scheme by computing
dm command for next cycle instead of maintianing last
step error information.
2010-01-13: Implemented apdm.
a(n)=a(n-1)+ep*e(n-2) or
a(n)=0.5*(a(n-1)+a(n-2))+ep*e(n-2);
*/
const PARMS_T *parms=simu->parms;
RECON_T *recon=simu->recon;
assert(parms->sim.closeloop);
/*copy dm computed in last cycle. This is used in next cycle (already after perfevl) */
const SIM_CFG_T *simcfg=&(parms->sim);
const int isim=simu->isim;
{/*Auto adjusting epdm for testing different epdm*/
static int epdm_is_auto=0;
if(simcfg->epdm->p[0]<0){
epdm_is_auto=1;
simcfg->epdm->p[0]=0.5;
}
if(epdm_is_auto){
if((isim*10)<parms->sim.end){//initial steps
simcfg->epdm->p[0]=0.5;
}else if((isim*10)%parms->sim.end==0){
simcfg->epdm->p[0]=(double)isim/(double)parms->sim.end;
info("epdm is set to %.1f at step %d\n", simcfg->epdm->p[0], isim);
}
}
}
/*Do the servo filtering. First simulate a drop frame*/
int drop=0;
if(simu->dmerr && parms->sim.dtrat_skip){
if(parms->sim.dtrat_skip>0){
if((isim+1)%parms->sim.dtrat_skip==0){//evenly
drop=1;
}
}else if(parms->sim.dtrat_skip<0){//use random draws
double tmp=randu(simu->misc_rand);
if(tmp*(-parms->sim.dtrat_skip)<1.){
drop=1;
}
}
}
dcell *dmerr=0;
if(drop){
warning("Drop a frame at step %d\n", isim);
}else if(simu->dmerr){
dmerr=simu->dmerr;
}
//always run servo_filter even if dmerr is NULL.
int hiout=servo_filter(simu->dmint, dmerr);
if(parms->recon.split){
/*Low order in split tomography only. fused integrator*/
if(servo_filter(simu->Mint_lo, simu->Merr_lo) && parms->sim.fuseint){
/*accumulate to the main integrator.*/
addlow2dm(&simu->dmint->mint->p[0], simu, simu->Mint_lo->mpreint, 1);
}
}
/*The following are moved from the beginning to the end because the
gradients are now from last step.*/
dcellcp(&simu->dmcmd0,simu->dmint->mint->p[0]);
if(!parms->sim.fuseint){
addlow2dm(&simu->dmcmd0,simu,simu->Mint_lo->mint->p[0], 1);
}
for(int ipowfs=0; ipowfs<parms->npowfs; ipowfs++){
//Record dmpsol for this time step for each powfs before updating it (z^-1).
//Do not reference the data, even for dtrat==1
if(!parms->powfs[ipowfs].psol || !parms->powfs[ipowfs].dtrat) continue;
double alpha=(isim % parms->powfs[ipowfs].dtrat == 0)?0:1;
dcelladd(&simu->wfspsol->p[ipowfs], alpha, simu->dmpsol, 1./parms->powfs[ipowfs].dtrat);
}
dcellcp(&simu->dmpsol, simu->dmcmd0);
if(parms->recon.modal){
dcellzero(simu->dmcmd);
dcellmm(&simu->dmcmd, simu->recon->amod, simu->dmcmd0, "nn", 1);
//convert DM command from modal to zonal spae
}else if(simu->recon->actinterp && !parms->recon.psol){
//Extrapolate to edge actuators
dcellzero(simu->dmcmd);
dcellmm(&simu->dmcmd, simu->recon->actinterp, simu->dmcmd0, "nn", 1);
}else{
dcellcp(&simu->dmcmd, simu->dmcmd0);
}
//The DM commands are always on zonal modes from this moment
if(simu->ttmreal){
ttsplit_do(recon, simu->dmcmd, simu->ttmreal, parms->sim.lpttm);
}
if(parms->sim.focus2tel && hiout){
dcellcp(&simu->telfocusreal, simu->telfocusint);
dcellmm(&simu->telfocusint, recon->RFdm, simu->dmcmd, "nn", parms->sim.epfocus2tel);
}
if(recon->dither_m){
//Change phase in calc_dither_amp if phase of dithering is changed
//this is for step isim+1
double anglei=((isim+1)/recon->dither_dtrat)*(2*M_PI/recon->dither_npoint);
dcelladd(&simu->dmcmd, 1, recon->dither_m, sin(anglei));
}
if(!parms->dbg.ncpa_preload && recon->dm_ncpa){
info_once("Add NCPA after integrator\n");
dcelladd(&simu->dmcmd, 1, recon->dm_ncpa, 1);
}
if(parms->sim.dmclip || parms->sim.dmclipia || recon->actstuck){
dcell *tmp=dcelldup(simu->dmcmd);
if(recon->actstuck){//zero stuck actuators
act_stuck_cmd(recon->aloc, simu->dmerr, recon->actstuck);
}
clipdm(simu, simu->dmcmd);
dcelladd(&tmp, 1, simu->dmcmd, -1); //find what is clipped
dcelladd(&simu->dmint->mint->p[0], 1, tmp, -1);//remove from integrator (anti wind up)
dcelladd(&simu->dmpsol, 1, tmp, -1);//also feed to PSOL (is this really necessary?)
dcellfree(tmp);
}
/*This is after the integrator output and clipping*/
if(simu->dmhist){
for(int idm=0; idm<parms->ndm; idm++){
if(simu->dmhist->p[idm]){
dhistfill(&simu->dmhist->p[idm], simu->dmcmd->p[idm],0,
parms->dm[idm].histbin, parms->dm[idm].histn);
}
}
}
/*hysteresis. */
if(simu->hyst){
hyst_dcell(simu->hyst, simu->dmreal, simu->dmcmd);
}
if(recon->moao && !parms->gpu.moao){
warning_once("moao filter implemented with LPF\n");
if(simu->dm_wfs){
const int nwfs=parms->nwfs;
for(int iwfs=0; iwfs<nwfs; iwfs++){
int ipowfs=parms->wfs[iwfs].powfs;
int imoao=parms->powfs[ipowfs].moao;
if(imoao<0) continue;
double g=parms->moao[imoao].gdm;
dadd(&simu->dm_wfs->p[iwfs], 1-g, simu->dm_wfs->p[iwfs+nwfs], g);
}
}
if(simu->dm_evl){
const int nevl=parms->evl.nevl;
int imoao=parms->evl.moao;
double g=parms->moao[imoao].gdm;
for(int ievl=0; ievl<nevl; ievl++){
dadd(&simu->dm_evl->p[ievl], 1-g, simu->dm_evl->p[ievl+nevl], g);
}
}
}
if(simu->fsmint){
/*fsmerr is from gradients from this time step. so copy before update for correct delay*/
dcellcp(&simu->fsmreal, simu->fsmint->mint->p[0]);
servo_filter(simu->fsmint, simu->fsmerr);
/*Inject dithering command, for step isim+1*/
for(int iwfs=0; iwfs<parms->nwfs; iwfs++){
const int ipowfs=parms->wfs[iwfs].powfs;
if(parms->powfs[ipowfs].dither==1){//T/T dithering.
//adjust delay due to propagation, and computation delay.
const int adjust=parms->sim.alfsm+1-parms->powfs[ipowfs].dtrat;
//Use isim+1 because the command is for next time step.
//minus adjust for delay
double anglei=(2*M_PI/parms->powfs[ipowfs].dither_npoint);
double angle=((isim+1-adjust)/parms->powfs[ipowfs].dtrat)*anglei;
simu->fsmreal->p[iwfs]->p[0]-=parms->powfs[ipowfs].dither_amp*cos(angle);
simu->fsmreal->p[iwfs]->p[1]-=parms->powfs[ipowfs].dither_amp*sin(angle);
}
}
}
}
