Browse Source

cmProc5.h/c : Added initial implementation of cmReflectCalc.

master
kevin 9 years ago
parent
commit
28d2de022a
2 changed files with 115 additions and 181 deletions
  1. 91
    177
      cmProc5.c
  2. 24
    4
      cmProc5.h

+ 91
- 177
cmProc5.c View File

@@ -645,225 +645,139 @@ cmRC_t cmPhatWrite( cmPhat_t* p, const char* dirStr )
645 645
   return rc;
646 646
 }
647 647
 
648
-#ifdef NOTDEF
649
-cmRC_t cmPhatTest1( cmCtx* ctx, const char* dirStr )
650
-{
651
-  cmRC_t         rc             = cmOkRC;
652
-  cmGoldSigArg_t sa;
653
-  cmGoldSig_t*   s              = NULL;
654
-  cmPhat_t*      p              = NULL;
655
-  char*          path           = NULL;
656
-  unsigned       dspFrmCnt      = 256;
657
-  unsigned       listenDelaySmp = 8196;
658
-  double         noiseGain      = 0.05;
659
-  unsigned       chIdx          = 0;
660
-  cmSample_t*    yV             = NULL;
661
-  unsigned       yN             = 0;
662
-  double         phatAlpha      = 0.5;
663
-  unsigned       phatMult       = 4.0;
664
-  double         nonLinExpo     = 4.0;
665
-  cmVectArray_t* outVA          = NULL; 
666
-  cmVectArray_t* inVA           = NULL;
667
-  cmVectArray_t* statusVA       = NULL;
668
-  unsigned       bsiN           = 4;
669
-  unsigned       bsiV[bsiN];  // known signal onset in absolute samples 
670
-  unsigned       esiV[bsiN];  // known signal offset
671
-  unsigned       lsiV[bsiN];  // end of listen time (when cmPhatChExec()) is run.
672
-  unsigned       dsiV[bsiN];  // detection time
673
-  unsigned       i,j;
674
-  
675
-  sa.chN             =     1;
676
-  sa.srate           = 44100.0;
677
-  sa.lfsrN           =     8;
678
-  sa.mlsCoeff0       =  0x8e;
679
-  sa.mlsCoeff1       =  0x96;
680
-  sa.samplesPerChip  =    64;
681
-  sa.rcosBeta        =     0.5;
682
-  sa.rcosOSFact      =     4;
683
-  sa.carrierHz       = 17000.0;
684
-  sa.envMs           =    50.0;
685
-  
686
-  // allocate the the id signals
687
-  if( (s = cmGoldSigAlloc( ctx, NULL, &sa ) == NULL )
688
-    return cmErrMsg(&ctx->err, cmSubSysFailRC, "Signal allocate failed.");
689 648
 
690
-  // set the post signal listen delay to half the signal length
691
-  listenDelaySmp = s->sigN/2; 
692
-
693
-  // allocate a PHAT detector
694
-    if( (p = cmPhatAlloc(ctx,NULL,sa.chN,s->sigN, phatAlpha, phatMult, kDebugAtPhatFl ) == NULL )
695
-  {
696
-    rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "PHAT allocate failed.");
697
-    goto errLabel;
698
-  }
699
-
700
-  // register an id signal with the PHAT detector
701
-  if( cmPhatSetId(p, chIdx, s->ch[chIdx].mdV, s->sigN ) != cmOkRC )
702
-  {
703
-    rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "PHAT setId failed.");
704
-    goto errLabel;
705
-  }
649
+//=======================================================================================================================
650
+// 
651
+//
706 652
 
707
-  // generate an input test signal containing bsiN id signals
708
-  if( atSignalGen(s,chIdx,p->fhN,s->sigN,bsiV,bsiN,noiseGain,&yV,&yN) != cmOkRC )
653
+cmReflectCalc_t* cmReflectCalcAlloc( cmCtx* ctx, cmReflectCalc_t* p, const cmGoldSigArg_t* gsa, float phat_alpha, unsigned phat_mult )
654
+{
655
+  cmReflectCalc_t* op = cmObjAlloc(cmReflectCalc_t,ctx,p);
656
+  cmRC_t rc = cmOkRC;
657
+  
658
+  // allocate the Gold code signal generator
659
+  if( (p->gs = cmGoldSigAlloc(ctx,NULL,NULL)) == NULL )
709 660
   {
710
-    rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"Signal generation failed.");
661
+    rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Gold sig allocate failed.");
711 662
     goto errLabel;
712 663
   }
713 664
 
714
-  // bsiV[] now holds signal onsets. Set esiV[] to  signal offsets.
715
-  atVOU_AddVVS(esiV,bsiV,bsiN,s->sigN );
716
-
717
-  // set lsiV[] to end-of-listen location 
718
-  atVOU_AddVVS(lsiV,esiV,bsiN,listenDelaySmp);
719
-
720
-  // zero the detection vector
721
-  atVOU_Zero(dsiV,bsiN);
722
-
723
-  // allocate a vector array to record the PHAT input signals
724
-  if( cmVectArrayAlloc(ctx,&inVA,kSampleVaFl) != cmOkRC )
665
+  // allocate the PHAT object
666
+  if( (p->phat = cmPhatAlloc(ctx,NULL,0,0,0,0,0)) == NULL )
725 667
   {
726
-    rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray inVA alloc failed.");
668
+    rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"PHAT allocate failed.");
727 669
     goto errLabel;
728 670
   }
