libcm is a C development framework with an emphasis on audio signal processing applications.
Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.

cmProc5.c 23KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861
  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* p, unsigned chN, unsigned hN, float alpha, unsigned mult, unsigned flags )
  267. {
  268. cmPhat_t* op = cmObjAlloc(cmPhat_t,ctx,p);
  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. p->fhN = cmNextPowerOfTwo(mult*hN);
  275. // allocate the FFT object
  276. cmFftAllocSR(ctx,&p->fft,NULL,p->fhN,kToPolarFftFl);
  277. if( chN != 0 )
  278. if( cmPhatInit(op,chN,hN,alpha,mult,flags) != cmOkRC )
  279. cmPhatFree(&op);
  280. return op;
  281. }
  282. cmRC_t cmPhatFree( cmPhat_t** pp )
  283. {
  284. cmRC_t rc = cmOkRC;
  285. if( pp == NULL || *pp == NULL )
  286. return rc;
  287. cmPhat_t* p = *pp;
  288. if((rc = cmPhatFinal(p)) != cmOkRC )
  289. return rc;
  290. cmMemFree(p->t0V);
  291. cmMemFree(p->t1V);
  292. cmMemFree(p->dV);
  293. cmMemFree(p->xV);
  294. cmMemFree(p->fhM);
  295. cmMemFree(p->mhM);
  296. cmMemFree(p->wndV);
  297. cmObjFreeStatic(cmFftFreeSR, cmFftSR, p->fft);
  298. cmVectArrayFree(&p->ftVa);
  299. cmObjFree(pp);
  300. return rc;
  301. }
  302. cmRC_t cmPhatInit( cmPhat_t* p, unsigned chN, unsigned hN, float alpha, unsigned mult, unsigned flags )
  303. {
  304. cmRC_t rc = cmOkRC;
  305. if((rc = cmPhatFinal(cmOkRC)) != cmOkRC )
  306. return rc;
  307. p->fhN = cmNextPowerOfTwo(mult*hN);
  308. if((cmFftInitSR(&p->fft, NULL, p->fhN, kToPolarFftFl)) != cmOkRC )
  309. return rc;
  310. p->alpha = alpha;
  311. p->flags = flags;
  312. // allocate the delay line
  313. p->dV = cmMemResizeZ(cmSample_t,p->dV,p->fhN);
  314. p->di = 0;
  315. // allocate the linear buffer
  316. p->xV = cmMemResizeZ(cmSample_t,p->xV,p->fhN);
  317. p->t0V = cmMemResizeZ(cmComplexR_t,p->t0V,p->fhN);
  318. p->t1V = cmMemResizeZ(cmComplexR_t,p->t1V,p->fhN);
  319. // allocate the window function
  320. p->wndV = cmMemResizeZ(cmSample_t,p->wndV,p->fhN);
  321. cmVOS_Hann(p->wndV,p->fhN);
  322. // allocate the signal id matrix
  323. p->chN = chN;
  324. p->hN = hN;
  325. p->binN = p->fft.binCnt; //atFftRealBinCount(p->fftH);
  326. p->fhM = cmMemResizeZ(cmComplexR_t, p->fhM, p->fhN * chN);
  327. p->mhM = cmMemResizeZ(float, p->mhM, p->binN * chN);
  328. cmPhatReset(p);
  329. //if( cmIsFlag(p->flags,kDebugAtPhatFl))
  330. // cmVectArrayAlloc(ctx, &p->ftVa, kSampleVaFl );
  331. //else
  332. // p->ftVa = NULL;
  333. return rc;
  334. }
  335. cmRC_t cmPhatFinal( cmPhat_t* p )
  336. { return cmOkRC; }
  337. cmRC_t cmPhatReset( cmPhat_t* p )
  338. {
  339. p->di = 0;
  340. p->absIdx = 0;
  341. cmVOS_Zero(p->dV,p->fhN);
  342. return cmOkRC;
  343. }
  344. cmRC_t cmPhatSetId( cmPhat_t* p, unsigned chIdx, const cmSample_t* hV, unsigned hN )
  345. {
  346. unsigned i;
  347. assert( chIdx < p->chN );
  348. assert( hN == p->hN );
  349. // Allocate a window vector
  350. cmSample_t* wndV = cmMemAllocZ(cmSample_t,hN);
  351. cmVOS_Hann(wndV,hN);
  352. // get ptr to output column in p->fhM[].
  353. cmComplexR_t* yV = p->fhM + (chIdx*p->fhN);
  354. // Zero pad hV[hN] to p->fhN;
  355. assert( hN <= p->fhN );
  356. cmVOS_Zero(p->xV,p->fhN);
  357. cmVOS_Copy(p->xV,hV,hN);
  358. // Apply the window function to the id signal
  359. if(atIsFlag(p->flags,kHannAtPhatFl) )
  360. cmVOS_MultVVV(p->xV,hV,wndV,hN);
  361. // take FFT of id signal. The result is in fft->complexV and fft->magV,phsV
  362. cmFftExecSR(p->fft, p->xV, p->fhN );
  363. // Store the magnitude of the id signal
  364. //atFftComplexAbs(p->mhM + (chIdx*p->binN), yV, p->binN);
  365. cmVOR_Copy(p->mhM + (chIdx*p->binN), p->fft->magV, p->binN );
  366. // Scale the magnitude
  367. cmVOS_MultVS( p->mhM + (chIdx*p->binN), p->binN, p->alpha);
  368. // store the complex conjugate of the FFT result in yV[]
  369. //atFftComplexConj(yV,p->binN);
  370. for(i=0; i<p->binN; ++i)
  371. yV[i].i = -(p->fft->complexV[i].i);
  372. cmMemFree(wndV);
  373. return kOkAtRC;
  374. }
  375. cmSample_t* _cmPhatReadVector( cmCtx* ctx, cmPhat_t* p, const char* fn, unsigned* vnRef )
  376. {
  377. cmVectArray_t* vap = NULL;
  378. cmSample_t* v = NULL;
  379. // instantiate a VectArray from a file
  380. if( cmVectArrayAllocFromFile(ctx, &vap, fn ) != kOkAtRC )
  381. {
  382. atErrMsg(&p->obj.err,kFileReadFailAtRC,"Id component vector file read failed '%s'.",fn);
  383. goto errLabel;
  384. }
  385. // get the count of elements in the vector
  386. *vnRef = cmVectArrayEleCount(vap);
  387. // allocate memory to hold the vector
  388. v = cmMemAlloc(&p->obj.err,cmSample_t,*vnRef);
  389. // copy the vector from the vector array object into v[]
  390. if( cmVectArrayGetF(vap,v,vnRef) != kOkAtRC )
  391. {
  392. cmMemFree(v);
  393. v = NULL;
  394. atErrMsg(&p->obj.err,kFileReadFailAtRC,"Id component vector copy out failed '%s'.",fn);
  395. goto errLabel;
  396. }
  397. cmRptPrintf(p->obj.err.rpt,"%i : %s",*vnRef,fn);
  398. errLabel:
  399. cmVectArrayFree(&vap);
  400. return v;
  401. }
  402. cmRC_t cmPhatExec( cmPhat_t* p, const cmSample_t* xV, unsigned xN )
  403. {
  404. unsigned n = atMin(xN,p->fhN-p->di);
  405. // update the delay line
  406. cmVOS_Copy(p->dV+p->di,xV,n);
  407. if( n < xN )
  408. cmVOS_Copy(p->dV,xV+n,xN-n);
  409. p->di = atModIncr(p->di,xN,p->fhN);
  410. // p->absIdx is the absolute sample index associated with di
  411. p->absIdx += xN;
  412. return kOkAtRC;
  413. }
  414. void cmPhatChExec(
  415. cmPhat_t* p,
  416. unsigned chIdx,
  417. unsigned sessionId,
  418. unsigned roleId)
  419. {
  420. unsigned n0 = p->fhN - p->di;
  421. unsigned n1 = p->fhN - n0;
  422. // Linearize the delay line into xV[]
  423. cmVOS_Copy(p->xV, p->dV + p->di, n0 );
  424. cmVOS_Copy(p->xV+n0, p->dV, n1 );
  425. if( atIsFlag(p->flags,kDebugAtPhatFl))
  426. cmVectArrayAppendS(p->ftVa, p->xV, p->fhN );
  427. // apply a window function to the incoming signal
  428. if( atIsFlag(p->flags,kHannAtPhatFl) )
  429. cmVOS_MultVV(p->xV,p->fhN,p->wndV);
  430. // Take the FFT of the delay line.
  431. // p->t0V[p->binN] = fft(p->xV)
  432. //atFftRealForward(p->fftH, p->xV, p->fhN, p->t0V, p->binN );
  433. cmFftExecSR(p->fft, p->xV, p->fhN );
  434. // Calc. the Cross Power Spectrum (aka cross spectral density) of the
  435. // input signal with the id signal.
  436. // Note that the CPS is equivalent to the Fourier Transform of the
  437. // cross-correlation of the two signals.
  438. // t0V[] *= p->fhM[:,chIdx]
  439. //atFftComplexMult( p->t0V, p->fhM + (chIdx * p->fhN), p->binN );
  440. cmVOCR_MultVVV( p->t0V, p->fft->complexV, p->fhM + (chIdx * p->fhN), p->binN)
  441. // Calculate the magnitude of the CPS.
  442. // xV[] = | t0V[] |
  443. cmVOCR_Abs( p->xV, p->t0V, p->binN );
  444. // Weight the CPS by the scaled magnitude of the id signal
  445. // (we want to emphasize the limited frequencies where the
  446. // id signal contains energy)
  447. // t0V[] *= p->mhM[:,chIdx]
  448. if( p->alpha > 0 )
  449. cmVOCR_MultR_VV( p->t0V, p->mhM + (chIdx*p->binN), p->binN);
  450. // Divide through by the magnitude of the CPS
  451. // This has the effect of whitening the spectram and thereby
  452. // minimizing the effect of the magnitude correlation
  453. // while maximimizing the effect of the phase correlation.
  454. //
  455. // t0V[] /= xV[]
  456. cmVOCR_DivR_VV( p->t0V, p->xV, p->binN );
  457. // Take the IFFT of the weighted CPS to recover the cross correlation.
  458. // xV[] = IFFT(t0V[])
  459. //// ***** atFftRealInverse( p->fftH, p->t0V, p->xV, p->fhN );
  460. // Shift the correlation spike to mark the end of the id
  461. cmVOS_Rotate( p->xV, p->fhN, -((int)p->hN) );
  462. // normalize by the length of the correlation
  463. cmVOS_DivVS(p->xV,p->fhN,p->fhN);
  464. if( atIsFlag(p->flags,kDebugAtPhatFl))
  465. {
  466. cmVectArrayAppendS(p->ftVa, p->xV, p->fhN );
  467. cmSample_t v[] = { sessionId, roleId };
  468. cmVectArrayAppendS(p->ftVa, v, sizeof(v)/sizeof(v[0]));
  469. }
  470. }
  471. cmRC_t cmPhatWrite( cmPhat_t* p, const char* dirStr )
  472. {
  473. cmRC_t rc = kOkAtRC;
  474. if( atIsFlag(p->flags, kDebugAtPhatFl))
  475. {
  476. char* path = NULL;
  477. if( p->ftVa != NULL )
  478. if((rc = cmVectArrayWrite(p->ftVa, path = atMakePath(&p->obj.err,path,"cmPhatFT","va",dirStr,NULL) )) != kOkAtRC )
  479. rc = atErrMsg(&p->obj.err,rc,"PHAT debug file write failed.");
  480. cmMemFree(path);
  481. }
  482. return rc;
  483. }
  484. cmRC_t cmPhatTest1( cmCtx* ctx, const char* dirStr )
  485. {
  486. cmRC_t rc = kOkAtRC;
  487. atSignalArg_t sa;
  488. atSignal_t* s = NULL;
  489. cmPhat_t* p = NULL;
  490. char* path = NULL;
  491. unsigned dspFrmCnt = 256;
  492. unsigned listenDelaySmp = 8196;
  493. double noiseGain = 0.05;
  494. unsigned chIdx = 0;
  495. cmSample_t* yV = NULL;
  496. unsigned yN = 0;
  497. double phatAlpha = 0.5;
  498. unsigned phatMult = 4.0;
  499. double nonLinExpo = 4.0;
  500. cmVectArray_t* outVA = NULL;
  501. cmVectArray_t* inVA = NULL;
  502. cmVectArray_t* statusVA = NULL;
  503. unsigned bsiN = 4;
  504. unsigned bsiV[bsiN]; // known signal onset in absolute samples
  505. unsigned esiV[bsiN]; // known signal offset
  506. unsigned lsiV[bsiN]; // end of listen time (when cmPhatChExec()) is run.
  507. unsigned dsiV[bsiN]; // detection time
  508. unsigned i,j;
  509. sa.chN = 1;
  510. sa.srate = 44100.0;
  511. sa.lfsrN = 8;
  512. sa.mlsCoeff0 = 0x8e;
  513. sa.mlsCoeff1 = 0x96;
  514. sa.samplesPerChip = 64;
  515. sa.rcosBeta = 0.5;
  516. sa.rcosOSFact = 4;
  517. sa.carrierHz = 17000.0;
  518. sa.envMs = 50.0;
  519. // allocate the the id signals
  520. if( atSignalAlloc( ctx, &s, &sa ) != kOkAtRC )
  521. return atErrMsg(&ctx->err, kTestFailAtRC, "Signal allocate failed.");
  522. // set the post signal listen delay to half the signal length
  523. listenDelaySmp = s->sigN/2;
  524. // allocate a PHAT detector
  525. if( cmPhatAlloc(ctx,&p,sa.chN,s->sigN, phatAlpha, phatMult, kDebugAtPhatFl ) != kOkAtRC )
  526. {
  527. rc = atErrMsg(&ctx->err, kTestFailAtRC, "PHAT allocate failed.");
  528. goto errLabel;
  529. }
  530. // register an id signal with the PHAT detector
  531. if( cmPhatSetId(p, chIdx, s->ch[chIdx].mdV, s->sigN ) != kOkAtRC )
  532. {
  533. rc = atErrMsg(&ctx->err, kTestFailAtRC, "PHAT setId failed.");
  534. goto errLabel;
  535. }
  536. // generate an input test signal containing bsiN id signals
  537. if( atSignalGen(s,chIdx,p->fhN,s->sigN,bsiV,bsiN,noiseGain,&yV,&yN) != kOkAtRC )
  538. {
  539. rc = atErrMsg(&ctx->err,kTestFailAtRC,"Signal generation failed.");
  540. goto errLabel;
  541. }
  542. // bsiV[] now holds signal onsets. Set esiV[] to signal offsets.
  543. atVOU_AddVVS(esiV,bsiV,bsiN,s->sigN );
  544. // set lsiV[] to end-of-listen location
  545. atVOU_AddVVS(lsiV,esiV,bsiN,listenDelaySmp);
  546. // zero the detection vector
  547. atVOU_Zero(dsiV,bsiN);
  548. // allocate a vector array to record the PHAT input signals
  549. if( cmVectArrayAlloc(ctx,&inVA,kSampleVaFl) != kOkAtRC )
  550. {
  551. rc = atErrMsg(&ctx->err, kTestFailAtRC, "vectArray inVA alloc failed.");
  552. goto errLabel;
  553. }
  554. // allocate a vector array to record the PHAT correlation output signals
  555. if( cmVectArrayAlloc(ctx,&outVA,kSampleVaFl) != kOkAtRC )
  556. {
  557. rc = atErrMsg(&ctx->err, kTestFailAtRC, "vectArray outVA alloc failed.");
  558. goto errLabel;
  559. }
  560. // allocate a vector array to record the PHAT status
  561. if( cmVectArrayAlloc(ctx,&statusVA,kSampleVaFl) != kOkAtRC )
  562. {
  563. rc = atErrMsg(&ctx->err, kTestFailAtRC, "vectArray statusVA alloc failed.");
  564. goto errLabel;
  565. }
  566. // for each 'dspFrmCnt' samples in the input signal
  567. for(i=0,j=0; j<bsiN && i<=yN-dspFrmCnt; i+=dspFrmCnt)
  568. {
  569. // store a copy of the input signal
  570. cmVectArrayAppendS(inVA,yV+i,dspFrmCnt);
  571. // feed the next dspFrmCnt samples to the PHAT detector
  572. cmPhatExec(p,yV+i,dspFrmCnt);
  573. // if the approximate end of an id signal is encountered
  574. if( lsiV[j] <= i && i < lsiV[j] + dspFrmCnt )
  575. {
  576. // execute the PHAT correlator
  577. cmPhatChExec( p, chIdx, -1, -1 );
  578. // apply non-linear exponent to the correlation vector
  579. cmVOS_PowV(p->xV,p->fhN,nonLinExpo);
  580. // locate the corr. peak inside the listening window
  581. // (the detection window is last 'detectWndSmp' samples in the corr. vector )
  582. unsigned detectWndSmp = 2*listenDelaySmp;
  583. dsiV[j] = cmVOS_ArgMax( p->xV + p->fhN - detectWndSmp, detectWndSmp);
  584. // convert the pk index to absolute time
  585. dsiV[j] = i + dspFrmCnt - detectWndSmp + dsiV[j];
  586. // sig beg sig end detect begin dtct end detect
  587. cmSample_t v[] = { bsiV[j], esiV[j], lsiV[j]-detectWndSmp, lsiV[j], dsiV[j] };
  588. // store the detection information
  589. cmVectArrayAppendS(statusVA,v,sizeof(v)/sizeof(v[0]));
  590. // store the correlation output vector
  591. cmVectArrayAppendS(outVA,p->xV,p->fhN);
  592. j += 1;
  593. }
  594. }
  595. // write inVA
  596. if( cmVectArrayWrite(inVA,path = atMakePath(&ctx->err,path,"phatIn","va",dirStr,NULL)) != kOkAtRC )
  597. {
  598. rc = atErrMsg(&ctx->err, kTestFailAtRC, "vectArray outVA write failed.");
  599. goto errLabel;
  600. }
  601. // write outVA
  602. if( cmVectArrayWrite(outVA,path = atMakePath(&ctx->err,path,"phatOut","va",dirStr,NULL)) != kOkAtRC )
  603. {
  604. rc = atErrMsg(&ctx->err, kTestFailAtRC, "vectArray outVA write failed.");
  605. goto errLabel;
  606. }
  607. // write statusVA
  608. if( cmVectArrayWrite(statusVA,path = atMakePath(&ctx->err,path,"phatStatus","va",dirStr,NULL)) != kOkAtRC )
  609. {
  610. rc = atErrMsg(&ctx->err, kTestFailAtRC, "vectArray statusVA write failed.");
  611. goto errLabel;
  612. }
  613. errLabel:
  614. cmVectArrayFree(&outVA);
  615. cmVectArrayFree(&inVA);
  616. if( cmPhatFree(&p) != kOkAtRC )
  617. atErrMsg(&ctx->err,kTestFailAtRC,"PHAT free failed.");
  618. if( atSignalFree(&s) != kOkAtRC )
  619. atErrMsg(&ctx->err,kTestFailAtRC,"Signal free failed.");
  620. return rc;
  621. }
  622. cmRC_t cmPhatTest2( cmCtx* ctx )
  623. {
  624. cmRC_t rc = kOkAtRC;
  625. cmPhat_t* p = NULL;
  626. unsigned hN = 16;
  627. float alpha = 1.0;
  628. unsigned mult = 4;
  629. cmSample_t hV[] = { 4,3,2,1, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
  630. cmSample_t x0V[] = { 4,3,2,1, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
  631. cmSample_t x1V[] = { 0,0,0,0, 4,3,2,1, 0,0,0,0, 0,0,0,0 };
  632. cmSample_t x2V[] = { 0,0,0,0, 0,0,0,0, 4,3,2,1, 0,0,0,0 };
  633. cmSample_t x3V[] = { 0,0,0,0, 0,0,0,0, 0,0,0,0, 4,3,2,1 };
  634. cmSample_t* xV[] = { x0V, x1V, x2V, x3V };
  635. unsigned chN = sizeof(xV)/sizeof(xV[0]);
  636. unsigned i;
  637. if(cmPhatAlloc(ctx,&p,chN,hN,alpha,mult,kNoFlagsAtPhatFl) != kOkAtRC )
  638. {
  639. rc = atErrMsg(&ctx->err,kTestFailAtRC,"cmPhatAlloc() failed.");
  640. goto errLabel;
  641. }
  642. for(i=0; i<chN; ++i)
  643. if( cmPhatSetId(p,i,hV,hN) != kOkAtRC )
  644. rc = atErrMsg(&ctx->err,kTestFailAtRC,"cmPhatSetId() failed.");
  645. for(i=0; i<chN; ++i)
  646. {
  647. cmPhatReset(p);
  648. if( cmPhatExec(p,xV[i],hN) != kOkAtRC )
  649. {
  650. rc = atErrMsg(&ctx->err,kTestFailAtRC,"cmPhatExec() failed.");
  651. goto errLabel;
  652. }
  653. cmPhatChExec(p, i, -1, -1);
  654. cmVOS_PrintL(&ctx->printRpt,"x:",p->xV,1,p->fhN);
  655. }
  656. errLabel:
  657. cmPhatFree(&p);
  658. return rc;
  659. }