1 /*
2 * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
3 *
4 * Use of this source code is governed by a BSD-style license
5 * that can be found in the LICENSE file in the root of the source
6 * tree. An additional intellectual property rights grant can be found
7 * in the file PATENTS. All contributing project authors may
8 * be found in the AUTHORS file in the root of the source tree.
9 */
10
11 #include <memory.h>
12 #include <string.h>
13 #ifdef WEBRTC_ANDROID
14 #include <stdlib.h>
15 #endif
16
17 #include "modules/audio_coding/codecs/isac/main/source/pitch_estimator.h"
18 #include "modules/audio_coding/codecs/isac/main/source/isac_vad.h"
19
WebRtcIsac_AllPoleFilter(double * InOut,double * Coef,size_t lengthInOut,int orderCoef)20 static void WebRtcIsac_AllPoleFilter(double* InOut,
21 double* Coef,
22 size_t lengthInOut,
23 int orderCoef) {
24 /* the state of filter is assumed to be in InOut[-1] to InOut[-orderCoef] */
25 double scal;
26 double sum;
27 size_t n;
28 int k;
29
30 //if (fabs(Coef[0]-1.0)<0.001) {
31 if ( (Coef[0] > 0.9999) && (Coef[0] < 1.0001) )
32 {
33 for(n = 0; n < lengthInOut; n++)
34 {
35 sum = Coef[1] * InOut[-1];
36 for(k = 2; k <= orderCoef; k++){
37 sum += Coef[k] * InOut[-k];
38 }
39 *InOut++ -= sum;
40 }
41 }
42 else
43 {
44 scal = 1.0 / Coef[0];
45 for(n=0;n<lengthInOut;n++)
46 {
47 *InOut *= scal;
48 for(k=1;k<=orderCoef;k++){
49 *InOut -= scal*Coef[k]*InOut[-k];
50 }
51 InOut++;
52 }
53 }
54 }
55
WebRtcIsac_AllZeroFilter(double * In,double * Coef,size_t lengthInOut,int orderCoef,double * Out)56 static void WebRtcIsac_AllZeroFilter(double* In,
57 double* Coef,
58 size_t lengthInOut,
59 int orderCoef,
60 double* Out) {
61 /* the state of filter is assumed to be in In[-1] to In[-orderCoef] */
62
63 size_t n;
64 int k;
65 double tmp;
66
67 for(n = 0; n < lengthInOut; n++)
68 {
69 tmp = In[0] * Coef[0];
70
71 for(k = 1; k <= orderCoef; k++){
72 tmp += Coef[k] * In[-k];
73 }
74
75 *Out++ = tmp;
76 In++;
77 }
78 }
79
WebRtcIsac_ZeroPoleFilter(double * In,double * ZeroCoef,double * PoleCoef,size_t lengthInOut,int orderCoef,double * Out)80 static void WebRtcIsac_ZeroPoleFilter(double* In,
81 double* ZeroCoef,
82 double* PoleCoef,
83 size_t lengthInOut,
84 int orderCoef,
85 double* Out) {
86 /* the state of the zero section is assumed to be in In[-1] to In[-orderCoef] */
87 /* the state of the pole section is assumed to be in Out[-1] to Out[-orderCoef] */
88
89 WebRtcIsac_AllZeroFilter(In,ZeroCoef,lengthInOut,orderCoef,Out);
90 WebRtcIsac_AllPoleFilter(Out,PoleCoef,lengthInOut,orderCoef);
91 }
92
93
WebRtcIsac_AutoCorr(double * r,const double * x,size_t N,size_t order)94 void WebRtcIsac_AutoCorr(double* r, const double* x, size_t N, size_t order) {
95 size_t lag, n;
96 double sum, prod;
97 const double *x_lag;
98
99 for (lag = 0; lag <= order; lag++)
100 {
101 sum = 0.0f;
102 x_lag = &x[lag];
103 prod = x[0] * x_lag[0];
104 for (n = 1; n < N - lag; n++) {
105 sum += prod;
106 prod = x[n] * x_lag[n];
107 }
108 sum += prod;
109 r[lag] = sum;
110 }
111
112 }
113
WebRtcIsac_BwExpand(double * out,double * in,double coef,size_t length)114 static void WebRtcIsac_BwExpand(double* out,
115 double* in,
116 double coef,
117 size_t length) {
118 size_t i;
119 double chirp;
120
121 chirp = coef;
122
123 out[0] = in[0];
124 for (i = 1; i < length; i++) {
125 out[i] = chirp * in[i];
126 chirp *= coef;
127 }
128 }
129
WebRtcIsac_WeightingFilter(const double * in,double * weiout,double * whiout,WeightFiltstr * wfdata)130 void WebRtcIsac_WeightingFilter(const double* in,
131 double* weiout,
132 double* whiout,
133 WeightFiltstr* wfdata) {
134 double tmpbuffer[PITCH_FRAME_LEN + PITCH_WLPCBUFLEN];
135 double corr[PITCH_WLPCORDER+1], rc[PITCH_WLPCORDER+1];
136 double apol[PITCH_WLPCORDER+1], apolr[PITCH_WLPCORDER+1];
137 double rho=0.9, *inp, *dp, *dp2;
138 double whoutbuf[PITCH_WLPCBUFLEN + PITCH_WLPCORDER];
139 double weoutbuf[PITCH_WLPCBUFLEN + PITCH_WLPCORDER];
140 double *weo, *who, opol[PITCH_WLPCORDER+1], ext[PITCH_WLPCWINLEN];
141 int k, n, endpos, start;
142
143 /* Set up buffer and states */
144 memcpy(tmpbuffer, wfdata->buffer, sizeof(double) * PITCH_WLPCBUFLEN);
145 memcpy(tmpbuffer+PITCH_WLPCBUFLEN, in, sizeof(double) * PITCH_FRAME_LEN);
146 memcpy(wfdata->buffer, tmpbuffer+PITCH_FRAME_LEN, sizeof(double) * PITCH_WLPCBUFLEN);
147
148 dp=weoutbuf;
149 dp2=whoutbuf;
150 for (k=0;k<PITCH_WLPCORDER;k++) {
151 *dp++ = wfdata->weostate[k];
152 *dp2++ = wfdata->whostate[k];
153 opol[k]=0.0;
154 }
155 opol[0]=1.0;
156 opol[PITCH_WLPCORDER]=0.0;
157 weo=dp;
158 who=dp2;
159
160 endpos=PITCH_WLPCBUFLEN + PITCH_SUBFRAME_LEN;
161 inp=tmpbuffer + PITCH_WLPCBUFLEN;
162
163 for (n=0; n<PITCH_SUBFRAMES; n++) {
164 /* Windowing */
165 start=endpos-PITCH_WLPCWINLEN;
166 for (k=0; k<PITCH_WLPCWINLEN; k++) {
167 ext[k]=wfdata->window[k]*tmpbuffer[start+k];
168 }
169
170 /* Get LPC polynomial */
171 WebRtcIsac_AutoCorr(corr, ext, PITCH_WLPCWINLEN, PITCH_WLPCORDER);
172 corr[0]=1.01*corr[0]+1.0; /* White noise correction */
173 WebRtcIsac_LevDurb(apol, rc, corr, PITCH_WLPCORDER);
174 WebRtcIsac_BwExpand(apolr, apol, rho, PITCH_WLPCORDER+1);
175
176 /* Filtering */
177 WebRtcIsac_ZeroPoleFilter(inp, apol, apolr, PITCH_SUBFRAME_LEN, PITCH_WLPCORDER, weo);
178 WebRtcIsac_ZeroPoleFilter(inp, apolr, opol, PITCH_SUBFRAME_LEN, PITCH_WLPCORDER, who);
179
180 inp+=PITCH_SUBFRAME_LEN;
181 endpos+=PITCH_SUBFRAME_LEN;
182 weo+=PITCH_SUBFRAME_LEN;
183 who+=PITCH_SUBFRAME_LEN;
184 }
185
186 /* Export filter states */
187 for (k=0;k<PITCH_WLPCORDER;k++) {
188 wfdata->weostate[k]=weoutbuf[PITCH_FRAME_LEN+k];
189 wfdata->whostate[k]=whoutbuf[PITCH_FRAME_LEN+k];
190 }
191
192 /* Export output data */
193 memcpy(weiout, weoutbuf+PITCH_WLPCORDER, sizeof(double) * PITCH_FRAME_LEN);
194 memcpy(whiout, whoutbuf+PITCH_WLPCORDER, sizeof(double) * PITCH_FRAME_LEN);
195 }
196