int dprepend(dbuf_t *d, size_t delta) {
int res = dresize(d, d->size + delta);
if(res) {
memmove(&d->buf[delta], &d->buf[0], d->size - delta);
memset(&d->buf[0], 0, delta);
}
return res;
}
/**
Sum all the columns of PSDs (excluding first column), scaled by scale
*/
void psd_sum(dmat *psd, double scale){
for(int ix=0; ix<psd->nx; ix++){
double tmp=0;
for(int iy=1; iy<psd->ny; iy++){
tmp+=IND(psd, ix, iy);
}
IND(psd, ix, 1)=tmp*scale;
}
dresize(psd, psd->nx, 2);
}
/**
Convert PSD into time series.*/
dmat* psd2time(const dmat *psdin, rand_t *rstat, double dt, int nstepin){
if(!psdin){
error("psdin cannot be null\n");
}
long nstep=nextpow2(nstepin);
double df=1./(dt*nstep);
dmat *fs=dlinspace(0, df, nstep);
dmat *psd=NULL;
if(psdin->ny==1){//[alpha, beta, fmin, fmax] discribes power law with cut on/off freq.
psd=dnew(nstep, 1);
double alpha=psdin->p[0];
double beta=psdin->p[1];
long i0=1, imax=nstep;
if(psdin->nx>2){
i0=(long)round(psdin->p[2]/df);
if(i0<1) i0=1;
}
if(psdin->nx>3){
imax=(long)round(psdin->p[3]/df);
}
info("fmin=%g, fmax=%g, df=%g, i0=%ld, imax=%ld\n",
psdin->p[2], psdin->p[3], df, i0, imax);
for(long i=i0; i<imax; i++){
psd->p[i]=beta*pow(i*df, alpha);
}
}else if(psdin->ny==2){
if(psdin->nx<2){
error("Invalid PSD\n");
}
psd=dinterp1(psdin, 0, fs, 1e-40);
psd->p[0]=0;/*disable pistion. */
}else{
error("psdin is invalid format.\n");
}
cmat *wshat=cnew(nstep, 1);
//cfft2plan(wshat, -1);
for(long i=0; i<nstep; i++){
wshat->p[i]=sqrt(psd->p[i]*df)*COMPLEX(randn(rstat), randn(rstat));
}
cfft2(wshat, -1);
dmat *out=NULL;
creal2d(&out, 0, wshat, 1);
cfree(wshat);
dfree(psd);
dfree(fs);
dresize(out, nstepin, 1);
return out;
}
/**
Initialize. al is additional latency
*/
SERVO_T *servo_new(dcell *merr, const dmat *ap, int al, double dt, const dmat *ep){
SERVO_T *st=calloc(1, sizeof(SERVO_T));
if(ap){
st->ap=ddup(ap);
}else{
st->ap=dnew(2,1);
st->ap->p[0]=1;
}
if(st->ap->nx<2){
dresize(st->ap, 2, 1);//2 element to ensure we keep integrator history.
}
st->mint=cellnew(st->ap->nx, 1);
st->dt=dt;
st->al=al;
st->merrhist=cellnew(st->al+1, 1);
servo_update(st, ep);
if(merr && merr->nx!=0 && merr->ny!=0 && merr->p[0]){
servo_init(st, merr);
}
return st;
}
int
dgrow(dbuf_t * d, size_t delta)
{
return dresize(d, d->size + delta);
}
/**
Find vibration peaks in the PSD by comparing the PSD against a LPF version plus noise.
*/
dmat *psd_vibid(const dmat *psdin){
double *f=psdin->p;
double *psd=psdin->p+psdin->nx;
dmat *y=dsub(psdin, 0, 0, 1, 1);
const double gain=0.1;
const double gain2=0.05;
int inpeak=0;
double ylpf0=y->p[1];
double dylpf0=fabs(y->p[1]-y->p[0]);
double ylpf=0, dylpf=0;
int nmaxp=100;
dmat *res=dnew(4, nmaxp);
double thres=25e-18;/*threshold: 5 nm*/
double sumxy=0, sumy=0, sum=0;
int count=0;
for(long i=1; i<psdin->nx-1; i++){
if(!inpeak){
//second order LPF
ylpf0=(1.-gain)*ylpf0+y->p[i]*gain;
ylpf=(1.-gain)*ylpf+ylpf0*gain;
double diff=y->p[i]-y->p[i-1];
if(diff>0){
dylpf0=(1.-gain2)*dylpf0+diff*gain2;
dylpf=(1.-gain2)*dylpf+dylpf0*gain2;
}
if(y->p[i+1]>ylpf+dylpf*5 && f[i]>1){//beginning of peak
inpeak=1;
if(count>0 && f[i] < f[(int)IND(res,3,count-1)] + 0.1){
//combine with last peak if within 1 Hz.
count--;
}else{
IND(res,2,count)=i;
sumxy=f[i]*psd[i];//for CoG
sumy=psd[i];//for CoG
sum=0;//integration
}
}
}else{
//continuation of peak
sumxy+=f[i]*psd[i];
sumy+=psd[i];
sum+=(f[i]-f[i-1])*(psd[i]+psd[i-1]);
if(y->p[i]<ylpf+dylpf && y->p[i+1]<ylpf+dylpf){//end of peak
inpeak=0;
if(sum*0.5>thres){
IND(res,0,count)=sumxy/sumy;
IND(res,1,count)=sum*0.5;
IND(res,3,count)=i;
count++;
if(count==nmaxp){
nmaxp*=2;
dresize(res, 4, nmaxp);
}
}
}
}
}
dfree(y);
dresize(res, 4, count);
return res;
}