729 671
 
730
-  // allocate a vector array to record the PHAT correlation output signals
731
-  if( cmVectArrayAlloc(ctx,&outVA,kSampleVaFl) != cmOkRC )
732
-  {
733
-    rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray outVA alloc failed.");
734
-    goto errLabel;
735
-  }
672
+  op->va = cmVectArrayAlloc(ctx,kSampleVaFl);
736 673
 
737
-  // allocate a vector array to record the PHAT status
738
-  if( cmVectArrayAlloc(ctx,&statusVA,kSampleVaFl) != cmOkRC )
739
-  {
740
-    rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray statusVA alloc failed.");
741
-    goto errLabel;
742
-  }
674
+  // allocate 'this'
675
+  if( gsa != NULL )  
676
+    rc = cmReflectCalcInit(op,gsa,phat_alpha,phat_mult);
677
+      
743 678
 
679
+ errLabel:
680
+      if( rc != cmOkRC )
681
+        cmReflectCalcFree(&op);
682
+                        
744 683
   
745
-  // for each 'dspFrmCnt' samples in the input signal
746
-  for(i=0,j=0; j<bsiN && i<=yN-dspFrmCnt; i+=dspFrmCnt)
747
-  {
748
-    // store a copy of the input signal
749
-    cmVectArrayAppendS(inVA,yV+i,dspFrmCnt);
750
-
751
-    // feed the next dspFrmCnt samples to the PHAT detector
752
-    cmPhatExec(p,yV+i,dspFrmCnt);
684
+  return op;
685
+  
686
+}
753 687
 
754
-    // if the approximate end of an id signal is encountered
755
-    if( lsiV[j] <= i && i < lsiV[j] + dspFrmCnt )
756
-    {
757
-      // execute the PHAT correlator
758
-      cmPhatChExec( p, chIdx, -1, -1 );
688
+cmRC_t cmReflectCalcFree( cmReflectCalc_t** pp )
689
+{
690
+  cmRC_t rc = cmOkRC;
759 691
 
760
-      // apply non-linear exponent to the correlation vector
761
-      cmVOS_PowV(p->xV,p->fhN,nonLinExpo);
692
+  if( pp == NULL || *pp == NULL )
693
+    return rc;
762 694
 
763
-      // locate the corr. peak inside the listening window
764
-      // (the detection window is last 'detectWndSmp' samples in the corr. vector )
765
-      unsigned detectWndSmp = 2*listenDelaySmp;
766
-      dsiV[j] = cmVOS_ArgMax( p->xV + p->fhN - detectWndSmp,  detectWndSmp);
695
+  cmReflectCalc_t* p = *pp;
767 696
 
768
-      // convert the pk index to absolute time
769
-      dsiV[j] = i + dspFrmCnt - detectWndSmp + dsiV[j];
697
+  if((rc = cmReflectCalcFinal(p)) != cmOkRC )
698
+    return rc;
770 699
 
771
-      //                 sig beg  sig end  detect begin          dtct end    detect
772
-      cmSample_t v[] = { bsiV[j], esiV[j], lsiV[j]-detectWndSmp, lsiV[j],    dsiV[j] };
700
+  cmVectArrayFree(&p->va);
701
+  cmGoldSigFree(&p->gs); 
702
+  cmPhatFree(&p->phat);
703
+  
704
+  cmMemFree(p);
705
+  *pp = NULL;
773 706
 
774
-      // store the detection information
775
-      cmVectArrayAppendS(statusVA,v,sizeof(v)/sizeof(v[0]));
707
+  return rc;
708
+}
776 709
 
777
-      // store the correlation output vector
778
-      cmVectArrayAppendS(outVA,p->xV,p->fhN);
779
-      
780
-      j += 1;
781
-    }
782
-  }
710
+cmRC_t cmReflectCalcInit( cmReflectCalc_t* p, const cmGoldSigArg_t* gsa, float phat_alpha, unsigned phat_mult )
711
+{
712
+  cmRC_t rc;
713
+  if((rc = cmReflectCalcFinal(p)) != cmOkRC )
714
+    return rc;
783 715
 
784
-  // write inVA
785
-  if( cmVectArrayWrite(inVA,path = atMakePath(&ctx->err,path,"phatIn","va",dirStr,NULL)) != cmOkRC )
716
+  //  initialize the Gold code signal generator
717
+  if((rc = cmGoldSigInit(p->gs,gsa)) != cmOkRC )
786 718
   {
787
-    rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray outVA write failed.");
719
+    rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Gold code signal initialize failed.");
788 720
     goto errLabel;
789 721
   }
790
-
791
-  // write outVA
792
-  if( cmVectArrayWrite(outVA,path = atMakePath(&ctx->err,path,"phatOut","va",dirStr,NULL)) != cmOkRC )
722
+  
723
+  unsigned phat_chN   = 1;
724
+  unsigned phat_hN    = p->gs->sigN;
725
+  unsigned phat_flags = 0;
726
+  unsigned phat_chIdx = 0;
727
+  
728
+  // initialize the PHAT
729
+  if((rc = cmPhatInit(p->phat,phat_chN,phat_hN,phat_alpha,phat_mult,phat_flags)) != cmOkRC )
793 730
   {
794
-    rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray outVA write failed.");
731
+    rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"PHAT intialize failed.");
795 732
     goto errLabel;
796 733
   }
