FreeCalypso > hg > efr-experiments

annotate src/az_lsp.c @ 2:c511bfb36c2a

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
beginning of EFR2 decoder, using AMR version of AGC module
author Mychaela Falconia <falcon@freecalypso.org>
date Wed, 03 Apr 2024 05:47:51 +0000
parents 56410792419a
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
0
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
1 /***********************************************************************
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
2 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
3 * FUNCTION: Az_lsp
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
4 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
5 * PURPOSE: Compute the LSPs from the LP coefficients (order=10)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
6 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
7 * DESCRIPTION:
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
8 * - The sum and difference filters are computed and divided by
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
9 * 1+z^{-1} and 1-z^{-1}, respectively.
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
10 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
11 * f1[i] = a[i] + a[11-i] - f1[i-1] ; i=1,...,5
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
12 * f2[i] = a[i] - a[11-i] + f2[i-1] ; i=1,...,5
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
13 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
14 * - The roots of F1(z) and F2(z) are found using Chebyshev polynomial
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
15 * evaluation. The polynomials are evaluated at 60 points regularly
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
16 * spaced in the frequency domain. The sign change interval is
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
17 * subdivided 4 times to better track the root.
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
18 * The LSPs are found in the cosine domain [1,-1].
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
19 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
20 * - If less than 10 roots are found, the LSPs from the past frame are
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
21 * used.
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
22 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
23 ***********************************************************************/
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
24
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
25 #include "typedef.h"
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
26 #include "basic_op.h"
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
27 #include "oper_32b.h"
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
28 #include "count.h"
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
29 #include "cnst.h"
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
30
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
31 #include "grid.tab"
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
32
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
33 /* M = LPC order, NC = M/2 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
34
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
35 #define NC M/2
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
36
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
37 /* local function */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
38
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
39 static Word16 Chebps (Word16 x, Word16 f[], Word16 n);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
40
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
41 void Az_lsp (
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
42 Word16 a[], /* (i) : predictor coefficients */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
43 Word16 lsp[], /* (o) : line spectral pairs */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
44 Word16 old_lsp[] /* (i) : old lsp[] (in case not found 10 roots) */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
45 )
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
46 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
47 Word16 i, j, nf, ip;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
48 Word16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
49 Word16 x, y, sign, exp;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
50 Word16 *coef;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
51 Word16 f1[M / 2 + 1], f2[M / 2 + 1];
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
52 Word32 t0;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
53
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
54 /*-------------------------------------------------------------*
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
55 * find the sum and diff. pol. F1(z) and F2(z) *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
56 * F1(z) <--- F1(z)/(1+z**-1) & F2(z) <--- F2(z)/(1-z**-1) *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
57 * *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
58 * f1[0] = 1.0; *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
59 * f2[0] = 1.0; *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
60 * *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
61 * for (i = 0; i< NC; i++) *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
62 * { *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
63 * f1[i+1] = a[i+1] + a[M-i] - f1[i] ; *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
64 * f2[i+1] = a[i+1] - a[M-i] + f2[i] ; *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
65 * } *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
66 *-------------------------------------------------------------*/
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
67
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
68 f1[0] = 1024; move16 (); /* f1[0] = 1.0 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
69 f2[0] = 1024; move16 (); /* f2[0] = 1.0 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
70
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
71 for (i = 0; i < NC; i++)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
72 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
73 t0 = L_mult (a[i + 1], 8192); /* x = (a[i+1] + a[M-i]) >> 2 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
74 t0 = L_mac (t0, a[M - i], 8192);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
75 x = extract_h (t0);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
76 /* f1[i+1] = a[i+1] + a[M-i] - f1[i] */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
77 f1[i + 1] = sub (x, f1[i]);move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
78
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
79 t0 = L_mult (a[i + 1], 8192); /* x = (a[i+1] - a[M-i]) >> 2 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
80 t0 = L_msu (t0, a[M - i], 8192);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
81 x = extract_h (t0);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
82 /* f2[i+1] = a[i+1] - a[M-i] + f2[i] */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
83 f2[i + 1] = add (x, f2[i]);move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
84 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
85
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
86 /*-------------------------------------------------------------*
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
87 * find the LSPs using the Chebychev pol. evaluation *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
88 *-------------------------------------------------------------*/
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
89
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
90 nf = 0; move16 (); /* number of found frequencies */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
91 ip = 0; move16 (); /* indicator for f1 or f2 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
92
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
93 coef = f1; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
94
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
95 xlow = grid[0]; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
96 ylow = Chebps (xlow, coef, NC);move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
97
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
98 j = 0;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
99 test (); test ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
100 /* while ( (nf < M) && (j < grid_points) ) */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
101 while ((sub (nf, M) < 0) && (sub (j, grid_points) < 0))
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
102 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
103 j++;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
104 xhigh = xlow; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
105 yhigh = ylow; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
106 xlow = grid[j]; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
107 ylow = Chebps (xlow, coef, NC);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
108 move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
109
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
110 test ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
111 if (L_mult (ylow, yhigh) <= (Word32) 0L)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
112 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
113
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
114 /* divide 4 times the interval */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
115
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
116 for (i = 0; i < 4; i++)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
117 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
118 /* xmid = (xlow + xhigh)/2 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
119 xmid = add (shr (xlow, 1), shr (xhigh, 1));
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
120 ymid = Chebps (xmid, coef, NC);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
121 move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
122
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
123 test ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
124 if (L_mult (ylow, ymid) <= (Word32) 0L)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
125 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
126 yhigh = ymid; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
127 xhigh = xmid; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
128 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
129 else
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
130 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
131 ylow = ymid; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
132 xlow = xmid; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
133 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
134 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
135
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
136 /*-------------------------------------------------------------*
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
137 * Linear interpolation *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
138 * xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
139 *-------------------------------------------------------------*/
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
140
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
141 x = sub (xhigh, xlow);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
142 y = sub (yhigh, ylow);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
143
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
144 test ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
145 if (y == 0)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
146 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
147 xint = xlow; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
148 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
149 else
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
150 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
151 sign = y; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
152 y = abs_s (y);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
153 exp = norm_s (y);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
154 y = shl (y, exp);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
155 y = div_s ((Word16) 16383, y);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
156 t0 = L_mult (x, y);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
157 t0 = L_shr (t0, sub (20, exp));
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
158 y = extract_l (t0); /* y= (xhigh-xlow)/(yhigh-ylow) */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
159
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
160 test ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
161 if (sign < 0)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
162 y = negate (y);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
163
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
164 t0 = L_mult (ylow, y);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
165 t0 = L_shr (t0, 11);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
166 xint = sub (xlow, extract_l (t0)); /* xint = xlow - ylow*y */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
167 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
168
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
169 lsp[nf] = xint; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
170 xlow = xint; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
171 nf++;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
172
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
173 test ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
174 if (ip == 0)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
175 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
176 ip = 1; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
177 coef = f2; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
178 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
179 else
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
180 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
181 ip = 0; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
182 coef = f1; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
183 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
184 ylow = Chebps (xlow, coef, NC);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
185 move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
186
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
187 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
188 test (); test ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
189 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
190
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
191 /* Check if M roots found */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
192
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
193 test ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
194 if (sub (nf, M) < 0)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
195 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
196 for (i = 0; i < M; i++)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
197 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
198 lsp[i] = old_lsp[i]; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
199 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
200
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
201 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
202 return;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
203 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
204
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
205 /************************************************************************
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
206 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
207 * FUNCTION: Chebps
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
208 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
209 * PURPOSE: Evaluates the Chebyshev polynomial series
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
210 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
211 * DESCRIPTION:
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
212 * - The polynomial order is n = m/2 = 5
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
213 * - The polynomial F(z) (F1(z) or F2(z)) is given by
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
214 * F(w) = 2 exp(-j5w) C(x)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
215 * where
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
216 * C(x) = T_n(x) + f(1)T_n-1(x) + ... +f(n-1)T_1(x) + f(n)/2
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
217 * and T_m(x) = cos(mw) is the mth order Chebyshev polynomial ( x=cos(w) )
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
218 * - The function returns the value of C(x) for the input x.
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
219 *
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
220 ***********************************************************************/
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
221
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
222 static Word16 Chebps (Word16 x, Word16 f[], Word16 n)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
223 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
224 Word16 i, cheb;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
225 Word16 b0_h, b0_l, b1_h, b1_l, b2_h, b2_l;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
226 Word32 t0;
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
227
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
228 b2_h = 256; move16 (); /* b2 = 1.0 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
229 b2_l = 0; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
230
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
231 t0 = L_mult (x, 512); /* 2*x */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
232 t0 = L_mac (t0, f[1], 8192); /* + f[1] */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
233 L_Extract (t0, &b1_h, &b1_l); /* b1 = 2*x + f[1] */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
234
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
235 for (i = 2; i < n; i++)
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
236 {
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
237 t0 = Mpy_32_16 (b1_h, b1_l, x); /* t0 = 2.0*x*b1 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
238 t0 = L_shl (t0, 1);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
239 t0 = L_mac (t0, b2_h, (Word16) 0x8000); /* t0 = 2.0*x*b1 - b2 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
240 t0 = L_msu (t0, b2_l, 1);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
241 t0 = L_mac (t0, f[i], 8192); /* t0 = 2.0*x*b1 - b2 + f[i] */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
242
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
243 L_Extract (t0, &b0_h, &b0_l); /* b0 = 2.0*x*b1 - b2 + f[i]*/
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
244
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
245 b2_l = b1_l; move16 (); /* b2 = b1; */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
246 b2_h = b1_h; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
247 b1_l = b0_l; move16 (); /* b1 = b0; */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
248 b1_h = b0_h; move16 ();
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
249 }
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
250
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
251 t0 = Mpy_32_16 (b1_h, b1_l, x); /* t0 = x*b1; */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
252 t0 = L_mac (t0, b2_h, (Word16) 0x8000); /* t0 = x*b1 - b2 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
253 t0 = L_msu (t0, b2_l, 1);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
254 t0 = L_mac (t0, f[i], 4096); /* t0 = x*b1 - b2 + f[i]/2 */
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
255
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
256 t0 = L_shl (t0, 6);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
257
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
258 cheb = extract_h (t0);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
259
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
260 return (cheb);
56410792419a src: original EFR source from ETSI
Mychaela Falconia <falcon@freecalypso.org>
parents:
diff changeset
261 }