File: delaytracking.c

    1   /*
    2    * delaytracking.c
    3    *
    4    * Code generation for function 'delaytracking'
    5    *
    6    */
    7   
    8   /* Include files */
    9   #include "rt_nonfinite.h"
   10   #include "gjbf.h"
   11   #include "delaytracking.h"
   12   #include "power.h"
   13   #include "arweights.h"
   14   #include "round1.h"
   15   #include "error.h"
   16   #include "any.h"
   17   #include "sqrt.h"
   18   #include "sum.h"
   19   #include "isfinite.h"
   20   #include "all.h"
   21   #include "gjbf_data.h"
   22   #include "blas.h"
   23   
   24   /* Variable Definitions */
   25   static emlrtRSInfo lb_emlrtRSI = { 26, "delaytracking",
   26     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   27   
   28   static emlrtRSInfo mb_emlrtRSI = { 27, "delaytracking",
   29     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   30   
   31   static emlrtRSInfo nb_emlrtRSI = { 28, "delaytracking",
   32     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   33   
   34   static emlrtRSInfo ob_emlrtRSI = { 29, "delaytracking",
   35     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   36   
   37   static emlrtRSInfo pb_emlrtRSI = { 31, "delaytracking",
   38     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   39   
   40   static emlrtRSInfo qb_emlrtRSI = { 34, "delaytracking",
   41     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   42   
   43   static emlrtRSInfo rb_emlrtRSI = { 35, "delaytracking",
   44     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   45   
   46   static emlrtRSInfo sb_emlrtRSI = { 36, "delaytracking",
   47     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   48   
   49   static emlrtRSInfo tb_emlrtRSI = { 39, "delaytracking",
   50     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   51   
   52   static emlrtRSInfo ub_emlrtRSI = { 60, "delaytracking",
   53     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   54   
   55   static emlrtRSInfo vb_emlrtRSI = { 63, "delaytracking",
   56     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   57   
   58   static emlrtDCInfo e_emlrtDCI = { 84, 35, "delaytracking",
   59     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m", 1
   60   };
   61   
   62   static emlrtBCInfo e_emlrtBCI = { -1, -1, 84, 35, "xPrev", "delaytracking",
   63     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m", 0
   64   };
   65   
   66   static emlrtDCInfo f_emlrtDCI = { 85, 11, "delaytracking",
   67     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m", 1
   68   };
   69   
   70   static emlrtBCInfo f_emlrtBCI = { 1, 882, 85, 11, "x", "delaytracking",
   71     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m", 0
   72   };
   73   
   74   static emlrtECInfo b_emlrtECI = { -1, 84, 5, "delaytracking",
   75     "E:\\micro array project\\Microphone Array\\beamform test\\delaytracking.m" };
   76   
   77   /* Function Definitions */
   78   void delaytracking(const emlrtStack *sp, const real_T x[14112], const
   79                      emxArray_real_T *xPrev, real_T fs, const real_T spos[3],
   80                      const real_T mpos[48], real_T c, real_T iw, real_T XSF[14112],
   81                      real_T wghts[16])
   82   {
   83     boolean_T b1;
   84     boolean_T bv8[14112];
   85     boolean_T dist[16];
   86     boolean_T guard1 = false;
   87     boolean_T tmp_data[14112];
   88     int32_T tmp_size[2];
   89     boolean_T bv9[3];
   90     real_T b_mpos[48];
   91     int32_T ixstart;
   92     real_T dv0[48];
   93     int32_T ix;
   94     real_T b_dist[16];
   95     boolean_T bv10[48];
   96     real_T mtmp;
   97     boolean_T exitg1;
   98     int32_T k;
   99     int32_T loop_ub;
  100     real_T xPrev_data[1764];
  101     int32_T i28;
  102     int32_T xPrev_size_idx_0;
  103     int32_T b_loop_ub;
  104     int32_T b_tmp_size[1];
  105     real_T b_tmp_data[1764];
  106     int32_T iv22[1];
  107     emlrtStack st;
  108     st.prev = sp;
  109     st.tls = sp->tls;
  110     b1 = false;
  111     covrtLogFcn(&emlrtCoverageInstance, 1U, 0);
  112   
  113     /* % delay tracking */
  114     /*   XSF = delaytracking(x, xPrev, fs, spos, mpos, c, iw) */
  115     /*  INPUT */
  116     /*  * |x|-each column response a mic track */
  117     /*  * |xPrev|-data from a previous window. This data allows for real data to be padded */
  118     /*    at the front of each shifted track .Simply zeros if no previous data       */
  119     /*  * |fs|- sampling rate (Hertz) */
  120     /*  * |spos| - 3x1 source location point (col vector) (meters) */
  121     /*  * |mpos| - column matrix of mic positions (meters) */
  122     /*  * |c| - speed of sound (meters/sec) */
  123     /*  * |iw| -(optional) if set to 1, the dsb will apply an inverse distance  */
  124     /*          weighting on the microphones, useful for near-field/immersive */
  125     /*          applications.  Any other value or its absense will result in */
  126     /*          the uniform weight dbs. */
  127     /* OUTPUT */
  128     /*  * |y| - delay tracking */
  129     /* % Function Declaration */
  130     /* % Argument Error Checking */
  131     covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 0, false);
  132     covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 0, false);
  133     covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 1, true);
  134     b_isfinite(x, bv8);
  135     st.site = &lb_emlrtRSI;
  136     all(&st, bv8, dist);
  137     if (!covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 2, b_all(dist))) {
  138       covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 0, true);
  139       covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 1, true);
  140       st.site = &mb_emlrtRSI;
  141       error(&st);
  142     } else {
  143       covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 0, false);
  144       covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 1, false);
  145       guard1 = false;
  146       if (covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 3, xPrev->size[0] == 0)) {
  147         guard1 = true;
  148       } else {
  149         covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 4, true);
  150         g_isfinite(xPrev, tmp_data, tmp_size);
  151         st.site = &nb_emlrtRSI;
  152         f_all(&st, tmp_data, tmp_size, dist);
  153         if (!covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 5, b_all(dist))) {
  154           guard1 = true;
  155         } else {
  156           covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 1, false);
  157           covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 2, false);
  158           covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 6, false);
  159           covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 7, true);
  160           covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 8, true);
  161           if ((!covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 9, c_isfinite(fs))) ||
  162               covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 10, fs <= 0.0)) {
  163             covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 2, true);
  164             covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 3, true);
  165             covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 1);
  166             st.site = &pb_emlrtRSI;
  167             b_error(&st);
  168           } else {
  169             covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 2, false);
  170             covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 3, false);
  171             covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 11, false);
  172             covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 12, true);
  173             d_isfinite(spos, bv9);
  174             if (!covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 13, c_all(bv9))) {
  175               covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 3, true);
  176               covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 4, true);
  177               covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 2);
  178               st.site = &qb_emlrtRSI;
  179               c_error(&st);
  180             } else {
  181               covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 14, false);
  182               covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 15, false);
  183               covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 16, false);
  184               covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 3, false);
  185               covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 4, false);
  186               covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 17, true);
  187               e_isfinite(mpos, bv10);
  188               st.site = &rb_emlrtRSI;
  189               d_all(&st, bv10, dist);
  190               if (!covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 18, b_all(dist)))
  191               {
  192                 covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 4, true);
  193                 covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 5, true);
  194                 st.site = &sb_emlrtRSI;
  195                 u_error(&st);
  196               } else {
  197                 covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 19, false);
  198                 covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 4, false);
  199                 covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 5, false);
  200                 covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 20, false);
  201                 covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 21, true);
  202                 covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 22, true);
  203                 if ((!covrtLogCond(&emlrtCoverageInstance, 1U, 0U, 23, c_isfinite
  204                                    (c))) || covrtLogCond(&emlrtCoverageInstance,
  205                      1U, 0U, 24, c <= 0.0)) {
  206                   covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 5, true);
  207                   covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 6, true);
  208                   covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 3);
  209                   st.site = &tb_emlrtRSI;
  210                   e_error(&st);
  211                 } else {
  212                   covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 5, false);
  213                   covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 6, false);
  214                 }
  215               }
  216             }
  217           }
  218         }
  219       }
  220   
  221       if (guard1) {
  222         covrtLogMcdc(&emlrtCoverageInstance, 1U, 0U, 1, true);
  223         covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 2, true);
  224         st.site = &ob_emlrtRSI;
  225         n_error(&st);
  226       }
  227     }
  228   
  229     covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 4);
  230   
  231     /* % Setup */
  232     /*  Initialize vars and get some information about the target audio */
  233     /*  total number of samples in each audio track */
  234     /*  number of microphones in the array */
  235     memset(&XSF[0], 0, 14112U * sizeof(real_T));
  236   
  237     /*  initialize shifted track matrix */
  238     /* % Beamform */
  239     /*  delaying each microphone track to bring the target signal into alignment */
  240     /*  then sum all tracks together. */
  241     /*  d_i = \sqrt{(mpos{i,x}\ ^\_\ s_x)^2+(mpos_{i,y}\ ^\_\ s_y)^2+ */
  242     /*  (mpos{i,z}\ ^\_\ s_z)^2} \quad i \in [1, M]  */
  243     /*   \tau_i = f_s\left(\frac{d_{max}\ ^\_\ d_i}{c}\right)  */
  244     /*  Find the distances of each mic to the source */
  245     /*  spos*ones: expend one spos to every mpos so as to subtract */
  246     for (ixstart = 0; ixstart < 3; ixstart++) {
  247       for (ix = 0; ix < 16; ix++) {
  248         b_mpos[ixstart + 3 * ix] = mpos[ixstart + 3 * ix] - spos[ixstart];
  249       }
  250     }
  251   
  252     power(b_mpos, dv0);
  253     sum(dv0, b_dist);
  254     st.site = &ub_emlrtRSI;
  255     b_sqrt(&st, b_dist);
  256     for (ixstart = 0; ixstart < 16; ixstart++) {
  257       dist[ixstart] = (b_dist[ixstart] == 0.0);
  258     }
  259   
  260     if (covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 7, any(dist))) {
  261       st.site = &vb_emlrtRSI;
  262       x_error(&st);
  263     }
  264   
  265     if (covrtLogIf(&emlrtCoverageInstance, 1U, 0U, 8, iw == 1.0)) {
  266       covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 5);
  267       arweights(b_dist, wghts);
  268   
  269       /*  inverse distance weighting */
  270     } else {
  271       covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 6);
  272       for (ixstart = 0; ixstart < 16; ixstart++) {
  273         wghts[ixstart] = 1.0;
  274       }
  275   
  276       /*  no weighting */
  277     }
  278   
  279     covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 7);
  280   
  281     /*  Divide distances by the speed of sound to find duration of travel */
  282     /*  for each signal.  Take that time relative to the furthest mic, */
  283     /*  since all other tracks will be delayed to match the wave that */
  284     /*  took the longest to be recieved. */
  285     ixstart = 1;
  286     mtmp = b_dist[0];
  287     if (muDoubleScalarIsNaN(b_dist[0])) {
  288       ix = 2;
  289       exitg1 = false;
  290       while ((!exitg1) && (ix < 17)) {
  291         ixstart = ix;
  292         if (!muDoubleScalarIsNaN(b_dist[ix - 1])) {
  293           mtmp = b_dist[ix - 1];
  294           exitg1 = true;
  295         } else {
  296           ix++;
  297         }
  298       }
  299     }
  300   
  301     if (ixstart < 16) {
  302       while (ixstart + 1 < 17) {
  303         if (b_dist[ixstart] > mtmp) {
  304           mtmp = b_dist[ixstart];
  305         }
  306   
  307         ixstart++;
  308       }
  309     }
  310   
  311     /*  Convert TDOA to integer delays */
  312     for (ixstart = 0; ixstart < 16; ixstart++) {
  313       b_dist[ixstart] = (mtmp - b_dist[ixstart]) / c * fs;
  314     }
  315   
  316     b_round(b_dist);
  317   
  318     /*  Shift each track.we're recycling x into xPrev on the next iteration */
  319     /*  take delay amount from xPrev to new xShift windows  */
  320     k = 0;
  321     while (k < 16) {
  322       covrtLogFor(&emlrtCoverageInstance, 1U, 0U, 0, 1);
  323       covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 8);
  324       mtmp = ((real_T)xPrev->size[0] - b_dist[k]) + 1.0;
  325       if (mtmp > xPrev->size[0]) {
  326         ix = 1;
  327         ixstart = 1;
  328       } else {
  329         if (mtmp != (int32_T)muDoubleScalarFloor(mtmp)) {
  330           emlrtIntegerCheckR2012b(mtmp, &e_emlrtDCI, sp);
  331         }
  332   
  333         ixstart = xPrev->size[0];
  334         ix = (int32_T)mtmp;
  335         if (!((ix >= 1) && (ix <= ixstart))) {
  336           emlrtDynamicBoundsCheckR2012b(ix, 1, ixstart, &e_emlrtBCI, sp);
  337         }
  338   
  339         ixstart = xPrev->size[0];
  340         i28 = xPrev->size[0];
  341         if (!((i28 >= 1) && (i28 <= ixstart))) {
  342           emlrtDynamicBoundsCheckR2012b(i28, 1, ixstart, &e_emlrtBCI, sp);
  343         }
  344   
  345         ixstart = i28 + 1;
  346       }
  347   
  348       if (1.0 > 882.0 - b_dist[k]) {
  349         loop_ub = 0;
  350       } else {
  351         mtmp = 882.0 - b_dist[k];
  352         if (mtmp != (int32_T)muDoubleScalarFloor(mtmp)) {
  353           emlrtIntegerCheckR2012b(mtmp, &f_emlrtDCI, sp);
  354         }
  355   
  356         loop_ub = (int32_T)mtmp;
  357         if (!((loop_ub >= 1) && (loop_ub <= 882))) {
  358           emlrtDynamicBoundsCheckR2012b(loop_ub, 1, 882, &f_emlrtBCI, sp);
  359         }
  360       }
  361   
  362       xPrev_size_idx_0 = (ixstart - ix) + loop_ub;
  363       b_loop_ub = ixstart - ix;
  364       for (i28 = 0; i28 < b_loop_ub; i28++) {
  365         xPrev_data[i28] = xPrev->data[((ix + i28) + xPrev->size[0] * k) - 1];
  366       }
  367   
  368       for (i28 = 0; i28 < loop_ub; i28++) {
  369         xPrev_data[(i28 + ixstart) - ix] = x[i28 + 882 * k];
  370       }
  371   
  372       b_tmp_size[0] = xPrev_size_idx_0;
  373       for (ixstart = 0; ixstart < xPrev_size_idx_0; ixstart++) {
  374         b_tmp_data[ixstart] = wghts[k] * xPrev_data[ixstart];
  375       }
  376   
  377       if (!b1) {
  378         iv22[0] = 882;
  379         b1 = true;
  380       }
  381   
  382       emlrtSubAssignSizeCheckR2012b(iv22, 1, b_tmp_size, 1, &b_emlrtECI, sp);
  383       memcpy(&XSF[k * 882], &b_tmp_data[0], 882U * sizeof(real_T));
  384       k++;
  385       if (*emlrtBreakCheckR2012bFlagVar != 0) {
  386         emlrtBreakCheckR2012b(sp);
  387       }
  388     }
  389   
  390     covrtLogFor(&emlrtCoverageInstance, 1U, 0U, 0, 0);
  391     covrtLogBasicBlock(&emlrtCoverageInstance, 1U, 9);
  392   
  393     /*  output */
  394     /*  function dsb */
  395   }
  396   
  397   /* End of code generation (delaytracking.c) */
  398