max30101_algo.cpp

00001 /*******************************************************************************
00002 * Copyright (C) 2018 Maxim Integrated Products, Inc., All Rights Reserved.
00003 *
00004 * Permission is hereby granted, free of charge, to any person obtaining a
00005 * copy of this software and associated documentation files (the "Software"),
00006 * to deal in the Software without restriction, including without limitation
00007 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
00008 * and/or sell copies of the Software, and to permit persons to whom the
00009 * Software is furnished to do so, subject to the following conditions:
00010 *
00011 * The above copyright notice and this permission notice shall be included
00012 * in all copies or substantial portions of the Software.
00013 *
00014 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00015 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00016 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00017 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
00018 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00019 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00020 * OTHER DEALINGS IN THE SOFTWARE.
00021 *
00022 * Except as contained in this notice, the name of Maxim Integrated
00023 * Products, Inc. shall not be used except as stated in the Maxim Integrated
00024 * Products, Inc. Branding Policy.
00025 *
00026 * The mere transfer of this software does not imply any licenses
00027 * of trade secrets, proprietary technology, copyrights, patents,
00028 * trademarks, maskwork rights, or any other form of intellectual
00029 * property whatsoever. Maxim Integrated Products, Inc. retains all
00030 * ownership rights.
00031 *******************************************************************************
00032 */
00033 
00034 #include "max30101_algo.h"
00035 
00036 //helper functions for the heart rate and SpO2 function
00037 uint16_t avg_dc_est(int32_t *p, uint16_t x);
00038 
00039 void lp_dfir_flt(int16_t din0, int16_t din1, int16_t din2, int16_t *dout0, int16_t *dout1, int16_t *dout2) ;
00040 int32_t mul16(int16_t x, int16_t y);
00041 
00042 //
00043 //  Heart Rate/SpO2 Monitor function takes sample input 'dinIR' and dinRed.
00044 //  Other inputs:
00045 //      ns -> Sample Counter, increments with each sample input.
00046 //      SampRate -> Input data real-time sample rate.
00047 //      dinLShft -> Number of left shifts for data to be 16 bit wide.
00048 //      compSpO2 -> If '1' compute SpO2 value,else compute HR only.
00049 
00050 //
00051 //  Outputs:
00052 //      ir_ac_comp  -> AC component of the IR signal.
00053 //      red_ac_comp -> AC component of the Red signal.
00054 //      ir_ac_mag   -> Peak to Peak magnitude of the IR signal.
00055 //      red_ac_mag  -> Peak to Peak magnitude of the Red signal.
00056 //      HRbpm       -> Heart Rate in beats per minute.
00057 //      SpO2        -> SpO2 value as %saturation.
00058 //      DRdy        -> '1' when new data is available.
00059 //
00060 
00061 void HRSpO2Func(uint32_t dinIR, uint32_t dinRed, uint32_t dinGreen, uint32_t ns, uint16_t SampRate, uint16_t compSpO2,
00062     int16_t *ir_ac_comp, int16_t *red_ac_comp, int16_t *green_ac_comp, int16_t *ir_ac_mag, int16_t *red_ac_mag,
00063     int16_t *green_ac_mag,    uint16_t *HRbpm2, uint16_t *SpO2B, uint16_t *DRdy)
00064 {
00065     static int32_t ir_avg_reg = 0;
00066     static int32_t red_avg_reg = 0;
00067     static int32_t green_avg_reg = 0;
00068 
00069     static int16_t ir_ac_sig_cur = 0;
00070     static int16_t ir_ac_sig_min = 0;
00071     static int16_t ir_ac_sig_max = 0;
00072     static int16_t ir_avg_est;
00073 
00074     static int16_t ir_pedge = 0, ir_nedge = 0;
00075     static int16_t ir_pzxic, ir_pzxip;
00076     static int16_t ir_nzxic;
00077 
00078     static int16_t red_ac_sig_cur = 0;
00079     static int16_t red_ac_sig_min = 0;
00080     static int16_t red_ac_sig_max = 0;
00081     static int16_t red_avg_est;
00082 
00083     static int16_t green_avg_est;
00084     static int16_t green_ac_sig_cur = 0;
00085     //static int16_t green_ac_sig_cur=0;
00086     static int16_t green_ac_sig_pre;
00087     static int16_t green_ac_sig_max ;
00088     static int16_t  green_ac_sig_min;
00089     static int16_t green_mac_FIFO[5];
00090     int16_t meanGreenMagFIFO;
00091     int16_t minAmpForHeartBeat ;
00092 
00093     uint32_t  IRData, RedData, greenData, rnum, rden, rdens;
00094     uint16_t  zeros_in_HrQue = 0, posCount = 0;
00095     static uint32_t prevPeakLoc = 0;
00096     static int16_t IrFIFO[100];
00097     static int16_t HrQue[10], lastKnownGoodHr[10];
00098     static int16_t SPO2Que[5];
00099     int16_t SPO2score[5];
00100     static uint16_t HrQindex = 0, lengthOfposCountExceeding = 0;
00101     static uint16_t initHrQueCounter = 0, fingerOff = 0;
00102 
00103     static int16_t HrQueSmoothing[3];
00104     static int16_t SPO2QueSmoothing[3];
00105 
00106     int16_t  k, j;
00107     uint32_t peakLoc ;
00108     int16_t bufferIdx1,  bufferIdx2;
00109     int16_t maxFIFO, IdxMaxFIFO ;
00110     int16_t HRperiod2, HRComp2, deltaHR;
00111     int16_t cSpO2 = 0, SpO2;
00112 
00113     int16_t HrCount = 0, HrSum = 0, meanGreenMagFIFOcounter = 0;
00114     int16_t SPO2D = 0, meanHrQ;
00115     int16_t dx[99], cumsumX[99];
00116     static int16_t SPO2QueCounter = 0 ; //, lastDisplayedHrValue;
00117 
00118     int16_t validSPO2Count = 0;
00119     int16_t validSPO2Sum = 0;
00120     int16_t SPO2scoreAverage =  0;
00121     int16_t SPO2scoreSum = 0 ;
00122     // int16_t deltaMeanLastKnownGoodHr = 0, meanLastKnownGoodHr = 0;
00123     // int16_t counterMeanLastKnownGoodHr = 0;
00124 
00125 
00126     /* clear some vars if fresh new start */
00127     if ((ns == 0) || (fingerOff > 300)) {
00128         ir_avg_reg = 0;
00129         red_avg_reg = 0;
00130         green_avg_reg = 0;
00131 
00132         ir_ac_sig_cur = 0;
00133         ir_ac_sig_min = 0;
00134         ir_ac_sig_max = 0;
00135 
00136         ir_avg_est = 0;
00137         green_avg_est = 0;
00138         red_avg_est = 0 ;
00139 
00140         ir_pedge = 0;
00141         ir_nedge = 0;
00142         ir_pzxic = 0;
00143         ir_pzxip = 0;
00144         ir_nzxic = 0 ;
00145         //ir_nzxip = 0;
00146         red_ac_sig_cur = 0;
00147         red_ac_sig_min = 0;
00148         red_ac_sig_max = 0;
00149 
00150         prevPeakLoc = 0 ;
00151         bufferIdx1 = 0 ;
00152         bufferIdx2 = 0;
00153         HrQindex = 0;
00154         initHrQueCounter = 0;
00155         lengthOfposCountExceeding = 0 ;
00156         fingerOff = 0;
00157         HRComp2 = 0;
00158 
00159         for (k = 0 ; k < 100 ; k++) {
00160             IrFIFO[k] = 0;
00161         }
00162 
00163         for (k = 0 ; k < 10 ; k++) {
00164             HrQue[k] = 0;
00165             lastKnownGoodHr[k] = 0;
00166         }
00167 
00168         for (k = 0 ; k < 3 ; k++) {
00169             HrQueSmoothing[k] = 70;
00170             SPO2QueSmoothing[k] = 97;
00171         }
00172 
00173         for (k = 0 ; k < 5 ; k++) {
00174             SPO2Que[k] = 97;
00175             SPO2score[k] = 0;
00176             green_mac_FIFO[k] = 0;
00177         }
00178 
00179         SPO2QueCounter = 0;
00180         *SpO2B = 97;
00181         *HRbpm2 = 0;
00182         *DRdy = 0 ;
00183     }
00184 
00185 
00186     /* Save current state */
00187     green_ac_sig_pre = green_ac_sig_cur;
00188 
00189     /* Process next data sample */
00190     minAmpForHeartBeat = 0;
00191     IRData  = dinIR;
00192     RedData = dinRed;
00193     greenData = dinGreen ;
00194 
00195     ir_avg_est  = avg_dc_est(&ir_avg_reg, IRData);
00196     red_avg_est = avg_dc_est(&red_avg_reg, RedData);
00197     green_avg_est = avg_dc_est(&green_avg_reg, greenData);
00198 
00199     lp_dfir_flt((uint16_t)(IRData - ir_avg_est), (uint16_t)(RedData - red_avg_est),
00200         (uint16_t)(greenData - green_avg_est),  &ir_ac_sig_cur, &red_ac_sig_cur, &green_ac_sig_cur);
00201 
00202     *ir_ac_comp = ir_ac_sig_cur;
00203     *red_ac_comp = red_ac_sig_cur;
00204     *green_ac_comp = green_ac_sig_cur;
00205 
00206     /* save to FIFO */
00207     for (k = 1 ; k < 100 ; k++) {
00208         IrFIFO[100 - k] = IrFIFO[99 - k];
00209     }
00210     IrFIFO[0] =  green_ac_sig_cur ; // invert
00211     for (k = 0 ; k < 97 ; k++) {
00212         dx[k] = IrFIFO[k + 2] - IrFIFO[k] ;
00213     }
00214     dx[97] = dx[96];
00215     dx[98] = dx[96];
00216 
00217     for (k = 0 ; k < 99 ; k++) {
00218         if (dx[k] > 0) {
00219             dx[k] = 1;
00220         } else {
00221             dx[k] = 0;
00222         }
00223     }
00224 
00225     cumsumX[0] = 0;
00226     for (k = 1; k < 99 ; k++) {
00227         if (dx[k] > 0) {
00228             cumsumX[k] =  cumsumX[k - 1] + dx[k] ;
00229         } else {
00230             cumsumX[k] =  0;
00231         }
00232     }
00233 
00234     /* determine noise
00235      * ignore less than 3 consecutive non-zeros's
00236      * detect # of sign change
00237      */
00238     posCount = 0;
00239     for (k = 1; k < 99 ; k++) {
00240         if (cumsumX[k] > 0) {
00241             posCount ++ ;
00242         } else if (cumsumX[k] == 0) {
00243             if (posCount < 4  && k >= 4) {
00244                 for (j = k - 1; j > k - posCount - 1; j--) {
00245                     cumsumX[j] = 0 ;
00246                 }
00247             }
00248             posCount = 0;
00249         }
00250     }
00251 
00252     /* ignore less than 3 consecutive zeros's */
00253     posCount = 0;
00254     for (k = 1; k < 99 ; k++) {
00255         if (cumsumX[k] == 0) {
00256             posCount ++ ;
00257         } else if (cumsumX[k] > 0) {
00258             if (posCount < 4  && k >= 4) {
00259                 for (j = k - 1; j > k - posCount - 1; j--) {
00260                     cumsumX[j] = 100 ;
00261                 }
00262             }
00263             posCount = 0;
00264         }
00265     }
00266 
00267     /* detect # of sign change */
00268     posCount = 0; /* sign change counter */
00269     for (k = 0; k < 98 ; k++) {
00270         if (cumsumX[k] == 0  && cumsumX[k + 1] > 0) {
00271             posCount ++;
00272         }
00273     }
00274     if (posCount >= 4) {
00275         lengthOfposCountExceeding ++ ;
00276     } else {
00277         lengthOfposCountExceeding = 0 ;
00278     }
00279 
00280     /*  Detect IR channel positive zero crossing (rising edge) */
00281     if ((green_ac_sig_pre < 0) && (green_ac_sig_cur >= 0) && fingerOff == 0) {
00282         *ir_ac_mag = ir_ac_sig_max - ir_ac_sig_min;
00283         *red_ac_mag = red_ac_sig_max - red_ac_sig_min;
00284         *green_ac_mag = green_ac_sig_max - green_ac_sig_min;
00285         if (*green_ac_mag > 0) {
00286             for (k = 0; k < 4 ; k++) {
00287                 green_mac_FIFO[k] = green_mac_FIFO[k + 1];
00288             }
00289             green_mac_FIFO[4] = *green_ac_mag ;
00290             if (green_mac_FIFO[4] > 1000) {
00291                 green_mac_FIFO[4] = 1000;
00292             }
00293         }
00294         meanGreenMagFIFO = 0;
00295         meanGreenMagFIFOcounter = 0;
00296         for (k = 0; k < 5 ; k++) {
00297             if (green_mac_FIFO[k] > 0) {
00298                 meanGreenMagFIFO = meanGreenMagFIFO + green_mac_FIFO[k] ;
00299                 meanGreenMagFIFOcounter++;
00300             }
00301         }
00302         if (meanGreenMagFIFOcounter >= 2) {
00303             meanGreenMagFIFO = meanGreenMagFIFO / meanGreenMagFIFOcounter ;
00304             minAmpForHeartBeat = meanGreenMagFIFO / 4 ; //25% of mean of past heart beat
00305         } else {
00306             minAmpForHeartBeat = 75;
00307         }
00308         if (minAmpForHeartBeat < 75) {
00309             minAmpForHeartBeat = 75;
00310         }
00311         if (minAmpForHeartBeat > 400) {
00312             minAmpForHeartBeat = 400;
00313         }
00314 
00315         ir_pedge = 1;
00316         ir_nedge = 0;
00317         ir_ac_sig_max = 0;
00318         ir_pzxip = ir_pzxic;
00319         ir_pzxic = ns;
00320         bufferIdx1 = ir_pzxic - ir_nzxic;
00321         bufferIdx2 = ir_pzxic - ir_pzxip;
00322 
00323         if ((*green_ac_mag) > minAmpForHeartBeat && (*green_ac_mag) < 20000 && bufferIdx1 >= 0
00324             && bufferIdx1 < 100 && bufferIdx2 >= 0 && bufferIdx2 < 100 && bufferIdx1 < bufferIdx2) { // was <5000
00325             maxFIFO = -32766;
00326 
00327             IdxMaxFIFO = 0;
00328             for (j = bufferIdx1; j <= bufferIdx2; j++) { // find max peak
00329                 if (IrFIFO[j] > maxFIFO) {
00330                     maxFIFO = IrFIFO[j];
00331                     IdxMaxFIFO  = j;
00332                 }
00333             }
00334             peakLoc = ir_pzxic - IdxMaxFIFO + 1 ;
00335 
00336             if (prevPeakLoc != 0) {
00337                 HRperiod2 = (uint16_t)(peakLoc -  prevPeakLoc);
00338                 if (HRperiod2 > 33 && HRperiod2 < 134) {
00339                     HRComp2 = (6000 / HRperiod2);
00340                     fingerOff = 0 ;
00341                 } else {
00342                     HRComp2 = 0 ;
00343                 }
00344             } else {
00345                 HRComp2 = 0 ;
00346             }
00347 
00348             if (initHrQueCounter < 10  && HRComp2 > 0) {
00349                 HrQue[HrQindex] = HRComp2;
00350                 HrQindex++;
00351                 initHrQueCounter ++;
00352                 if (HrQindex == 10) {
00353                     HrQindex  = 0;
00354                 }
00355             }
00356 
00357             if (initHrQueCounter > 7 && lengthOfposCountExceeding <= 3) {
00358                 if (HRComp2 > 0) {
00359 
00360                     HrCount = 0;
00361                     HrSum = 0;
00362                     zeros_in_HrQue = 0;
00363                     for (k = 1 ; k < initHrQueCounter ; k++) {
00364                         if (HrQue[k] > 0) {
00365                             HrSum += HrQue[k];
00366                             HrCount ++;
00367                         } else {
00368                             zeros_in_HrQue ++;
00369                         }
00370                     }
00371                     meanHrQ = HrSum / HrCount ;
00372                     deltaHR = lastKnownGoodHr[0] / 10;
00373 
00374                     if (HRComp2 >  lastKnownGoodHr[0] - deltaHR &&  HRComp2  < lastKnownGoodHr[0] + deltaHR) {
00375                         for (k = 1 ; k < 10 ; k++) {
00376                             HrQue[10 - k] = HrQue[9 - k];
00377                         }
00378                         HrQue[0] = HRComp2;
00379                     } /* HR smoothing using FIFO queue */
00380 
00381                     if (zeros_in_HrQue <= 2) {
00382                         for (k = 1 ; k < 3 ; k++) {
00383                             HrQueSmoothing[3 - k] = HrQueSmoothing[2 - k];
00384                         }
00385                         HrQueSmoothing[0] = meanHrQ ;
00386                         HRComp2 = ((HrQueSmoothing[0] << 2) + (HrQueSmoothing[1] << 1) + (HrQueSmoothing[2] << 1)) >> 3;
00387                         *HRbpm2 = HRComp2 ;
00388 
00389                         for (k = 1 ; k < 10 ; k++) {
00390                             lastKnownGoodHr[10 - k] = lastKnownGoodHr[9 - k];
00391                         }
00392                         lastKnownGoodHr[0] = HRComp2;
00393                     }
00394                 }
00395 
00396             } else if (initHrQueCounter < 7) { /* before que is filled up, display whatever it got. */
00397                 *HRbpm2 =  HRComp2;
00398 
00399             } else {
00400                 //  *HRbpm2 =  0 ;
00401                 HrCount = 0;
00402                 HrSum = 0;
00403                 for (k = 0 ; k < 10 ; k++) {
00404                     if (lastKnownGoodHr[k] > 0) {
00405                         HrSum = HrSum + lastKnownGoodHr[k];
00406                         HrCount++;
00407                     }
00408                 }
00409                 if (HrCount > 0) {
00410                     *HRbpm2 = HrSum / HrCount;
00411                 } else {
00412                     *HRbpm2 = 0;
00413                 }
00414             }
00415             prevPeakLoc = peakLoc ; /* save peakLoc into Static var */
00416 
00417             if (compSpO2) {
00418                 rnum = (ir_avg_reg >> 20) * (*red_ac_mag);
00419                 rden = (red_avg_reg >> 20) * (*ir_ac_mag);
00420                 rdens = (rden >> 15);
00421                 if (rdens > 0) {
00422                     cSpO2 = 110 - (((25 * rnum) / (rdens)) >> 15);
00423                 }
00424 
00425                 if (cSpO2 >= 100) {
00426                     SpO2 = 100;
00427                 } else if (cSpO2 <= 70) {
00428                     SpO2 = 70;
00429                 } else {
00430                     SpO2 = cSpO2;
00431                 }
00432 
00433                 SPO2Que[SPO2QueCounter ] = SpO2;
00434 
00435                 for (k = 0 ; k < 5 ; k++) {
00436                     SPO2score[k] = 0;
00437                     for (j = 0 ; j < 5 ; j++)
00438                         if (abs(SPO2Que[k] - SPO2Que[j]) > 5) {
00439                             SPO2score[k] ++;
00440                         }
00441                 }
00442 
00443                 SPO2scoreSum =  0;
00444                 for (k = 0 ; k < 5 ; k++) {
00445                     SPO2scoreSum += SPO2score[k] ;
00446                 }
00447                 SPO2scoreAverage = SPO2scoreSum / 5;
00448                 for (k = 1 ; k < 5 ; k++) {
00449                     SPO2score[k] = SPO2score[k] - SPO2scoreAverage;
00450                 }
00451 
00452                 validSPO2Count = 0;
00453                 validSPO2Sum = 0;
00454                 for (k = 1 ; k < 5 ; k++) {
00455                     if (SPO2score[k] <= 0) { // add for HR to report
00456                         validSPO2Sum += SPO2Que[k];
00457                         validSPO2Count ++;
00458                     }
00459                 }
00460                 if (validSPO2Count > 0) {
00461                     SPO2D = (validSPO2Sum / validSPO2Count) - 1;
00462                 }
00463                 if (SPO2D > 100) {
00464                     SPO2D = 100;
00465                 }
00466 
00467                 SPO2QueCounter ++;
00468                 if (SPO2QueCounter == 5) {
00469                     SPO2QueCounter = 0;
00470                 }
00471 
00472                 for (k = 1 ; k < 3 ; k++) {
00473                     SPO2QueSmoothing[3 - k] = SPO2QueSmoothing[2 - k];
00474                 }
00475                 SPO2QueSmoothing[0] = SPO2D;
00476                 *SpO2B = ((SPO2QueSmoothing[0] << 2) + (SPO2QueSmoothing[1] << 1) + (SPO2QueSmoothing[2] << 1)) >> 3;
00477 
00478                 if (*SpO2B > 100) {
00479                     *SpO2B = 100 ;
00480                 }
00481 
00482             } else {
00483                 SpO2 = 0;
00484                 *SpO2B = 0;
00485             }
00486             *DRdy = 1;
00487 
00488         }
00489     }
00490 
00491     /* Detect IR channel negative zero crossing (falling edge) */
00492     if ((green_ac_sig_pre > 0) && (green_ac_sig_cur <= 0)) {
00493         ir_pedge = 0;
00494         ir_nedge = 1;
00495         ir_ac_sig_min = 0;
00496         ir_nzxic = ns;
00497     }
00498 
00499     /* Find Maximum IR & Red values in positive cycle */
00500     if (ir_pedge && (green_ac_sig_cur > green_ac_sig_pre)) {
00501         ir_ac_sig_max  = ir_ac_sig_cur;
00502         red_ac_sig_max = red_ac_sig_cur;
00503         green_ac_sig_max = green_ac_sig_cur;
00504     }
00505 
00506     /* Find minimum IR & Red values in negative cycle */
00507     if (ir_nedge && (green_ac_sig_cur < green_ac_sig_pre)) {
00508         ir_ac_sig_min  = ir_ac_sig_cur;
00509         red_ac_sig_min = red_ac_sig_cur;
00510         green_ac_sig_min = green_ac_sig_cur;
00511     }
00512 
00513     if (IRData < 50000) {
00514         // finger-off
00515         fingerOff++;
00516         *DRdy = 0;
00517     } else {
00518         fingerOff = 0 ;
00519     }
00520 
00521     if (*SpO2B ==  0  ||   *HRbpm2 == 0) {
00522         *DRdy = 0;
00523     }
00524 }
00525 
00526 /*
00527  * Average DC Estimator
00528  */
00529 uint16_t avg_dc_est(int32_t *p, uint16_t x)
00530 {
00531     *p += ((((int32_t) x << 15) - *p) >> 4);
00532     return (*p >> 15);
00533 }
00534 
00535 /*
00536  * Symmetric Dual Low Pass FIR Filter
00537  */
00538 void lp_dfir_flt(int16_t din0, int16_t din1, int16_t din2, int16_t *dout0, int16_t *dout1, int16_t *dout2)
00539 {
00540     static const uint16_t FIRCoeffs[12] = {688, 1283, 2316, 3709, 5439, 7431,
00541             9561, 11666, 13563, 15074, 16047, 16384
00542         };
00543 
00544     static int16_t cbuf0[32], cbuf1[32], cbuf2[32];
00545     static int16_t offset = 0;
00546     int32_t y0, y1, y2;
00547     int16_t i;
00548 
00549     cbuf0[offset] = din0;
00550     cbuf1[offset] = din1;
00551     cbuf2[offset] = din2;
00552 
00553     y0 = mul16(FIRCoeffs[11], cbuf0[(offset - 11) & 0x1F]);
00554     y1 = mul16(FIRCoeffs[11], cbuf1[(offset - 11) & 0x1F]);
00555     y2 = mul16(FIRCoeffs[11], cbuf2[(offset - 11) & 0x1F]);
00556 
00557 
00558     for (i = 0; i < 11; i++) {
00559         y0 += mul16(FIRCoeffs[i], cbuf0[(offset - i) & 0x1F] + cbuf0[(offset - 22 + i) & 0x1F]);
00560         y1 += mul16(FIRCoeffs[i], cbuf1[(offset - i) & 0x1F] + cbuf1[(offset - 22 + i) & 0x1F]);
00561         y2 += mul16(FIRCoeffs[i], cbuf2[(offset - i) & 0x1F] + cbuf2[(offset - 22 + i) & 0x1F]);
00562     }
00563     offset = (offset + 1) & 0x1F;
00564 
00565     *dout0 = (y0 >> 15);
00566     *dout1 = (y1 >> 15);
00567     *dout2 = (y2 >> 15);
00568 }
00569 
00570 /*
00571  * Integer multiplier
00572  */
00573 int32_t mul16(int16_t x, int16_t y)
00574 {
00575     return (int32_t)(x * y);
00576 }
00577