797 734
 
798
-  // write statusVA
799
-  if( cmVectArrayWrite(statusVA,path = atMakePath(&ctx->err,path,"phatStatus","va",dirStr,NULL)) != cmOkRC )
735
+  // register a target signal with the PHAT
736
+  if((rc = cmPhatSetId( p->phat, phat_chIdx, p->gs->ch[phat_chIdx].mdV, p->gs->sigN )) != cmOkRC )
800 737
   {
801
-    rc = cmErrMsg(&ctx->err, cmSubSysFailRC, "vectArray statusVA write failed.");
738
+    rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"PHAT signal registration failed.");
802 739
     goto errLabel;
803 740
   }
804 741
 
805
- errLabel:
806
-  cmVectArrayFree(&outVA);
807
-  cmVectArrayFree(&inVA);
808
-
809
-  if( cmPhatFree(&p) != cmOkRC )
810
-    cmErrMsg(&ctx->err,cmSubSysFailRC,"PHAT free failed.");
811
-
812
-  if( atSignalFree(&s) != cmOkRC )
813
-    cmErrMsg(&ctx->err,cmSubSysFailRC,"Signal free failed.");
742
+  
743
+  p->xi     = 0;
744
+  p->zeroFl = false;
814 745
 
746
+ errLabel:
747
+  
815 748
   return rc;
816 749
 }
817 750
 
818
-cmRC_t cmPhatTest2( cmCtx* ctx )
751
+cmRC_t cmReflectCalcFinal( cmReflectCalc_t* p )
819 752
 {
820
-  cmRC_t    rc    = cmOkRC;
821
-  cmPhat_t* p     = NULL;
822
-  unsigned  hN    = 16;
823
-  float     alpha = 1.0;
824
-  unsigned  mult  = 4;
825
-
826
-  cmSample_t hV[]  = { 4,3,2,1, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
827
-  cmSample_t x0V[] = { 4,3,2,1, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
828
-  cmSample_t x1V[] = { 0,0,0,0, 4,3,2,1, 0,0,0,0, 0,0,0,0 };
829
-  cmSample_t x2V[] = { 0,0,0,0, 0,0,0,0, 4,3,2,1, 0,0,0,0 };
830
-  cmSample_t x3V[] = { 0,0,0,0, 0,0,0,0, 0,0,0,0, 4,3,2,1 };
831
-
832
-  cmSample_t* xV[] = { x0V, x1V, x2V, x3V };
833
-  unsigned    chN  = sizeof(xV)/sizeof(xV[0]);
834
-  unsigned    i;
835
-  
836
-  if(cmPhatAlloc(ctx,&p,chN,hN,alpha,mult,kNoFlagsAtPhatFl) != cmOkRC )
837
-  {
838
-    rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"cmPhatAlloc() failed.");
839
-    goto errLabel;
840
-  }
753
+  cmGoldSigFinal(p->gs);
754
+  cmPhatFinal(p->phat);
755
+  return cmOkRC;
756
+}
841 757
 
842
-  for(i=0; i<chN; ++i)
843
-    if( cmPhatSetId(p,i,hV,hN) != cmOkRC )
844
-      rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"cmPhatSetId() failed.");
845
-  
758
+cmRC_t cmReflectCalcExec( cmReflectCalc_t* p, const cmSample_t xV, cmSample_t* yV, unsigned xyN )
759
+{
760
+  unsigned i;
846 761
   
847
-  for(i=0; i<chN; ++i)
762
+  for(i=0; i<xyN; ++i,++p->xi)
848 763
   {
849
-    cmPhatReset(p);
850
-    
851
-    if( cmPhatExec(p,xV[i],hN) != cmOkRC )
852
-    {
853
-      rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"cmPhatExec() failed.");
854
-      goto errLabel;
855
-    }
856
-
857
-    cmPhatChExec(p, i, -1, -1);
858
-    cmVOS_PrintL(&ctx->printRpt,"x:",p->xV,1,p->fhN);
859
-  }
860
-
764
+    if( p->xi < p->gs->sigN )
765
+      yV[i] = p->gs->ch[0].mdV[p->xi];
766
+    else
767
+      yV[i] = 0;
861 768
 
