dwtdenoising.c

用离散小波变换实现语音去噪

/*
 * File: dwtdenoising.c
 *
 * Real-Time Workshop code generated for Simulink model dwtdenoising.
 *
 * Model version                        : 1.17
 * Real-Time Workshop file version      : 6.6  (R2007a)  01-Feb-2007
 * Real-Time Workshop file generated on : Mon Dec 10 11:23:11 2007
 * TLC version                          : 6.6 (Jan 16 2007)
 * C source code generated on           : Mon Dec 10 11:23:12 2007
 */

#include "dwtdenoising.h"
#include "dwtdenoising_private.h"

/* Block signals (auto storage) */

#pragma DATA_ALIGN(dwtdenoising_B, 8)

BlockIO_dwtdenoising dwtdenoising_B;

/* Block states (auto storage) */
#pragma DATA_ALIGN(dwtdenoising_DWork, 8)

D_Work_dwtdenoising dwtdenoising_DWork;

/* Real-time model */
RT_MODEL_dwtdenoising dwtdenoising_M_;
RT_MODEL_dwtdenoising *dwtdenoising_M = &dwtdenoising_M_;

/* DSP/BIOS Task Block: <S3>/S-Function1 (dspbios_task) */
void Task0_fcn(void)
{
  /* Wait until application is initialized properly */
  SEM_pend(&startSEM, SYS_FOREVER);
  TSK_prolog( TSK_self() );
  while (1) {
    /* Call the system: <Root>/Denoising Algorithm */

    /* Output and update for function-call system: '<Root>/Denoising Algorithm' */
    {
      {
        /* local block i/o variables */
        real_T rtb_DyadicAnalysisFilterBank_o1[32];
        real_T rtb_DyadicAnalysisFilterBank_o2[16];
        real_T rtb_DyadicAnalysisFilterBank_o3[8];
        real_T rtb_DyadicAnalysisFilterBank_o4[8];
        real_T rtb_DeadZone1[32];
        real_T rtb_DeadZone2[16];
        real_T rtb_DeadZone3[8];
        real_T rtb_DeadZone4[8];

        /* Output: Signal Processing Blockset Delay (sdspdelay) - '<S4>/Integer Delay' */
        {
          /* Single channel input (Delay = 21000, Samples per channel = 32) */
          int_T buffstart = dwtdenoising_DWork.IntegerDelay_CIRC_BUF_IDX;
          while (buffstart >= 21000)
            buffstart -= 21000;

          {
            int_T copy1size = buffstart <= (20968)? 32 : 21000 - buffstart;
            memcpy((byte_T *)rtb_DeadZone1, (byte_T *)
                   &dwtdenoising_DWork.IntegerDelay_IC_BUFF[0] + (buffstart*
                    sizeof(real_T)), copy1size * sizeof(real_T));
            memcpy((byte_T *)rtb_DeadZone1 + copy1size * sizeof(real_T), (byte_T
                    *)&dwtdenoising_DWork.IntegerDelay_IC_BUFF[0], (32 -
                    copy1size) * sizeof(real_T));
          }
        }

        /* DeadZone: '<S5>/Dead Zone1' */
        {
          int_T i1;
          real_T *y0 = rtb_DeadZone1;
          const real_T *u0 = rtb_DeadZone1;
          for (i1=0; i1 < 32; i1++) {
            if (u0[i1] >= dwtdenoising_P.DeadZone1_End) {
              y0[i1] = u0[i1] - dwtdenoising_P.DeadZone1_End;
            } else if (u0[i1] > dwtdenoising_P.DeadZone1_Start) {
              y0[i1] = 0.0;
            } else {
              y0[i1] = u0[i1] - dwtdenoising_P.DeadZone1_Start;
            }
          }
        }

        /* Output: Signal Processing Blockset Delay (sdspdelay) - '<S4>/Integer Delay1' */
        {
          /* Single channel input (Delay = 7000, Samples per channel = 16) */
          int_T buffstart = dwtdenoising_DWork.IntegerDelay1_CIRC_BUF_IDX;
          while (buffstart >= 7000)
            buffstart -= 7000;

          {
            int_T copy1size = buffstart <= (6984)? 16 : 7000 - buffstart;
            memcpy((byte_T *)rtb_DeadZone2, (byte_T *)
                   &dwtdenoising_DWork.IntegerDelay1_IC_BUFF[0] + (buffstart*
                    sizeof(real_T)), copy1size * sizeof(real_T));
            memcpy((byte_T *)rtb_DeadZone2 + copy1size * sizeof(real_T), (byte_T
                    *)&dwtdenoising_DWork.IntegerDelay1_IC_BUFF[0], (16 -
                    copy1size) * sizeof(real_T));
          }
        }

        /* DeadZone: '<S5>/Dead Zone2' */
        {
          int_T i1;
          real_T *y0 = rtb_DeadZone2;
          const real_T *u0 = rtb_DeadZone2;
          for (i1=0; i1 < 16; i1++) {
            if (u0[i1] >= dwtdenoising_P.DeadZone2_End) {
              y0[i1] = u0[i1] - dwtdenoising_P.DeadZone2_End;
            } else if (u0[i1] > dwtdenoising_P.DeadZone2_Start) {
              y0[i1] = 0.0;
            } else {
              y0[i1] = u0[i1] - dwtdenoising_P.DeadZone2_Start;
            }
          }
        }

        /* Output: Signal Processing Blockset Delay (sdspdelay) - '<S4>/Integer Delay2' */
        {
          /* Single channel input (Delay = 7000, Samples per channel = 8) */
          memcpy((byte_T *)rtb_DeadZone3, (byte_T *)
                 &dwtdenoising_DWork.IntegerDelay2_IC_BUFF[0] +
                 (dwtdenoising_DWork.IntegerDelay2_CIRC_BUF_IDX*sizeof(real_T)),
                 8 * sizeof(real_T));
        }

        /* DeadZone: '<S5>/Dead Zone3' */
        {
          int_T i1;
          real_T *y0 = rtb_DeadZone3;
          const real_T *u0 = rtb_DeadZone3;
          for (i1=0; i1 < 8; i1++) {
            if (u0[i1] >= dwtdenoising_P.DeadZone3_End) {
              y0[i1] = u0[i1] - dwtdenoising_P.DeadZone3_End;
            } else if (u0[i1] > dwtdenoising_P.DeadZone3_Start) {
              y0[i1] = 0.0;
            } else {
              y0[i1] = u0[i1] - dwtdenoising_P.DeadZone3_Start;
            }
          }
        }

        /* Output: Signal Processing Blockset Delay (sdspdelay) - '<S4>/Integer Delay3' */
        {
          /* Single channel input (Delay = 7000, Samples per channel = 8) */
          memcpy((byte_T *)rtb_DeadZone4, (byte_T *)
                 &dwtdenoising_DWork.IntegerDelay3_IC_BUFF[0] +
                 (dwtdenoising_DWork.IntegerDelay3_CIRC_BUF_IDX*sizeof(real_T)),
                 8 * sizeof(real_T));
        }

        /* DeadZone: '<S5>/Dead Zone4' */
        {
          int_T i1;
          real_T *y0 = rtb_DeadZone4;
          const real_T *u0 = rtb_DeadZone4;
          for (i1=0; i1 < 8; i1++) {
            if (u0[i1] >= dwtdenoising_P.DeadZone4_End) {
              y0[i1] = u0[i1] - dwtdenoising_P.DeadZone4_End;
            } else if (u0[i1] > dwtdenoising_P.DeadZone4_Start) {
              y0[i1] = 0.0;
            } else {
              y0[i1] = u0[i1] - dwtdenoising_P.DeadZone4_Start;
            }
          }
        }

        /* Signal Processing Blockset IDWT S-Function (sdspidwt) - '<S2>/Dyadic Synthesis Filter Bank' */
        {
          /* Dyadic synthesis filter bank */
          /* Asymmetric Tree */
          const int_T longTapBytes = 1 * 4 * sizeof(real_T);
          const int_T shortTapBytes = 1 * 4 * sizeof(real_T);
          const int_T inElem0Bytes = 8 * sizeof(real_T);
          const int_T inElem0BytesTimes4 = inElem0Bytes * 4;
          int32_T *longFiltTapIdx =
            &dwtdenoising_DWork.DyadicSynthesisFilterBank_Lon_b[0];
          int32_T *shortFiltTapIdx =
            &dwtdenoising_DWork.DyadicSynthesisFilterBank_Sho_h[0];
          byte_T *longFiltTapBuf = (byte_T *)
            &dwtdenoising_DWork.DyadicSynthesisFilterBank_LongT[0];
          byte_T *shortFiltTapBuf = (byte_T *)
            &dwtdenoising_DWork.DyadicSynthesisFilterBank_Short[0];
          byte_T *memBase = (byte_T *)
            &dwtdenoising_DWork.DyadicSynthesisFilterBank_WorkB[0];
          int_T inFrameSize = 8;       /* first level input frame size */
          int_T level = 0;
          byte_T *inputAddr = memBase;
          byte_T *outputAddr = memBase;

          /* Local array of pointers to the output port locations */
          byte_T *inPortAddr[4];
          inPortAddr[0] = (byte_T *)rtb_DeadZone1;
          inPortAddr[1] = (byte_T *)rtb_DeadZone2;
          inPortAddr[2] = (byte_T *)rtb_DeadZone3;
          inPortAddr[3] = (byte_T *)rtb_DeadZone4;

          /* Iterate through all tree levels */
          for (level=0; level < 3; level++) {
            /* calculate input and output addresses to work buffer */
            if (level > 0) {
              if (level == 1) {
                outputAddr = memBase + inElem0BytesTimes4;
              } else {
                inputAddr = memBase + (inElem0BytesTimes4 << (level-2));
                outputAddr = memBase + (inElem0BytesTimes4 << (level-1));
              }
            }

            {
              /* Memory address generations */
              const byte_T *inputToLongFilt = (level == 0) ? inPortAddr[2] :
                inPortAddr[2-level];
              const byte_T *inputToShortFilt = (level == 0) ? inPortAddr[3] :
                inputAddr;
              byte_T *output = (level == 2) ? (byte_T *)dwtdenoising_B.Output :
                outputAddr;

              /* Two channel synthesis filter bank */
              MWDSP_2ChSBank_DF_DD((real_T *)inputToLongFilt,
                                   (real_T *)inputToShortFilt,
                                   (real_T *)output,
                                   (real_T *)longFiltTapBuf,
                                   (real_T *)shortFiltTapBuf,
                                   (real_T *)
                                   &dwtdenoising_P.DyadicSynthesisFilterBa_HF[0],
                                   (real_T *)
                                   &dwtdenoising_P.DyadicSynthesisFilterBa_LF[0],
                                   longFiltTapIdx,
                                   shortFiltTapIdx,
                                   1, inFrameSize, 4, 4);

              /* Update parameters for next level */
              longFiltTapIdx++;
              shortFiltTapIdx++;
              longFiltTapBuf += longTapBytes;
              shortFiltTapBuf += shortTapBytes;
              inFrameSize <<= 1;
            }
          }
        }

        /* S-Function Block: <S2>/DAC (c6711dsk_dac) */
        {
          const real_T DACScaleFactor = 32768.0;
          const real_T SaturateUpperLimit = 1.0;
          const real_T SaturateLowerLimit = -1.0;
          const real_T *restrict i_ptr = dwtdenoising_B.Output;
          short *restrict o_ptr;       /* drop volatile */
          void *blkDacBuffPtr;
          int_T i = 0;
          blkDacBuffPtr = getDacBuff();

          // Preserve DAC buffer source address for later use in CACHE write back
          o_ptr = (short *) blkDacBuffPtr;

#pragma UNROLL(2)
#pragma MUST_ITERATE(64,64,64)

          for (i=0; i<64; i++) {
            o_ptr[i] = sat_mpy(i_ptr[i], DACScaleFactor, SaturateUpperLimit,
                               SaturateLowerLimit);
          }

          CACHE_wbL2( (void *) blkDacBuffPtr, 512, CACHE_WAIT );
        }

        /* S-Function Block: <S2>/ADC (c6711dsk_adc) */
        {
          const real_T ADCScaleFactor = 1.0 / 32768.0;
          const short *restrict i_buf;
          real_T *restrict o_buf = dwtdenoising_B.ADC;
          int_T i = 0;
          i_buf = (const short *) getAdcBuff();
          CACHE_wbInvL2( (void *) i_buf, 512, CACHE_WAIT );

#pragma UNROLL(2)
#pragma MUST_ITERATE(64,64,64)

          for (i=0; i<64; i++) {
            o_buf[i] = (real_T)(int) i_buf[i] * ADCScaleFactor;
          }
        }

        /* Signal Processing Blockset DWT S-Function (sdspdwt) - '<S2>/Dyadic Analysis Filter Bank' */
        {
          byte_T *sums = (byte_T *)
            &dwtdenoising_DWork.DyadicAnalysisFilterBank_Sums[0];
          byte_T *memBase = (byte_T *)
            &dwtdenoising_DWork.DyadicAnalysisFilterBank_WorkBu[0];
          byte_T *tapBuf = (byte_T *)
            &dwtdenoising_DWork.DyadicAnalysisFilterBank_TapDel[0];
          int32_T *tapIdx = &dwtdenoising_DWork.DyadicAnalysisFilterBank_TapD_m
            [0];
          int32_T *phaseIdx =
            &dwtdenoising_DWork.DyadicAnalysisFilterBank_PhaseI[0];

          /* Initialize parameters for the first level filter bank */
          int_T inFrameSize = 64;
          int_T outElem = 64 >> 1;
          int_T offset = 64;
          int_T level;

          /* loop through all numLevels filter bank levels */
          for (level=0; level < 3; level++) {
            const byte_T *in = (level == 0) ? (const byte_T *)dwtdenoising_B.ADC
              : memBase;

            /* all filter outputs from the same level are temporally stored in
               the bottom half of WorkBuffer.
               store high-pass filter result in location mem
               store low-pass filter result  in location mem+outElemTimesBpe */
            byte_T *mem = memBase + offset * sizeof(real_T);
            int_T outElemTimesBpe = outElem * sizeof(real_T);