libcm is a C development framework with an emphasis on audio signal processing applications.
Du kannst nicht mehr als 25 Themen auswählen Themen müssen mit entweder einem Buchstaben oder einer Ziffer beginnen. Sie können Bindestriche („-“) enthalten und bis zu 35 Zeichen lang sein.

cmProc5.c 24KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021
  1. #include "cmPrefix.h"
  2. #include "cmGlobal.h"
  3. #include "cmRpt.h"
  4. #include "cmErr.h"
  5. #include "cmCtx.h"
  6. #include "cmMem.h"
  7. #include "cmMallocDebug.h"
  8. #include "cmLinkedHeap.h"
  9. #include "cmFloatTypes.h"
  10. #include "cmComplexTypes.h"
  11. #include "cmFileSys.h"
  12. #include "cmJson.h"
  13. #include "cmSymTbl.h"
  14. #include "cmAudioFile.h"
  15. #include "cmText.h"
  16. #include "cmProcObj.h"
  17. #include "cmProcTemplate.h"
  18. #include "cmMath.h"
  19. #include "cmFile.h"
  20. #include "cmTime.h"
  21. #include "cmMidi.h"
  22. #include "cmProc.h"
  23. #include "cmProc2.h"
  24. #include "cmProc5.h"
  25. #include "cmVectOps.h"
  26. //=======================================================================================================================
  27. cmGoertzel* cmGoertzelAlloc( cmCtx* c, cmGoertzel* p, double srate, const double* fcHzV, unsigned chCnt, unsigned procSmpCnt, unsigned hopSmpCnt, unsigned wndSmpCnt )
  28. {
  29. cmGoertzel* op = cmObjAlloc(cmGoertzel,c,p);
  30. op->shb = cmShiftBufAlloc(c,NULL,0,0,0);
  31. if( srate > 0 )
  32. if( cmGoertzelInit(op,srate,fcHzV,chCnt,procSmpCnt,wndSmpCnt,hopSmpCnt) != cmOkRC )
  33. cmGoertzelFree(&op);
  34. return op;
  35. }
  36. cmRC_t cmGoertzelFree( cmGoertzel** pp )
  37. {
  38. cmRC_t rc = cmOkRC;
  39. if( pp==NULL || *pp==NULL )
  40. return rc;
  41. cmGoertzel* p = *pp;
  42. if((rc = cmGoertzelFinal(p)) != cmOkRC )
  43. return rc;
  44. cmShiftBufFree(&p->shb);
  45. cmMemFree(p->ch);
  46. cmMemFree(p->wnd);
  47. cmObjFree(pp);
  48. return rc;
  49. }
  50. cmRC_t cmGoertzelInit( cmGoertzel* p, double srate, const double* fcHzV, unsigned chCnt, unsigned procSmpCnt, unsigned hopSmpCnt, unsigned wndSmpCnt )
  51. {
  52. cmRC_t rc;
  53. unsigned i;
  54. if((rc = cmGoertzelFinal(p)) != cmOkRC )
  55. return rc;
  56. p->ch = cmMemResizeZ(cmGoertzelCh,p->ch,chCnt);
  57. p->chCnt = chCnt;
  58. p->srate = srate;
  59. p->wnd = cmMemResizeZ(cmSample_t,p->wnd,wndSmpCnt);
  60. cmVOS_Hann(p->wnd,wndSmpCnt);
  61. cmShiftBufInit(p->shb,procSmpCnt,wndSmpCnt,hopSmpCnt);
  62. for(i=0; i<p->chCnt; ++i)
  63. {
  64. cmGoertzelSetFcHz(p,i,fcHzV[i]);
  65. }
  66. return rc;
  67. }
  68. cmRC_t cmGoertzelFinal( cmGoertzel* p )
  69. { return cmOkRC; }
  70. cmRC_t cmGoertzelSetFcHz( cmGoertzel* p, unsigned chIdx, double hz )
  71. {
  72. assert( chIdx < p->chCnt );
  73. p->ch[chIdx].hz = hz;
  74. p->ch[chIdx].coeff = 2*cos(2*M_PI*hz/p->srate);
  75. return cmOkRC;
  76. }
  77. cmRC_t cmGoertzelExec( cmGoertzel* p, const cmSample_t* inpV, unsigned procSmpCnt, double* outV, unsigned chCnt )
  78. {
  79. unsigned i,j;
  80. while( cmShiftBufExec(p->shb,inpV,procSmpCnt) )
  81. {
  82. unsigned xn = p->shb->wndSmpCnt;
  83. cmSample_t x[ xn ];
  84. cmVOS_MultVVV(x,xn,p->wnd,p->shb->outV);
  85. for(i=0; i<chCnt; ++i)
  86. {
  87. cmGoertzelCh* ch = p->ch + i;
  88. ch->s2 = x[0];
  89. ch->s1 = x[1] + 2 * x[0] * ch->coeff;
  90. for(j=2; j<xn; ++j)
  91. {
  92. ch->s0 = x[j] + ch->coeff * ch->s1 - ch->s2;
  93. ch->s2 = ch->s1;
  94. ch->s1 = ch->s0;
  95. }
  96. outV[i] = ch->s2*ch->s2 + ch->s1*ch->s1 - ch->coeff * ch->s2 * ch->s1;
  97. }
  98. }
  99. return cmOkRC;
  100. }
  101. //=======================================================================================================================
  102. double _cmGoldSigSinc( double t, double T )
  103. {
  104. double x = t/T;
  105. return x == 0 ? 1.0 : sin(M_PI*x)/(M_PI*x);
  106. }
  107. void _cmGoldSigRaisedCos( cmSample_t* yV, int yN, double sPc, double beta )
  108. {
  109. int i;
  110. for(i=0; i<yN; ++i)
  111. {
  112. double t = i - yN/2;
  113. double den = 1 - (4*(beta*beta)*(t*t) / (sPc*sPc));
  114. double a;
  115. if(fabs(den) < 0.00001 )
  116. a = 1;
  117. else
  118. a = cos(M_PI * beta * t/ sPc ) / den;
  119. yV[i] = _cmGoldSigSinc(t,sPc) * a;
  120. }
  121. }
  122. void _cmGoldSigConv( cmGoldSig_t* p, unsigned chIdx )
  123. {
  124. int i;
  125. int sPc = p->a.samplesPerChip;
  126. int osf = p->a.rcosOSFact;
  127. // for each bit in the spreading-code
  128. for(i=0; i<p->mlsN; ++i)
  129. {
  130. int j = (i*sPc) + sPc/2; // index into bbV[] of center of impulse response
  131. int k = j - (sPc*osf)/2; // index into bbV[] of start of impulse response
  132. int h;
  133. // for each sample in the impulse response
  134. for(h=0; h<p->rcosN; ++h,++k)
  135. {
  136. while( k<0 )
  137. k += p->sigN;
  138. while( k>=p->sigN )
  139. k -= p->sigN;
  140. p->ch[chIdx].bbV[k] += p->ch[chIdx].pnV[i] * p->rcosV[h];
  141. }
  142. }
  143. }
  144. void _cmGoldSigModulate( cmGoldSig_t* p, unsigned chIdx )
  145. {
  146. unsigned i;
  147. double rps = 2.0 * M_PI * p->a.carrierHz / p->a.srate;
  148. cmSample_t* yV = p->ch[chIdx].mdV;
  149. cmSample_t* bbV = p->ch[chIdx].bbV;
  150. for(i=0; i<p->sigN; ++i)
  151. yV[ i ] = bbV[i]*cos(rps*i) + bbV[i]*sin(rps*i);
  152. // apply a half Hann envelope to the onset/offset of the id signal
  153. if( p->a.envMs > 0 )
  154. {
  155. unsigned wndMs = p->a.envMs * 2;
  156. unsigned wndN = wndMs * p->a.srate / 1000;
  157. wndN += wndN % 2 ? 0 : 1; // force the window length to be odd
  158. unsigned wNo2 = wndN/2 + 1;
  159. cmSample_t wndV[ wndN ];
  160. cmVOS_Hann(wndV,wndN);
  161. cmVOS_MultVV(yV,wNo2,wndV);
  162. cmVOS_MultVV(yV + p->sigN - wNo2, wNo2, wndV + wNo2 - 1);
  163. }
  164. }
  165. cmGoldSig_t* cmGoldSigAlloc( cmCtx* ctx, cmGoldSig_t* p, const cmGoldSigArg_t* a )
  166. {
  167. cmGoldSig_t* op = cmObjAlloc(cmGoldSig_t,ctx,p);
  168. op->fir = cmFIRAllocKaiser(ctx, NULL, 0, 0, 0, 0, 0, 0, 0 );
  169. if( a != NULL )
  170. if( cmGoldSigInit(op,a) != cmOkRC )
  171. cmGoldSigFree(&op);
  172. return op;
  173. }
  174. cmRC_t cmGoldSigFree( cmGoldSig_t** pp )
  175. {
  176. cmRC_t rc = cmOkRC;
  177. if( pp == NULL || *pp == NULL )
  178. return rc;
  179. cmGoldSig_t* p = *pp;
  180. if((rc = cmGoldSigFinal(p)) != cmOkRC )
  181. return rc;
  182. unsigned i;
  183. for(i=0; i<p->a.chN; ++i)
  184. {
  185. cmMemFree(p->ch[i].bbV);
  186. cmMemFree(p->ch[i].mdV);
  187. }
  188. cmFIRFree(&p->fir);
  189. cmMemFree(p->ch);
  190. cmMemFree(p->rcosV);
  191. cmMemFree(p->pnM);
  192. cmMemFree(p);
  193. *pp = NULL;
  194. return rc;
  195. }
  196. cmRC_t cmGoldSigInit( cmGoldSig_t* p, const cmGoldSigArg_t* a )
  197. {
  198. cmRC_t rc = cmOkRC;
  199. unsigned i;
  200. p->a = *a; // store arg recd
  201. p->ch = cmMemResizeZ(cmGoldSigCh_t,p->ch,a->chN); // alloc channel array
  202. p->mlsN = (1 << a->lfsrN) - 1; // calc spreading code length
  203. p->rcosN = a->samplesPerChip * a->rcosOSFact; // calc rcos imp. resp. length
  204. p->rcosN += (p->rcosN % 2)==0; // force rcos imp. length odd
  205. p->rcosV = cmMemResizeZ(cmSample_t,p->rcosV,p->rcosN); // alloc rcos imp. resp. vector
  206. p->pnM = cmMemResizeZ(int,p->pnM,p->mlsN*a->chN); // alloc spreading-code mtx
  207. p->sigN = p->mlsN * a->samplesPerChip; // calc audio signal length
  208. // generate spreading codes
  209. if( cmGenGoldCodes(a->lfsrN, a->mlsCoeff0, a->mlsCoeff1, a->chN, p->pnM, p->mlsN ) == false )
  210. {
  211. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Unable to generate sufficient balanced Gold codes.");
  212. goto errLabel;
  213. }
  214. // generate the rcos impulse response
  215. _cmGoldSigRaisedCos(p->rcosV,p->rcosN,a->samplesPerChip,a->rcosBeta);
  216. if(1)
  217. {
  218. double passHz = 20000.0;
  219. double stopHz = 17000.0;
  220. double passDb = 1.0;
  221. double stopDb = 90.0;
  222. unsigned flags = 0;
  223. if( cmFIRInitKaiser(p->fir, 64, a->srate, passHz, stopHz, passDb, stopDb, flags ) != cmOkRC )
  224. {
  225. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Unable to allocate internal FIR.");
  226. goto errLabel;
  227. }
  228. p->rcosN = p->fir->coeffCnt;
  229. p->rcosV = cmMemResizeZ(cmSample_t,p->rcosV,p->rcosN);
  230. cmVOS_CopyD(p->rcosV,p->rcosN,p->fir->coeffV);
  231. }
  232. // for each channel
  233. for(i=0; i<a->chN; ++i)
  234. {
  235. // Note: if (i*p->mlsN) is set to 0 in the following line then all channels
  236. // will use the same spreading code.
  237. p->ch[i].pnV = p->pnM + (i*p->mlsN); // get ch. spreading code
  238. p->ch[i].bbV = cmMemResizeZ(cmSample_t,p->ch[i].bbV,p->sigN); // alloc baseband signal vector
  239. p->ch[i].mdV = cmMemResizeZ(cmSample_t,p->ch[i].mdV,p->sigN); // alloc output audio vector
  240. // Convolve spreading code with rcos impulse reponse to form baseband signal.
  241. _cmGoldSigConv(p, i );
  242. // Modulate baseband signal to carrier frq. and apply attack/decay envelope.
  243. _cmGoldSigModulate(p, i );
  244. }
  245. errLabel:
  246. if((rc = cmErrLastRC(&p->obj.err)) != cmOkRC )
  247. cmGoldSigFree(&p);
  248. return rc;
  249. }
  250. cmRC_t cmGoldSigFinal( cmGoldSig_t* p )
  251. { return cmFIRFinal(p->fir); }
  252. cmRC_t cmGoldSigWrite( cmCtx* ctx, cmGoldSig_t* p, const char* fn )
  253. {
  254. cmVectArray_t* vap = NULL;
  255. unsigned i;
  256. vap = cmVectArrayAlloc(ctx,kSampleVaFl);
  257. for(i=0; i<p->a.chN; ++i)
  258. {
  259. cmVectArrayAppendS(vap,p->ch[i].bbV,p->sigN);
  260. cmVectArrayAppendS(vap,p->ch[i].mdV,p->sigN);
  261. }
  262. cmVectArrayWrite(vap,fn);
  263. cmVectArrayFree(&vap);
  264. return cmOkRC;
  265. }
  266. cmRC_t cmGoldSigGen( cmGoldSig_t* p, unsigned chIdx, unsigned prefixN, unsigned dsN, unsigned *bsiV, unsigned bsiN, double noiseGain, cmSample_t** yVRef, unsigned* yNRef )
  267. {
  268. unsigned yN = prefixN + bsiN * (p->sigN + dsN);
  269. cmSample_t* yV = cmMemAllocZ(cmSample_t,yN);
  270. unsigned i;
  271. cmVOS_Random(yV, yN, -noiseGain, noiseGain );
  272. for(i=0; i<bsiN; ++i)
  273. {
  274. bsiV[i] = prefixN + i*(p->sigN + dsN);
  275. cmVOS_AddVV(yV + bsiV[i], p->sigN, p->ch[chIdx].mdV );
  276. }
  277. if( yVRef != NULL )
  278. *yVRef = yV;
  279. if( yNRef != NULL )
  280. *yNRef = yN;
  281. return cmOkRC;
  282. }
  283. //=======================================================================================================================
  284. cmPhat_t* cmPhatAlloc( cmCtx* ctx, cmPhat_t* ap, unsigned chN, unsigned hN, float alpha, unsigned mult, unsigned flags )
  285. {
  286. cmPhat_t* p = cmObjAlloc(cmPhat_t,ctx,ap);
  287. // The FFT buffer and the delay line is at least twice the size of the
  288. // id signal. This will guarantee that at least one complete id signal
  289. // is inside the buffer. In practice it means that it is possible
  290. // that there will be two id's in the buffer therefore if there are
  291. // two correlation spikes it is important that we take the second.
  292. unsigned fhN = cmNextPowerOfTwo(mult*hN);
  293. // allocate the FFT object
  294. cmFftAllocSR(ctx,&p->fft,NULL,fhN,kToPolarFftFl);
  295. cmIFftAllocRS(ctx,&p->ifft,fhN/2 + 1 );
  296. // allocate the vect array
  297. p->ftVa = cmVectArrayAlloc(ctx, kSampleVaFl );
  298. if( chN != 0 )
  299. if( cmPhatInit(p,chN,hN,alpha,mult,flags) != cmOkRC )
  300. cmPhatFree(&p);
  301. return p;
  302. }
  303. cmRC_t cmPhatFree( cmPhat_t** pp )
  304. {
  305. cmRC_t rc = cmOkRC;
  306. if( pp == NULL || *pp == NULL )
  307. return rc;
  308. cmPhat_t* p = *pp;
  309. if((rc = cmPhatFinal(p)) != cmOkRC )
  310. return rc;
  311. cmMemFree(p->t0V);
  312. cmMemFree(p->t1V);
  313. cmMemFree(p->dV);
  314. cmMemFree(p->xV);
  315. cmMemFree(p->fhM);
  316. cmMemFree(p->mhM);
  317. cmMemFree(p->wndV);
  318. cmObjFreeStatic(cmFftFreeSR, cmFftSR, p->fft);
  319. cmObjFreeStatic(cmIFftFreeRS, cmIFftRS, p->ifft);
  320. cmVectArrayFree(&p->ftVa);
  321. cmObjFree(pp);
  322. return rc;
  323. }
  324. cmRC_t cmPhatInit( cmPhat_t* p, unsigned chN, unsigned hN, float alpha, unsigned mult, unsigned flags )
  325. {
  326. cmRC_t rc = cmOkRC;
  327. if((rc = cmPhatFinal(cmOkRC)) != cmOkRC )
  328. return rc;
  329. p->fhN = cmNextPowerOfTwo(mult*hN);
  330. if((cmFftInitSR(&p->fft, NULL, p->fhN, kToPolarFftFl)) != cmOkRC )
  331. return rc;
  332. if((cmIFftInitRS(&p->ifft, p->fft.binCnt )) != cmOkRC )
  333. return rc;
  334. p->alpha = alpha;
  335. p->flags = flags;
  336. // allocate the delay line
  337. p->dV = cmMemResizeZ(cmSample_t,p->dV,p->fhN);
  338. p->di = 0;
  339. // allocate the linear buffer
  340. p->xV = cmMemResizeZ(cmSample_t,p->xV,p->fhN);
  341. p->t0V = cmMemResizeZ(cmComplexR_t,p->t0V,p->fhN);
  342. p->t1V = cmMemResizeZ(cmComplexR_t,p->t1V,p->fhN);
  343. // allocate the window function
  344. p->wndV = cmMemResizeZ(cmSample_t,p->wndV,p->fhN);
  345. cmVOS_Hann(p->wndV,p->fhN);
  346. // allocate the signal id matrix
  347. p->chN = chN;
  348. p->hN = hN;
  349. p->binN = p->fft.binCnt; //atFftRealBinCount(p->fftH);
  350. p->fhM = cmMemResizeZ(cmComplexR_t, p->fhM, p->fhN * chN);
  351. p->mhM = cmMemResizeZ(float, p->mhM, p->binN * chN);
  352. cmPhatReset(p);
  353. if( cmIsFlag(p->flags,kDebugAtPhatFl))
  354. cmVectArrayClear(p->ftVa);
  355. return rc;
  356. }
  357. cmRC_t cmPhatFinal( cmPhat_t* p )
  358. { return cmOkRC; }
  359. cmRC_t cmPhatReset( cmPhat_t* p )
  360. {
  361. p->di = 0;
  362. p->absIdx = 0;
  363. cmVOS_Zero(p->dV,p->fhN);
  364. return cmOkRC;
  365. }
  366. cmRC_t cmPhatSetId( cmPhat_t* p, unsigned chIdx, const cmSample_t* hV, unsigned hN )
  367. {
  368. unsigned i;
  369. assert( chIdx < p->chN );
  370. assert( hN == p->hN );
  371. // Allocate a window vector
  372. cmSample_t* wndV = cmMemAllocZ(cmSample_t,hN);
  373. cmVOS_Hann(wndV,hN);
  374. // get ptr to output column in p->fhM[].
  375. cmComplexR_t* yV = p->fhM + (chIdx*p->fhN);
  376. // Zero pad hV[hN] to p->fhN;
  377. assert( hN <= p->fhN );
  378. cmVOS_Zero(p->xV,p->fhN);
  379. cmVOS_Copy(p->xV,hN,hV);
  380. // Apply the window function to the id signal
  381. if(cmIsFlag(p->flags,kHannAtPhatFl) )
  382. cmVOS_MultVVV(p->xV,hN,hV,wndV);
  383. // take FFT of id signal. The result is in fft->complexV and fft->magV,phsV
  384. cmFftExecSR(&p->fft, p->xV, p->fhN );
  385. // Store the magnitude of the id signal
  386. //atFftComplexAbs(p->mhM + (chIdx*p->binN), yV, p->binN);
  387. cmVOF_CopyR(p->mhM + (chIdx*p->binN), p->binN, p->fft.magV );
  388. // Scale the magnitude
  389. cmVOS_MultVS( p->mhM + (chIdx*p->binN), p->binN, p->alpha);
  390. // store the complex conjugate of the FFT result in yV[]
  391. //atFftComplexConj(yV,p->binN);
  392. for(i=0; i<p->binN; ++i)
  393. yV[i] = cmCconjR(p->fft.complexV[i]);
  394. cmMemFree(wndV);
  395. return cmOkRC;
  396. }
  397. cmSample_t* _cmPhatReadVector( cmCtx* ctx, cmPhat_t* p, const char* fn, unsigned* vnRef )
  398. {
  399. cmVectArray_t* vap = NULL;
  400. cmSample_t* v = NULL;
  401. cmRC_t rc = cmOkRC;
  402. // instantiate a VectArray from a file
  403. if( (vap = cmVectArrayAllocFromFile(ctx, fn )) == NULL )
  404. {
  405. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Id component vector file read failed '%s'.",fn);
  406. goto errLabel;
  407. }
  408. // get the count of elements in the vector
  409. *vnRef = cmVectArrayEleCount(vap);
  410. // allocate memory to hold the vector
  411. v = cmMemAlloc(cmSample_t,*vnRef);
  412. // copy the vector from the vector array object into v[]
  413. if((rc = cmVectArrayGetF(vap,v,vnRef)) != cmOkRC )
  414. {
  415. cmMemFree(v);
  416. v = NULL;
  417. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Id component vector copy out failed '%s'.",fn);
  418. goto errLabel;
  419. }
  420. cmRptPrintf(p->obj.err.rpt,"%i : %s",*vnRef,fn);
  421. errLabel:
  422. cmVectArrayFree(&vap);
  423. return v;
  424. }
  425. cmRC_t cmPhatExec( cmPhat_t* p, const cmSample_t* xV, unsigned xN )
  426. {
  427. unsigned n = cmMin(xN,p->fhN-p->di);
  428. // update the delay line
  429. cmVOS_Copy(p->dV+p->di,n,xV);
  430. if( n < xN )
  431. cmVOS_Copy(p->dV,xN-n,xV+n);
  432. p->di = cmModIncr(p->di,xN,p->fhN);
  433. // p->absIdx is the absolute sample index associated with di
  434. p->absIdx += xN;
  435. return cmOkRC;
  436. }
  437. void cmPhatChExec(
  438. cmPhat_t* p,
  439. unsigned chIdx,
  440. unsigned sessionId,
  441. unsigned roleId)
  442. {
  443. unsigned n0 = p->fhN - p->di;
  444. unsigned n1 = p->fhN - n0;
  445. // Linearize the delay line into xV[]
  446. cmVOS_Copy(p->xV, n0, p->dV + p->di );
  447. cmVOS_Copy(p->xV+n0, n1, p->dV );
  448. if( cmIsFlag(p->flags,kDebugAtPhatFl))
  449. cmVectArrayAppendS(p->ftVa, p->xV, p->fhN );
  450. // apply a window function to the incoming signal
  451. if( cmIsFlag(p->flags,kHannAtPhatFl) )
  452. cmVOS_MultVV(p->xV,p->fhN,p->wndV);
  453. // Take the FFT of the delay line.
  454. // p->t0V[p->binN] = fft(p->xV)
  455. //atFftRealForward(p->fftH, p->xV, p->fhN, p->t0V, p->binN );
  456. cmFftExecSR(&p->fft, p->xV, p->fhN );
  457. // Calc. the Cross Power Spectrum (aka cross spectral density) of the
  458. // input signal with the id signal.
  459. // Note that the CPS is equivalent to the Fourier Transform of the
  460. // cross-correlation of the two signals.
  461. // t0V[] *= p->fhM[:,chIdx]
  462. //atFftComplexMult( p->t0V, p->fhM + (chIdx * p->fhN), p->binN );
  463. cmVOCR_MultVVV( p->t0V, p->fft.complexV, p->fhM + (chIdx * p->fhN), p->binN);
  464. // Calculate the magnitude of the CPS.
  465. // xV[] = | t0V[] |
  466. cmVOCR_Abs( p->xV, p->t0V, p->binN );
  467. // Weight the CPS by the scaled magnitude of the id signal
  468. // (we want to emphasize the limited frequencies where the
  469. // id signal contains energy)
  470. // t0V[] *= p->mhM[:,chIdx]
  471. if( p->alpha > 0 )
  472. cmVOCR_MultVFV( p->t0V, p->mhM + (chIdx*p->binN), p->binN);
  473. // Divide through by the magnitude of the CPS
  474. // This has the effect of whitening the spectram and thereby
  475. // minimizing the effect of the magnitude correlation
  476. // while maximimizing the effect of the phase correlation.
  477. //
  478. // t0V[] /= xV[]
  479. cmVOCR_DivVFV( p->t0V, p->xV, p->binN );
  480. // Take the IFFT of the weighted CPS to recover the cross correlation.
  481. // xV[] = IFFT(t0V[])
  482. cmIFftExecRS( &p->ifft, p->t0V );
  483. // Normalize the result by the length of the transform.
  484. cmVOS_DivVVS( p->xV, p->fhN, p->ifft.outV, p->fhN );
  485. // Shift the correlation spike to mark the end of the id
  486. cmVOS_Rotate( p->xV, p->fhN, -((int)p->hN) );
  487. // normalize by the length of the correlation
  488. cmVOS_DivVS(p->xV,p->fhN,p->fhN);
  489. if( cmIsFlag(p->flags,kDebugAtPhatFl))
  490. {
  491. cmVectArrayAppendS(p->ftVa, p->xV, p->fhN );
  492. cmSample_t v[] = { sessionId, roleId };
  493. cmVectArrayAppendS(p->ftVa, v, sizeof(v)/sizeof(v[0]));
  494. }
  495. }
  496. cmRC_t cmPhatWrite( cmPhat_t* p, const char* dirStr )
  497. {
  498. cmRC_t rc = cmOkRC;
  499. if( cmIsFlag(p->flags, kDebugAtPhatFl))
  500. {
  501. const char* path = NULL;
  502. if( cmVectArrayCount(p->ftVa) )
  503. if((rc = cmVectArrayWrite(p->ftVa, path = cmFsMakeFn(path,"cmPhatFT","va",dirStr,NULL) )) != cmOkRC )
  504. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"PHAT debug file write failed.");
  505. cmFsFreeFn(path);
  506. }
  507. return rc;
  508. }
  509. //=======================================================================================================================
  510. //
  511. //
  512. cmReflectCalc_t* cmReflectCalcAlloc( cmCtx* ctx, cmReflectCalc_t* p, const cmGoldSigArg_t* gsa, float phat_alpha, unsigned phat_mult )
  513. {
  514. cmReflectCalc_t* op = cmObjAlloc(cmReflectCalc_t,ctx,p);
  515. cmRC_t rc = cmOkRC;
  516. // allocate the Gold code signal generator
  517. if( (op->gs = cmGoldSigAlloc(ctx,NULL,NULL)) == NULL )
  518. {
  519. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Gold sig allocate failed.");
  520. goto errLabel;
  521. }
  522. // allocate the PHAT object
  523. if( (op->phat = cmPhatAlloc(ctx,NULL,0,0,0,0,0)) == NULL )
  524. {
  525. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"PHAT allocate failed.");
  526. goto errLabel;
  527. }
  528. op->phVa = cmVectArrayAlloc(ctx,kSampleVaFl);
  529. op->xVa = cmVectArrayAlloc(ctx,kSampleVaFl);
  530. op->yVa = cmVectArrayAlloc(ctx,kSampleVaFl);
  531. // allocate 'this'
  532. if( gsa != NULL )
  533. rc = cmReflectCalcInit(op,gsa,phat_alpha,phat_mult);
  534. errLabel:
  535. if( rc != cmOkRC )
  536. cmReflectCalcFree(&op);
  537. return op;
  538. }
  539. cmRC_t cmReflectCalcFree( cmReflectCalc_t** pp )
  540. {
  541. cmRC_t rc = cmOkRC;
  542. if( pp == NULL || *pp == NULL )
  543. return rc;
  544. cmReflectCalc_t* p = *pp;
  545. cmReflectCalcWrite(p,"/Users/kevin/temp/kc");
  546. if((rc = cmReflectCalcFinal(p)) != cmOkRC )
  547. return rc;
  548. cmMemFree(p->t0V);
  549. cmMemFree(p->t1V);
  550. cmVectArrayFree(&p->phVa);
  551. cmVectArrayFree(&p->xVa);
  552. cmVectArrayFree(&p->yVa);
  553. cmGoldSigFree(&p->gs);
  554. cmPhatFree(&p->phat);
  555. cmMemFree(p);
  556. *pp = NULL;
  557. return rc;
  558. }
  559. cmRC_t cmReflectCalcInit( cmReflectCalc_t* p, const cmGoldSigArg_t* gsa, float phat_alpha, unsigned phat_mult )
  560. {
  561. cmRC_t rc;
  562. if((rc = cmReflectCalcFinal(p)) != cmOkRC )
  563. return rc;
  564. // initialize the Gold code signal generator
  565. if((rc = cmGoldSigInit(p->gs,gsa)) != cmOkRC )
  566. {
  567. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Gold code signal initialize failed.");
  568. goto errLabel;
  569. }
  570. unsigned phat_chN = 1;
  571. unsigned phat_hN = p->gs->sigN;
  572. unsigned phat_flags = 0;
  573. unsigned phat_chIdx = 0;
  574. // initialize the PHAT
  575. if((rc = cmPhatInit(p->phat,phat_chN,phat_hN,phat_alpha,phat_mult,phat_flags)) != cmOkRC )
  576. {
  577. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"PHAT intialize failed.");
  578. goto errLabel;
  579. }
  580. // register a target signal with the PHAT
  581. if((rc = cmPhatSetId( p->phat, phat_chIdx, p->gs->ch[phat_chIdx].mdV, p->gs->sigN )) != cmOkRC )
  582. {
  583. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"PHAT signal registration failed.");
  584. goto errLabel;
  585. }
  586. p->xi = 0;
  587. p->tN = 5;
  588. p->t0V = cmMemResizeZ(unsigned,p->t0V,p->tN);
  589. p->t1V = cmMemResizeZ(unsigned,p->t1V,p->tN);
  590. p->ti = 0;
  591. p->t = 0;
  592. errLabel:
  593. return rc;
  594. }
  595. cmRC_t cmReflectCalcFinal( cmReflectCalc_t* p )
  596. {
  597. cmGoldSigFinal(p->gs);
  598. cmPhatFinal(p->phat);
  599. return cmOkRC;
  600. }
  601. /*
  602. cmRC_t cmReflectCalcExec( cmReflectCalc_t* p, const cmSample_t* xV, cmSample_t* yV, unsigned xyN )
  603. {
  604. unsigned i;
  605. // feed audio into the PHAT's buffer
  606. cmPhatExec(p->phat,xV,xyN);
  607. // fill the output buffer
  608. for(i=0; i<xyN; ++i,++p->xi)
  609. {
  610. if( p->xi < p->gs->sigN )
  611. yV[i] = p->gs->ch[0].mdV[p->xi];
  612. else
  613. yV[i] = 0;
  614. // if the PHAT has a complete buffer
  615. if( p->xi == p->phat->fhN )
  616. {
  617. p->xi = 0;
  618. // execute the correlation
  619. cmPhatChExec(p->phat,0,0,0);
  620. // p->phat->xV[fhN] now holds the correlation result
  621. if( p->phVa != NULL )
  622. cmVectArrayAppendS(p->phVa,p->phat->xV,p->phat->fhN );
  623. }
  624. }
  625. cmVectArrayAppendS(p->xVa,xV,xyN);
  626. cmVectArrayAppendS(p->yVa,yV,xyN);
  627. return cmOkRC;
  628. }
  629. */
  630. cmRC_t cmReflectCalcExec( cmReflectCalc_t* p, const cmSample_t* xV, cmSample_t* yV, unsigned xyN )
  631. {
  632. unsigned i;
  633. unsigned xyN0 = xyN;
  634. while(xyN0)
  635. {
  636. // feed audio into the PHAT's buffer
  637. unsigned di = p->phat->di;
  638. unsigned n = cmMin(xyN0,p->phat->fhN - di );
  639. cmPhatExec(p->phat,xV,n);
  640. if( di + n == p->phat->fhN )
  641. {
  642. // execute the correlation
  643. cmPhatChExec(p->phat,0,0,0);
  644. // p->phat->xV[fhN] now holds the correlation result
  645. // get the peak index
  646. p->t1V[p->ti] = cmVOS_MaxIndex(p->phat->xV,p->phat->fhN,1);
  647. printf("%i %i\n",p->t,p->t1V[p->ti]);
  648. p->ti = (p->ti + 1) % p->tN;
  649. // store the correlation result
  650. if( p->phVa != NULL )
  651. cmVectArrayAppendS(p->phVa,p->phat->xV,p->phat->fhN );
  652. }
  653. xyN0 -= n;
  654. }
  655. // fill the output buffer
  656. for(i=0; i<xyN; ++i)
  657. {
  658. if( p->xi == 0 )
  659. p->t0V[p->ti] = p->t + i;
  660. if( p->xi < p->gs->sigN )
  661. yV[i] = p->gs->ch[0].mdV[p->xi];
  662. else
  663. yV[i] = 0;
  664. p->xi = (p->xi+1) % p->phat->fhN;
  665. }
  666. p->t += xyN;
  667. if( p->xVa != NULL )
  668. cmVectArrayAppendS(p->xVa,xV,xyN);
  669. if( p->yVa != NULL )
  670. cmVectArrayAppendS(p->yVa,yV,xyN);
  671. return cmOkRC;
  672. }
  673. cmRC_t cmReflectCalcWrite( cmReflectCalc_t* p, const char* dirStr )
  674. {
  675. cmRC_t rc = cmOkRC;
  676. if( p->xVa != NULL)
  677. cmVectArrayWriteDirFn(p->xVa, dirStr, "reflect_calc_x.va" );
  678. if( p->yVa != NULL )
  679. cmVectArrayWriteDirFn(p->yVa, dirStr, "reflect_calc_y.va" );
  680. if( p->phVa != NULL )
  681. cmVectArrayWriteDirFn(p->phVa,dirStr, "reflect_calc_ph.va");
  682. return rc;
  683. }
  684. //=======================================================================================================================
  685. //
  686. //
  687. cmNlmsEc_t* cmNlmsEcAlloc( cmCtx* ctx, cmNlmsEc_t* ap, float mu, unsigned hN, unsigned delayN )
  688. {
  689. cmNlmsEc_t* p = cmObjAlloc(cmNlmsEc_t,ctx,ap);
  690. // allocate the vect array
  691. p->eVa = cmVectArrayAlloc(ctx, kFloatVaFl );
  692. if( mu != 0 )
  693. if( cmNlmsEcInit(p,mu,hN,delayN) != cmOkRC )
  694. cmNlmsEcFree(&p);
  695. return p;
  696. }
  697. cmRC_t cmNlmsEcFree( cmNlmsEc_t** pp )
  698. {
  699. cmRC_t rc = cmOkRC;
  700. if( pp == NULL || *pp == NULL )
  701. return rc;
  702. cmNlmsEc_t* p = *pp;
  703. if((rc = cmNlmsEcFinal(p)) != cmOkRC )
  704. return rc;
  705. cmMemFree(p->wV);
  706. cmMemFree(p->hV);
  707. cmVectArrayFree(&p->eVa);
  708. cmObjFree(pp);
  709. return rc;
  710. }
  711. cmRC_t cmNlmsEcInit( cmNlmsEc_t* p, float mu, unsigned hN, unsigned delayN )
  712. {
  713. cmRC_t rc = cmOkRC;
  714. if((rc = cmNlmsEcFinal(p)) != cmOkRC )
  715. return rc;
  716. p->mu = mu;
  717. p->hN = hN;
  718. p->delayN = delayN;
  719. p->wV = cmMemResizeZ(cmSample_t,p->wV,hN);
  720. p->hV = cmMemResizeZ(cmSample_t,p->hV,hN);
  721. p->dV = cmMemResizeZ(cmSample_t,p->dV,delayN);
  722. p->w0i = 0;
  723. return rc;
  724. }
  725. cmRC_t cmNlmsEcFinal( cmNlmsEc_t* p )
  726. { return cmOkRC; }
  727. /*
  728. for n=M:N
  729. uv = u(n:-1:n-M+1);
  730. e(n) = d(n)-w'*uv;
  731. w=w+mu/(a + uv'*uv ) * uv * conj(e(n));
  732. endfor
  733. e = e(:).^2;
  734. */
  735. cmRC_t cmNlmsEcExec( cmNlmsEc_t* p, const cmSample_t* xV, const cmSample_t* fV, cmSample_t* yV, unsigned xyN )
  736. {
  737. unsigned i;
  738. for(i=0; i<xyN; ++i)
  739. {
  740. cmSample_t y = 0;
  741. unsigned j;
  742. unsigned k = 0;
  743. float a = 0.00001;
  744. p->hV[p->w0i] = xV[i];
  745. for(j=p->w0i,k=0; j<p->hN; ++j,++k)
  746. y += p->hV[j] * p->wV[k];
  747. for(j=0; j<p->w0i; ++j,++k)
  748. y += p->hV[j] * p->wV[k];
  749. p->w0i = (p->w0i+1) % p->hN;
  750. float e = fV[i] - y;
  751. cmSample_t z = 0;
  752. for(j=0; j<p->hN; ++j)
  753. z += p->hV[j] * p->hV[j];
  754. for(j=0; j<p->hN; ++j)
  755. p->wV[j] += p->mu/(a + z) * p->hV[j] * e;
  756. }
  757. return cmOkRC;
  758. }
  759. cmRC_t cmNlmsEcWrite( cmNlmsEc_t* p, const cmChar_t* dirStr )
  760. {
  761. if( p->eVa != NULL )
  762. cmVectArrayWriteDirFn(p->eVa,dirStr, "nlms_err.va");
  763. return cmOkRC;
  764. }