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

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100
  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, double srate, float mu, unsigned hN, unsigned delayN )
  688. {
  689. cmNlmsEc_t* p = cmObjAlloc(cmNlmsEc_t,ctx,ap);
  690. bool debugFl = false;
  691. // allocate the vect array's
  692. p->uVa = debugFl ? cmVectArrayAlloc(ctx, kFloatVaFl ) : NULL;
  693. p->fVa = debugFl ? cmVectArrayAlloc(ctx, kFloatVaFl ) : NULL;
  694. p->eVa = debugFl ? cmVectArrayAlloc(ctx, kFloatVaFl ) : NULL;
  695. if( srate != 0 )
  696. if( cmNlmsEcInit(p,srate,mu,hN,delayN) != cmOkRC )
  697. cmNlmsEcFree(&p);
  698. return p;
  699. }
  700. cmRC_t cmNlmsEcFree( cmNlmsEc_t** pp )
  701. {
  702. cmRC_t rc = cmOkRC;
  703. if( pp == NULL || *pp == NULL )
  704. return rc;
  705. cmNlmsEc_t* p = *pp;
  706. if((rc = cmNlmsEcFinal(p)) != cmOkRC )
  707. return rc;
  708. cmMemFree(p->wV);
  709. cmMemFree(p->hV);
  710. cmVectArrayFree(&p->eVa);
  711. cmObjFree(pp);
  712. return rc;
  713. }
  714. cmRC_t cmNlmsEcInit( cmNlmsEc_t* p, double srate, float mu, unsigned hN, unsigned delayN )
  715. {
  716. cmRC_t rc = cmOkRC;
  717. if((rc = cmNlmsEcFinal(p)) != cmOkRC )
  718. return rc;
  719. assert( srate >= hN );
  720. assert( srate >= delayN );
  721. p->mu = mu;
  722. p->hN = hN;
  723. p->delayN = cmMax(1,delayN);
  724. p->dN = srate;
  725. p->delayV = cmMemResizeZ(cmSample_t, p->delayV, srate );
  726. p->di = 0;
  727. p->wV = cmMemResizeZ(double,p->wV,srate);
  728. p->hV = cmMemResizeZ(double,p->hV,srate);
  729. p->w0i = 0;
  730. return rc;
  731. }
  732. cmRC_t cmNlmsEcFinal( cmNlmsEc_t* p )
  733. { return cmOkRC; }
  734. cmRC_t cmNlmsEcExec( cmNlmsEc_t* p, const cmSample_t* xV, const cmSample_t* fV, cmSample_t* yV, unsigned xyN )
  735. {
  736. // See: http://www.eit.lth.se/fileadmin/eit/courses/ett042/CE/CE2e.pdf
  737. // and http://www.eit.lth.se/fileadmin/eit/courses/ett042/CE/CE3e.pdf
  738. unsigned i;
  739. for(i=0; i<xyN; ++i)
  740. {
  741. double y = 0;
  742. unsigned k = 0;
  743. double a = 0.001;
  744. unsigned j;
  745. // Insert the next sample into the filter delay line.
  746. // Note that rather than shifting the delay line on each iteration we
  747. // increment the input location and then align it with the zeroth
  748. // weight below.
  749. p->hV[p->w0i] = p->delayV[ p->di ];
  750. p->delayV[ p->di ] = xV[i];
  751. p->di = (p->di + 1) % p->delayN;
  752. // calculate the output of the delay w0i:hN
  753. for(j=p->w0i,k=0; j<p->hN; ++j,++k)
  754. y += p->hV[j] * p->wV[k];
  755. // calcuate the output of the delay 0:w0i
  756. for(j=0; j<p->w0i; ++j,++k)
  757. y += p->hV[j] * p->wV[k];
  758. // calculate the error which is also the filter output
  759. double e = fV[i] - y;
  760. yV[i] = e;
  761. //
  762. double z = 0;
  763. for(j=0; j<p->hN; ++j)
  764. z += p->hV[j] * p->hV[j];
  765. // update weights 0 through w0i
  766. for(j=p->w0i,k=0; j<p->hN; ++j,++k)
  767. p->wV[k] += (p->mu/(a + z)) * p->hV[j] * e;
  768. // update weights w0i through hN
  769. for(j=0; j<p->w0i; ++j,++k)
  770. p->wV[k] += (p->mu/(a + z)) * p->hV[j] * e;
  771. // advance the delay
  772. p->w0i = (p->w0i+1) % p->hN;
  773. }
  774. if( p->uVa != NULL )
  775. cmVectArrayAppendS(p->uVa,xV,xyN);
  776. if( p->fVa != NULL )
  777. cmVectArrayAppendS(p->fVa,fV,xyN);
  778. if( p->eVa != NULL )
  779. cmVectArrayAppendS(p->eVa,yV,xyN);
  780. return cmOkRC;
  781. }
  782. cmRC_t cmNlmsEcWrite( cmNlmsEc_t* p, const cmChar_t* dirStr )
  783. {
  784. if( p->uVa != NULL )
  785. cmVectArrayWriteDirFn(p->uVa, dirStr, "nlms_unfiltered.va");
  786. if( p->fVa != NULL )
  787. cmVectArrayWriteDirFn(p->fVa, dirStr, "nlms_filtered.va");
  788. if( p->eVa != NULL )
  789. cmVectArrayWriteDirFn(p->eVa, dirStr, "nlms_out.va");
  790. return cmOkRC;
  791. }
  792. void cmNlmsEcSetMu( cmNlmsEc_t* p, float mu )
  793. {
  794. if( mu < 0 )
  795. p->mu = 0.0001;
  796. else
  797. if( mu >= 1 )
  798. p->mu = 0.99;
  799. else
  800. p->mu = mu;
  801. }
  802. void cmNlmsEcSetDelayN( cmNlmsEc_t* p, unsigned delayN )
  803. {
  804. if( delayN > p->dN)
  805. delayN = p->dN;
  806. else
  807. if( delayN < 1 )
  808. delayN = 1;
  809. cmVOS_Zero(p->delayV,p->delayN);
  810. p->delayN = delayN;
  811. }
  812. void cmNlmsEcSetIrN( cmNlmsEc_t* p, unsigned hN )
  813. {
  814. if( hN > p->dN )
  815. hN = p->dN;
  816. else
  817. if( hN < 1 )
  818. hN = 1;
  819. cmVOD_Zero(p->wV,p->hN);
  820. cmVOD_Zero(p->hV,p->hN);
  821. p->hN = hN;
  822. }