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 23KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869
  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. if( a != NULL )
  169. if( cmGoldSigInit(op,a) != cmOkRC )
  170. cmGoldSigFree(&op);
  171. return op;
  172. }
  173. cmRC_t cmGoldSigFree( cmGoldSig_t** pp )
  174. {
  175. cmRC_t rc = cmOkRC;
  176. if( pp == NULL || *pp == NULL )
  177. return rc;
  178. cmGoldSig_t* p = *pp;
  179. if((rc = cmGoldSigFinal(p)) != cmOkRC )
  180. return rc;
  181. unsigned i;
  182. for(i=0; i<p->a.chN; ++i)
  183. {
  184. cmMemFree(p->ch[i].bbV);
  185. cmMemFree(p->ch[i].mdV);
  186. }
  187. cmMemFree(p->ch);
  188. cmMemFree(p->rcosV);
  189. cmMemFree(p->pnM);
  190. cmMemFree(p);
  191. *pp = NULL;
  192. return rc;
  193. }
  194. cmRC_t cmGoldSigInit( cmGoldSig_t* p, const cmGoldSigArg_t* a )
  195. {
  196. cmRC_t rc = cmOkRC;
  197. unsigned i;
  198. p->a = *a; // store arg recd
  199. p->ch = cmMemResizeZ(cmGoldSigCh_t,p->ch,a->chN); // alloc channel array
  200. p->mlsN = (1 << a->lfsrN) - 1; // calc spreading code length
  201. p->rcosN = a->samplesPerChip * a->rcosOSFact; // calc rcos imp. resp. length
  202. p->rcosN += (p->rcosN % 2)==0; // force rcos imp. length odd
  203. p->rcosV = cmMemResizeZ(cmSample_t,p->rcosV,p->rcosN); // alloc rcos imp. resp. vector
  204. p->pnM = cmMemResizeZ(int,p->pnM,p->mlsN*a->chN); // alloc spreading-code mtx
  205. p->sigN = p->mlsN * a->samplesPerChip; // calc audio signal length
  206. // generate spreading codes
  207. if( cmGenGoldCodes(a->lfsrN, a->mlsCoeff0, a->mlsCoeff1, a->chN, p->pnM, p->mlsN ) == false )
  208. {
  209. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Unable to generate sufficient balanced Gold codes.");
  210. goto errLabel;
  211. }
  212. // generate the rcos impulse response
  213. _cmGoldSigRaisedCos(p->rcosV,p->rcosN,a->samplesPerChip,a->rcosBeta);
  214. // for each channel
  215. for(i=0; i<a->chN; ++i)
  216. {
  217. // Note: if (i*p->mlsN) is set to 0 in the following line then all channels
  218. // will use the same spreading code.
  219. p->ch[i].pnV = p->pnM + (i*p->mlsN); // get ch. spreading code
  220. p->ch[i].bbV = cmMemResizeZ(cmSample_t,p->ch[i].bbV,p->sigN); // alloc baseband signal vector
  221. p->ch[i].mdV = cmMemResizeZ(cmSample_t,p->ch[i].mdV,p->sigN); // alloc output audio vector
  222. // Convolve spreading code with rcos impulse reponse to form baseband signal.
  223. _cmGoldSigConv(p, i );
  224. // Modulate baseband signal to carrier frq. and apply attack/decay envelope.
  225. _cmGoldSigModulate(p, i );
  226. }
  227. errLabel:
  228. if((rc = cmErrLastRC(&p->obj.err)) != cmOkRC )
  229. cmGoldSigFree(&p);
  230. return rc;
  231. }
  232. cmRC_t cmGoldSigFinal( cmGoldSig_t* p )
  233. { return cmOkRC; }
  234. cmRC_t cmGoldSigWrite( cmCtx* ctx, cmGoldSig_t* p, const char* fn )
  235. {
  236. cmVectArray_t* vap = NULL;
  237. unsigned i;
  238. vap = cmVectArrayAlloc(ctx,kSampleVaFl);
  239. for(i=0; i<p->a.chN; ++i)
  240. {
  241. cmVectArrayAppendS(vap,p->ch[i].bbV,p->sigN);
  242. cmVectArrayAppendS(vap,p->ch[i].mdV,p->sigN);
  243. }
  244. cmVectArrayWrite(vap,fn);
  245. cmVectArrayFree(&vap);
  246. return cmOkRC;
  247. }
  248. cmRC_t cmGoldSigGen( cmGoldSig_t* p, unsigned chIdx, unsigned prefixN, unsigned dsN, unsigned *bsiV, unsigned bsiN, double noiseGain, cmSample_t** yVRef, unsigned* yNRef )
  249. {
  250. unsigned yN = prefixN + bsiN * (p->sigN + dsN);
  251. cmSample_t* yV = cmMemAllocZ(cmSample_t,yN);
  252. unsigned i;
  253. cmVOS_Random(yV, yN, -noiseGain, noiseGain );
  254. for(i=0; i<bsiN; ++i)
  255. {
  256. bsiV[i] = prefixN + i*(p->sigN + dsN);
  257. cmVOS_AddVV(yV + bsiV[i], p->sigN, p->ch[chIdx].mdV );
  258. }
  259. if( yVRef != NULL )
  260. *yVRef = yV;
  261. if( yNRef != NULL )
  262. *yNRef = yN;
  263. return cmOkRC;
  264. }
  265. //=======================================================================================================================
  266. cmPhat_t* cmPhatAlloc( cmCtx* ctx, cmPhat_t* ap, unsigned chN, unsigned hN, float alpha, unsigned mult, unsigned flags )
  267. {
  268. cmPhat_t* p = cmObjAlloc(cmPhat_t,ctx,ap);
  269. // The FFT buffer and the delay line is at least twice the size of the
  270. // id signal. This will guarantee that at least one complete id signal
  271. // is inside the buffer. In practice it means that it is possible
  272. // that there will be two id's in the buffer therefore if there are
  273. // two correlation spikes it is important that we take the second.
  274. unsigned fhN = cmNextPowerOfTwo(mult*hN);
  275. // allocate the FFT object
  276. cmFftAllocSR(ctx,&p->fft,NULL,fhN,kToPolarFftFl);
  277. cmIFftAllocRS(ctx,&p->ifft,fhN/2 + 1 );
  278. if( chN != 0 )
  279. if( cmPhatInit(p,chN,hN,alpha,mult,flags) != cmOkRC )
  280. cmPhatFree(&p);
  281. return p;
  282. }
  283. cmRC_t cmPhatFree( cmPhat_t** pp )
  284. {
  285. cmRC_t rc = cmOkRC;
  286. if( pp == NULL || *pp == NULL )
  287. return rc;
  288. cmPhat_t* p = *pp;
  289. if((rc = cmPhatFinal(p)) != cmOkRC )
  290. return rc;
  291. cmMemFree(p->t0V);
  292. cmMemFree(p->t1V);
  293. cmMemFree(p->dV);
  294. cmMemFree(p->xV);
  295. cmMemFree(p->fhM);
  296. cmMemFree(p->mhM);
  297. cmMemFree(p->wndV);
  298. cmObjFreeStatic(cmFftFreeSR, cmFftSR, p->fft);
  299. cmObjFreeStatic(cmIFftFreeRS, cmIFftRS, p->ifft);
  300. cmVectArrayFree(&p->ftVa);
  301. cmObjFree(pp);
  302. return rc;
  303. }
  304. cmRC_t cmPhatInit( cmPhat_t* p, unsigned chN, unsigned hN, float alpha, unsigned mult, unsigned flags )
  305. {
  306. cmRC_t rc = cmOkRC;
  307. if((rc = cmPhatFinal(cmOkRC)) != cmOkRC )
  308. return rc;
  309. p->fhN = cmNextPowerOfTwo(mult*hN);
  310. if((cmFftInitSR(&p->fft, NULL, p->fhN, kToPolarFftFl)) != cmOkRC )
  311. return rc;
  312. if((cmIFftInitRS(&p->ifft, p->fft.binCnt )) != cmOkRC )
  313. return rc;
  314. p->alpha = alpha;
  315. p->flags = flags;
  316. // allocate the delay line
  317. p->dV = cmMemResizeZ(cmSample_t,p->dV,p->fhN);
  318. p->di = 0;
  319. // allocate the linear buffer
  320. p->xV = cmMemResizeZ(cmSample_t,p->xV,p->fhN);
  321. p->t0V = cmMemResizeZ(cmComplexR_t,p->t0V,p->fhN);
  322. p->t1V = cmMemResizeZ(cmComplexR_t,p->t1V,p->fhN);
  323. // allocate the window function
  324. p->wndV = cmMemResizeZ(cmSample_t,p->wndV,p->fhN);
  325. cmVOS_Hann(p->wndV,p->fhN);
  326. // allocate the signal id matrix
  327. p->chN = chN;
  328. p->hN = hN;
  329. p->binN = p->fft.binCnt; //atFftRealBinCount(p->fftH);
  330. p->fhM = cmMemResizeZ(cmComplexR_t, p->fhM, p->fhN * chN);
  331. p->mhM = cmMemResizeZ(float, p->mhM, p->binN * chN);
  332. cmPhatReset(p);
  333. //if( cmIsFlag(p->flags,kDebugAtPhatFl))
  334. // cmVectArrayAlloc(ctx, &p->ftVa, kSampleVaFl );
  335. //else
  336. // p->ftVa = NULL;
  337. return rc;
  338. }
  339. cmRC_t cmPhatFinal( cmPhat_t* p )
  340. { return cmOkRC; }
  341. cmRC_t cmPhatReset( cmPhat_t* p )
  342. {
  343. p->di = 0;
  344. p->absIdx = 0;
  345. cmVOS_Zero(p->dV,p->fhN);
  346. return cmOkRC;
  347. }
  348. cmRC_t cmPhatSetId( cmPhat_t* p, unsigned chIdx, const cmSample_t* hV, unsigned hN )
  349. {
  350. unsigned i;
  351. assert( chIdx < p->chN );
  352. assert( hN == p->hN );
  353. // Allocate a window vector
  354. cmSample_t* wndV = cmMemAllocZ(cmSample_t,hN);
  355. cmVOS_Hann(wndV,hN);
  356. // get ptr to output column in p->fhM[].
  357. cmComplexR_t* yV = p->fhM + (chIdx*p->fhN);
  358. // Zero pad hV[hN] to p->fhN;
  359. assert( hN <= p->fhN );
  360. cmVOS_Zero(p->xV,p->fhN);
  361. cmVOS_Copy(p->xV,hN,hV);
  362. // Apply the window function to the id signal
  363. if(cmIsFlag(p->flags,kHannAtPhatFl) )
  364. cmVOS_MultVVV(p->xV,hN,hV,wndV);
  365. // take FFT of id signal. The result is in fft->complexV and fft->magV,phsV
  366. cmFftExecSR(&p->fft, p->xV, p->fhN );
  367. // Store the magnitude of the id signal
  368. //atFftComplexAbs(p->mhM + (chIdx*p->binN), yV, p->binN);
  369. cmVOF_CopyR(p->mhM + (chIdx*p->binN), p->binN, p->fft.magV );
  370. // Scale the magnitude
  371. cmVOS_MultVS( p->mhM + (chIdx*p->binN), p->binN, p->alpha);
  372. // store the complex conjugate of the FFT result in yV[]
  373. //atFftComplexConj(yV,p->binN);
  374. for(i=0; i<p->binN; ++i)
  375. yV[i] = cmCconjR(p->fft.complexV[i]);
  376. cmMemFree(wndV);
  377. return cmOkRC;
  378. }
  379. cmSample_t* _cmPhatReadVector( cmCtx* ctx, cmPhat_t* p, const char* fn, unsigned* vnRef )
  380. {
  381. cmVectArray_t* vap = NULL;
  382. cmSample_t* v = NULL;
  383. cmRC_t rc = cmOkRC;
  384. // instantiate a VectArray from a file
  385. if( (vap = cmVectArrayAllocFromFile(ctx, fn )) == NULL )
  386. {
  387. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Id component vector file read failed '%s'.",fn);
  388. goto errLabel;
  389. }
  390. // get the count of elements in the vector
  391. *vnRef = cmVectArrayEleCount(vap);
  392. // allocate memory to hold the vector
  393. v = cmMemAlloc(cmSample_t,*vnRef);
  394. // copy the vector from the vector array object into v[]
  395. if((rc = cmVectArrayGetF(vap,v,vnRef)) != cmOkRC )
  396. {
  397. cmMemFree(v);
  398. v = NULL;
  399. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Id component vector copy out failed '%s'.",fn);
  400. goto errLabel;
  401. }
  402. cmRptPrintf(p->obj.err.rpt,"%i : %s",*vnRef,fn);
  403. errLabel:
  404. cmVectArrayFree(&vap);
  405. return v;
  406. }
  407. cmRC_t cmPhatExec( cmPhat_t* p, const cmSample_t* xV, unsigned xN )
  408. {
  409. unsigned n = cmMin(xN,p->fhN-p->di);
  410. // update the delay line
  411. cmVOS_Copy(p->dV+p->di,n,xV);
  412. if( n < xN )
  413. cmVOS_Copy(p->dV,xN-n,xV+n);
  414. p->di = cmModIncr(p->di,xN,p->fhN);
  415. // p->absIdx is the absolute sample index associated with di
  416. p->absIdx += xN;
  417. return cmOkRC;
  418. }
  419. void cmPhatChExec(
  420. cmPhat_t* p,
  421. unsigned chIdx,
  422. unsigned sessionId,
  423. unsigned roleId)
  424. {
  425. unsigned n0 = p->fhN - p->di;
  426. unsigned n1 = p->fhN - n0;
  427. // Linearize the delay line into xV[]
  428. cmVOS_Copy(p->xV, n0, p->dV + p->di );
  429. cmVOS_Copy(p->xV+n0, n1, p->dV );
  430. if( cmIsFlag(p->flags,kDebugAtPhatFl))
  431. cmVectArrayAppendS(p->ftVa, p->xV, p->fhN );
  432. // apply a window function to the incoming signal
  433. if( cmIsFlag(p->flags,kHannAtPhatFl) )
  434. cmVOS_MultVV(p->xV,p->fhN,p->wndV);
  435. // Take the FFT of the delay line.
  436. // p->t0V[p->binN] = fft(p->xV)
  437. //atFftRealForward(p->fftH, p->xV, p->fhN, p->t0V, p->binN );
  438. cmFftExecSR(&p->fft, p->xV, p->fhN );
  439. // Calc. the Cross Power Spectrum (aka cross spectral density) of the
  440. // input signal with the id signal.
  441. // Note that the CPS is equivalent to the Fourier Transform of the
  442. // cross-correlation of the two signals.
  443. // t0V[] *= p->fhM[:,chIdx]
  444. //atFftComplexMult( p->t0V, p->fhM + (chIdx * p->fhN), p->binN );
  445. cmVOCR_MultVVV( p->t0V, p->fft.complexV, p->fhM + (chIdx * p->fhN), p->binN);
  446. // Calculate the magnitude of the CPS.
  447. // xV[] = | t0V[] |
  448. cmVOCR_Abs( p->xV, p->t0V, p->binN );
  449. // Weight the CPS by the scaled magnitude of the id signal
  450. // (we want to emphasize the limited frequencies where the
  451. // id signal contains energy)
  452. // t0V[] *= p->mhM[:,chIdx]
  453. if( p->alpha > 0 )
  454. cmVOCR_MultVFV( p->t0V, p->mhM + (chIdx*p->binN), p->binN);
  455. // Divide through by the magnitude of the CPS
  456. // This has the effect of whitening the spectram and thereby
  457. // minimizing the effect of the magnitude correlation
  458. // while maximimizing the effect of the phase correlation.
  459. //
  460. // t0V[] /= xV[]
  461. cmVOCR_DivVFV( p->t0V, p->xV, p->binN );
  462. // Take the IFFT of the weighted CPS to recover the cross correlation.
  463. // xV[] = IFFT(t0V[])
  464. cmIFftExecRS( &p->ifft, p->t0V );
  465. // Normalize the result by the length of the transform.
  466. cmVOS_DivVVS( p->xV, p->fhN, p->ifft.outV, p->fhN );
  467. // Shift the correlation spike to mark the end of the id
  468. cmVOS_Rotate( p->xV, p->fhN, -((int)p->hN) );
  469. // normalize by the length of the correlation
  470. cmVOS_DivVS(p->xV,p->fhN,p->fhN);
  471. if( cmIsFlag(p->flags,kDebugAtPhatFl))
  472. {
  473. cmVectArrayAppendS(p->ftVa, p->xV, p->fhN );
  474. cmSample_t v[] = { sessionId, roleId };
  475. cmVectArrayAppendS(p->ftVa, v, sizeof(v)/sizeof(v[0]));
  476. }
  477. }
  478. cmRC_t cmPhatWrite( cmPhat_t* p, const char* dirStr )
  479. {
  480. cmRC_t rc = cmOkRC;
  481. if( cmIsFlag(p->flags, kDebugAtPhatFl))
  482. {
  483. const char* path = NULL;
  484. if( p->ftVa != NULL )
  485. if((rc = cmVectArrayWrite(p->ftVa, path = cmFsMakeFn(path,"cmPhatFT","va",dirStr,NULL) )) != cmOkRC )
  486. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"PHAT debug file write failed.");
  487. cmFsFreeFn(path);
  488. }
  489. return rc;
  490. }
  491. #ifdef NOTDEF
  492. cmRC_t cmPhatTest1( cmCtx* ctx, const char* dirStr )
  493. {
  494. cmRC_t rc = cmOkRC;
  495. cmGoldSigArg_t sa;
  496. cmGoldSig_t* s = NULL;
  497. cmPhat_t* p = NULL;
  498. char* path = NULL;
  499. unsigned dspFrmCnt = 256;
  500. unsigned listenDelaySmp = 8196;
  501. double noiseGain = 0.05;
  502. unsigned chIdx = 0;
  503. cmSample_t* yV = NULL;
  504. unsigned yN = 0;
  505. double phatAlpha = 0.5;
  506. unsigned phatMult = 4.0;
  507. double nonLinExpo = 4.0;
  508. cmVectArray_t* outVA = NULL;
  509. cmVectArray_t* inVA = NULL;
  510. cmVectArray_t* statusVA = NULL;
  511. unsigned bsiN = 4;
  512. unsigned bsiV[bsiN]; // known signal onset in absolute samples
  513. unsigned esiV[bsiN]; // known signal offset
  514. unsigned lsiV[bsiN]; // end of listen time (when cmPhatChExec()) is run.
  515. unsigned dsiV[bsiN]; // detection time
  516. unsigned i,j;
  517. sa.chN = 1;
  518. sa.srate = 44100.0;
  519. sa.lfsrN = 8;
  520. sa.mlsCoeff0 = 0x8e;
  521. sa.mlsCoeff1 = 0x96;
  522. sa.samplesPerChip = 64;
  523. sa.rcosBeta = 0.5;
  524. sa.rcosOSFact = 4;
  525. sa.carrierHz = 17000.0;
  526. sa.envMs = 50.0;
  527. // allocate the the id signals
  528. if( (s = cmGoldSigAlloc( ctx, NULL, &sa ) == NULL )
  529. return cmErrMsg(&ctx->err, cmSubSysFailRC, "Signal allocate failed.");
  530. // set the post signal listen delay to half the signal length
  531. listenDelaySmp = s->sigN/2;
  532. // allocate a PHAT detector
  533. if( (p = cmPhatAlloc(ctx,NULL,sa.chN,s->sigN, phatAlpha, phatMult, kDebugAtPhatFl ) == NULL )
  534. {
  535. rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "PHAT allocate failed.");
  536. goto errLabel;
  537. }
  538. // register an id signal with the PHAT detector
  539. if( cmPhatSetId(p, chIdx, s->ch[chIdx].mdV, s->sigN ) != cmOkRC )
  540. {
  541. rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "PHAT setId failed.");
  542. goto errLabel;
  543. }
  544. // generate an input test signal containing bsiN id signals
  545. if( atSignalGen(s,chIdx,p->fhN,s->sigN,bsiV,bsiN,noiseGain,&yV,&yN) != cmOkRC )
  546. {
  547. rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"Signal generation failed.");
  548. goto errLabel;
  549. }
  550. // bsiV[] now holds signal onsets. Set esiV[] to signal offsets.
  551. atVOU_AddVVS(esiV,bsiV,bsiN,s->sigN );
  552. // set lsiV[] to end-of-listen location
  553. atVOU_AddVVS(lsiV,esiV,bsiN,listenDelaySmp);
  554. // zero the detection vector
  555. atVOU_Zero(dsiV,bsiN);
  556. // allocate a vector array to record the PHAT input signals
  557. if( cmVectArrayAlloc(ctx,&inVA,kSampleVaFl) != cmOkRC )
  558. {
  559. rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray inVA alloc failed.");
  560. goto errLabel;
  561. }
  562. // allocate a vector array to record the PHAT correlation output signals
  563. if( cmVectArrayAlloc(ctx,&outVA,kSampleVaFl) != cmOkRC )
  564. {
  565. rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray outVA alloc failed.");
  566. goto errLabel;
  567. }
  568. // allocate a vector array to record the PHAT status
  569. if( cmVectArrayAlloc(ctx,&statusVA,kSampleVaFl) != cmOkRC )
  570. {
  571. rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray statusVA alloc failed.");
  572. goto errLabel;
  573. }
  574. // for each 'dspFrmCnt' samples in the input signal
  575. for(i=0,j=0; j<bsiN && i<=yN-dspFrmCnt; i+=dspFrmCnt)
  576. {
  577. // store a copy of the input signal
  578. cmVectArrayAppendS(inVA,yV+i,dspFrmCnt);
  579. // feed the next dspFrmCnt samples to the PHAT detector
  580. cmPhatExec(p,yV+i,dspFrmCnt);
  581. // if the approximate end of an id signal is encountered
  582. if( lsiV[j] <= i && i < lsiV[j] + dspFrmCnt )
  583. {
  584. // execute the PHAT correlator
  585. cmPhatChExec( p, chIdx, -1, -1 );
  586. // apply non-linear exponent to the correlation vector
  587. cmVOS_PowV(p->xV,p->fhN,nonLinExpo);
  588. // locate the corr. peak inside the listening window
  589. // (the detection window is last 'detectWndSmp' samples in the corr. vector )
  590. unsigned detectWndSmp = 2*listenDelaySmp;
  591. dsiV[j] = cmVOS_ArgMax( p->xV + p->fhN - detectWndSmp, detectWndSmp);
  592. // convert the pk index to absolute time
  593. dsiV[j] = i + dspFrmCnt - detectWndSmp + dsiV[j];
  594. // sig beg sig end detect begin dtct end detect
  595. cmSample_t v[] = { bsiV[j], esiV[j], lsiV[j]-detectWndSmp, lsiV[j], dsiV[j] };
  596. // store the detection information
  597. cmVectArrayAppendS(statusVA,v,sizeof(v)/sizeof(v[0]));
  598. // store the correlation output vector
  599. cmVectArrayAppendS(outVA,p->xV,p->fhN);
  600. j += 1;
  601. }
  602. }
  603. // write inVA
  604. if( cmVectArrayWrite(inVA,path = atMakePath(&ctx->err,path,"phatIn","va",dirStr,NULL)) != cmOkRC )
  605. {
  606. rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray outVA write failed.");
  607. goto errLabel;
  608. }
  609. // write outVA
  610. if( cmVectArrayWrite(outVA,path = atMakePath(&ctx->err,path,"phatOut","va",dirStr,NULL)) != cmOkRC )
  611. {
  612. rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray outVA write failed.");
  613. goto errLabel;
  614. }
  615. // write statusVA
  616. if( cmVectArrayWrite(statusVA,path = atMakePath(&ctx->err,path,"phatStatus","va",dirStr,NULL)) != cmOkRC )
  617. {
  618. rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray statusVA write failed.");
  619. goto errLabel;
  620. }
  621. errLabel:
  622. cmVectArrayFree(&outVA);
  623. cmVectArrayFree(&inVA);
  624. if( cmPhatFree(&p) != cmOkRC )
  625. cmErrMsg(&ctx->err,cmSubSysFailRC,"PHAT free failed.");
  626. if( atSignalFree(&s) != cmOkRC )
  627. cmErrMsg(&ctx->err,cmSubSysFailRC,"Signal free failed.");
  628. return rc;
  629. }
  630. cmRC_t cmPhatTest2( cmCtx* ctx )
  631. {
  632. cmRC_t rc = cmOkRC;
  633. cmPhat_t* p = NULL;
  634. unsigned hN = 16;
  635. float alpha = 1.0;
  636. unsigned mult = 4;
  637. cmSample_t hV[] = { 4,3,2,1, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
  638. cmSample_t x0V[] = { 4,3,2,1, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
  639. cmSample_t x1V[] = { 0,0,0,0, 4,3,2,1, 0,0,0,0, 0,0,0,0 };
  640. cmSample_t x2V[] = { 0,0,0,0, 0,0,0,0, 4,3,2,1, 0,0,0,0 };
  641. cmSample_t x3V[] = { 0,0,0,0, 0,0,0,0, 0,0,0,0, 4,3,2,1 };
  642. cmSample_t* xV[] = { x0V, x1V, x2V, x3V };
  643. unsigned chN = sizeof(xV)/sizeof(xV[0]);
  644. unsigned i;
  645. if(cmPhatAlloc(ctx,&p,chN,hN,alpha,mult,kNoFlagsAtPhatFl) != cmOkRC )
  646. {
  647. rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"cmPhatAlloc() failed.");
  648. goto errLabel;
  649. }
  650. for(i=0; i<chN; ++i)
  651. if( cmPhatSetId(p,i,hV,hN) != cmOkRC )
  652. rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"cmPhatSetId() failed.");
  653. for(i=0; i<chN; ++i)
  654. {
  655. cmPhatReset(p);
  656. if( cmPhatExec(p,xV[i],hN) != cmOkRC )
  657. {
  658. rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"cmPhatExec() failed.");
  659. goto errLabel;
  660. }
  661. cmPhatChExec(p, i, -1, -1);
  662. cmVOS_PrintL(&ctx->printRpt,"x:",p->xV,1,p->fhN);
  663. }
  664. errLabel:
  665. cmPhatFree(&p);
  666. return rc;
  667. }
  668. #endif