libcm is a C development framework with an emphasis on audio signal processing applications.
Вы не можете выбрать более 25 тем Темы должны начинаться с буквы или цифры, могут содержать дефисы(-) и должны содержать не более 35 символов.

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. }