862
-  errLabel:
769
+    if( p->xi == p->phat->fhN )
770
+    {
771
+      p->xi = 0;
863 772
 
864
-  cmPhatFree(&p);
773
+      cmPhatChExec(p->phat,0,0,0);
865 774
 
775
+      if( p->va != NULL )
776
+        cmVectArrayAppendS(p->va,p->phat->xV,p->phat->fhN );
777
+    }
866 778
     
867
-  return rc;
779
+  }
780
+
781
+  return cmOkRC;
782
+  
868 783
 }
869
-#endif

+ 24
- 4
cmProc5.h View File

@@ -156,8 +156,7 @@ extern "C" {
156 156
   // Allocate a PHAT based multi-channel correlator.
157 157
   // 'chN'  is the maximum count of id signals to be set via cmPhatSetId().
158 158
   // 'hN' is the the length of the id signal in samples.
159
-  // 'alpha' weight used to emphasize the frequencies where the
160
-  // id signal contains energy.
159
+  // 'alpha' weight used to emphasize the frequencies where the id signal contains energy.
161 160
   // 'mult' * 'hN' is the correlation length (fhN)
162 161
   // 'flags' See kDebugAtPhatFl and kWndAtPhatFl.
163 162
   cmPhat_t* cmPhatAlloc(  cmCtx* ctx, cmPhat_t* p, unsigned chN, unsigned hN, float alpha, unsigned mult, unsigned flags );
@@ -191,9 +190,30 @@ extern "C" {
191 190
 
192 191
   cmRC_t cmPhatWrite( cmPhat_t* p, const char* dirStr );
193 192
 
194
-  cmRC_t cmPhatTest1( cmCtx* ctx, const char* dirFn );
195
-  cmRC_t cmPhatTest2( cmCtx* ctx );
196 193
 
194
+  //=======================================================================================================================
195
+  // 
196
+  //
197
+
198
+  typedef struct
199
+  {
200
+    cmObj obj;
201
+
202
+    cmGoldSig_t*     gs;
203
+    unsigned         xi;  // index into gs->ch[0].mdV[] of the next sample to output
204
+    bool             zeroFl;
205
+    cmPhat_t*        phat;
206
+    cmVectArray_t*   va;
207
+  } cmReflectCalc_t;
208
+
209
+  
210
+  cmReflectCalc_t* cmReflectCalcAlloc( cmCtx* ctx, cmReflectCalc_t* p, const cmGoldSigArg_t* gsa, float phat_alpha, unsigned phat_mult );
211
+  cmRC_t cmReflectCalcFree( cmReflectCalc_t** pp );
212
+  cmRC_t cmReflectCalcInit( cmReflectCalc_t* p, const cmGoldSigArg_t* gsa, float phat_alpha, unsigned phat_mult );
213
+  cmRC_t cmReflectCalcFinal( cmReflectCalc_t* p );
214
+  cmRC_t cmReflectCalcExec( cmReflectCalc_t* p, const cmSample_t xV, cmSample_t* yV, unsigned xyN );
215
+
216
+  
197 217
   
198 218
 #ifdef __cplusplus
199 219
 }

Loading…
Cancel
Save