Line data Source code
1 : /******************************************************************************
2 : * ETSI TS 103 634 V1.5.1 *
3 : * Low Complexity Communication Codec Plus (LC3plus) *
4 : * *
5 : * Copyright licence is solely granted through ETSI Intellectual Property *
6 : * Rights Policy, 3rd April 2019. No patent licence is granted by implication, *
7 : * estoppel or otherwise. *
8 : ******************************************************************************/
9 :
10 : #include "defines.h"
11 : #include "functions.h"
12 :
13 0 : void plc_phEcu_LF_peak_analysis_fx(Word16 * plocs, /* i/o 0 ... Lprot/2 +1*/
14 : Word16 * n_plocs, /* i/o 0.. MAX_PLOCS */
15 : Word32 * L_f0estQ16, /* i/o Q16*/
16 : const Word16 *mag, /* i: Qx */
17 : const Word16 stPhECU_f0hzLtpBinQ7, const Word16 stPhECU_f0gainLtpQ15,
18 : const Word16 nSubm, Word16 maxPlocs,
19 : Word8 *scratchBuffer /* Size = 6 * MAX_PLOCS + 42 */
20 : )
21 :
22 : {
23 : Counter i, j;
24 : Word16 n_plocs_ana, peakLF_Xval, tmp, f_abs_ind, plocsIntersectFlag;
25 :
26 : Word32 L_fQ7, *L_f0est_prelQ16;
27 0 : Word16 num_prel = 0, *plocs_prel;
28 : Word16 prel_low, prel_high, start, fin;
29 : Word16 *plocs_old;
30 : Word32 *L_plocsi_old;
31 :
32 : #ifdef DYNMEM_COUNT
33 : Dyn_Mem_In("plc_phEcu_LF_peak_analysis_fx", sizeof(struct {
34 : Counter i, j;
35 : Word16 n_plocs_ana, peakLF_Xval, tmp, f_abs_ind, plocsIntersectFlag;
36 : Word32 L_fQ7, *L_f0est_prelQ16;
37 : Word16 num_prel, *plocs_prel;
38 : Word16 prel_low, prel_high, start, fin;
39 : Word16 *plocs_old;
40 : Word32 *L_plocsi_old;
41 : }));
42 : #endif
43 :
44 :
45 :
46 0 : L_f0est_prelQ16 = (Word32 *)scratchAlign(scratchBuffer, 0); /* Size = 4 * 7 */
47 0 : plocs_prel = (Word16 *)scratchAlign(L_f0est_prelQ16, sizeof(*L_f0est_prelQ16) * 7); /* Size = 2 * 7 */
48 0 : plocs_old = (Word16 *)scratchAlign(plocs_prel, sizeof(*plocs_prel) * 7); /* Size = 2 * MAX_PLOCS */
49 0 : L_plocsi_old = (Word32 *)scratchAlign(plocs_old, sizeof(*plocs_old) * maxPlocs); /* Size = 4 * MAX_PLOCS */
50 :
51 0 : test(); test();
52 0 : IF ((*n_plocs > 0) && sub(stPhECU_f0gainLtpQ15, ((Word16)(0.25 * 32768.0))) > 0 &&
53 : sub(stPhECU_f0hzLtpBinQ7, (Word16)(2.75 * 128.0)) < 0)
54 : {
55 :
56 : /* % analyze/apply f0Ltp to avoid intermodulation effects below f0 of ~180 Hz
57 : % we only do the f0Ltp-replacement(s) if there is already an established
58 : % fft peak in the region ~fRes to 2.5*fres
59 : fft_peak_eval_plocs = 1:3;
60 : plocsIntersectFlag = intersect(plocs, fft_peak_eval_plocs ); % check for 1,2,3 in plocs */
61 :
62 0 : plocsIntersectFlag = 0; move16();
63 0 : peakLF_Xval = 0; move16();
64 0 : n_plocs_ana = s_min(*n_plocs, 3);
65 0 : FOR (i = 0; i < n_plocs_ana; i++)
66 : {
67 0 : tmp = plocs[i]; move16();
68 0 : if (sub(tmp, 2) <= 0) /* C index 0, 1,2 checked , [DC, 62.5 Hz, 125Hz ] */
69 : {
70 0 : plocsIntersectFlag = add(i, 1);
71 : }
72 0 : peakLF_Xval = s_max(mag[tmp], peakLF_Xval);
73 : }
74 :
75 0 : num_prel = 0; move16();
76 0 : IF (plocsIntersectFlag != 0)
77 : { /* fft-peak at 0, 62 or 125 Hz */
78 : /* analyze if ltp-based f0 need to be added or not */
79 0 : peakLF_Xval = mult_r(peakLF_Xval, (Word16)(.375 * 32768.0)); /* now as a limit */
80 :
81 0 : FOR (i = 1; i <= nSubm; i++)
82 : {
83 0 : L_fQ7 = L_mult0(i, stPhECU_f0hzLtpBinQ7); /* fractional index stored in L_plocsi */
84 0 : f_abs_ind = L_shr_pos(L_add(L_fQ7, 64), 7); /* integer bin index stored in plocs */
85 :
86 0 : test();
87 0 : IF ((L_sub(L_fQ7, 819) <= 0) && /* % only apply up to ~400hz , 819 = 400/62.5*128 */
88 : (sub(mag[f_abs_ind], peakLF_Xval) >
89 : 0)) /* % only set as preliminary if relative peak strength is signficant*/
90 : {
91 0 : L_f0est_prelQ16[num_prel] = L_shl_pos(L_fQ7, 9); move32();
92 0 : plocs_prel[num_prel] = f_abs_ind; move16();
93 0 : num_prel = add(num_prel, 1);
94 : }
95 : }
96 : } /*intersectFlag*/
97 :
98 : /* now replace/ merge new preliminary added peaks with existing plocs and L_f0estQ16 */
99 : /* note that a previous fake/merged magnitude-determined peak may be replaced by two separated side peaks */
100 :
101 : /* a general non-optimized list-merging solution below */
102 0 : test();
103 0 : IF ((num_prel > 0) && (sub(add(num_prel, *n_plocs), MAX_PLOCS) <= 0) /* skip in case plocs list is too large */
104 : )
105 : {
106 0 : prel_low = plocs_prel[0]; move16();
107 0 : prel_high = plocs_prel[sub(num_prel, 1)]; move16();
108 :
109 : /* initial assumption:: all original peaks (1 or 2 of them) are positioned below prel_low */
110 0 : start = (*n_plocs); /* at this point 'start' is the location_c where to add any harmonics peaks */
111 :
112 0 : FOR (i = sub(*n_plocs, 1); i >= 0; i--)
113 : {
114 0 : if (sub(plocs[i], prel_low) >= 0)
115 : {
116 0 : start = i; move16();
117 : }
118 : }
119 0 : start = sub(start, 1); /* end of old section to copy before the added/merged section */
120 0 : start = s_max(start, -1); /* limit for loop later */
121 : /*% dbg check low part for a sucessful replace/merge */
122 0 : if (start >= 0 && start < *n_plocs)
123 : {
124 0 : ASSERT(plocs[start] < plocs_prel[0]);
125 : }
126 :
127 0 : sub(0, 0);
128 0 : IF (prel_high < plocs[0])
129 : {
130 0 : fin = 0; move16(); /*% keep all plocs , just concat */
131 : }
132 : ELSE
133 : {
134 0 : fin = *n_plocs;
135 0 : FOR (i = 0; i < *n_plocs; i++)
136 : {
137 0 : sub(0, 0);
138 0 : if (plocs[i] <= prel_high)
139 : {
140 0 : fin = i; move16();
141 : }
142 : }
143 0 : fin = add(fin, 1); /* first element in high part of old plocs to be copied */
144 : }
145 :
146 : /*% dbg check high part for a sucessful replace/merge */
147 0 : if (fin >= 0 && fin < *n_plocs)
148 : {
149 0 : ASSERT(plocs_prel[sub(num_prel, 1)] < plocs[fin]);
150 : }
151 :
152 : /*
153 : % actual replace/merge of added integer locations and fractional freqs. into plocs/f0list list ;
154 : % three loops in BASOP
155 : plocs = [ plocs(1:(start)) ; plocs_prel ; plocs((fin):end) ];
156 : f0est = [ f0est(1:(start)) ; f0est_prel; f0est((fin):end) ];
157 : */
158 :
159 0 : FOR (i = 0; i < *n_plocs; i++)
160 : {
161 0 : plocs_old[i] = plocs[i]; move16();
162 0 : L_plocsi_old[i] = L_f0estQ16[i]; move32();
163 : }
164 :
165 : /*
166 : j=0;
167 : FOR(i=0; i <= start; i++)
168 : {
169 : plocs[i] = plocs_old[i]; move16();
170 : L_f0estQ16[i] = L_plocsi_old[i]; move32();
171 : j++;
172 : }
173 : */
174 :
175 0 : j = add(start, 1);
176 :
177 0 : ASSERT(j>=0);
178 :
179 0 : FOR (i = 0; i < num_prel; i++) /* NB this section may both insert or overwrite old plocs */
180 : {
181 0 : plocs[j] = plocs_prel[i]; move16();
182 0 : L_f0estQ16[j] = L_f0est_prelQ16[i]; move32();
183 0 : j++;
184 : }
185 0 : FOR (i = fin; i < *n_plocs; i++) /* copy the tail of the list */
186 : {
187 0 : plocs[j] = plocs_old[i]; move16();
188 0 : L_f0estQ16[j] = L_plocsi_old[i]; move32();
189 0 : j++;
190 : }
191 :
192 0 : *n_plocs = j; move16(); /* update total length */
193 : } /* num_prel >0*/
194 : } /* gain/hz Limits */
195 :
196 : #ifdef DYNMEM_COUNT
197 : Dyn_Mem_Out();
198 : #endif
199 :
200 0 : }
201 :
202 :
|
