1 /*
2 * Copyright (C) 2017 Analog Devices, Inc.
3 *
4 * This library is free software; you can redistribute it and/or
5 * modify it under the terms of the GNU Lesser General Public
6 * License as published by the Free Software Foundation; either
7 * version 2.1 of the License, or (at your option) any later version.
8 *
9 * This library is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * Lesser General Public License for more details.
13 */
14
15 /* Include Files */
16 #include "rt_nonfinite.h"
17 #include "internal_design_filter_cg.h"
18 #include "internal_design_filter_cg_emxutil.h"
19 #include <stdio.h>
20
21 /* Type Definitions */
22 #ifndef typedef_struct_T
23 #define typedef_struct_T
24
25 typedef struct {
26 double nfft[2048];
27 double Fs;
28 double w[2048];
29 double fvflag;
30 char range[8];
31 double centerdc;
32 char configlevel[7];
33 } struct_T;
34
35 #endif /*typedef_struct_T*/
36
37 /* Function Declarations */
38 static void analogresp(const char type[2], const double f[2048], double
39 Fconverter, const double b1_data[], const int b1_size[2], const
40 emxArray_creal_T *a1, const double b2_data[], const int b2_size[2], const
41 emxArray_creal_T *a2, creal_T abc[2048]);
42 static boolean_T anyNonFinite(const emxArray_creal_T *x);
43 static void b_abs(const emxArray_creal_T *x, emxArray_real_T *y);
44 static void b_acos(double *x);
45 static void b_analogresp(const char type[2], const emxArray_real_T *f, double
46 Fconverter, const double b1_data[], const int b1_size[2], const
47 emxArray_creal_T *a1, const double b2_data[], const int b2_size[2], const
48 emxArray_creal_T *a2, emxArray_creal_T *abc);
49 static void b_butter_cg(double Wn, double num[4], emxArray_creal_T *den);
50 static void b_cos(emxArray_real_T *x);
51 static void b_determineBestFractionLength(const double tap_store[128], double
52 taps[128]);
53 static void b_exp(creal_T x[2048]);
54 static void b_firfreqz(const double b[15], const struct_T *options, creal_T h
55 [2048], double w[2048]);
56 static void b_firpm_cg(double order, const double ff[4], const emxArray_real_T
57 *amplitudes, const emxArray_real_T *frequencies, const emxArray_real_T
58 *weights, emxArray_real_T *h, boolean_T *valid, double *err);
59 static void b_fix(double *x);
60 static void b_freqs_cg(const double b_data[], const int b_size[2], const
61 emxArray_creal_T *a, const emxArray_real_T *w, emxArray_creal_T *h);
62 static void b_freqz_cg(const double b[15], const double w[2048], double Fs,
63 creal_T hh[2048]);
64 static void b_generateCascadedResponseRx(const char enables[4], const
65 emxArray_real_T *w, double Fs, const double hb1_coeff[15], const double
66 hb2_coeff[7], const double hb3_coeff_data[], const int hb3_coeff_size[2],
67 const double dec_int3_coeff_data[], const int dec_int3_coeff_size[2],
68 emxArray_creal_T *combinedResponse);
69 static double b_log2(double x);
70 static void b_polyval(const double p[7], const creal_T x[2048],
71 creal_T y[2048]);
72 static void b_power(const double a[2048], double y[2048]);
73 static void b_rdivide(const emxArray_creal_T *x, const emxArray_creal_T *y,
74 emxArray_creal_T *z);
75 static void b_sinc(emxArray_real_T *x);
76 static void b_sqrt(double *x);
77 static boolean_T b_strcmp(const char a[2]);
78 static double b_sum(const emxArray_real_T *x);
79 static void b_us(const double o[7], double u[7]);
80 static void b_xscal(int n, const creal_T a, emxArray_creal_T *x, int ix0, int
81 incx);
82 static void b_xzlartg(const creal_T f, const creal_T g, double *cs,
83 creal_T *sn);
84 static void butter_cg(double Wn, double num[2], creal_T den_data[], int
85 den_size[2]);
86 static void c_abs(const emxArray_real_T *x, emxArray_real_T *y);
87 static void c_analogresp(const emxArray_real_T *f, double Fconverter, const
88 double b1_data[], const int b1_size[2], const emxArray_creal_T *a1, const
89 double b2_data[], const int b2_size[2], const emxArray_creal_T *a2,
90 emxArray_creal_T *abc);
91 static void c_exp(emxArray_creal_T *x);
92 static void c_firfreqz(const double b[7], const struct_T *options, creal_T h
93 [2048], double w[2048]);
94 static void c_fix(emxArray_real_T *x);
95 static void c_freqz_cg(const double b[7], const double w[2048], double Fs,
96 creal_T hh[2048]);
97 static void c_generateCascadedResponseRx(const char enables[4], const
98 emxArray_real_T *w, double Fs, const double hb1_coeff[15], const double
99 hb2_coeff[7], const double hb3_coeff_data[], const int hb3_coeff_size[2],
100 const double dec_int3_coeff_data[], const int dec_int3_coeff_size[2], const
101 emxArray_real_T *extraTaps, emxArray_creal_T *combinedResponse);
102 static void c_polyval(const double p_data[], const int p_size[2], const creal_T
103 x[2048], creal_T y[2048]);
104 static void c_power(const emxArray_real_T *a, emxArray_real_T *y);
105 static void c_rdivide(const emxArray_real_T *x, const emxArray_real_T *y,
106 emxArray_real_T *z);
107 static void c_sqrt(creal_T *x);
108 static boolean_T c_strcmp(const char a[2]);
109 static double c_sum(const double x[128]);
110 static void c_us(const double o_data[], const int o_size[2], double u_data[],
111 int u_size[2]);
112 static double dBinv(double dBinput);
113 static void d_abs(const double x[128], double y[128]);
114 static void d_analogresp(const emxArray_real_T *f, double Fconverter, const
115 double b1_data[], const int b1_size[2], const emxArray_creal_T *a1, const
116 double b2_data[], const int b2_size[2], const emxArray_creal_T *a2,
117 emxArray_creal_T *abc);
118 static void d_firfreqz(double b_data[], int b_size[2], const struct_T *options,
119 creal_T h[2048], double w[2048]);
120 static void d_freqz_cg(const double b_data[], const int b_size[2], const double
121 w[2048], double Fs, creal_T hh[2048]);
122 static void d_polyval(const double p[29], const creal_T x[2048],
123 creal_T y[2048]);
124 static void d_us(const double o[15], double u[29]);
125 static double db2mag(double ydb);
126 static void determineBestFractionLength(const emxArray_real_T *tap_store, double
127 i, double M, emxArray_real_T *taps);
128 static int div_s32_floor(int numerator, int denominator);
129 static void e_firfreqz(const double b[29], const struct_T *options, creal_T h
130 [2048], double w[2048]);
131 static void e_freqz_cg(const double b[29], const double w[2048], double Fs,
132 creal_T hh[2048]);
133 static void e_polyval(const double p[13], const creal_T x[2048],
134 creal_T y[2048]);
135 static void e_us(const double o[7], double u[13]);
136 static void eig(const emxArray_creal_T *A, emxArray_creal_T *V);
137 static int eml_zlahqr(emxArray_creal_T *h);
138 static void f_firfreqz(const double b[13], const struct_T *options, creal_T h
139 [2048], double w[2048]);
140 static void f_freqz_cg(const double b[13], const double w[2048], double Fs,
141 creal_T hh[2048]);
142 static void f_polyval(const double p[57], const creal_T x[2048],
143 creal_T y[2048]);
144 static void f_us(const double o[15], double u[57]);
145 static void firfreqz(const struct_T *options, creal_T h[2048], double w[2048]);
146 static void firpm_cg(double order, const double ff[4], const emxArray_real_T
147 *amplitudes, const emxArray_real_T *frequencies, const
148 emxArray_real_T *weights, emxArray_real_T *h);
149 static void firpmgrid_cg(double nfilt, const double ff[4], emxArray_real_T
150 *gridactual);
151 static void freqs_cg(const double b_data[], const int b_size[2], const
152 emxArray_creal_T *a, const double w[2048], creal_T h[2048]);
153 static void freqz_cg(const double w[2048], double Fs, creal_T hh[2048]);
154 static void g_firfreqz(const double b[57], const struct_T *options, creal_T h
155 [2048], double w[2048]);
156 static void g_freqz_cg(const double b[57], const double w[2048], double Fs,
157 creal_T hh[2048]);
158 static void g_polyval(const double p[43], const creal_T x[2048],
159 creal_T y[2048]);
160 static void g_us(const double o[15], double u[43]);
161 static void generateCascadedResponseRx(const char enables[4], const double w
162 [2048], double Fs, const double hb1_coeff[15], const double hb2_coeff[7],
163 const double hb3_coeff_data[], const int hb3_coeff_size[2], const double
164 dec_int3_coeff_data[], const int dec_int3_coeff_size[2], creal_T
165 combinedResponse[2048]);
166 static void h_firfreqz(const double b[43], const struct_T *options, creal_T h
167 [2048], double w[2048]);
168 static void h_freqz_cg(const double b[43], const double w[2048], double Fs,
169 creal_T hh[2048]);
170 static void h_polyval(const double p[19], const creal_T x[2048],
171 creal_T y[2048]);
172 static void h_us(const double o[7], double u[19]);
173 static void i_firfreqz(const double b[19], const struct_T *options, creal_T h
174 [2048], double w[2048]);
175 static void i_freqz_cg(const double b[19], const double w[2048], double Fs,
176 creal_T hh[2048]);
177 static void i_polyval(const double p[85], const creal_T x[2048],
178 creal_T y[2048]);
179 static void i_us(const double o[15], double u[85]);
180 static void interp1(const emxArray_real_T *varargin_1, const emxArray_real_T
181 *varargin_2, const emxArray_real_T *varargin_3,
182 emxArray_real_T *Vq);
183 static void j_firfreqz(const double b[85], const struct_T *options, creal_T h
184 [2048], double w[2048]);
185 static void j_freqz_cg(const double b[85], const double w[2048], double Fs,
186 creal_T hh[2048]);
187 static void j_polyval(const emxArray_real_T *p, const emxArray_creal_T *x,
188 emxArray_creal_T *y);
189 static void k_freqz_cg(const emxArray_real_T *w, double Fs,
190 emxArray_creal_T *hh);
191 static void l_freqz_cg(const double b[15], const emxArray_real_T *w, double Fs,
192 emxArray_creal_T *hh);
193 static void lp2lp_cg(const emxArray_creal_T *a, const emxArray_real_T *b, double
194 wo, emxArray_creal_T *at, emxArray_real_T *bt, double *dt);
195 static void m_freqz_cg(const double b[7], const emxArray_real_T *w, double Fs,
196 emxArray_creal_T *hh);
197 static double mag2db(double y);
198 static double mpower(double a, double b);
199 static void n_freqz_cg(const emxArray_real_T *b, const emxArray_real_T *w,
200 double Fs, emxArray_creal_T *hh);
201 static void o_freqz_cg(const double b[29], const emxArray_real_T *w, double Fs,
202 emxArray_creal_T *hh);
203 static void p_freqz_cg(const double b[13], const emxArray_real_T *w, double Fs,
204 emxArray_creal_T *hh);
205 static void poly(const emxArray_creal_T *x, emxArray_creal_T *c);
206 static void polyval(const double p[15], const creal_T x[2048], creal_T y[2048]);
207 static void power(const double a[2048], double y[2048]);
208 static void q_freqz_cg(const double b[57], const emxArray_real_T *w, double Fs,
209 emxArray_creal_T *hh);
210 static void r_freqz_cg(const double b[43], const emxArray_real_T *w, double Fs,
211 emxArray_creal_T *hh);
212 static void rdivide(const emxArray_real_T *x, double y, emxArray_real_T *z);
213 static creal_T recip(const creal_T y);
214 static double remezdd(double k, double n, double m, const emxArray_real_T *x);
215 static void remezm(double nfilt, const double edge[4], const emxArray_real_T
216 *grid, emxArray_real_T *des, emxArray_real_T *wt,
217 emxArray_real_T *h, double *dev, boolean_T *valid);
218 static void removeTrailingZero(const double b_data[], const int b_size[2], const
219 emxArray_creal_T *a, double bR_data[], int bR_size[2], emxArray_creal_T *aR);
220 static double rt_atan2d_snf(double u0, double u1);
221 static double rt_hypotd_snf(double u0, double u1);
222 static double rt_powd_snf(double u0, double u1);
223 static double rt_remd_snf(double u0, double u1);
224 static double rt_roundd_snf(double u);
225 static void s_freqz_cg(const double b[19], const emxArray_real_T *w, double Fs,
226 emxArray_creal_T *hh);
227 static void sinc(double x[2048]);
228 static double sum(const double x[2048]);
229 static void t_freqz_cg(const double b[85], const emxArray_real_T *w, double Fs,
230 emxArray_creal_T *hh);
231 static void us(const double o[15], double u[15]);
232 static void vector_poly(const emxArray_creal_T *x, emxArray_creal_T *c);
233 static double xdlapy3(double x1, double x2, double x3);
234 static void xgehrd(emxArray_creal_T *a);
235 static double xnrm2(int n, const emxArray_creal_T *x, int ix0);
236 static void xscal(int n, const creal_T a, emxArray_creal_T *x, int ix0);
237 static void xzgeev(const emxArray_creal_T *A, int *info, emxArray_creal_T
238 *alpha1, emxArray_creal_T *beta1);
239 static void xzhgeqz(const emxArray_creal_T *A, int ilo, int ihi, int *info,
240 emxArray_creal_T *alpha1, emxArray_creal_T *beta1);
241 static creal_T xzlarfg(creal_T *alpha1, creal_T *x);
242 static void xzlartg(const creal_T f, const creal_T g, double *cs, creal_T *sn,
243 creal_T *r);
244 static void zp2ss_cg(emxArray_creal_T *a, emxArray_real_T *b,
245 emxArray_real_T *c,
246 double *d);
247
248 /* Function Definitions */
249
250 /*
251 * UNTITLED Summary of this function goes here
252 * Detailed explanation goes here
253 * Arguments : const char type[2]
254 * const double f[2048]
255 * double Fconverter
256 * const double b1_data[]
257 * const int b1_size[2]
258 * const emxArray_creal_T *a1
259 * const double b2_data[]
260 * const int b2_size[2]
261 * const emxArray_creal_T *a2
262 * creal_T abc[2048]
263 * Return Type : void
264 */
analogresp(const char type[2],const double f[2048],double Fconverter,const double b1_data[],const int b1_size[2],const emxArray_creal_T * a1,const double b2_data[],const int b2_size[2],const emxArray_creal_T * a2,creal_T abc[2048])265 static void analogresp(const char type[2], const double f[2048], double
266 Fconverter, const double b1_data[], const int b1_size[2], const
267 emxArray_creal_T *a1, const double b2_data[], const int b2_size[2], const
268 emxArray_creal_T *a2, creal_T abc[2048])
269 {
270 boolean_T b_bool;
271 int kstr;
272 int exitg1;
273 static const char cv33[2] = { 'T', 'x' };
274
275 static const char cv34[2] = { 'R', 'x' };
276
277 double dv14[2048];
278 double dv15[2048];
279 static creal_T dcv1[2048];
280 double abc_im;
281 double im;
282 b_bool = false;
283 kstr = 0;
284 do {
285 exitg1 = 0;
286 if (kstr + 1 < 3) {
287 if (type[kstr] != cv33[kstr]) {
288 exitg1 = 1;
289 } else {
290 kstr++;
291 }
292 } else {
293 b_bool = true;
294 exitg1 = 1;
295 }
296 } while (exitg1 == 0);
297
298 if (b_bool) {
299 kstr = 0;
300 } else {
301 b_bool = false;
302 kstr = 0;
303 do {
304 exitg1 = 0;
305 if (kstr + 1 < 3) {
306 if (type[kstr] != cv34[kstr]) {
307 exitg1 = 1;
308 } else {
309 kstr++;
310 }
311 } else {
312 b_bool = true;
313 exitg1 = 1;
314 }
315 } while (exitg1 == 0);
316
317 if (b_bool) {
318 kstr = 1;
319 } else {
320 kstr = -1;
321 }
322 }
323
324 switch (kstr) {
325 case 0:
326 for (kstr = 0; kstr < 2048; kstr++) {
327 dv14[kstr] = f[kstr] / Fconverter;
328 }
329
330 sinc(dv14);
331 for (kstr = 0; kstr < 2048; kstr++) {
332 dv15[kstr] = 6.2831853071795862 * f[kstr];
333 }
334
335 freqs_cg(b1_data, b1_size, a1, dv15, abc);
336 for (kstr = 0; kstr < 2048; kstr++) {
337 dv15[kstr] = 6.2831853071795862 * f[kstr];
338 }
339
340 freqs_cg(b2_data, b2_size, a2, dv15, dcv1);
341 for (kstr = 0; kstr < 2048; kstr++) {
342 abc_im = dv14[kstr] * abc[kstr].re;
343 im = dv14[kstr] * abc[kstr].im;
344 abc[kstr].re = abc_im * dcv1[kstr].re - im * dcv1[kstr].im;
345 abc[kstr].im = abc_im * dcv1[kstr].im + im * dcv1[kstr].re;
346 }
347 break;
348
349 case 1:
350 for (kstr = 0; kstr < 2048; kstr++) {
351 dv14[kstr] = 6.2831853071795862 * f[kstr];
352 }
353
354 freqs_cg(b1_data, b1_size, a1, dv14, abc);
355 for (kstr = 0; kstr < 2048; kstr++) {
356 dv14[kstr] = 6.2831853071795862 * f[kstr];
357 }
358
359 freqs_cg(b2_data, b2_size, a2, dv14, dcv1);
360 for (kstr = 0; kstr < 2048; kstr++) {
361 dv14[kstr] = f[kstr] / Fconverter;
362 }
363
364 sinc(dv14);
365 power(dv14, dv15);
366 for (kstr = 0; kstr < 2048; kstr++) {
367 abc_im = abc[kstr].re * dcv1[kstr].im + abc[kstr].im * dcv1[kstr].re;
368 abc[kstr].re = dv15[kstr] * (abc[kstr].re * dcv1[kstr].re - abc[kstr].im *
369 dcv1[kstr].im);
370 abc[kstr].im = dv15[kstr] * abc_im;
371 }
372 break;
373
374 default:
375 /* Default to Rx */
376 for (kstr = 0; kstr < 2048; kstr++) {
377 dv14[kstr] = 6.2831853071795862 * f[kstr];
378 }
379
380 freqs_cg(b1_data, b1_size, a1, dv14, abc);
381 for (kstr = 0; kstr < 2048; kstr++) {
382 dv14[kstr] = 6.2831853071795862 * f[kstr];
383 }
384
385 freqs_cg(b2_data, b2_size, a2, dv14, dcv1);
386 for (kstr = 0; kstr < 2048; kstr++) {
387 dv14[kstr] = f[kstr] / Fconverter;
388 }
389
390 sinc(dv14);
391 power(dv14, dv15);
392 for (kstr = 0; kstr < 2048; kstr++) {
393 abc_im = abc[kstr].re * dcv1[kstr].im + abc[kstr].im * dcv1[kstr].re;
394 abc[kstr].re = dv15[kstr] * (abc[kstr].re * dcv1[kstr].re - abc[kstr].im *
395 dcv1[kstr].im);
396 abc[kstr].im = dv15[kstr] * abc_im;
397 }
398 break;
399 }
400 }
401
402 /*
403 * Arguments : const emxArray_creal_T *x
404 * Return Type : boolean_T
405 */
anyNonFinite(const emxArray_creal_T * x)406 static boolean_T anyNonFinite(const emxArray_creal_T *x)
407 {
408 boolean_T p;
409 int nx;
410 int k;
411 nx = x->size[0] * x->size[1];
412 p = true;
413 for (k = 0; k + 1 <= nx; k++) {
414 if (p && ((!(rtIsInf(x->data[k].re) || rtIsInf(x->data[k].im))) &&
415 (!(rtIsNaN(x->data[k].re) || rtIsNaN(x->data[k].im))))) {
416 p = true;
417 } else {
418 p = false;
419 }
420 }
421
422 return !p;
423 }
424
425 /*
426 * Arguments : const emxArray_creal_T *x
427 * emxArray_real_T *y
428 * Return Type : void
429 */
b_abs(const emxArray_creal_T * x,emxArray_real_T * y)430 static void b_abs(const emxArray_creal_T *x, emxArray_real_T *y)
431 {
432 int k;
433 k = y->size[0] * y->size[1];
434 y->size[0] = 1;
435 y->size[1] = x->size[1];
436 emxEnsureCapacity_real_T(y, k);
437 for (k = 0; k + 1 <= x->size[1]; k++) {
438 y->data[k] = rt_hypotd_snf(x->data[k].re, x->data[k].im);
439 }
440 }
441
442 /*
443 * Arguments : double *x
444 * Return Type : void
445 */
b_acos(double * x)446 static void b_acos(double *x)
447 {
448 *x = acos(*x);
449 }
450
451 /*
452 * UNTITLED Summary of this function goes here
453 * Detailed explanation goes here
454 * Arguments : const char type[2]
455 * const emxArray_real_T *f
456 * double Fconverter
457 * const double b1_data[]
458 * const int b1_size[2]
459 * const emxArray_creal_T *a1
460 * const double b2_data[]
461 * const int b2_size[2]
462 * const emxArray_creal_T *a2
463 * emxArray_creal_T *abc
464 * Return Type : void
465 */
b_analogresp(const char type[2],const emxArray_real_T * f,double Fconverter,const double b1_data[],const int b1_size[2],const emxArray_creal_T * a1,const double b2_data[],const int b2_size[2],const emxArray_creal_T * a2,emxArray_creal_T * abc)466 static void b_analogresp(const char type[2], const emxArray_real_T *f, double
467 Fconverter, const double b1_data[], const int b1_size[2], const
468 emxArray_creal_T *a1, const double b2_data[], const int b2_size[2], const
469 emxArray_creal_T *a2, emxArray_creal_T *abc)
470 {
471 boolean_T b_bool;
472 int kstr;
473 int exitg1;
474 static const char cv46[2] = { 'T', 'x' };
475
476 emxArray_creal_T *r17;
477 emxArray_real_T *r18;
478 emxArray_real_T *r19;
479 static const char cv47[2] = { 'R', 'x' };
480
481 int loop_ub;
482 double abc_re;
483 double abc_im;
484 double re;
485 double im;
486 b_bool = false;
487 kstr = 0;
488 do {
489 exitg1 = 0;
490 if (kstr + 1 < 3) {
491 if (type[kstr] != cv46[kstr]) {
492 exitg1 = 1;
493 } else {
494 kstr++;
495 }
496 } else {
497 b_bool = true;
498 exitg1 = 1;
499 }
500 } while (exitg1 == 0);
501
502 if (b_bool) {
503 kstr = 0;
504 } else {
505 b_bool = false;
506 kstr = 0;
507 do {
508 exitg1 = 0;
509 if (kstr + 1 < 3) {
510 if (type[kstr] != cv47[kstr]) {
511 exitg1 = 1;
512 } else {
513 kstr++;
514 }
515 } else {
516 b_bool = true;
517 exitg1 = 1;
518 }
519 } while (exitg1 == 0);
520
521 if (b_bool) {
522 kstr = 1;
523 } else {
524 kstr = -1;
525 }
526 }
527
528 emxInit_creal_T(&r17, 2);
529 emxInit_real_T(&r18, 2);
530 emxInit_real_T(&r19, 2);
531 switch (kstr) {
532 case 0:
533 rdivide(f, Fconverter, r19);
534 b_sinc(r19);
535 kstr = r18->size[0] * r18->size[1];
536 r18->size[0] = 1;
537 r18->size[1] = f->size[1];
538 emxEnsureCapacity_real_T(r18, kstr);
539 loop_ub = f->size[0] * f->size[1];
540 for (kstr = 0; kstr < loop_ub; kstr++) {
541 r18->data[kstr] = 6.2831853071795862 * f->data[kstr];
542 }
543
544 b_freqs_cg(b1_data, b1_size, a1, r18, abc);
545 kstr = r18->size[0] * r18->size[1];
546 r18->size[0] = 1;
547 r18->size[1] = f->size[1];
548 emxEnsureCapacity_real_T(r18, kstr);
549 loop_ub = f->size[0] * f->size[1];
550 for (kstr = 0; kstr < loop_ub; kstr++) {
551 r18->data[kstr] = 6.2831853071795862 * f->data[kstr];
552 }
553
554 b_freqs_cg(b2_data, b2_size, a2, r18, r17);
555 kstr = abc->size[0] * abc->size[1];
556 abc->size[0] = 1;
557 abc->size[1] = r19->size[1];
558 emxEnsureCapacity_creal_T(abc, kstr);
559 loop_ub = r19->size[0] * r19->size[1];
560 for (kstr = 0; kstr < loop_ub; kstr++) {
561 abc_re = r19->data[kstr] * abc->data[kstr].re;
562 abc_im = r19->data[kstr] * abc->data[kstr].im;
563 re = r17->data[kstr].re;
564 im = r17->data[kstr].im;
565 abc->data[kstr].re = abc_re * re - abc_im * im;
566 abc->data[kstr].im = abc_re * im + abc_im * re;
567 }
568 break;
569
570 case 1:
571 kstr = r19->size[0] * r19->size[1];
572 r19->size[0] = 1;
573 r19->size[1] = f->size[1];
574 emxEnsureCapacity_real_T(r19, kstr);
575 loop_ub = f->size[0] * f->size[1];
576 for (kstr = 0; kstr < loop_ub; kstr++) {
577 r19->data[kstr] = 6.2831853071795862 * f->data[kstr];
578 }
579
580 b_freqs_cg(b1_data, b1_size, a1, r19, abc);
581 kstr = r19->size[0] * r19->size[1];
582 r19->size[0] = 1;
583 r19->size[1] = f->size[1];
584 emxEnsureCapacity_real_T(r19, kstr);
585 loop_ub = f->size[0] * f->size[1];
586 for (kstr = 0; kstr < loop_ub; kstr++) {
587 r19->data[kstr] = 6.2831853071795862 * f->data[kstr];
588 }
589
590 b_freqs_cg(b2_data, b2_size, a2, r19, r17);
591 rdivide(f, Fconverter, r19);
592 b_sinc(r19);
593 c_power(r19, r18);
594 kstr = abc->size[0] * abc->size[1];
595 abc->size[0] = 1;
596 emxEnsureCapacity_creal_T(abc, kstr);
597 kstr = abc->size[0];
598 loop_ub = abc->size[1];
599 loop_ub *= kstr;
600 for (kstr = 0; kstr < loop_ub; kstr++) {
601 abc_re = abc->data[kstr].re * r17->data[kstr].re - abc->data[kstr].im *
602 r17->data[kstr].im;
603 abc_im = abc->data[kstr].re * r17->data[kstr].im + abc->data[kstr].im *
604 r17->data[kstr].re;
605 abc->data[kstr].re = r18->data[kstr] * abc_re;
606 abc->data[kstr].im = r18->data[kstr] * abc_im;
607 }
608 break;
609
610 default:
611 /* Default to Rx */
612 kstr = r19->size[0] * r19->size[1];
613 r19->size[0] = 1;
614 r19->size[1] = f->size[1];
615 emxEnsureCapacity_real_T(r19, kstr);
616 loop_ub = f->size[0] * f->size[1];
617 for (kstr = 0; kstr < loop_ub; kstr++) {
618 r19->data[kstr] = 6.2831853071795862 * f->data[kstr];
619 }
620
621 b_freqs_cg(b1_data, b1_size, a1, r19, abc);
622 kstr = r19->size[0] * r19->size[1];
623 r19->size[0] = 1;
624 r19->size[1] = f->size[1];
625 emxEnsureCapacity_real_T(r19, kstr);
626 loop_ub = f->size[0] * f->size[1];
627 for (kstr = 0; kstr < loop_ub; kstr++) {
628 r19->data[kstr] = 6.2831853071795862 * f->data[kstr];
629 }
630
631 b_freqs_cg(b2_data, b2_size, a2, r19, r17);
632 rdivide(f, Fconverter, r19);
633 b_sinc(r19);
634 c_power(r19, r18);
635 kstr = abc->size[0] * abc->size[1];
636 abc->size[0] = 1;
637 emxEnsureCapacity_creal_T(abc, kstr);
638 kstr = abc->size[0];
639 loop_ub = abc->size[1];
640 loop_ub *= kstr;
641 for (kstr = 0; kstr < loop_ub; kstr++) {
642 abc_re = abc->data[kstr].re * r17->data[kstr].re - abc->data[kstr].im *
643 r17->data[kstr].im;
644 abc_im = abc->data[kstr].re * r17->data[kstr].im + abc->data[kstr].im *
645 r17->data[kstr].re;
646 abc->data[kstr].re = r18->data[kstr] * abc_re;
647 abc->data[kstr].im = r18->data[kstr] * abc_im;
648 }
649 break;
650 }
651
652 emxFree_real_T(&r19);
653 emxFree_real_T(&r18);
654 emxFree_creal_T(&r17);
655 }
656
657 /*
658 * BUTTER_CG Butterworth digital and analog filter design. Codegen support
659 *
660 * This function is based on 'butter' by The MathWorks Inc.
661 * Arguments : double Wn
662 * double num[4]
663 * emxArray_creal_T *den
664 * Return Type : void
665 */
b_butter_cg(double Wn,double num[4],emxArray_creal_T * den)666 static void b_butter_cg(double Wn, double num[4], emxArray_creal_T *den)
667 {
668 emxArray_creal_T *a;
669 emxArray_real_T *b;
670 emxArray_real_T *c;
671 emxArray_creal_T *b_a;
672 emxArray_real_T *b_b;
673 double d;
674 double y_re;
675 double y_im;
676 int k;
677 static const double c_a[4] = { 0.0, 0.0, 0.0, 0.037037037037037035 };
678
679 double ai;
680 emxInit_creal_T(&a, 2);
681 emxInit_real_T1(&b, 1);
682 emxInit_real_T(&c, 2);
683 emxInit_creal_T(&b_a, 2);
684 emxInit_real_T1(&b_b, 1);
685
686 /* Set types we will only use */
687 /* Cast to enforce precision rules */
688 /* step 1: get analog, pre-warped frequencies */
689 /* step 2: convert to low-pass prototype estimate */
690 /* lowpass */
691 /* Cast to enforce precision rules */
692 /* step 3: Get N-th order Butterworth analog lowpass prototype */
693 /* Transform to state-space */
694 zp2ss_cg(a, b, c, &d);
695
696 /* step 4: Transform to lowpass, bandpass, highpass, or bandstop of desired Wn */
697 /* Lowpass */
698 lp2lp_cg(a, b, Wn, b_a, b_b, &d);
699
700 /* step 5: Use Bilinear transformation to find discrete equivalent: */
701 /* nargout <= 3 */
702 /* Transform to zero-pole-gain and polynomial forms: */
703 /* nargout <= 2 */
704 poly(b_a, den);
705
706 /* This internal function returns more exact numerator vectors */
707 /* for the num/den case. */
708 /* Wn input is two element band edge vector */
709 /* --------------------------------- */
710 /* lowpass */
711 y_re = den->data[0].re;
712 y_im = den->data[0].im;
713 k = 0;
714 emxFree_real_T(&b_b);
715 emxFree_creal_T(&b_a);
716 emxFree_real_T(&c);
717 emxFree_real_T(&b);
718 emxFree_creal_T(&a);
719 while (k <= den->size[1] - 2) {
720 d = 0.0 * y_im + 0.0 * y_re;
721 y_re = (0.0 * y_re - 0.0 * y_im) + den->data[k + 1].re;
722 y_im = d + den->data[k + 1].im;
723 k++;
724 }
725
726 for (k = 0; k < 4; k++) {
727 d = c_a[k] * y_re;
728 ai = c_a[k] * y_im;
729 if (ai == 0.0) {
730 d /= 0.037037037037037035;
731 } else if (d == 0.0) {
732 d = 0.0;
733 } else {
734 d /= 0.037037037037037035;
735 }
736
737 num[k] = d;
738 }
739
740 /* num = poly(a-b*c)+(d-1)*den; */
741 }
742
743 /*
744 * Arguments : emxArray_real_T *x
745 * Return Type : void
746 */
b_cos(emxArray_real_T * x)747 static void b_cos(emxArray_real_T *x)
748 {
749 int nx;
750 int k;
751 nx = x->size[1];
752 for (k = 0; k + 1 <= nx; k++) {
753 x->data[k] = cos(x->data[k]);
754 }
755 }
756
757 /*
758 * Codegen workaround for fixed fi call requirements
759 * Arguments : const double tap_store[128]
760 * double taps[128]
761 * Return Type : void
762 */
b_determineBestFractionLength(const double tap_store[128],double taps[128])763 static void b_determineBestFractionLength(const double tap_store[128], double
764 taps[128])
765 {
766 double r[2048];
767 int ixstart;
768 double b_r[128];
769 double dv16[128];
770 double u;
771 double e[16];
772 double v;
773 double dv17[128];
774 short i57;
775 double dv18[128];
776 double dv19[128];
777 double dv20[128];
778 double dv21[128];
779 double dv22[128];
780 double dv23[128];
781 double dv24[128];
782 double dv25[128];
783 double dv26[128];
784 double dv27[128];
785 double dv28[128];
786 double dv29[128];
787 double dv30[128];
788 double dv31[128];
789 int itmp;
790 int ix;
791 boolean_T exitg1;
792 memset(&r[0], 0, sizeof(double) << 11);
793 for (ixstart = 0; ixstart < 128; ixstart++) {
794 u = tap_store[ixstart] * 2.0;
795 v = fabs(u);
796 if (v < 4.503599627370496E+15) {
797 if (v >= 0.5) {
798 u = floor(u + 0.5);
799 } else {
800 u *= 0.0;
801 }
802 }
803
804 if (u < 32768.0) {
805 if (u >= -32768.0) {
806 i57 = (short)u;
807 } else {
808 i57 = MIN_int16_T;
809 }
810 } else if (u >= 32768.0) {
811 i57 = MAX_int16_T;
812 } else {
813 i57 = 0;
814 }
815
816 r[ixstart << 4] = (double)i57 * 0.5;
817 b_r[ixstart] = r[ixstart << 4] - tap_store[ixstart];
818 u = tap_store[ixstart] * 4.0;
819 v = fabs(u);
820 if (v < 4.503599627370496E+15) {
821 if (v >= 0.5) {
822 u = floor(u + 0.5);
823 } else {
824 u *= 0.0;
825 }
826 }
827
828 if (u < 32768.0) {
829 if (u >= -32768.0) {
830 i57 = (short)u;
831 } else {
832 i57 = MIN_int16_T;
833 }
834 } else if (u >= 32768.0) {
835 i57 = MAX_int16_T;
836 } else {
837 i57 = 0;
838 }
839
840 r[1 + (ixstart << 4)] = (double)i57 * 0.25;
841 }
842
843 d_abs(b_r, dv16);
844 e[0] = c_sum(dv16);
845 for (ixstart = 0; ixstart < 128; ixstart++) {
846 b_r[ixstart] = r[1 + (ixstart << 4)] - tap_store[ixstart];
847 u = tap_store[ixstart] * 8.0;
848 v = fabs(u);
849 if (v < 4.503599627370496E+15) {
850 if (v >= 0.5) {
851 u = floor(u + 0.5);
852 } else {
853 u *= 0.0;
854 }
855 }
856
857 if (u < 32768.0) {
858 if (u >= -32768.0) {
859 i57 = (short)u;
860 } else {
861 i57 = MIN_int16_T;
862 }
863 } else if (u >= 32768.0) {
864 i57 = MAX_int16_T;
865 } else {
866 i57 = 0;
867 }
868
869 r[2 + (ixstart << 4)] = (double)i57 * 0.125;
870 }
871
872 d_abs(b_r, dv17);
873 e[1] = c_sum(dv17);
874 for (ixstart = 0; ixstart < 128; ixstart++) {
875 b_r[ixstart] = r[2 + (ixstart << 4)] - tap_store[ixstart];
876 u = tap_store[ixstart] * 16.0;
877 v = fabs(u);
878 if (v < 4.503599627370496E+15) {
879 if (v >= 0.5) {
880 u = floor(u + 0.5);
881 } else {
882 u *= 0.0;
883 }
884 }
885
886 if (u < 32768.0) {
887 if (u >= -32768.0) {
888 i57 = (short)u;
889 } else {
890 i57 = MIN_int16_T;
891 }
892 } else if (u >= 32768.0) {
893 i57 = MAX_int16_T;
894 } else {
895 i57 = 0;
896 }
897
898 r[3 + (ixstart << 4)] = (double)i57 * 0.0625;
899 }
900
901 d_abs(b_r, dv18);
902 e[2] = c_sum(dv18);
903 for (ixstart = 0; ixstart < 128; ixstart++) {
904 b_r[ixstart] = r[3 + (ixstart << 4)] - tap_store[ixstart];
905 u = tap_store[ixstart] * 32.0;
906 v = fabs(u);
907 if (v < 4.503599627370496E+15) {
908 if (v >= 0.5) {
909 u = floor(u + 0.5);
910 } else {
911 u *= 0.0;
912 }
913 }
914
915 if (u < 32768.0) {
916 if (u >= -32768.0) {
917 i57 = (short)u;
918 } else {
919 i57 = MIN_int16_T;
920 }
921 } else if (u >= 32768.0) {
922 i57 = MAX_int16_T;
923 } else {
924 i57 = 0;
925 }
926
927 r[4 + (ixstart << 4)] = (double)i57 * 0.03125;
928 }
929
930 d_abs(b_r, dv19);
931 e[3] = c_sum(dv19);
932 for (ixstart = 0; ixstart < 128; ixstart++) {
933 b_r[ixstart] = r[4 + (ixstart << 4)] - tap_store[ixstart];
934 u = tap_store[ixstart] * 64.0;
935 v = fabs(u);
936 if (v < 4.503599627370496E+15) {
937 if (v >= 0.5) {
938 u = floor(u + 0.5);
939 } else {
940 u *= 0.0;
941 }
942 }
943
944 if (u < 32768.0) {
945 if (u >= -32768.0) {
946 i57 = (short)u;
947 } else {
948 i57 = MIN_int16_T;
949 }
950 } else if (u >= 32768.0) {
951 i57 = MAX_int16_T;
952 } else {
953 i57 = 0;
954 }
955
956 r[5 + (ixstart << 4)] = (double)i57 * 0.015625;
957 }
958
959 d_abs(b_r, dv20);
960 e[4] = c_sum(dv20);
961 for (ixstart = 0; ixstart < 128; ixstart++) {
962 b_r[ixstart] = r[5 + (ixstart << 4)] - tap_store[ixstart];
963 u = tap_store[ixstart] * 128.0;
964 v = fabs(u);
965 if (v < 4.503599627370496E+15) {
966 if (v >= 0.5) {
967 u = floor(u + 0.5);
968 } else {
969 u *= 0.0;
970 }
971 }
972
973 if (u < 32768.0) {
974 if (u >= -32768.0) {
975 i57 = (short)u;
976 } else {
977 i57 = MIN_int16_T;
978 }
979 } else if (u >= 32768.0) {
980 i57 = MAX_int16_T;
981 } else {
982 i57 = 0;
983 }
984
985 r[6 + (ixstart << 4)] = (double)i57 * 0.0078125;
986 }
987
988 d_abs(b_r, dv21);
989 e[5] = c_sum(dv21);
990 for (ixstart = 0; ixstart < 128; ixstart++) {
991 b_r[ixstart] = r[6 + (ixstart << 4)] - tap_store[ixstart];
992 u = tap_store[ixstart] * 256.0;
993 v = fabs(u);
994 if (v < 4.503599627370496E+15) {
995 if (v >= 0.5) {
996 u = floor(u + 0.5);
997 } else {
998 u *= 0.0;
999 }
1000 }
1001
1002 if (u < 32768.0) {
1003 if (u >= -32768.0) {
1004 i57 = (short)u;
1005 } else {
1006 i57 = MIN_int16_T;
1007 }
1008 } else if (u >= 32768.0) {
1009 i57 = MAX_int16_T;
1010 } else {
1011 i57 = 0;
1012 }
1013
1014 r[7 + (ixstart << 4)] = (double)i57 * 0.00390625;
1015 }
1016
1017 d_abs(b_r, dv22);
1018 e[6] = c_sum(dv22);
1019 for (ixstart = 0; ixstart < 128; ixstart++) {
1020 b_r[ixstart] = r[7 + (ixstart << 4)] - tap_store[ixstart];
1021 u = tap_store[ixstart] * 512.0;
1022 v = fabs(u);
1023 if (v < 4.503599627370496E+15) {
1024 if (v >= 0.5) {
1025 u = floor(u + 0.5);
1026 } else {
1027 u *= 0.0;
1028 }
1029 }
1030
1031 if (u < 32768.0) {
1032 if (u >= -32768.0) {
1033 i57 = (short)u;
1034 } else {
1035 i57 = MIN_int16_T;
1036 }
1037 } else if (u >= 32768.0) {
1038 i57 = MAX_int16_T;
1039 } else {
1040 i57 = 0;
1041 }
1042
1043 r[8 + (ixstart << 4)] = (double)i57 * 0.001953125;
1044 }
1045
1046 d_abs(b_r, dv23);
1047 e[7] = c_sum(dv23);
1048 for (ixstart = 0; ixstart < 128; ixstart++) {
1049 b_r[ixstart] = r[8 + (ixstart << 4)] - tap_store[ixstart];
1050 u = tap_store[ixstart] * 1024.0;
1051 v = fabs(u);
1052 if (v < 4.503599627370496E+15) {
1053 if (v >= 0.5) {
1054 u = floor(u + 0.5);
1055 } else {
1056 u *= 0.0;
1057 }
1058 }
1059
1060 if (u < 32768.0) {
1061 if (u >= -32768.0) {
1062 i57 = (short)u;
1063 } else {
1064 i57 = MIN_int16_T;
1065 }
1066 } else if (u >= 32768.0) {
1067 i57 = MAX_int16_T;
1068 } else {
1069 i57 = 0;
1070 }
1071
1072 r[9 + (ixstart << 4)] = (double)i57 * 0.0009765625;
1073 }
1074
1075 d_abs(b_r, dv24);
1076 e[8] = c_sum(dv24);
1077 for (ixstart = 0; ixstart < 128; ixstart++) {
1078 b_r[ixstart] = r[9 + (ixstart << 4)] - tap_store[ixstart];
1079 u = tap_store[ixstart] * 2048.0;
1080 v = fabs(u);
1081 if (v < 4.503599627370496E+15) {
1082 if (v >= 0.5) {
1083 u = floor(u + 0.5);
1084 } else {
1085 u *= 0.0;
1086 }
1087 }
1088
1089 if (u < 32768.0) {
1090 if (u >= -32768.0) {
1091 i57 = (short)u;
1092 } else {
1093 i57 = MIN_int16_T;
1094 }
1095 } else if (u >= 32768.0) {
1096 i57 = MAX_int16_T;
1097 } else {
1098 i57 = 0;
1099 }
1100
1101 r[10 + (ixstart << 4)] = (double)i57 * 0.00048828125;
1102 }
1103
1104 d_abs(b_r, dv25);
1105 e[9] = c_sum(dv25);
1106 for (ixstart = 0; ixstart < 128; ixstart++) {
1107 b_r[ixstart] = r[10 + (ixstart << 4)] - tap_store[ixstart];
1108 u = tap_store[ixstart] * 4096.0;
1109 v = fabs(u);
1110 if (v < 4.503599627370496E+15) {
1111 if (v >= 0.5) {
1112 u = floor(u + 0.5);
1113 } else {
1114 u *= 0.0;
1115 }
1116 }
1117
1118 if (u < 32768.0) {
1119 if (u >= -32768.0) {
1120 i57 = (short)u;
1121 } else {
1122 i57 = MIN_int16_T;
1123 }
1124 } else if (u >= 32768.0) {
1125 i57 = MAX_int16_T;
1126 } else {
1127 i57 = 0;
1128 }
1129
1130 r[11 + (ixstart << 4)] = (double)i57 * 0.000244140625;
1131 }
1132
1133 d_abs(b_r, dv26);
1134 e[10] = c_sum(dv26);
1135 for (ixstart = 0; ixstart < 128; ixstart++) {
1136 b_r[ixstart] = r[11 + (ixstart << 4)] - tap_store[ixstart];
1137 u = tap_store[ixstart] * 8192.0;
1138 v = fabs(u);
1139 if (v < 4.503599627370496E+15) {
1140 if (v >= 0.5) {
1141 u = floor(u + 0.5);
1142 } else {
1143 u *= 0.0;
1144 }
1145 }
1146
1147 if (u < 32768.0) {
1148 if (u >= -32768.0) {
1149 i57 = (short)u;
1150 } else {
1151 i57 = MIN_int16_T;
1152 }
1153 } else if (u >= 32768.0) {
1154 i57 = MAX_int16_T;
1155 } else {
1156 i57 = 0;
1157 }
1158
1159 r[12 + (ixstart << 4)] = (double)i57 * 0.0001220703125;
1160 }
1161
1162 d_abs(b_r, dv27);
1163 e[11] = c_sum(dv27);
1164 for (ixstart = 0; ixstart < 128; ixstart++) {
1165 b_r[ixstart] = r[12 + (ixstart << 4)] - tap_store[ixstart];
1166 u = tap_store[ixstart] * 16384.0;
1167 v = fabs(u);
1168 if (v < 4.503599627370496E+15) {
1169 if (v >= 0.5) {
1170 u = floor(u + 0.5);
1171 } else {
1172 u *= 0.0;
1173 }
1174 }
1175
1176 if (u < 32768.0) {
1177 if (u >= -32768.0) {
1178 i57 = (short)u;
1179 } else {
1180 i57 = MIN_int16_T;
1181 }
1182 } else if (u >= 32768.0) {
1183 i57 = MAX_int16_T;
1184 } else {
1185 i57 = 0;
1186 }
1187
1188 r[13 + (ixstart << 4)] = (double)i57 * 6.103515625E-5;
1189 }
1190
1191 d_abs(b_r, dv28);
1192 e[12] = c_sum(dv28);
1193 for (ixstart = 0; ixstart < 128; ixstart++) {
1194 b_r[ixstart] = r[13 + (ixstart << 4)] - tap_store[ixstart];
1195 u = tap_store[ixstart] * 32768.0;
1196 v = fabs(u);
1197 if (v < 4.503599627370496E+15) {
1198 if (v >= 0.5) {
1199 u = floor(u + 0.5);
1200 } else {
1201 u *= 0.0;
1202 }
1203 }
1204
1205 if (u < 32768.0) {
1206 if (u >= -32768.0) {
1207 i57 = (short)u;
1208 } else {
1209 i57 = MIN_int16_T;
1210 }
1211 } else if (u >= 32768.0) {
1212 i57 = MAX_int16_T;
1213 } else {
1214 i57 = 0;
1215 }
1216
1217 r[14 + (ixstart << 4)] = (double)i57 * 3.0517578125E-5;
1218 }
1219
1220 d_abs(b_r, dv29);
1221 e[13] = c_sum(dv29);
1222 for (ixstart = 0; ixstart < 128; ixstart++) {
1223 b_r[ixstart] = r[14 + (ixstart << 4)] - tap_store[ixstart];
1224 u = tap_store[ixstart] * 65536.0;
1225 v = fabs(u);
1226 if (v < 4.503599627370496E+15) {
1227 if (v >= 0.5) {
1228 u = floor(u + 0.5);
1229 } else {
1230 u *= 0.0;
1231 }
1232 }
1233
1234 if (u < 32768.0) {
1235 if (u >= -32768.0) {
1236 i57 = (short)u;
1237 } else {
1238 i57 = MIN_int16_T;
1239 }
1240 } else if (u >= 32768.0) {
1241 i57 = MAX_int16_T;
1242 } else {
1243 i57 = 0;
1244 }
1245
1246 r[15 + (ixstart << 4)] = (double)i57 * 1.52587890625E-5;
1247 }
1248
1249 d_abs(b_r, dv30);
1250 e[14] = c_sum(dv30);
1251 for (ixstart = 0; ixstart < 128; ixstart++) {
1252 b_r[ixstart] = r[15 + (ixstart << 4)] - tap_store[ixstart];
1253 }
1254
1255 d_abs(b_r, dv31);
1256 e[15] = c_sum(dv31);
1257 ixstart = 1;
1258 u = e[0];
1259 itmp = 0;
1260 if (rtIsNaN(e[0])) {
1261 ix = 2;
1262 exitg1 = false;
1263 while ((!exitg1) && (ix < 17)) {
1264 ixstart = ix;
1265 if (!rtIsNaN(e[ix - 1])) {
1266 u = e[ix - 1];
1267 itmp = ix - 1;
1268 exitg1 = true;
1269 } else {
1270 ix++;
1271 }
1272 }
1273 }
1274
1275 if (ixstart < 16) {
1276 while (ixstart + 1 < 17) {
1277 if (e[ixstart] < u) {
1278 u = e[ixstart];
1279 itmp = ixstart;
1280 }
1281
1282 ixstart++;
1283 }
1284 }
1285
1286 for (ixstart = 0; ixstart < 128; ixstart++) {
1287 taps[ixstart] = r[itmp + (ixstart << 4)];
1288 }
1289 }
1290
1291 /*
1292 * Arguments : creal_T x[2048]
1293 * Return Type : void
1294 */
b_exp(creal_T x[2048])1295 static void b_exp(creal_T x[2048])
1296 {
1297 int k;
1298 double x_re;
1299 double r;
1300 for (k = 0; k < 2048; k++) {
1301 if (x[k].im == 0.0) {
1302 x_re = exp(x[k].re);
1303 r = 0.0;
1304 } else if (rtIsInf(x[k].im) && rtIsInf(x[k].re) && (x[k].re < 0.0)) {
1305 x_re = 0.0;
1306 r = 0.0;
1307 } else {
1308 r = exp(x[k].re / 2.0);
1309 x_re = r * (r * cos(x[k].im));
1310 r *= r * sin(x[k].im);
1311 }
1312
1313 x[k].re = x_re;
1314 x[k].im = r;
1315 }
1316 }
1317
1318 /*
1319 * Make b a row
1320 * Arguments : const double b[15]
1321 * const struct_T *options
1322 * creal_T h[2048]
1323 * double w[2048]
1324 * Return Type : void
1325 */
b_firfreqz(const double b[15],const struct_T * options,creal_T h[2048],double w[2048])1326 static void b_firfreqz(const double b[15], const struct_T *options, creal_T h
1327 [2048], double w[2048])
1328 {
1329 int i64;
1330 creal_T dcv2[2048];
1331 double digw;
1332 double b_digw[2048];
1333 double h_re;
1334 double brm;
1335 double d;
1336
1337 /* -------------------------------------------------------------------------- */
1338 /* Actual Frequency Response Computation */
1339 /* if fvflag, */
1340 /* Frequency vector specified. Use Horner's method of polynomial */
1341 /* evaluation at the frequency points and divide the numerator */
1342 /* by the denominator. */
1343 /* */
1344 /* Note: we use positive i here because of the relationship */
1345 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
1346 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
1347 /* */
1348 /* Fs was specified, freq. vector is in Hz */
1349 /* Convert from Hz to rad/sample for computational purposes */
1350 /* Digital frequency must be used for this calculation */
1351 for (i64 = 0; i64 < 2048; i64++) {
1352 w[i64] = options->w[i64];
1353 digw = 6.2831853071795862 * options->w[i64] / options->Fs;
1354 dcv2[i64].re = digw * 0.0;
1355 dcv2[i64].im = digw;
1356 b_digw[i64] = digw;
1357 }
1358
1359 b_exp(dcv2);
1360 polyval(b, dcv2, h);
1361 for (i64 = 0; i64 < 2048; i64++) {
1362 dcv2[i64].re = 14.0 * (b_digw[i64] * 0.0);
1363 dcv2[i64].im = 14.0 * b_digw[i64];
1364 }
1365
1366 b_exp(dcv2);
1367 for (i64 = 0; i64 < 2048; i64++) {
1368 h_re = h[i64].re;
1369 if (dcv2[i64].im == 0.0) {
1370 if (h[i64].im == 0.0) {
1371 h[i64].re /= dcv2[i64].re;
1372 h[i64].im = 0.0;
1373 } else if (h[i64].re == 0.0) {
1374 h[i64].re = 0.0;
1375 h[i64].im /= dcv2[i64].re;
1376 } else {
1377 h[i64].re /= dcv2[i64].re;
1378 h[i64].im /= dcv2[i64].re;
1379 }
1380 } else if (dcv2[i64].re == 0.0) {
1381 if (h[i64].re == 0.0) {
1382 h[i64].re = h[i64].im / dcv2[i64].im;
1383 h[i64].im = 0.0;
1384 } else if (h[i64].im == 0.0) {
1385 h[i64].re = 0.0;
1386 h[i64].im = -(h_re / dcv2[i64].im);
1387 } else {
1388 h[i64].re = h[i64].im / dcv2[i64].im;
1389 h[i64].im = -(h_re / dcv2[i64].im);
1390 }
1391 } else {
1392 brm = fabs(dcv2[i64].re);
1393 digw = fabs(dcv2[i64].im);
1394 if (brm > digw) {
1395 digw = dcv2[i64].im / dcv2[i64].re;
1396 d = dcv2[i64].re + digw * dcv2[i64].im;
1397 h[i64].re = (h[i64].re + digw * h[i64].im) / d;
1398 h[i64].im = (h[i64].im - digw * h_re) / d;
1399 } else if (digw == brm) {
1400 if (dcv2[i64].re > 0.0) {
1401 digw = 0.5;
1402 } else {
1403 digw = -0.5;
1404 }
1405
1406 if (dcv2[i64].im > 0.0) {
1407 d = 0.5;
1408 } else {
1409 d = -0.5;
1410 }
1411
1412 h[i64].re = (h[i64].re * digw + h[i64].im * d) / brm;
1413 h[i64].im = (h[i64].im * digw - h_re * d) / brm;
1414 } else {
1415 digw = dcv2[i64].re / dcv2[i64].im;
1416 d = dcv2[i64].im + digw * dcv2[i64].re;
1417 h[i64].re = (digw * h[i64].re + h[i64].im) / d;
1418 h[i64].im = (digw * h[i64].im - h_re) / d;
1419 }
1420 }
1421 }
1422 }
1423
1424 /*
1425 * FIRPM Parks-McClellan optimal equiripple FIR filter design.
1426 *
1427 * This function is based on 'firpm' by The MathWorks Inc.
1428 * Arguments : double order
1429 * const double ff[4]
1430 * const emxArray_real_T *amplitudes
1431 * const emxArray_real_T *frequencies
1432 * const emxArray_real_T *weights
1433 * emxArray_real_T *h
1434 * boolean_T *valid
1435 * double *err
1436 * Return Type : void
1437 */
b_firpm_cg(double order,const double ff[4],const emxArray_real_T * amplitudes,const emxArray_real_T * frequencies,const emxArray_real_T * weights,emxArray_real_T * h,boolean_T * valid,double * err)1438 static void b_firpm_cg(double order, const double ff[4], const emxArray_real_T
1439 *amplitudes, const emxArray_real_T *frequencies, const emxArray_real_T
1440 *weights, emxArray_real_T *h, boolean_T *valid, double *err)
1441 {
1442 emxArray_real_T *grid;
1443 emxArray_real_T *des;
1444 emxArray_real_T *wt;
1445 emxArray_real_T *b_h;
1446 int i52;
1447 emxArray_real_T *c_h;
1448 double b_ff[4];
1449 double x;
1450 boolean_T b_valid;
1451 int h_idx_0;
1452 double d2;
1453 int i53;
1454 int i54;
1455 int loop_ub;
1456 emxInit_real_T(&grid, 2);
1457 emxInit_real_T(&des, 2);
1458 emxInit_real_T(&wt, 2);
1459 emxInit_real_T(&b_h, 2);
1460
1461 /* */
1462 firpmgrid_cg(order + 1.0, ff, grid);
1463
1464 /* orgFreqIndx = frequencies; */
1465 /* */
1466 /* positionsOfNewFreqIndx = zeros(size(grid)); */
1467 /* for ind = 1:length(positionsOfNewFreqIndx) */
1468 /* */
1469 /* [~,indx] = min( abs(orgFreqIndx-grid(ind)) ); */
1470 /* */
1471 /* positionsOfNewFreqIndx(ind) = indx; */
1472 /* end */
1473 /* */
1474 /* wt = weights(positionsOfNewFreqIndx); */
1475 /* des = amplitudes(positionsOfNewFreqIndx); */
1476 interp1(frequencies, amplitudes, grid, des);
1477 interp1(frequencies, weights, grid, wt);
1478
1479 /* Workaround */
1480 /* ftype = 2; */
1481 /* sign_val = 1; */
1482 /* Always bandpass designs */
1483 /* cast to enforce precision rules */
1484 /* Call actual design algorithm */
1485 rdivide(grid, 2.0, b_h);
1486 emxFree_real_T(&grid);
1487 for (i52 = 0; i52 < 4; i52++) {
1488 b_ff[i52] = ff[i52] / 2.0;
1489 }
1490
1491 emxInit_real_T(&c_h, 2);
1492 remezm(order + 1.0, b_ff, b_h, des, wt, c_h, &x, &b_valid);
1493 h_idx_0 = c_h->size[0] * c_h->size[1];
1494 i52 = h->size[0] * h->size[1];
1495 h->size[0] = 1;
1496 h->size[1] = h_idx_0;
1497 emxEnsureCapacity_real_T(h, i52);
1498 emxFree_real_T(&wt);
1499 emxFree_real_T(&des);
1500 for (i52 = 0; i52 < h_idx_0; i52++) {
1501 h->data[h->size[0] * i52] = c_h->data[i52];
1502 }
1503
1504 emxFree_real_T(&c_h);
1505
1506 /* make it a row */
1507 d2 = (double)h->size[1] - rt_remd_snf(order + 1.0, 2.0);
1508 if (1.0 > d2) {
1509 i52 = 1;
1510 h_idx_0 = 1;
1511 i53 = 0;
1512 } else {
1513 i52 = (int)d2;
1514 h_idx_0 = -1;
1515 i53 = 1;
1516 }
1517
1518 i54 = b_h->size[0] * b_h->size[1];
1519 b_h->size[0] = 1;
1520 b_h->size[1] = (h->size[1] + div_s32_floor(i53 - i52, h_idx_0)) + 1;
1521 emxEnsureCapacity_real_T(b_h, i54);
1522 loop_ub = h->size[1];
1523 for (i54 = 0; i54 < loop_ub; i54++) {
1524 b_h->data[b_h->size[0] * i54] = h->data[h->size[0] * i54];
1525 }
1526
1527 loop_ub = div_s32_floor(i53 - i52, h_idx_0);
1528 for (i53 = 0; i53 <= loop_ub; i53++) {
1529 b_h->data[b_h->size[0] * (i53 + h->size[1])] = h->data[(i52 + h_idx_0 * i53)
1530 - 1];
1531 }
1532
1533 i52 = h->size[0] * h->size[1];
1534 h->size[0] = 1;
1535 h->size[1] = b_h->size[1];
1536 emxEnsureCapacity_real_T(h, i52);
1537 loop_ub = b_h->size[1];
1538 for (i52 = 0; i52 < loop_ub; i52++) {
1539 h->data[h->size[0] * i52] = b_h->data[b_h->size[0] * i52];
1540 }
1541
1542 if (1 > h->size[1]) {
1543 i52 = 1;
1544 h_idx_0 = 1;
1545 i53 = 0;
1546 } else {
1547 i52 = h->size[1];
1548 h_idx_0 = -1;
1549 i53 = 1;
1550 }
1551
1552 i54 = b_h->size[0] * b_h->size[1];
1553 b_h->size[0] = 1;
1554 b_h->size[1] = div_s32_floor(i53 - i52, h_idx_0) + 1;
1555 emxEnsureCapacity_real_T(b_h, i54);
1556 loop_ub = div_s32_floor(i53 - i52, h_idx_0);
1557 for (i53 = 0; i53 <= loop_ub; i53++) {
1558 b_h->data[b_h->size[0] * i53] = h->data[(i52 + h_idx_0 * i53) - 1];
1559 }
1560
1561 i52 = h->size[0] * h->size[1];
1562 h->size[0] = 1;
1563 h->size[1] = b_h->size[1];
1564 emxEnsureCapacity_real_T(h, i52);
1565 loop_ub = b_h->size[1];
1566 for (i52 = 0; i52 < loop_ub; i52++) {
1567 h->data[h->size[0] * i52] = b_h->data[b_h->size[0] * i52];
1568 }
1569
1570 emxFree_real_T(&b_h);
1571 *valid = b_valid;
1572 *err = fabs(x);
1573 }
1574
1575 /*
1576 * Arguments : double *x
1577 * Return Type : void
1578 */
b_fix(double * x)1579 static void b_fix(double *x)
1580 {
1581 if (*x < 0.0) {
1582 *x = ceil(*x);
1583 } else {
1584 *x = floor(*x);
1585 }
1586 }
1587
1588 /*
1589 * FREQS Laplace-transform (s-domain) frequency response with codegen support
1590 *
1591 * This function is based on 'freqs' by The MathWorks Inc.
1592 * Arguments : const double b_data[]
1593 * const int b_size[2]
1594 * const emxArray_creal_T *a
1595 * const emxArray_real_T *w
1596 * emxArray_creal_T *h
1597 * Return Type : void
1598 */
b_freqs_cg(const double b_data[],const int b_size[2],const emxArray_creal_T * a,const emxArray_real_T * w,emxArray_creal_T * h)1599 static void b_freqs_cg(const double b_data[], const int b_size[2], const
1600 emxArray_creal_T *a, const emxArray_real_T *w, emxArray_creal_T *h)
1601 {
1602 emxArray_creal_T *s;
1603 emxArray_creal_T *b_a;
1604 double b_b_data[4];
1605 int b_b_size[2];
1606 int i40;
1607 int loop_ub;
1608 emxArray_creal_T *y;
1609 boolean_T b11;
1610 emxArray_real_T c_b_data;
1611 int k;
1612 double a_re;
1613 double a_im;
1614 double s_re;
1615 double s_im;
1616 emxInit_creal_T(&s, 2);
1617 emxInit_creal_T(&b_a, 2);
1618 removeTrailingZero(b_data, b_size, a, b_b_data, b_b_size, b_a);
1619 i40 = s->size[0] * s->size[1];
1620 s->size[0] = 1;
1621 s->size[1] = w->size[1];
1622 emxEnsureCapacity_creal_T(s, i40);
1623 loop_ub = w->size[0] * w->size[1];
1624 for (i40 = 0; i40 < loop_ub; i40++) {
1625 s->data[i40].re = w->data[i40] * 0.0;
1626 s->data[i40].im = w->data[i40];
1627 }
1628
1629 emxInit_creal_T(&y, 2);
1630 i40 = y->size[0] * y->size[1];
1631 y->size[0] = 1;
1632 y->size[1] = s->size[1];
1633 emxEnsureCapacity_creal_T(y, i40);
1634 if ((y->size[1] == 0) || (b_a->size[1] == 0)) {
1635 b11 = true;
1636 } else {
1637 b11 = false;
1638 }
1639
1640 if (!b11) {
1641 i40 = y->size[0] * y->size[1];
1642 y->size[0] = 1;
1643 emxEnsureCapacity_creal_T(y, i40);
1644 loop_ub = y->size[1];
1645 for (i40 = 0; i40 < loop_ub; i40++) {
1646 y->data[y->size[0] * i40] = b_a->data[0];
1647 }
1648
1649 for (k = 0; k <= b_a->size[1] - 2; k++) {
1650 i40 = y->size[0] * y->size[1];
1651 y->size[0] = 1;
1652 y->size[1] = s->size[1];
1653 emxEnsureCapacity_creal_T(y, i40);
1654 a_re = b_a->data[k + 1].re;
1655 a_im = b_a->data[k + 1].im;
1656 loop_ub = s->size[0] * s->size[1];
1657 for (i40 = 0; i40 < loop_ub; i40++) {
1658 s_re = s->data[i40].re * y->data[i40].re - s->data[i40].im * y->data[i40]
1659 .im;
1660 s_im = s->data[i40].re * y->data[i40].im + s->data[i40].im * y->data[i40]
1661 .re;
1662 y->data[i40].re = s_re + a_re;
1663 y->data[i40].im = s_im + a_im;
1664 }
1665 }
1666 }
1667
1668 c_b_data.data = (double *)&b_b_data;
1669 c_b_data.size = (int *)&b_b_size;
1670 c_b_data.allocatedSize = 4;
1671 c_b_data.numDimensions = 2;
1672 c_b_data.canFreeData = false;
1673 j_polyval(&c_b_data, s, b_a);
1674 b_rdivide(b_a, y, h);
1675 emxFree_creal_T(&y);
1676 emxFree_creal_T(&b_a);
1677 emxFree_creal_T(&s);
1678 }
1679
1680 /*
1681 * FREQZ_CG Frequency response of digital filter with codegen support
1682 *
1683 * This function is based on 'freqz' by The MathWorks Inc.
1684 * Arguments : const double b[15]
1685 * const double w[2048]
1686 * double Fs
1687 * creal_T hh[2048]
1688 * Return Type : void
1689 */
b_freqz_cg(const double b[15],const double w[2048],double Fs,creal_T hh[2048])1690 static void b_freqz_cg(const double b[15], const double w[2048], double Fs,
1691 creal_T hh[2048])
1692 {
1693 static struct_T options;
1694 int i10;
1695 static const char cv14[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
1696
1697 double b_w[2048];
1698 static const char cv15[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
1699
1700 /* Cast to enforce precision rules */
1701 options.Fs = Fs;
1702 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
1703 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
1704
1705 /* Remaining are default or for advanced use */
1706 options.fvflag = 1.0;
1707 for (i10 = 0; i10 < 8; i10++) {
1708 options.range[i10] = cv14[i10];
1709 }
1710
1711 options.centerdc = 0.0;
1712 for (i10 = 0; i10 < 7; i10++) {
1713 options.configlevel[i10] = cv15[i10];
1714 }
1715
1716 b_firfreqz(b, &options, hh, b_w);
1717
1718 /* Generate the default structure to pass to freqzplot */
1719 /* If rad/sample, Fs is empty */
1720 }
1721
1722 /*
1723 * Arguments : const char enables[4]
1724 * const emxArray_real_T *w
1725 * double Fs
1726 * const double hb1_coeff[15]
1727 * const double hb2_coeff[7]
1728 * const double hb3_coeff_data[]
1729 * const int hb3_coeff_size[2]
1730 * const double dec_int3_coeff_data[]
1731 * const int dec_int3_coeff_size[2]
1732 * emxArray_creal_T *combinedResponse
1733 * Return Type : void
1734 */
b_generateCascadedResponseRx(const char enables[4],const emxArray_real_T * w,double Fs,const double hb1_coeff[15],const double hb2_coeff[7],const double hb3_coeff_data[],const int hb3_coeff_size[2],const double dec_int3_coeff_data[],const int dec_int3_coeff_size[2],emxArray_creal_T * combinedResponse)1735 static void b_generateCascadedResponseRx(const char enables[4], const
1736 emxArray_real_T *w, double Fs, const double hb1_coeff[15], const double
1737 hb2_coeff[7], const double hb3_coeff_data[], const int hb3_coeff_size[2],
1738 const double dec_int3_coeff_data[], const int dec_int3_coeff_size[2],
1739 emxArray_creal_T *combinedResponse)
1740 {
1741 boolean_T b_bool;
1742 int ix;
1743 int exitg1;
1744 emxArray_creal_T *d2;
1745 emxArray_creal_T *d3;
1746 static const char cv35[4] = { '2', '1', '1', '1' };
1747
1748 double u[15];
1749 double tmp_data[29];
1750 int tmp_size[2];
1751 double b_u[30];
1752 double c_u[14];
1753 double d_u[60];
1754 double e_u[45];
1755 double f_u[21];
1756 double g_u[90];
1757 emxArray_real_T b_tmp_data;
1758 int iy;
1759 double h_u[7];
1760 int k;
1761 static const char cv36[4] = { '1', '2', '1', '1' };
1762
1763 static const char cv37[4] = { '1', '1', '2', '1' };
1764
1765 double combinedResponse_re;
1766 double combinedResponse_im;
1767 double d2_re;
1768 double d2_im;
1769 static const char cv38[4] = { '2', '2', '1', '1' };
1770
1771 static const char cv39[4] = { '2', '1', '2', '1' };
1772
1773 static const char cv40[4] = { '1', '2', '2', '1' };
1774
1775 static const char cv41[4] = { '2', '2', '2', '1' };
1776
1777 static const char cv42[4] = { '1', '1', '1', '3' };
1778
1779 static const char cv43[4] = { '2', '1', '1', '3' };
1780
1781 static const char cv44[4] = { '1', '2', '1', '3' };
1782
1783 static const char cv45[4] = { '2', '2', '1', '3' };
1784
1785 /* Cast */
1786 b_bool = false;
1787 ix = 1;
1788 do {
1789 exitg1 = 0;
1790 if (ix < 5) {
1791 if (enables[ix - 1] != '1') {
1792 exitg1 = 1;
1793 } else {
1794 ix++;
1795 }
1796 } else {
1797 b_bool = true;
1798 exitg1 = 1;
1799 }
1800 } while (exitg1 == 0);
1801
1802 if (b_bool) {
1803 ix = 0;
1804 } else {
1805 b_bool = false;
1806 ix = 0;
1807 do {
1808 exitg1 = 0;
1809 if (ix + 1 < 5) {
1810 if (enables[ix] != cv35[ix]) {
1811 exitg1 = 1;
1812 } else {
1813 ix++;
1814 }
1815 } else {
1816 b_bool = true;
1817 exitg1 = 1;
1818 }
1819 } while (exitg1 == 0);
1820
1821 if (b_bool) {
1822 ix = 1;
1823 } else {
1824 b_bool = false;
1825 ix = 0;
1826 do {
1827 exitg1 = 0;
1828 if (ix + 1 < 5) {
1829 if (enables[ix] != cv36[ix]) {
1830 exitg1 = 1;
1831 } else {
1832 ix++;
1833 }
1834 } else {
1835 b_bool = true;
1836 exitg1 = 1;
1837 }
1838 } while (exitg1 == 0);
1839
1840 if (b_bool) {
1841 ix = 2;
1842 } else {
1843 b_bool = false;
1844 ix = 0;
1845 do {
1846 exitg1 = 0;
1847 if (ix + 1 < 5) {
1848 if (enables[ix] != cv37[ix]) {
1849 exitg1 = 1;
1850 } else {
1851 ix++;
1852 }
1853 } else {
1854 b_bool = true;
1855 exitg1 = 1;
1856 }
1857 } while (exitg1 == 0);
1858
1859 if (b_bool) {
1860 ix = 3;
1861 } else {
1862 b_bool = false;
1863 ix = 0;
1864 do {
1865 exitg1 = 0;
1866 if (ix + 1 < 5) {
1867 if (enables[ix] != cv38[ix]) {
1868 exitg1 = 1;
1869 } else {
1870 ix++;
1871 }
1872 } else {
1873 b_bool = true;
1874 exitg1 = 1;
1875 }
1876 } while (exitg1 == 0);
1877
1878 if (b_bool) {
1879 ix = 4;
1880 } else {
1881 b_bool = false;
1882 ix = 0;
1883 do {
1884 exitg1 = 0;
1885 if (ix + 1 < 5) {
1886 if (enables[ix] != cv39[ix]) {
1887 exitg1 = 1;
1888 } else {
1889 ix++;
1890 }
1891 } else {
1892 b_bool = true;
1893 exitg1 = 1;
1894 }
1895 } while (exitg1 == 0);
1896
1897 if (b_bool) {
1898 ix = 5;
1899 } else {
1900 b_bool = false;
1901 ix = 0;
1902 do {
1903 exitg1 = 0;
1904 if (ix + 1 < 5) {
1905 if (enables[ix] != cv40[ix]) {
1906 exitg1 = 1;
1907 } else {
1908 ix++;
1909 }
1910 } else {
1911 b_bool = true;
1912 exitg1 = 1;
1913 }
1914 } while (exitg1 == 0);
1915
1916 if (b_bool) {
1917 ix = 6;
1918 } else {
1919 b_bool = false;
1920 ix = 0;
1921 do {
1922 exitg1 = 0;
1923 if (ix + 1 < 5) {
1924 if (enables[ix] != cv41[ix]) {
1925 exitg1 = 1;
1926 } else {
1927 ix++;
1928 }
1929 } else {
1930 b_bool = true;
1931 exitg1 = 1;
1932 }
1933 } while (exitg1 == 0);
1934
1935 if (b_bool) {
1936 ix = 7;
1937 } else {
1938 b_bool = false;
1939 ix = 0;
1940 do {
1941 exitg1 = 0;
1942 if (ix + 1 < 5) {
1943 if (enables[ix] != cv42[ix]) {
1944 exitg1 = 1;
1945 } else {
1946 ix++;
1947 }
1948 } else {
1949 b_bool = true;
1950 exitg1 = 1;
1951 }
1952 } while (exitg1 == 0);
1953
1954 if (b_bool) {
1955 ix = 8;
1956 } else {
1957 b_bool = false;
1958 ix = 0;
1959 do {
1960 exitg1 = 0;
1961 if (ix + 1 < 5) {
1962 if (enables[ix] != cv43[ix]) {
1963 exitg1 = 1;
1964 } else {
1965 ix++;
1966 }
1967 } else {
1968 b_bool = true;
1969 exitg1 = 1;
1970 }
1971 } while (exitg1 == 0);
1972
1973 if (b_bool) {
1974 ix = 9;
1975 } else {
1976 b_bool = false;
1977 ix = 0;
1978 do {
1979 exitg1 = 0;
1980 if (ix + 1 < 5) {
1981 if (enables[ix] != cv44[ix]) {
1982 exitg1 = 1;
1983 } else {
1984 ix++;
1985 }
1986 } else {
1987 b_bool = true;
1988 exitg1 = 1;
1989 }
1990 } while (exitg1 == 0);
1991
1992 if (b_bool) {
1993 ix = 10;
1994 } else {
1995 b_bool = false;
1996 ix = 0;
1997 do {
1998 exitg1 = 0;
1999 if (ix + 1 < 5) {
2000 if (enables[ix] != cv45[ix]) {
2001 exitg1 = 1;
2002 } else {
2003 ix++;
2004 }
2005 } else {
2006 b_bool = true;
2007 exitg1 = 1;
2008 }
2009 } while (exitg1 == 0);
2010
2011 if (b_bool) {
2012 ix = 11;
2013 } else {
2014 ix = -1;
2015 }
2016 }
2017 }
2018 }
2019 }
2020 }
2021 }
2022 }
2023 }
2024 }
2025 }
2026 }
2027
2028 emxInit_creal_T(&d2, 2);
2029 emxInit_creal_T(&d3, 2);
2030 switch (ix) {
2031 case 0:
2032 /* only FIR */
2033 k_freqz_cg(w, Fs, combinedResponse);
2034 break;
2035
2036 case 1:
2037 /* Hb1 */
2038 memset(&u[0], 0, 15U * sizeof(double));
2039 ix = 0;
2040 iy = 0;
2041 for (k = 0; k < 15; k++) {
2042 u[iy] = hb1_coeff[ix];
2043 ix++;
2044 iy++;
2045 }
2046
2047 l_freqz_cg(u, w, Fs, combinedResponse);
2048 break;
2049
2050 case 2:
2051 /* Hb2 */
2052 for (ix = 0; ix < 7; ix++) {
2053 h_u[ix] = 0.0;
2054 }
2055
2056 ix = 0;
2057 iy = 0;
2058 for (k = 0; k < 7; k++) {
2059 h_u[iy] = hb2_coeff[ix];
2060 ix++;
2061 iy++;
2062 }
2063
2064 m_freqz_cg(h_u, w, Fs, combinedResponse);
2065 break;
2066
2067 case 3:
2068 /* Hb3 */
2069 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
2070 b_tmp_data.data = (double *)&tmp_data;
2071 b_tmp_data.size = (int *)&tmp_size;
2072 b_tmp_data.allocatedSize = 29;
2073 b_tmp_data.numDimensions = 2;
2074 b_tmp_data.canFreeData = false;
2075 n_freqz_cg(&b_tmp_data, w, Fs, combinedResponse);
2076 break;
2077
2078 case 4:
2079 /* Hb2,Hb1 */
2080 memset(&b_u[0], 0, 30U * sizeof(double));
2081 ix = 0;
2082 iy = 0;
2083 for (k = 0; k < 15; k++) {
2084 b_u[iy] = hb1_coeff[ix];
2085 ix++;
2086 iy += 2;
2087 }
2088
2089 o_freqz_cg(*(double (*)[29])&b_u[0], w, Fs, combinedResponse);
2090 for (ix = 0; ix < 7; ix++) {
2091 h_u[ix] = 0.0;
2092 }
2093
2094 ix = 0;
2095 iy = 0;
2096 for (k = 0; k < 7; k++) {
2097 h_u[iy] = hb2_coeff[ix];
2098 ix++;
2099 iy++;
2100 }
2101
2102 m_freqz_cg(h_u, w, Fs, d2);
2103 ix = combinedResponse->size[0] * combinedResponse->size[1];
2104 combinedResponse->size[0] = 1;
2105 emxEnsureCapacity_creal_T(combinedResponse, ix);
2106 ix = combinedResponse->size[0];
2107 iy = combinedResponse->size[1];
2108 iy *= ix;
2109 for (ix = 0; ix < iy; ix++) {
2110 combinedResponse_re = combinedResponse->data[ix].re;
2111 combinedResponse_im = combinedResponse->data[ix].im;
2112 d2_re = d2->data[ix].re;
2113 d2_im = d2->data[ix].im;
2114 combinedResponse->data[ix].re = combinedResponse_re * d2_re -
2115 combinedResponse_im * d2_im;
2116 combinedResponse->data[ix].im = combinedResponse_re * d2_im +
2117 combinedResponse_im * d2_re;
2118 }
2119 break;
2120
2121 case 5:
2122 /* Hb3,Hb1 */
2123 memset(&b_u[0], 0, 30U * sizeof(double));
2124 ix = 0;
2125 iy = 0;
2126 for (k = 0; k < 15; k++) {
2127 b_u[iy] = hb1_coeff[ix];
2128 ix++;
2129 iy += 2;
2130 }
2131
2132 o_freqz_cg(*(double (*)[29])&b_u[0], w, Fs, combinedResponse);
2133 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
2134 b_tmp_data.data = (double *)&tmp_data;
2135 b_tmp_data.size = (int *)&tmp_size;
2136 b_tmp_data.allocatedSize = 29;
2137 b_tmp_data.numDimensions = 2;
2138 b_tmp_data.canFreeData = false;
2139 n_freqz_cg(&b_tmp_data, w, Fs, d2);
2140 ix = combinedResponse->size[0] * combinedResponse->size[1];
2141 combinedResponse->size[0] = 1;
2142 emxEnsureCapacity_creal_T(combinedResponse, ix);
2143 ix = combinedResponse->size[0];
2144 iy = combinedResponse->size[1];
2145 iy *= ix;
2146 for (ix = 0; ix < iy; ix++) {
2147 combinedResponse_re = combinedResponse->data[ix].re;
2148 combinedResponse_im = combinedResponse->data[ix].im;
2149 d2_re = d2->data[ix].re;
2150 d2_im = d2->data[ix].im;
2151 combinedResponse->data[ix].re = combinedResponse_re * d2_re -
2152 combinedResponse_im * d2_im;
2153 combinedResponse->data[ix].im = combinedResponse_re * d2_im +
2154 combinedResponse_im * d2_re;
2155 }
2156 break;
2157
2158 case 6:
2159 /* Hb3,Hb2 */
2160 memset(&c_u[0], 0, 14U * sizeof(double));
2161 ix = 0;
2162 iy = 0;
2163 for (k = 0; k < 7; k++) {
2164 c_u[iy] = hb2_coeff[ix];
2165 ix++;
2166 iy += 2;
2167 }
2168
2169 p_freqz_cg(*(double (*)[13])&c_u[0], w, Fs, combinedResponse);
2170 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
2171 b_tmp_data.data = (double *)&tmp_data;
2172 b_tmp_data.size = (int *)&tmp_size;
2173 b_tmp_data.allocatedSize = 29;
2174 b_tmp_data.numDimensions = 2;
2175 b_tmp_data.canFreeData = false;
2176 n_freqz_cg(&b_tmp_data, w, Fs, d2);
2177 ix = combinedResponse->size[0] * combinedResponse->size[1];
2178 combinedResponse->size[0] = 1;
2179 emxEnsureCapacity_creal_T(combinedResponse, ix);
2180 ix = combinedResponse->size[0];
2181 iy = combinedResponse->size[1];
2182 iy *= ix;
2183 for (ix = 0; ix < iy; ix++) {
2184 combinedResponse_re = combinedResponse->data[ix].re;
2185 combinedResponse_im = combinedResponse->data[ix].im;
2186 d2_re = d2->data[ix].re;
2187 d2_im = d2->data[ix].im;
2188 combinedResponse->data[ix].re = combinedResponse_re * d2_re -
2189 combinedResponse_im * d2_im;
2190 combinedResponse->data[ix].im = combinedResponse_re * d2_im +
2191 combinedResponse_im * d2_re;
2192 }
2193 break;
2194
2195 case 7:
2196 /* Hb3,Hb2,Hb1 */
2197 memset(&d_u[0], 0, 60U * sizeof(double));
2198 ix = 0;
2199 iy = 0;
2200 for (k = 0; k < 15; k++) {
2201 d_u[iy] = hb1_coeff[ix];
2202 ix++;
2203 iy += 4;
2204 }
2205
2206 q_freqz_cg(*(double (*)[57])&d_u[0], w, Fs, combinedResponse);
2207 memset(&c_u[0], 0, 14U * sizeof(double));
2208 ix = 0;
2209 iy = 0;
2210 for (k = 0; k < 7; k++) {
2211 c_u[iy] = hb2_coeff[ix];
2212 ix++;
2213 iy += 2;
2214 }
2215
2216 p_freqz_cg(*(double (*)[13])&c_u[0], w, Fs, d2);
2217 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
2218 b_tmp_data.data = (double *)&tmp_data;
2219 b_tmp_data.size = (int *)&tmp_size;
2220 b_tmp_data.allocatedSize = 29;
2221 b_tmp_data.numDimensions = 2;
2222 b_tmp_data.canFreeData = false;
2223 n_freqz_cg(&b_tmp_data, w, Fs, d3);
2224 ix = combinedResponse->size[0] * combinedResponse->size[1];
2225 combinedResponse->size[0] = 1;
2226 emxEnsureCapacity_creal_T(combinedResponse, ix);
2227 ix = combinedResponse->size[0];
2228 iy = combinedResponse->size[1];
2229 iy *= ix;
2230 for (ix = 0; ix < iy; ix++) {
2231 combinedResponse_re = combinedResponse->data[ix].re * d2->data[ix].re -
2232 combinedResponse->data[ix].im * d2->data[ix].im;
2233 combinedResponse_im = combinedResponse->data[ix].re * d2->data[ix].im +
2234 combinedResponse->data[ix].im * d2->data[ix].re;
2235 d2_re = d3->data[ix].re;
2236 d2_im = d3->data[ix].im;
2237 combinedResponse->data[ix].re = combinedResponse_re * d2_re -
2238 combinedResponse_im * d2_im;
2239 combinedResponse->data[ix].im = combinedResponse_re * d2_im +
2240 combinedResponse_im * d2_re;
2241 }
2242 break;
2243
2244 case 8:
2245 /* Dec/Int3 */
2246 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
2247 b_tmp_data.data = (double *)&tmp_data;
2248 b_tmp_data.size = (int *)&tmp_size;
2249 b_tmp_data.allocatedSize = 29;
2250 b_tmp_data.numDimensions = 2;
2251 b_tmp_data.canFreeData = false;
2252 n_freqz_cg(&b_tmp_data, w, Fs, combinedResponse);
2253
2254 /* RECHECK ALL DEC BY 3 */
2255 break;
2256
2257 case 9:
2258 /* Dec/Int3,Hb1 */
2259 memset(&e_u[0], 0, 45U * sizeof(double));
2260 ix = 0;
2261 iy = 0;
2262 for (k = 0; k < 15; k++) {
2263 e_u[iy] = hb1_coeff[ix];
2264 ix++;
2265 iy += 3;
2266 }
2267
2268 r_freqz_cg(*(double (*)[43])&e_u[0], w, Fs, combinedResponse);
2269 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
2270 b_tmp_data.data = (double *)&tmp_data;
2271 b_tmp_data.size = (int *)&tmp_size;
2272 b_tmp_data.allocatedSize = 29;
2273 b_tmp_data.numDimensions = 2;
2274 b_tmp_data.canFreeData = false;
2275 n_freqz_cg(&b_tmp_data, w, Fs, d2);
2276 ix = combinedResponse->size[0] * combinedResponse->size[1];
2277 combinedResponse->size[0] = 1;
2278 emxEnsureCapacity_creal_T(combinedResponse, ix);
2279 ix = combinedResponse->size[0];
2280 iy = combinedResponse->size[1];
2281 iy *= ix;
2282 for (ix = 0; ix < iy; ix++) {
2283 combinedResponse_re = combinedResponse->data[ix].re;
2284 combinedResponse_im = combinedResponse->data[ix].im;
2285 d2_re = d2->data[ix].re;
2286 d2_im = d2->data[ix].im;
2287 combinedResponse->data[ix].re = combinedResponse_re * d2_re -
2288 combinedResponse_im * d2_im;
2289 combinedResponse->data[ix].im = combinedResponse_re * d2_im +
2290 combinedResponse_im * d2_re;
2291 }
2292 break;
2293
2294 case 10:
2295 /* Dec/Int3,Hb2 */
2296 memset(&f_u[0], 0, 21U * sizeof(double));
2297 ix = 0;
2298 iy = 0;
2299 for (k = 0; k < 7; k++) {
2300 f_u[iy] = hb2_coeff[ix];
2301 ix++;
2302 iy += 3;
2303 }
2304
2305 s_freqz_cg(*(double (*)[19])&f_u[0], w, Fs, combinedResponse);
2306 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
2307 b_tmp_data.data = (double *)&tmp_data;
2308 b_tmp_data.size = (int *)&tmp_size;
2309 b_tmp_data.allocatedSize = 29;
2310 b_tmp_data.numDimensions = 2;
2311 b_tmp_data.canFreeData = false;
2312 n_freqz_cg(&b_tmp_data, w, Fs, d2);
2313 ix = combinedResponse->size[0] * combinedResponse->size[1];
2314 combinedResponse->size[0] = 1;
2315 emxEnsureCapacity_creal_T(combinedResponse, ix);
2316 ix = combinedResponse->size[0];
2317 iy = combinedResponse->size[1];
2318 iy *= ix;
2319 for (ix = 0; ix < iy; ix++) {
2320 combinedResponse_re = combinedResponse->data[ix].re;
2321 combinedResponse_im = combinedResponse->data[ix].im;
2322 d2_re = d2->data[ix].re;
2323 d2_im = d2->data[ix].im;
2324 combinedResponse->data[ix].re = combinedResponse_re * d2_re -
2325 combinedResponse_im * d2_im;
2326 combinedResponse->data[ix].im = combinedResponse_re * d2_im +
2327 combinedResponse_im * d2_re;
2328 }
2329 break;
2330
2331 case 11:
2332 /* Dec/Int3,Hb2,Hb1 {Hm4,Hm2c34,Hm1} */
2333 memset(&g_u[0], 0, 90U * sizeof(double));
2334 ix = 0;
2335 iy = 0;
2336 for (k = 0; k < 15; k++) {
2337 g_u[iy] = hb1_coeff[ix];
2338 ix++;
2339 iy += 6;
2340 }
2341
2342 t_freqz_cg(*(double (*)[85])&g_u[0], w, Fs, combinedResponse);
2343 memset(&f_u[0], 0, 21U * sizeof(double));
2344 ix = 0;
2345 iy = 0;
2346 for (k = 0; k < 7; k++) {
2347 f_u[iy] = hb2_coeff[ix];
2348 ix++;
2349 iy += 3;
2350 }
2351
2352 s_freqz_cg(*(double (*)[19])&f_u[0], w, Fs, d2);
2353 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
2354 b_tmp_data.data = (double *)&tmp_data;
2355 b_tmp_data.size = (int *)&tmp_size;
2356 b_tmp_data.allocatedSize = 29;
2357 b_tmp_data.numDimensions = 2;
2358 b_tmp_data.canFreeData = false;
2359 n_freqz_cg(&b_tmp_data, w, Fs, d3);
2360 ix = combinedResponse->size[0] * combinedResponse->size[1];
2361 combinedResponse->size[0] = 1;
2362 emxEnsureCapacity_creal_T(combinedResponse, ix);
2363 ix = combinedResponse->size[0];
2364 iy = combinedResponse->size[1];
2365 iy *= ix;
2366 for (ix = 0; ix < iy; ix++) {
2367 combinedResponse_re = combinedResponse->data[ix].re * d2->data[ix].re -
2368 combinedResponse->data[ix].im * d2->data[ix].im;
2369 combinedResponse_im = combinedResponse->data[ix].re * d2->data[ix].im +
2370 combinedResponse->data[ix].im * d2->data[ix].re;
2371 d2_re = d3->data[ix].re;
2372 d2_im = d3->data[ix].im;
2373 combinedResponse->data[ix].re = combinedResponse_re * d2_re -
2374 combinedResponse_im * d2_im;
2375 combinedResponse->data[ix].im = combinedResponse_re * d2_im +
2376 combinedResponse_im * d2_re;
2377 }
2378 break;
2379 }
2380
2381 emxFree_creal_T(&d3);
2382 emxFree_creal_T(&d2);
2383
2384 /* Add filter extra filter to end of cascade */
2385 }
2386
2387 /*
2388 * Arguments : double x
2389 * Return Type : double
2390 */
b_log2(double x)2391 static double b_log2(double x)
2392 {
2393 double f;
2394 double t;
2395 int eint;
2396 if (x == 0.0) {
2397 f = rtMinusInf;
2398 } else if (x < 0.0) {
2399 f = rtNaN;
2400 } else if ((!rtIsInf(x)) && (!rtIsNaN(x))) {
2401 t = frexp(x, &eint);
2402 if (t == 0.5) {
2403 f = (double)eint - 1.0;
2404 } else if ((eint == 1) && (t < 0.75)) {
2405 f = log(2.0 * t) / 0.69314718055994529;
2406 } else {
2407 f = log(t) / 0.69314718055994529 + (double)eint;
2408 }
2409 } else {
2410 f = x;
2411 }
2412
2413 return f;
2414 }
2415
2416 /*
2417 * Arguments : const double p[7]
2418 * const creal_T x[2048]
2419 * creal_T y[2048]
2420 * Return Type : void
2421 */
b_polyval(const double p[7],const creal_T x[2048],creal_T y[2048])2422 static void b_polyval(const double p[7], const creal_T x[2048], creal_T y[2048])
2423 {
2424 int i11;
2425 int k;
2426 double x_im;
2427 for (i11 = 0; i11 < 2048; i11++) {
2428 y[i11].re = p[0];
2429 y[i11].im = 0.0;
2430 }
2431
2432 for (k = 0; k < 6; k++) {
2433 for (i11 = 0; i11 < 2048; i11++) {
2434 x_im = x[i11].re * y[i11].im + x[i11].im * y[i11].re;
2435 y[i11].re = (x[i11].re * y[i11].re - x[i11].im * y[i11].im) + p[k + 1];
2436 y[i11].im = x_im;
2437 }
2438 }
2439 }
2440
2441 /*
2442 * Arguments : const double a[2048]
2443 * double y[2048]
2444 * Return Type : void
2445 */
b_power(const double a[2048],double y[2048])2446 static void b_power(const double a[2048], double y[2048])
2447 {
2448 int k;
2449 for (k = 0; k < 2048; k++) {
2450 y[k] = a[k] * a[k];
2451 }
2452 }
2453
2454 /*
2455 * Arguments : const emxArray_creal_T *x
2456 * const emxArray_creal_T *y
2457 * emxArray_creal_T *z
2458 * Return Type : void
2459 */
b_rdivide(const emxArray_creal_T * x,const emxArray_creal_T * y,emxArray_creal_T * z)2460 static void b_rdivide(const emxArray_creal_T *x, const emxArray_creal_T *y,
2461 emxArray_creal_T *z)
2462 {
2463 int i29;
2464 int loop_ub;
2465 double x_re;
2466 double x_im;
2467 double y_re;
2468 double y_im;
2469 double brm;
2470 double bim;
2471 double s;
2472 i29 = z->size[0] * z->size[1];
2473 z->size[0] = 1;
2474 z->size[1] = x->size[1];
2475 emxEnsureCapacity_creal_T(z, i29);
2476 loop_ub = x->size[0] * x->size[1];
2477 for (i29 = 0; i29 < loop_ub; i29++) {
2478 x_re = x->data[i29].re;
2479 x_im = x->data[i29].im;
2480 y_re = y->data[i29].re;
2481 y_im = y->data[i29].im;
2482 if (y_im == 0.0) {
2483 if (x_im == 0.0) {
2484 z->data[i29].re = x_re / y_re;
2485 z->data[i29].im = 0.0;
2486 } else if (x_re == 0.0) {
2487 z->data[i29].re = 0.0;
2488 z->data[i29].im = x_im / y_re;
2489 } else {
2490 z->data[i29].re = x_re / y_re;
2491 z->data[i29].im = x_im / y_re;
2492 }
2493 } else if (y_re == 0.0) {
2494 if (x_re == 0.0) {
2495 z->data[i29].re = x_im / y_im;
2496 z->data[i29].im = 0.0;
2497 } else if (x_im == 0.0) {
2498 z->data[i29].re = 0.0;
2499 z->data[i29].im = -(x_re / y_im);
2500 } else {
2501 z->data[i29].re = x_im / y_im;
2502 z->data[i29].im = -(x_re / y_im);
2503 }
2504 } else {
2505 brm = fabs(y_re);
2506 bim = fabs(y_im);
2507 if (brm > bim) {
2508 s = y_im / y_re;
2509 bim = y_re + s * y_im;
2510 z->data[i29].re = (x_re + s * x_im) / bim;
2511 z->data[i29].im = (x_im - s * x_re) / bim;
2512 } else if (bim == brm) {
2513 if (y_re > 0.0) {
2514 s = 0.5;
2515 } else {
2516 s = -0.5;
2517 }
2518
2519 if (y_im > 0.0) {
2520 bim = 0.5;
2521 } else {
2522 bim = -0.5;
2523 }
2524
2525 z->data[i29].re = (x_re * s + x_im * bim) / brm;
2526 z->data[i29].im = (x_im * s - x_re * bim) / brm;
2527 } else {
2528 s = y_re / y_im;
2529 bim = y_im + s * y_re;
2530 z->data[i29].re = (s * x_re + x_im) / bim;
2531 z->data[i29].im = (s * x_im - x_re) / bim;
2532 }
2533 }
2534 }
2535 }
2536
2537 /*
2538 * Arguments : emxArray_real_T *x
2539 * Return Type : void
2540 */
b_sinc(emxArray_real_T * x)2541 static void b_sinc(emxArray_real_T *x)
2542 {
2543 int i72;
2544 int k;
2545 i72 = x->size[1];
2546 for (k = 0; k < i72; k++) {
2547 if (fabs(x->data[k]) < 1.0020841800044864E-292) {
2548 x->data[k] = 1.0;
2549 } else {
2550 x->data[k] *= 3.1415926535897931;
2551 x->data[k] = sin(x->data[k]) / x->data[k];
2552 }
2553 }
2554 }
2555
2556 /*
2557 * Arguments : double *x
2558 * Return Type : void
2559 */
b_sqrt(double * x)2560 static void b_sqrt(double *x)
2561 {
2562 *x = sqrt(*x);
2563 }
2564
2565 /*
2566 * Arguments : const char a[2]
2567 * Return Type : boolean_T
2568 */
b_strcmp(const char a[2])2569 static boolean_T b_strcmp(const char a[2])
2570 {
2571 boolean_T b_bool;
2572 int kstr;
2573 int exitg1;
2574 static const char cv0[2] = { 'R', 'x' };
2575
2576 b_bool = false;
2577 kstr = 0;
2578 do {
2579 exitg1 = 0;
2580 if (kstr + 1 < 3) {
2581 if (a[kstr] != cv0[kstr]) {
2582 exitg1 = 1;
2583 } else {
2584 kstr++;
2585 }
2586 } else {
2587 b_bool = true;
2588 exitg1 = 1;
2589 }
2590 } while (exitg1 == 0);
2591
2592 return b_bool;
2593 }
2594
2595 /*
2596 * Arguments : const emxArray_real_T *x
2597 * Return Type : double
2598 */
b_sum(const emxArray_real_T * x)2599 static double b_sum(const emxArray_real_T *x)
2600 {
2601 double y;
2602 int k;
2603 if (x->size[1] == 0) {
2604 y = 0.0;
2605 } else {
2606 y = x->data[0];
2607 for (k = 2; k <= x->size[1]; k++) {
2608 y += x->data[k - 1];
2609 }
2610 }
2611
2612 return y;
2613 }
2614
2615 /*
2616 * Arguments : const double o[7]
2617 * double u[7]
2618 * Return Type : void
2619 */
b_us(const double o[7],double u[7])2620 static void b_us(const double o[7], double u[7])
2621 {
2622 int ix;
2623 int iy;
2624 int k;
2625 for (ix = 0; ix < 7; ix++) {
2626 u[ix] = 0.0;
2627 }
2628
2629 ix = 0;
2630 iy = 0;
2631 for (k = 0; k < 7; k++) {
2632 u[iy] = o[ix];
2633 ix++;
2634 iy++;
2635 }
2636 }
2637
2638 /*
2639 * Arguments : int n
2640 * const creal_T a
2641 * emxArray_creal_T *x
2642 * int ix0
2643 * int incx
2644 * Return Type : void
2645 */
b_xscal(int n,const creal_T a,emxArray_creal_T * x,int ix0,int incx)2646 static void b_xscal(int n, const creal_T a, emxArray_creal_T *x, int ix0, int
2647 incx)
2648 {
2649 int i62;
2650 int k;
2651 double x_re;
2652 double x_im;
2653 if (!(incx < 1)) {
2654 i62 = ix0 + incx * (n - 1);
2655 for (k = ix0; k <= i62; k += incx) {
2656 x_re = x->data[k - 1].re;
2657 x_im = x->data[k - 1].im;
2658 x->data[k - 1].re = a.re * x_re - a.im * x_im;
2659 x->data[k - 1].im = a.re * x_im + a.im * x_re;
2660 }
2661 }
2662 }
2663
2664 /*
2665 * Arguments : const creal_T f
2666 * const creal_T g
2667 * double *cs
2668 * creal_T *sn
2669 * Return Type : void
2670 */
b_xzlartg(const creal_T f,const creal_T g,double * cs,creal_T * sn)2671 static void b_xzlartg(const creal_T f, const creal_T g, double *cs, creal_T *sn)
2672 {
2673 double scale;
2674 double f2s;
2675 double x;
2676 double fs_re;
2677 double fs_im;
2678 double gs_re;
2679 double gs_im;
2680 boolean_T guard1 = false;
2681 double g2;
2682 double g2s;
2683 scale = fabs(f.re);
2684 f2s = fabs(f.im);
2685 if (f2s > scale) {
2686 scale = f2s;
2687 }
2688
2689 x = fabs(g.re);
2690 f2s = fabs(g.im);
2691 if (f2s > x) {
2692 x = f2s;
2693 }
2694
2695 if (x > scale) {
2696 scale = x;
2697 }
2698
2699 fs_re = f.re;
2700 fs_im = f.im;
2701 gs_re = g.re;
2702 gs_im = g.im;
2703 guard1 = false;
2704 if (scale >= 7.4428285367870146E+137) {
2705 do {
2706 fs_re *= 1.3435752215134178E-138;
2707 fs_im *= 1.3435752215134178E-138;
2708 gs_re *= 1.3435752215134178E-138;
2709 gs_im *= 1.3435752215134178E-138;
2710 scale *= 1.3435752215134178E-138;
2711 } while (!(scale < 7.4428285367870146E+137));
2712
2713 guard1 = true;
2714 } else if (scale <= 1.3435752215134178E-138) {
2715 if ((g.re == 0.0) && (g.im == 0.0)) {
2716 *cs = 1.0;
2717 sn->re = 0.0;
2718 sn->im = 0.0;
2719 } else {
2720 do {
2721 fs_re *= 7.4428285367870146E+137;
2722 fs_im *= 7.4428285367870146E+137;
2723 gs_re *= 7.4428285367870146E+137;
2724 gs_im *= 7.4428285367870146E+137;
2725 scale *= 7.4428285367870146E+137;
2726 } while (!(scale > 1.3435752215134178E-138));
2727
2728 guard1 = true;
2729 }
2730 } else {
2731 guard1 = true;
2732 }
2733
2734 if (guard1) {
2735 scale = fs_re * fs_re + fs_im * fs_im;
2736 g2 = gs_re * gs_re + gs_im * gs_im;
2737 x = g2;
2738 if (1.0 > g2) {
2739 x = 1.0;
2740 }
2741
2742 if (scale <= x * 2.0041683600089728E-292) {
2743 if ((f.re == 0.0) && (f.im == 0.0)) {
2744 *cs = 0.0;
2745 g2 = rt_hypotd_snf(gs_re, gs_im);
2746 sn->re = gs_re / g2;
2747 sn->im = -gs_im / g2;
2748 } else {
2749 g2s = sqrt(g2);
2750 *cs = rt_hypotd_snf(fs_re, fs_im) / g2s;
2751 x = fabs(f.re);
2752 f2s = fabs(f.im);
2753 if (f2s > x) {
2754 x = f2s;
2755 }
2756
2757 if (x > 1.0) {
2758 g2 = rt_hypotd_snf(f.re, f.im);
2759 fs_re = f.re / g2;
2760 fs_im = f.im / g2;
2761 } else {
2762 scale = 7.4428285367870146E+137 * f.re;
2763 f2s = 7.4428285367870146E+137 * f.im;
2764 g2 = rt_hypotd_snf(scale, f2s);
2765 fs_re = scale / g2;
2766 fs_im = f2s / g2;
2767 }
2768
2769 gs_re /= g2s;
2770 gs_im = -gs_im / g2s;
2771 sn->re = fs_re * gs_re - fs_im * gs_im;
2772 sn->im = fs_re * gs_im + fs_im * gs_re;
2773 }
2774 } else {
2775 f2s = sqrt(1.0 + g2 / scale);
2776 fs_re *= f2s;
2777 fs_im *= f2s;
2778 *cs = 1.0 / f2s;
2779 g2 += scale;
2780 fs_re /= g2;
2781 fs_im /= g2;
2782 sn->re = fs_re * gs_re - fs_im * -gs_im;
2783 sn->im = fs_re * -gs_im + fs_im * gs_re;
2784 }
2785 }
2786 }
2787
2788 /*
2789 * BUTTER_CG Butterworth digital and analog filter design. Codegen support
2790 *
2791 * This function is based on 'butter' by The MathWorks Inc.
2792 * Arguments : double Wn
2793 * double num[2]
2794 * creal_T den_data[]
2795 * int den_size[2]
2796 * Return Type : void
2797 */
butter_cg(double Wn,double num[2],creal_T den_data[],int den_size[2])2798 static void butter_cg(double Wn, double num[2], creal_T den_data[], int
2799 den_size[2])
2800 {
2801 emxArray_real_T *c;
2802 int i5;
2803 int tmp_size[2];
2804 creal_T tmp_data[1];
2805 int b_tmp_size[1];
2806 double b_tmp_data[1];
2807 emxArray_creal_T *a;
2808 emxArray_real_T *b;
2809 emxArray_creal_T *r1;
2810 emxArray_creal_T c_tmp_data;
2811 emxArray_real_T d_tmp_data;
2812 double d;
2813 int loop_ub;
2814 double y_im;
2815 double ar;
2816 double ai;
2817 emxInit_real_T(&c, 2);
2818
2819 /* Set types we will only use */
2820 /* Cast to enforce precision rules */
2821 /* step 1: get analog, pre-warped frequencies */
2822 /* step 2: convert to low-pass prototype estimate */
2823 /* lowpass */
2824 /* Cast to enforce precision rules */
2825 /* step 3: Get N-th order Butterworth analog lowpass prototype */
2826 /* Transform to state-space */
2827 /* ZP2SS Zero-pole to state-space conversion. Codegen support */
2828 /* */
2829 /* This function is based on 'zp2ss' by The MathWorks Inc. */
2830 /* Strip infinities and throw away. */
2831 /* Group into complex pairs */
2832 /* try */
2833 /* % z and p should have real elements and exact complex conjugate pair. */
2834 /* z = cplxpair(zF,0); */
2835 /* p = cplxpair(pF,0); */
2836 /* catch */
2837 /* % If fail, revert to use the old default tolerance. */
2838 /* % The use of tolerance in checking for real entries and conjugate pairs */
2839 /* % may result in misinterpretation for edge cases. Please review the */
2840 /* % process of how z and p are generated. */
2841 /* z = cplxpair(zF,1e6*nz*norm(zF)*eps + eps); */
2842 /* p = cplxpair(pF,1e6*np*norm(pF)*eps + eps); */
2843 /* end */
2844 /* Initialize state-space matrices for running series */
2845 /* If odd number of poles AND zeros, convert the pole and zero */
2846 /* at the end into state-space. */
2847 /* H(s) = (s-z1)/(s-p1) = (s + num(2)) / (s + den(2)) */
2848 /* If odd number of poles only, convert the pole at the */
2849 /* end into state-space. */
2850 /* H(s) = 1/(s-p1) = 1/(s + den(2)) */
2851 /* If odd number of zeros only, convert the zero at the */
2852 /* end, along with a pole-pair into state-space. */
2853 /* H(s) = (s+num(2))/(s^2+den(2)s+den(3)) */
2854 /* Now we have an even number of poles and zeros, although not */
2855 /* necessarily the same number - there may be more poles. */
2856 /* H(s) = (s^2+num(2)s+num(3))/(s^2+den(2)s+den(3)) */
2857 /* Loop through rest of pairs, connecting in series to build the model. */
2858 /* Take care of any left over unmatched pole pairs. */
2859 /* H(s) = 1/(s^2+den(2)s+den(3)) */
2860 /* Apply gain k: */
2861 /* step 4: Transform to lowpass, bandpass, highpass, or bandstop of desired Wn */
2862 /* Lowpass */
2863 i5 = c->size[0] * c->size[1];
2864 c->size[0] = 1;
2865 c->size[1] = 1;
2866 emxEnsureCapacity_real_T(c, i5);
2867 c->data[0] = 1.0;
2868 tmp_size[0] = 1;
2869 tmp_size[1] = 1;
2870 tmp_data[0].re = -1.0;
2871 tmp_data[0].im = 0.0;
2872 b_tmp_size[0] = 1;
2873 b_tmp_data[0] = 1.0;
2874 emxInit_creal_T(&a, 2);
2875 emxInit_real_T1(&b, 1);
2876 emxInit_creal_T(&r1, 2);
2877 c_tmp_data.data = (creal_T *)&tmp_data;
2878 c_tmp_data.size = (int *)&tmp_size;
2879 c_tmp_data.allocatedSize = 1;
2880 c_tmp_data.numDimensions = 2;
2881 c_tmp_data.canFreeData = false;
2882 d_tmp_data.data = (double *)&b_tmp_data;
2883 d_tmp_data.size = (int *)&b_tmp_size;
2884 d_tmp_data.allocatedSize = 1;
2885 d_tmp_data.numDimensions = 1;
2886 d_tmp_data.canFreeData = false;
2887 lp2lp_cg(&c_tmp_data, &d_tmp_data, Wn, a, b, &d);
2888
2889 /* step 5: Use Bilinear transformation to find discrete equivalent: */
2890 /* nargout <= 3 */
2891 /* Transform to zero-pole-gain and polynomial forms: */
2892 /* nargout <= 2 */
2893 poly(a, r1);
2894 den_size[0] = 1;
2895 den_size[1] = r1->size[1];
2896 loop_ub = r1->size[0] * r1->size[1];
2897 emxFree_real_T(&c);
2898 emxFree_real_T(&b);
2899 emxFree_creal_T(&a);
2900 for (i5 = 0; i5 < loop_ub; i5++) {
2901 den_data[i5] = r1->data[i5];
2902 }
2903
2904 emxFree_creal_T(&r1);
2905
2906 /* This internal function returns more exact numerator vectors */
2907 /* for the num/den case. */
2908 /* Wn input is two element band edge vector */
2909 /* --------------------------------- */
2910 /* lowpass */
2911 d = (0.0 * den_data[0].re - 0.0 * den_data[0].im) + den_data[1].re;
2912 y_im = (0.0 * den_data[0].im + 0.0 * den_data[0].re) + den_data[1].im;
2913 for (i5 = 0; i5 < 2; i5++) {
2914 ar = (double)i5 * d;
2915 ai = (double)i5 * y_im;
2916 if ((!(ai == 0.0)) && (ar == 0.0)) {
2917 ar = 0.0;
2918 }
2919
2920 num[i5] = ar;
2921 }
2922
2923 /* num = poly(a-b*c)+(d-1)*den; */
2924 }
2925
2926 /*
2927 * Arguments : const emxArray_real_T *x
2928 * emxArray_real_T *y
2929 * Return Type : void
2930 */
c_abs(const emxArray_real_T * x,emxArray_real_T * y)2931 static void c_abs(const emxArray_real_T *x, emxArray_real_T *y)
2932 {
2933 int k;
2934 k = y->size[0] * y->size[1];
2935 y->size[0] = 1;
2936 y->size[1] = x->size[1];
2937 emxEnsureCapacity_real_T(y, k);
2938 for (k = 0; k + 1 <= x->size[1]; k++) {
2939 y->data[k] = fabs(x->data[k]);
2940 }
2941 }
2942
2943 /*
2944 * UNTITLED Summary of this function goes here
2945 * Detailed explanation goes here
2946 * Arguments : const emxArray_real_T *f
2947 * double Fconverter
2948 * const double b1_data[]
2949 * const int b1_size[2]
2950 * const emxArray_creal_T *a1
2951 * const double b2_data[]
2952 * const int b2_size[2]
2953 * const emxArray_creal_T *a2
2954 * emxArray_creal_T *abc
2955 * Return Type : void
2956 */
c_analogresp(const emxArray_real_T * f,double Fconverter,const double b1_data[],const int b1_size[2],const emxArray_creal_T * a1,const double b2_data[],const int b2_size[2],const emxArray_creal_T * a2,emxArray_creal_T * abc)2957 static void c_analogresp(const emxArray_real_T *f, double Fconverter, const
2958 double b1_data[], const int b1_size[2], const emxArray_creal_T *a1, const
2959 double b2_data[], const int b2_size[2], const emxArray_creal_T *a2,
2960 emxArray_creal_T *abc)
2961 {
2962 emxArray_real_T *r25;
2963 int b_abc;
2964 int loop_ub;
2965 emxArray_creal_T *r26;
2966 emxArray_real_T *r27;
2967 double abc_re;
2968 double abc_im;
2969 emxInit_real_T(&r25, 2);
2970 b_abc = r25->size[0] * r25->size[1];
2971 r25->size[0] = 1;
2972 r25->size[1] = f->size[1];
2973 emxEnsureCapacity_real_T(r25, b_abc);
2974 loop_ub = f->size[0] * f->size[1];
2975 for (b_abc = 0; b_abc < loop_ub; b_abc++) {
2976 r25->data[b_abc] = 6.2831853071795862 * f->data[b_abc];
2977 }
2978
2979 b_freqs_cg(b1_data, b1_size, a1, r25, abc);
2980 b_abc = r25->size[0] * r25->size[1];
2981 r25->size[0] = 1;
2982 r25->size[1] = f->size[1];
2983 emxEnsureCapacity_real_T(r25, b_abc);
2984 loop_ub = f->size[0] * f->size[1];
2985 for (b_abc = 0; b_abc < loop_ub; b_abc++) {
2986 r25->data[b_abc] = 6.2831853071795862 * f->data[b_abc];
2987 }
2988
2989 emxInit_creal_T(&r26, 2);
2990 emxInit_real_T(&r27, 2);
2991 b_freqs_cg(b2_data, b2_size, a2, r25, r26);
2992 rdivide(f, Fconverter, r25);
2993 b_sinc(r25);
2994 c_power(r25, r27);
2995 b_abc = abc->size[0] * abc->size[1];
2996 abc->size[0] = 1;
2997 emxEnsureCapacity_creal_T(abc, b_abc);
2998 b_abc = abc->size[0];
2999 loop_ub = abc->size[1];
3000 loop_ub *= b_abc;
3001 emxFree_real_T(&r25);
3002 for (b_abc = 0; b_abc < loop_ub; b_abc++) {
3003 abc_re = abc->data[b_abc].re * r26->data[b_abc].re - abc->data[b_abc].im *
3004 r26->data[b_abc].im;
3005 abc_im = abc->data[b_abc].re * r26->data[b_abc].im + abc->data[b_abc].im *
3006 r26->data[b_abc].re;
3007 abc->data[b_abc].re = r27->data[b_abc] * abc_re;
3008 abc->data[b_abc].im = r27->data[b_abc] * abc_im;
3009 }
3010
3011 emxFree_real_T(&r27);
3012 emxFree_creal_T(&r26);
3013 }
3014
3015 /*
3016 * Arguments : emxArray_creal_T *x
3017 * Return Type : void
3018 */
c_exp(emxArray_creal_T * x)3019 static void c_exp(emxArray_creal_T *x)
3020 {
3021 int nx;
3022 int k;
3023 double x_re;
3024 double r;
3025 nx = x->size[1];
3026 for (k = 0; k + 1 <= nx; k++) {
3027 if (x->data[k].im == 0.0) {
3028 x_re = exp(x->data[k].re);
3029 r = 0.0;
3030 } else if (rtIsInf(x->data[k].im) && rtIsInf(x->data[k].re) && (x->data[k].
3031 re < 0.0)) {
3032 x_re = 0.0;
3033 r = 0.0;
3034 } else {
3035 r = exp(x->data[k].re / 2.0);
3036 x_re = r * (r * cos(x->data[k].im));
3037 r *= r * sin(x->data[k].im);
3038 }
3039
3040 x->data[k].re = x_re;
3041 x->data[k].im = r;
3042 }
3043 }
3044
3045 /*
3046 * Make b a row
3047 * Arguments : const double b[7]
3048 * const struct_T *options
3049 * creal_T h[2048]
3050 * double w[2048]
3051 * Return Type : void
3052 */
c_firfreqz(const double b[7],const struct_T * options,creal_T h[2048],double w[2048])3053 static void c_firfreqz(const double b[7], const struct_T *options, creal_T h
3054 [2048], double w[2048])
3055 {
3056 int i65;
3057 creal_T dcv3[2048];
3058 double digw;
3059 double b_digw[2048];
3060 double h_re;
3061 double brm;
3062 double d;
3063
3064 /* -------------------------------------------------------------------------- */
3065 /* Actual Frequency Response Computation */
3066 /* if fvflag, */
3067 /* Frequency vector specified. Use Horner's method of polynomial */
3068 /* evaluation at the frequency points and divide the numerator */
3069 /* by the denominator. */
3070 /* */
3071 /* Note: we use positive i here because of the relationship */
3072 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
3073 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
3074 /* */
3075 /* Fs was specified, freq. vector is in Hz */
3076 /* Convert from Hz to rad/sample for computational purposes */
3077 /* Digital frequency must be used for this calculation */
3078 for (i65 = 0; i65 < 2048; i65++) {
3079 w[i65] = options->w[i65];
3080 digw = 6.2831853071795862 * options->w[i65] / options->Fs;
3081 dcv3[i65].re = digw * 0.0;
3082 dcv3[i65].im = digw;
3083 b_digw[i65] = digw;
3084 }
3085
3086 b_exp(dcv3);
3087 b_polyval(b, dcv3, h);
3088 for (i65 = 0; i65 < 2048; i65++) {
3089 dcv3[i65].re = 6.0 * (b_digw[i65] * 0.0);
3090 dcv3[i65].im = 6.0 * b_digw[i65];
3091 }
3092
3093 b_exp(dcv3);
3094 for (i65 = 0; i65 < 2048; i65++) {
3095 h_re = h[i65].re;
3096 if (dcv3[i65].im == 0.0) {
3097 if (h[i65].im == 0.0) {
3098 h[i65].re /= dcv3[i65].re;
3099 h[i65].im = 0.0;
3100 } else if (h[i65].re == 0.0) {
3101 h[i65].re = 0.0;
3102 h[i65].im /= dcv3[i65].re;
3103 } else {
3104 h[i65].re /= dcv3[i65].re;
3105 h[i65].im /= dcv3[i65].re;
3106 }
3107 } else if (dcv3[i65].re == 0.0) {
3108 if (h[i65].re == 0.0) {
3109 h[i65].re = h[i65].im / dcv3[i65].im;
3110 h[i65].im = 0.0;
3111 } else if (h[i65].im == 0.0) {
3112 h[i65].re = 0.0;
3113 h[i65].im = -(h_re / dcv3[i65].im);
3114 } else {
3115 h[i65].re = h[i65].im / dcv3[i65].im;
3116 h[i65].im = -(h_re / dcv3[i65].im);
3117 }
3118 } else {
3119 brm = fabs(dcv3[i65].re);
3120 digw = fabs(dcv3[i65].im);
3121 if (brm > digw) {
3122 digw = dcv3[i65].im / dcv3[i65].re;
3123 d = dcv3[i65].re + digw * dcv3[i65].im;
3124 h[i65].re = (h[i65].re + digw * h[i65].im) / d;
3125 h[i65].im = (h[i65].im - digw * h_re) / d;
3126 } else if (digw == brm) {
3127 if (dcv3[i65].re > 0.0) {
3128 digw = 0.5;
3129 } else {
3130 digw = -0.5;
3131 }
3132
3133 if (dcv3[i65].im > 0.0) {
3134 d = 0.5;
3135 } else {
3136 d = -0.5;
3137 }
3138
3139 h[i65].re = (h[i65].re * digw + h[i65].im * d) / brm;
3140 h[i65].im = (h[i65].im * digw - h_re * d) / brm;
3141 } else {
3142 digw = dcv3[i65].re / dcv3[i65].im;
3143 d = dcv3[i65].im + digw * dcv3[i65].re;
3144 h[i65].re = (digw * h[i65].re + h[i65].im) / d;
3145 h[i65].im = (digw * h[i65].im - h_re) / d;
3146 }
3147 }
3148 }
3149 }
3150
3151 /*
3152 * Arguments : emxArray_real_T *x
3153 * Return Type : void
3154 */
c_fix(emxArray_real_T * x)3155 static void c_fix(emxArray_real_T *x)
3156 {
3157 int nx;
3158 int k;
3159 double b_x;
3160 nx = x->size[1];
3161 for (k = 0; k + 1 <= nx; k++) {
3162 if (x->data[k] < 0.0) {
3163 b_x = ceil(x->data[k]);
3164 } else {
3165 b_x = floor(x->data[k]);
3166 }
3167
3168 x->data[k] = b_x;
3169 }
3170 }
3171
3172 /*
3173 * FREQZ_CG Frequency response of digital filter with codegen support
3174 *
3175 * This function is based on 'freqz' by The MathWorks Inc.
3176 * Arguments : const double b[7]
3177 * const double w[2048]
3178 * double Fs
3179 * creal_T hh[2048]
3180 * Return Type : void
3181 */
c_freqz_cg(const double b[7],const double w[2048],double Fs,creal_T hh[2048])3182 static void c_freqz_cg(const double b[7], const double w[2048], double Fs,
3183 creal_T hh[2048])
3184 {
3185 static struct_T options;
3186 int i12;
3187 static const char cv16[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
3188
3189 double b_w[2048];
3190 static const char cv17[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
3191
3192 /* Cast to enforce precision rules */
3193 options.Fs = Fs;
3194 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
3195 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
3196
3197 /* Remaining are default or for advanced use */
3198 options.fvflag = 1.0;
3199 for (i12 = 0; i12 < 8; i12++) {
3200 options.range[i12] = cv16[i12];
3201 }
3202
3203 options.centerdc = 0.0;
3204 for (i12 = 0; i12 < 7; i12++) {
3205 options.configlevel[i12] = cv17[i12];
3206 }
3207
3208 c_firfreqz(b, &options, hh, b_w);
3209
3210 /* Generate the default structure to pass to freqzplot */
3211 /* If rad/sample, Fs is empty */
3212 }
3213
3214 /*
3215 * Arguments : const char enables[4]
3216 * const emxArray_real_T *w
3217 * double Fs
3218 * const double hb1_coeff[15]
3219 * const double hb2_coeff[7]
3220 * const double hb3_coeff_data[]
3221 * const int hb3_coeff_size[2]
3222 * const double dec_int3_coeff_data[]
3223 * const int dec_int3_coeff_size[2]
3224 * const emxArray_real_T *extraTaps
3225 * emxArray_creal_T *combinedResponse
3226 * Return Type : void
3227 */
c_generateCascadedResponseRx(const char enables[4],const emxArray_real_T * w,double Fs,const double hb1_coeff[15],const double hb2_coeff[7],const double hb3_coeff_data[],const int hb3_coeff_size[2],const double dec_int3_coeff_data[],const int dec_int3_coeff_size[2],const emxArray_real_T * extraTaps,emxArray_creal_T * combinedResponse)3228 static void c_generateCascadedResponseRx(const char enables[4], const
3229 emxArray_real_T *w, double Fs, const double hb1_coeff[15], const double
3230 hb2_coeff[7], const double hb3_coeff_data[], const int hb3_coeff_size[2],
3231 const double dec_int3_coeff_data[], const int dec_int3_coeff_size[2], const
3232 emxArray_real_T *extraTaps, emxArray_creal_T *combinedResponse)
3233 {
3234 boolean_T b_bool;
3235 int iy;
3236 int exitg1;
3237 emxArray_creal_T *d2;
3238 emxArray_creal_T *d3;
3239 static const char cv48[4] = { '2', '1', '1', '1' };
3240
3241 double u[15];
3242 int stages;
3243 double tmp_data[29];
3244 int tmp_size[2];
3245 double b_u[30];
3246 double c_u[14];
3247 double d_u[60];
3248 double e_u[45];
3249 double f_u[21];
3250 double g_u[90];
3251 emxArray_real_T b_tmp_data;
3252 double h_u[7];
3253 emxArray_real_T *i_u;
3254 int k;
3255 int c;
3256 static const char cv49[4] = { '1', '2', '1', '1' };
3257
3258 static const char cv50[4] = { '1', '1', '2', '1' };
3259
3260 double combinedResponse_re;
3261 double combinedResponse_im;
3262 double d2_re;
3263 double d2_im;
3264 static const char cv51[4] = { '2', '2', '1', '1' };
3265
3266 static const char cv52[4] = { '2', '1', '2', '1' };
3267
3268 static const char cv53[4] = { '1', '2', '2', '1' };
3269
3270 static const char cv54[4] = { '2', '2', '2', '1' };
3271
3272 static const char cv55[4] = { '1', '1', '1', '3' };
3273
3274 static const char cv56[4] = { '2', '1', '1', '3' };
3275
3276 static const char cv57[4] = { '1', '2', '1', '3' };
3277
3278 static const char cv58[4] = { '2', '2', '1', '3' };
3279
3280 /* Cast */
3281 b_bool = false;
3282 iy = 1;
3283 do {
3284 exitg1 = 0;
3285 if (iy < 5) {
3286 if (enables[iy - 1] != '1') {
3287 exitg1 = 1;
3288 } else {
3289 iy++;
3290 }
3291 } else {
3292 b_bool = true;
3293 exitg1 = 1;
3294 }
3295 } while (exitg1 == 0);
3296
3297 if (b_bool) {
3298 iy = 0;
3299 } else {
3300 b_bool = false;
3301 iy = 0;
3302 do {
3303 exitg1 = 0;
3304 if (iy + 1 < 5) {
3305 if (enables[iy] != cv48[iy]) {
3306 exitg1 = 1;
3307 } else {
3308 iy++;
3309 }
3310 } else {
3311 b_bool = true;
3312 exitg1 = 1;
3313 }
3314 } while (exitg1 == 0);
3315
3316 if (b_bool) {
3317 iy = 1;
3318 } else {
3319 b_bool = false;
3320 iy = 0;
3321 do {
3322 exitg1 = 0;
3323 if (iy + 1 < 5) {
3324 if (enables[iy] != cv49[iy]) {
3325 exitg1 = 1;
3326 } else {
3327 iy++;
3328 }
3329 } else {
3330 b_bool = true;
3331 exitg1 = 1;
3332 }
3333 } while (exitg1 == 0);
3334
3335 if (b_bool) {
3336 iy = 2;
3337 } else {
3338 b_bool = false;
3339 iy = 0;
3340 do {
3341 exitg1 = 0;
3342 if (iy + 1 < 5) {
3343 if (enables[iy] != cv50[iy]) {
3344 exitg1 = 1;
3345 } else {
3346 iy++;
3347 }
3348 } else {
3349 b_bool = true;
3350 exitg1 = 1;
3351 }
3352 } while (exitg1 == 0);
3353
3354 if (b_bool) {
3355 iy = 3;
3356 } else {
3357 b_bool = false;
3358 iy = 0;
3359 do {
3360 exitg1 = 0;
3361 if (iy + 1 < 5) {
3362 if (enables[iy] != cv51[iy]) {
3363 exitg1 = 1;
3364 } else {
3365 iy++;
3366 }
3367 } else {
3368 b_bool = true;
3369 exitg1 = 1;
3370 }
3371 } while (exitg1 == 0);
3372
3373 if (b_bool) {
3374 iy = 4;
3375 } else {
3376 b_bool = false;
3377 iy = 0;
3378 do {
3379 exitg1 = 0;
3380 if (iy + 1 < 5) {
3381 if (enables[iy] != cv52[iy]) {
3382 exitg1 = 1;
3383 } else {
3384 iy++;
3385 }
3386 } else {
3387 b_bool = true;
3388 exitg1 = 1;
3389 }
3390 } while (exitg1 == 0);
3391
3392 if (b_bool) {
3393 iy = 5;
3394 } else {
3395 b_bool = false;
3396 iy = 0;
3397 do {
3398 exitg1 = 0;
3399 if (iy + 1 < 5) {
3400 if (enables[iy] != cv53[iy]) {
3401 exitg1 = 1;
3402 } else {
3403 iy++;
3404 }
3405 } else {
3406 b_bool = true;
3407 exitg1 = 1;
3408 }
3409 } while (exitg1 == 0);
3410
3411 if (b_bool) {
3412 iy = 6;
3413 } else {
3414 b_bool = false;
3415 iy = 0;
3416 do {
3417 exitg1 = 0;
3418 if (iy + 1 < 5) {
3419 if (enables[iy] != cv54[iy]) {
3420 exitg1 = 1;
3421 } else {
3422 iy++;
3423 }
3424 } else {
3425 b_bool = true;
3426 exitg1 = 1;
3427 }
3428 } while (exitg1 == 0);
3429
3430 if (b_bool) {
3431 iy = 7;
3432 } else {
3433 b_bool = false;
3434 iy = 0;
3435 do {
3436 exitg1 = 0;
3437 if (iy + 1 < 5) {
3438 if (enables[iy] != cv55[iy]) {
3439 exitg1 = 1;
3440 } else {
3441 iy++;
3442 }
3443 } else {
3444 b_bool = true;
3445 exitg1 = 1;
3446 }
3447 } while (exitg1 == 0);
3448
3449 if (b_bool) {
3450 iy = 8;
3451 } else {
3452 b_bool = false;
3453 iy = 0;
3454 do {
3455 exitg1 = 0;
3456 if (iy + 1 < 5) {
3457 if (enables[iy] != cv56[iy]) {
3458 exitg1 = 1;
3459 } else {
3460 iy++;
3461 }
3462 } else {
3463 b_bool = true;
3464 exitg1 = 1;
3465 }
3466 } while (exitg1 == 0);
3467
3468 if (b_bool) {
3469 iy = 9;
3470 } else {
3471 b_bool = false;
3472 iy = 0;
3473 do {
3474 exitg1 = 0;
3475 if (iy + 1 < 5) {
3476 if (enables[iy] != cv57[iy]) {
3477 exitg1 = 1;
3478 } else {
3479 iy++;
3480 }
3481 } else {
3482 b_bool = true;
3483 exitg1 = 1;
3484 }
3485 } while (exitg1 == 0);
3486
3487 if (b_bool) {
3488 iy = 10;
3489 } else {
3490 b_bool = false;
3491 iy = 0;
3492 do {
3493 exitg1 = 0;
3494 if (iy + 1 < 5) {
3495 if (enables[iy] != cv58[iy]) {
3496 exitg1 = 1;
3497 } else {
3498 iy++;
3499 }
3500 } else {
3501 b_bool = true;
3502 exitg1 = 1;
3503 }
3504 } while (exitg1 == 0);
3505
3506 if (b_bool) {
3507 iy = 11;
3508 } else {
3509 iy = -1;
3510 }
3511 }
3512 }
3513 }
3514 }
3515 }
3516 }
3517 }
3518 }
3519 }
3520 }
3521 }
3522
3523 emxInit_creal_T(&d2, 2);
3524 emxInit_creal_T(&d3, 2);
3525 switch (iy) {
3526 case 0:
3527 /* only FIR */
3528 k_freqz_cg(w, Fs, combinedResponse);
3529 stages = 1;
3530 break;
3531
3532 case 1:
3533 /* Hb1 */
3534 memset(&u[0], 0, 15U * sizeof(double));
3535 stages = 0;
3536 iy = 0;
3537 for (k = 0; k < 15; k++) {
3538 u[iy] = hb1_coeff[stages];
3539 stages++;
3540 iy++;
3541 }
3542
3543 l_freqz_cg(u, w, Fs, combinedResponse);
3544 stages = 1;
3545 break;
3546
3547 case 2:
3548 /* Hb2 */
3549 for (stages = 0; stages < 7; stages++) {
3550 h_u[stages] = 0.0;
3551 }
3552
3553 stages = 0;
3554 iy = 0;
3555 for (k = 0; k < 7; k++) {
3556 h_u[iy] = hb2_coeff[stages];
3557 stages++;
3558 iy++;
3559 }
3560
3561 m_freqz_cg(h_u, w, Fs, combinedResponse);
3562 stages = 1;
3563 break;
3564
3565 case 3:
3566 /* Hb3 */
3567 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
3568 b_tmp_data.data = (double *)&tmp_data;
3569 b_tmp_data.size = (int *)&tmp_size;
3570 b_tmp_data.allocatedSize = 29;
3571 b_tmp_data.numDimensions = 2;
3572 b_tmp_data.canFreeData = false;
3573 n_freqz_cg(&b_tmp_data, w, Fs, combinedResponse);
3574 stages = 1;
3575 break;
3576
3577 case 4:
3578 /* Hb2,Hb1 */
3579 memset(&b_u[0], 0, 30U * sizeof(double));
3580 stages = 0;
3581 iy = 0;
3582 for (k = 0; k < 15; k++) {
3583 b_u[iy] = hb1_coeff[stages];
3584 stages++;
3585 iy += 2;
3586 }
3587
3588 o_freqz_cg(*(double (*)[29])&b_u[0], w, Fs, combinedResponse);
3589 for (stages = 0; stages < 7; stages++) {
3590 h_u[stages] = 0.0;
3591 }
3592
3593 stages = 0;
3594 iy = 0;
3595 for (k = 0; k < 7; k++) {
3596 h_u[iy] = hb2_coeff[stages];
3597 stages++;
3598 iy++;
3599 }
3600
3601 m_freqz_cg(h_u, w, Fs, d2);
3602 stages = combinedResponse->size[0] * combinedResponse->size[1];
3603 combinedResponse->size[0] = 1;
3604 emxEnsureCapacity_creal_T(combinedResponse, stages);
3605 iy = combinedResponse->size[0];
3606 stages = combinedResponse->size[1];
3607 iy *= stages;
3608 for (stages = 0; stages < iy; stages++) {
3609 combinedResponse_re = combinedResponse->data[stages].re;
3610 combinedResponse_im = combinedResponse->data[stages].im;
3611 d2_re = d2->data[stages].re;
3612 d2_im = d2->data[stages].im;
3613 combinedResponse->data[stages].re = combinedResponse_re * d2_re -
3614 combinedResponse_im * d2_im;
3615 combinedResponse->data[stages].im = combinedResponse_re * d2_im +
3616 combinedResponse_im * d2_re;
3617 }
3618
3619 stages = 2;
3620 break;
3621
3622 case 5:
3623 /* Hb3,Hb1 */
3624 memset(&b_u[0], 0, 30U * sizeof(double));
3625 stages = 0;
3626 iy = 0;
3627 for (k = 0; k < 15; k++) {
3628 b_u[iy] = hb1_coeff[stages];
3629 stages++;
3630 iy += 2;
3631 }
3632
3633 o_freqz_cg(*(double (*)[29])&b_u[0], w, Fs, combinedResponse);
3634 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
3635 b_tmp_data.data = (double *)&tmp_data;
3636 b_tmp_data.size = (int *)&tmp_size;
3637 b_tmp_data.allocatedSize = 29;
3638 b_tmp_data.numDimensions = 2;
3639 b_tmp_data.canFreeData = false;
3640 n_freqz_cg(&b_tmp_data, w, Fs, d2);
3641 stages = combinedResponse->size[0] * combinedResponse->size[1];
3642 combinedResponse->size[0] = 1;
3643 emxEnsureCapacity_creal_T(combinedResponse, stages);
3644 iy = combinedResponse->size[0];
3645 stages = combinedResponse->size[1];
3646 iy *= stages;
3647 for (stages = 0; stages < iy; stages++) {
3648 combinedResponse_re = combinedResponse->data[stages].re;
3649 combinedResponse_im = combinedResponse->data[stages].im;
3650 d2_re = d2->data[stages].re;
3651 d2_im = d2->data[stages].im;
3652 combinedResponse->data[stages].re = combinedResponse_re * d2_re -
3653 combinedResponse_im * d2_im;
3654 combinedResponse->data[stages].im = combinedResponse_re * d2_im +
3655 combinedResponse_im * d2_re;
3656 }
3657
3658 stages = 2;
3659 break;
3660
3661 case 6:
3662 /* Hb3,Hb2 */
3663 memset(&c_u[0], 0, 14U * sizeof(double));
3664 stages = 0;
3665 iy = 0;
3666 for (k = 0; k < 7; k++) {
3667 c_u[iy] = hb2_coeff[stages];
3668 stages++;
3669 iy += 2;
3670 }
3671
3672 p_freqz_cg(*(double (*)[13])&c_u[0], w, Fs, combinedResponse);
3673 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
3674 b_tmp_data.data = (double *)&tmp_data;
3675 b_tmp_data.size = (int *)&tmp_size;
3676 b_tmp_data.allocatedSize = 29;
3677 b_tmp_data.numDimensions = 2;
3678 b_tmp_data.canFreeData = false;
3679 n_freqz_cg(&b_tmp_data, w, Fs, d2);
3680 stages = combinedResponse->size[0] * combinedResponse->size[1];
3681 combinedResponse->size[0] = 1;
3682 emxEnsureCapacity_creal_T(combinedResponse, stages);
3683 iy = combinedResponse->size[0];
3684 stages = combinedResponse->size[1];
3685 iy *= stages;
3686 for (stages = 0; stages < iy; stages++) {
3687 combinedResponse_re = combinedResponse->data[stages].re;
3688 combinedResponse_im = combinedResponse->data[stages].im;
3689 d2_re = d2->data[stages].re;
3690 d2_im = d2->data[stages].im;
3691 combinedResponse->data[stages].re = combinedResponse_re * d2_re -
3692 combinedResponse_im * d2_im;
3693 combinedResponse->data[stages].im = combinedResponse_re * d2_im +
3694 combinedResponse_im * d2_re;
3695 }
3696
3697 stages = 2;
3698 break;
3699
3700 case 7:
3701 /* Hb3,Hb2,Hb1 */
3702 memset(&d_u[0], 0, 60U * sizeof(double));
3703 stages = 0;
3704 iy = 0;
3705 for (k = 0; k < 15; k++) {
3706 d_u[iy] = hb1_coeff[stages];
3707 stages++;
3708 iy += 4;
3709 }
3710
3711 q_freqz_cg(*(double (*)[57])&d_u[0], w, Fs, combinedResponse);
3712 memset(&c_u[0], 0, 14U * sizeof(double));
3713 stages = 0;
3714 iy = 0;
3715 for (k = 0; k < 7; k++) {
3716 c_u[iy] = hb2_coeff[stages];
3717 stages++;
3718 iy += 2;
3719 }
3720
3721 p_freqz_cg(*(double (*)[13])&c_u[0], w, Fs, d2);
3722 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
3723 b_tmp_data.data = (double *)&tmp_data;
3724 b_tmp_data.size = (int *)&tmp_size;
3725 b_tmp_data.allocatedSize = 29;
3726 b_tmp_data.numDimensions = 2;
3727 b_tmp_data.canFreeData = false;
3728 n_freqz_cg(&b_tmp_data, w, Fs, d3);
3729 stages = combinedResponse->size[0] * combinedResponse->size[1];
3730 combinedResponse->size[0] = 1;
3731 emxEnsureCapacity_creal_T(combinedResponse, stages);
3732 iy = combinedResponse->size[0];
3733 stages = combinedResponse->size[1];
3734 iy *= stages;
3735 for (stages = 0; stages < iy; stages++) {
3736 combinedResponse_re = combinedResponse->data[stages].re * d2->data[stages]
3737 .re - combinedResponse->data[stages].im * d2->data[stages].im;
3738 combinedResponse_im = combinedResponse->data[stages].re * d2->data[stages]
3739 .im + combinedResponse->data[stages].im * d2->data[stages].re;
3740 d2_re = d3->data[stages].re;
3741 d2_im = d3->data[stages].im;
3742 combinedResponse->data[stages].re = combinedResponse_re * d2_re -
3743 combinedResponse_im * d2_im;
3744 combinedResponse->data[stages].im = combinedResponse_re * d2_im +
3745 combinedResponse_im * d2_re;
3746 }
3747
3748 stages = 3;
3749 break;
3750
3751 case 8:
3752 /* Dec/Int3 */
3753 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
3754 b_tmp_data.data = (double *)&tmp_data;
3755 b_tmp_data.size = (int *)&tmp_size;
3756 b_tmp_data.allocatedSize = 29;
3757 b_tmp_data.numDimensions = 2;
3758 b_tmp_data.canFreeData = false;
3759 n_freqz_cg(&b_tmp_data, w, Fs, combinedResponse);
3760 stages = 1;
3761
3762 /* RECHECK ALL DEC BY 3 */
3763 break;
3764
3765 case 9:
3766 /* Dec/Int3,Hb1 */
3767 memset(&e_u[0], 0, 45U * sizeof(double));
3768 stages = 0;
3769 iy = 0;
3770 for (k = 0; k < 15; k++) {
3771 e_u[iy] = hb1_coeff[stages];
3772 stages++;
3773 iy += 3;
3774 }
3775
3776 r_freqz_cg(*(double (*)[43])&e_u[0], w, Fs, combinedResponse);
3777 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
3778 b_tmp_data.data = (double *)&tmp_data;
3779 b_tmp_data.size = (int *)&tmp_size;
3780 b_tmp_data.allocatedSize = 29;
3781 b_tmp_data.numDimensions = 2;
3782 b_tmp_data.canFreeData = false;
3783 n_freqz_cg(&b_tmp_data, w, Fs, d2);
3784 stages = combinedResponse->size[0] * combinedResponse->size[1];
3785 combinedResponse->size[0] = 1;
3786 emxEnsureCapacity_creal_T(combinedResponse, stages);
3787 iy = combinedResponse->size[0];
3788 stages = combinedResponse->size[1];
3789 iy *= stages;
3790 for (stages = 0; stages < iy; stages++) {
3791 combinedResponse_re = combinedResponse->data[stages].re;
3792 combinedResponse_im = combinedResponse->data[stages].im;
3793 d2_re = d2->data[stages].re;
3794 d2_im = d2->data[stages].im;
3795 combinedResponse->data[stages].re = combinedResponse_re * d2_re -
3796 combinedResponse_im * d2_im;
3797 combinedResponse->data[stages].im = combinedResponse_re * d2_im +
3798 combinedResponse_im * d2_re;
3799 }
3800
3801 stages = 2;
3802 break;
3803
3804 case 10:
3805 /* Dec/Int3,Hb2 */
3806 memset(&f_u[0], 0, 21U * sizeof(double));
3807 stages = 0;
3808 iy = 0;
3809 for (k = 0; k < 7; k++) {
3810 f_u[iy] = hb2_coeff[stages];
3811 stages++;
3812 iy += 3;
3813 }
3814
3815 s_freqz_cg(*(double (*)[19])&f_u[0], w, Fs, combinedResponse);
3816 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
3817 b_tmp_data.data = (double *)&tmp_data;
3818 b_tmp_data.size = (int *)&tmp_size;
3819 b_tmp_data.allocatedSize = 29;
3820 b_tmp_data.numDimensions = 2;
3821 b_tmp_data.canFreeData = false;
3822 n_freqz_cg(&b_tmp_data, w, Fs, d2);
3823 stages = combinedResponse->size[0] * combinedResponse->size[1];
3824 combinedResponse->size[0] = 1;
3825 emxEnsureCapacity_creal_T(combinedResponse, stages);
3826 iy = combinedResponse->size[0];
3827 stages = combinedResponse->size[1];
3828 iy *= stages;
3829 for (stages = 0; stages < iy; stages++) {
3830 combinedResponse_re = combinedResponse->data[stages].re;
3831 combinedResponse_im = combinedResponse->data[stages].im;
3832 d2_re = d2->data[stages].re;
3833 d2_im = d2->data[stages].im;
3834 combinedResponse->data[stages].re = combinedResponse_re * d2_re -
3835 combinedResponse_im * d2_im;
3836 combinedResponse->data[stages].im = combinedResponse_re * d2_im +
3837 combinedResponse_im * d2_re;
3838 }
3839
3840 stages = 2;
3841 break;
3842
3843 case 11:
3844 /* Dec/Int3,Hb2,Hb1 {Hm4,Hm2c34,Hm1} */
3845 memset(&g_u[0], 0, 90U * sizeof(double));
3846 stages = 0;
3847 iy = 0;
3848 for (k = 0; k < 15; k++) {
3849 g_u[iy] = hb1_coeff[stages];
3850 stages++;
3851 iy += 6;
3852 }
3853
3854 t_freqz_cg(*(double (*)[85])&g_u[0], w, Fs, combinedResponse);
3855 memset(&f_u[0], 0, 21U * sizeof(double));
3856 stages = 0;
3857 iy = 0;
3858 for (k = 0; k < 7; k++) {
3859 f_u[iy] = hb2_coeff[stages];
3860 stages++;
3861 iy += 3;
3862 }
3863
3864 s_freqz_cg(*(double (*)[19])&f_u[0], w, Fs, d2);
3865 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
3866 b_tmp_data.data = (double *)&tmp_data;
3867 b_tmp_data.size = (int *)&tmp_size;
3868 b_tmp_data.allocatedSize = 29;
3869 b_tmp_data.numDimensions = 2;
3870 b_tmp_data.canFreeData = false;
3871 n_freqz_cg(&b_tmp_data, w, Fs, d3);
3872 stages = combinedResponse->size[0] * combinedResponse->size[1];
3873 combinedResponse->size[0] = 1;
3874 emxEnsureCapacity_creal_T(combinedResponse, stages);
3875 iy = combinedResponse->size[0];
3876 stages = combinedResponse->size[1];
3877 iy *= stages;
3878 for (stages = 0; stages < iy; stages++) {
3879 combinedResponse_re = combinedResponse->data[stages].re * d2->data[stages]
3880 .re - combinedResponse->data[stages].im * d2->data[stages].im;
3881 combinedResponse_im = combinedResponse->data[stages].re * d2->data[stages]
3882 .im + combinedResponse->data[stages].im * d2->data[stages].re;
3883 d2_re = d3->data[stages].re;
3884 d2_im = d3->data[stages].im;
3885 combinedResponse->data[stages].re = combinedResponse_re * d2_re -
3886 combinedResponse_im * d2_im;
3887 combinedResponse->data[stages].im = combinedResponse_re * d2_im +
3888 combinedResponse_im * d2_re;
3889 }
3890
3891 stages = 3;
3892 break;
3893 }
3894
3895 emxFree_creal_T(&d3);
3896
3897 /* Add filter extra filter to end of cascade */
3898 if (!(extraTaps->size[1] == 0)) {
3899 emxInit_real_T(&i_u, 2);
3900 c = (int)rt_powd_snf(2.0, stages);
3901 iy = c * extraTaps->size[1];
3902 stages = i_u->size[0] * i_u->size[1];
3903 i_u->size[0] = 1;
3904 i_u->size[1] = iy;
3905 emxEnsureCapacity_real_T(i_u, stages);
3906 for (stages = 0; stages < iy; stages++) {
3907 i_u->data[stages] = 0.0;
3908 }
3909
3910 stages = 0;
3911 iy = 0;
3912 for (k = 1; k <= extraTaps->size[1]; k++) {
3913 i_u->data[iy] = extraTaps->data[stages];
3914 stages++;
3915 iy += c;
3916 }
3917
3918 stages = (i_u->size[1] - c) + 1;
3919 iy = i_u->size[0] * i_u->size[1];
3920 if (1 > stages) {
3921 i_u->size[1] = 0;
3922 } else {
3923 i_u->size[1] = stages;
3924 }
3925
3926 emxEnsureCapacity_real_T(i_u, iy);
3927 n_freqz_cg(i_u, w, Fs, d2);
3928 stages = combinedResponse->size[0] * combinedResponse->size[1];
3929 combinedResponse->size[0] = 1;
3930 emxEnsureCapacity_creal_T(combinedResponse, stages);
3931 iy = combinedResponse->size[0];
3932 stages = combinedResponse->size[1];
3933 iy *= stages;
3934 emxFree_real_T(&i_u);
3935 for (stages = 0; stages < iy; stages++) {
3936 combinedResponse_re = combinedResponse->data[stages].re;
3937 combinedResponse_im = combinedResponse->data[stages].im;
3938 d2_re = d2->data[stages].re;
3939 d2_im = d2->data[stages].im;
3940 combinedResponse->data[stages].re = combinedResponse_re * d2_re -
3941 combinedResponse_im * d2_im;
3942 combinedResponse->data[stages].im = combinedResponse_re * d2_im +
3943 combinedResponse_im * d2_re;
3944 }
3945 }
3946
3947 emxFree_creal_T(&d2);
3948 }
3949
3950 /*
3951 * Arguments : const double p_data[]
3952 * const int p_size[2]
3953 * const creal_T x[2048]
3954 * creal_T y[2048]
3955 * Return Type : void
3956 */
c_polyval(const double p_data[],const int p_size[2],const creal_T x[2048],creal_T y[2048])3957 static void c_polyval(const double p_data[], const int p_size[2], const creal_T
3958 x[2048], creal_T y[2048])
3959 {
3960 int i13;
3961 int k;
3962 double x_im;
3963 if (!(p_size[1] == 0)) {
3964 for (i13 = 0; i13 < 2048; i13++) {
3965 y[i13].re = p_data[0];
3966 y[i13].im = 0.0;
3967 }
3968
3969 for (k = 0; k <= p_size[1] - 2; k++) {
3970 for (i13 = 0; i13 < 2048; i13++) {
3971 x_im = x[i13].re * y[i13].im + x[i13].im * y[i13].re;
3972 y[i13].re = (x[i13].re * y[i13].re - x[i13].im * y[i13].im) + p_data[k +
3973 1];
3974 y[i13].im = x_im;
3975 }
3976 }
3977 }
3978 }
3979
3980 /*
3981 * Arguments : const emxArray_real_T *a
3982 * emxArray_real_T *y
3983 * Return Type : void
3984 */
c_power(const emxArray_real_T * a,emxArray_real_T * y)3985 static void c_power(const emxArray_real_T *a, emxArray_real_T *y)
3986 {
3987 int unnamed_idx_1;
3988 int k;
3989 unnamed_idx_1 = a->size[1];
3990 k = y->size[0] * y->size[1];
3991 y->size[0] = 1;
3992 y->size[1] = a->size[1];
3993 emxEnsureCapacity_real_T(y, k);
3994 for (k = 0; k + 1 <= unnamed_idx_1; k++) {
3995 y->data[k] = rt_powd_snf(a->data[k], 3.0);
3996 }
3997 }
3998
3999 /*
4000 * Arguments : const emxArray_real_T *x
4001 * const emxArray_real_T *y
4002 * emxArray_real_T *z
4003 * Return Type : void
4004 */
c_rdivide(const emxArray_real_T * x,const emxArray_real_T * y,emxArray_real_T * z)4005 static void c_rdivide(const emxArray_real_T *x, const emxArray_real_T *y,
4006 emxArray_real_T *z)
4007 {
4008 int i50;
4009 int loop_ub;
4010 i50 = z->size[0] * z->size[1];
4011 z->size[0] = 1;
4012 z->size[1] = x->size[1];
4013 emxEnsureCapacity_real_T(z, i50);
4014 loop_ub = x->size[0] * x->size[1];
4015 for (i50 = 0; i50 < loop_ub; i50++) {
4016 z->data[i50] = x->data[i50] / y->data[i50];
4017 }
4018 }
4019
4020 /*
4021 * Arguments : creal_T *x
4022 * Return Type : void
4023 */
c_sqrt(creal_T * x)4024 static void c_sqrt(creal_T *x)
4025 {
4026 double xr;
4027 double xi;
4028 double absxi;
4029 double absxr;
4030 xr = x->re;
4031 xi = x->im;
4032 if (xi == 0.0) {
4033 if (xr < 0.0) {
4034 absxi = 0.0;
4035 xr = sqrt(-xr);
4036 } else {
4037 absxi = sqrt(xr);
4038 xr = 0.0;
4039 }
4040 } else if (xr == 0.0) {
4041 if (xi < 0.0) {
4042 absxi = sqrt(-xi / 2.0);
4043 xr = -absxi;
4044 } else {
4045 absxi = sqrt(xi / 2.0);
4046 xr = absxi;
4047 }
4048 } else if (rtIsNaN(xr)) {
4049 absxi = xr;
4050 } else if (rtIsNaN(xi)) {
4051 absxi = xi;
4052 xr = xi;
4053 } else if (rtIsInf(xi)) {
4054 absxi = fabs(xi);
4055 xr = xi;
4056 } else if (rtIsInf(xr)) {
4057 if (xr < 0.0) {
4058 absxi = 0.0;
4059 xr = xi * -xr;
4060 } else {
4061 absxi = xr;
4062 xr = 0.0;
4063 }
4064 } else {
4065 absxr = fabs(xr);
4066 absxi = fabs(xi);
4067 if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307)) {
4068 absxr *= 0.5;
4069 absxi *= 0.5;
4070 absxi = rt_hypotd_snf(absxr, absxi);
4071 if (absxi > absxr) {
4072 absxi = sqrt(absxi) * sqrt(1.0 + absxr / absxi);
4073 } else {
4074 absxi = sqrt(absxi) * 1.4142135623730951;
4075 }
4076 } else {
4077 absxi = sqrt((rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
4078 }
4079
4080 if (xr > 0.0) {
4081 xr = 0.5 * (xi / absxi);
4082 } else {
4083 if (xi < 0.0) {
4084 xr = -absxi;
4085 } else {
4086 xr = absxi;
4087 }
4088
4089 absxi = 0.5 * (xi / xr);
4090 }
4091 }
4092
4093 x->re = absxi;
4094 x->im = xr;
4095 }
4096
4097 /*
4098 * Arguments : const char a[2]
4099 * Return Type : boolean_T
4100 */
c_strcmp(const char a[2])4101 static boolean_T c_strcmp(const char a[2])
4102 {
4103 boolean_T b_bool;
4104 int kstr;
4105 int exitg1;
4106 static const char cv32[2] = { 'T', 'x' };
4107
4108 b_bool = false;
4109 kstr = 0;
4110 do {
4111 exitg1 = 0;
4112 if (kstr + 1 < 3) {
4113 if (a[kstr] != cv32[kstr]) {
4114 exitg1 = 1;
4115 } else {
4116 kstr++;
4117 }
4118 } else {
4119 b_bool = true;
4120 exitg1 = 1;
4121 }
4122 } while (exitg1 == 0);
4123
4124 return b_bool;
4125 }
4126
4127 /*
4128 * Arguments : const double x[128]
4129 * Return Type : double
4130 */
c_sum(const double x[128])4131 static double c_sum(const double x[128])
4132 {
4133 double y;
4134 int k;
4135 y = x[0];
4136 for (k = 0; k < 127; k++) {
4137 y += x[k + 1];
4138 }
4139
4140 return y;
4141 }
4142
4143 /*
4144 * Arguments : const double o_data[]
4145 * const int o_size[2]
4146 * double u_data[]
4147 * int u_size[2]
4148 * Return Type : void
4149 */
c_us(const double o_data[],const int o_size[2],double u_data[],int u_size[2])4150 static void c_us(const double o_data[], const int o_size[2], double u_data[],
4151 int u_size[2])
4152 {
4153 int ix;
4154 int iy;
4155 int k;
4156 u_size[0] = 1;
4157 u_size[1] = (signed char)o_size[1];
4158 ix = (signed char)o_size[1];
4159 if (0 <= ix - 1) {
4160 memset(&u_data[0], 0, (unsigned int)(ix * (int)sizeof(double)));
4161 }
4162
4163 ix = 0;
4164 iy = 0;
4165 for (k = 1; k <= o_size[1]; k++) {
4166 u_data[iy] = o_data[ix];
4167 ix++;
4168 iy++;
4169 }
4170 }
4171
4172 /*
4173 * Arguments : double dBinput
4174 * Return Type : double
4175 */
dBinv(double dBinput)4176 static double dBinv(double dBinput)
4177 {
4178 double dBoutput;
4179 if (dBinput > -150.0) {
4180 dBoutput = rt_powd_snf(10.0, dBinput / 20.0);
4181 } else {
4182 dBoutput = 0.0;
4183 }
4184
4185 return dBoutput;
4186 }
4187
4188 /*
4189 * Arguments : const double x[128]
4190 * double y[128]
4191 * Return Type : void
4192 */
d_abs(const double x[128],double y[128])4193 static void d_abs(const double x[128], double y[128])
4194 {
4195 int k;
4196 for (k = 0; k < 128; k++) {
4197 y[k] = fabs(x[k]);
4198 }
4199 }
4200
4201 /*
4202 * UNTITLED Summary of this function goes here
4203 * Detailed explanation goes here
4204 * Arguments : const emxArray_real_T *f
4205 * double Fconverter
4206 * const double b1_data[]
4207 * const int b1_size[2]
4208 * const emxArray_creal_T *a1
4209 * const double b2_data[]
4210 * const int b2_size[2]
4211 * const emxArray_creal_T *a2
4212 * emxArray_creal_T *abc
4213 * Return Type : void
4214 */
d_analogresp(const emxArray_real_T * f,double Fconverter,const double b1_data[],const int b1_size[2],const emxArray_creal_T * a1,const double b2_data[],const int b2_size[2],const emxArray_creal_T * a2,emxArray_creal_T * abc)4215 static void d_analogresp(const emxArray_real_T *f, double Fconverter, const
4216 double b1_data[], const int b1_size[2], const emxArray_creal_T *a1, const
4217 double b2_data[], const int b2_size[2], const emxArray_creal_T *a2,
4218 emxArray_creal_T *abc)
4219 {
4220 emxArray_real_T *r28;
4221 emxArray_real_T *r29;
4222 int i56;
4223 int loop_ub;
4224 emxArray_creal_T *r30;
4225 double re;
4226 double im;
4227 double b_re;
4228 double b_im;
4229 emxInit_real_T(&r28, 2);
4230 emxInit_real_T(&r29, 2);
4231 rdivide(f, Fconverter, r28);
4232 b_sinc(r28);
4233 i56 = r29->size[0] * r29->size[1];
4234 r29->size[0] = 1;
4235 r29->size[1] = f->size[1];
4236 emxEnsureCapacity_real_T(r29, i56);
4237 loop_ub = f->size[0] * f->size[1];
4238 for (i56 = 0; i56 < loop_ub; i56++) {
4239 r29->data[i56] = 6.2831853071795862 * f->data[i56];
4240 }
4241
4242 b_freqs_cg(b1_data, b1_size, a1, r29, abc);
4243 i56 = r29->size[0] * r29->size[1];
4244 r29->size[0] = 1;
4245 r29->size[1] = f->size[1];
4246 emxEnsureCapacity_real_T(r29, i56);
4247 loop_ub = f->size[0] * f->size[1];
4248 for (i56 = 0; i56 < loop_ub; i56++) {
4249 r29->data[i56] = 6.2831853071795862 * f->data[i56];
4250 }
4251
4252 emxInit_creal_T(&r30, 2);
4253 b_freqs_cg(b2_data, b2_size, a2, r29, r30);
4254 i56 = abc->size[0] * abc->size[1];
4255 abc->size[0] = 1;
4256 abc->size[1] = r28->size[1];
4257 emxEnsureCapacity_creal_T(abc, i56);
4258 loop_ub = r28->size[0] * r28->size[1];
4259 emxFree_real_T(&r29);
4260 for (i56 = 0; i56 < loop_ub; i56++) {
4261 re = r28->data[i56] * abc->data[i56].re;
4262 im = r28->data[i56] * abc->data[i56].im;
4263 b_re = r30->data[i56].re;
4264 b_im = r30->data[i56].im;
4265 abc->data[i56].re = re * b_re - im * b_im;
4266 abc->data[i56].im = re * b_im + im * b_re;
4267 }
4268
4269 emxFree_real_T(&r28);
4270 emxFree_creal_T(&r30);
4271 }
4272
4273 /*
4274 * Make b a row
4275 * Arguments : double b_data[]
4276 * int b_size[2]
4277 * const struct_T *options
4278 * creal_T h[2048]
4279 * double w[2048]
4280 * Return Type : void
4281 */
d_firfreqz(double b_data[],int b_size[2],const struct_T * options,creal_T h[2048],double w[2048])4282 static void d_firfreqz(double b_data[], int b_size[2], const struct_T *options,
4283 creal_T h[2048], double w[2048])
4284 {
4285 int b_idx_0;
4286 int loop_ub;
4287 double b_b_data[29];
4288 creal_T dcv4[2048];
4289 double digw;
4290 double b_digw[2048];
4291 double h_re;
4292 double brm;
4293 double d;
4294
4295 /* -------------------------------------------------------------------------- */
4296 b_idx_0 = b_size[1];
4297 loop_ub = b_size[1];
4298 if (0 <= loop_ub - 1) {
4299 memcpy(&b_b_data[0], &b_data[0], (unsigned int)(loop_ub * (int)sizeof(double)));
4300 }
4301
4302 b_size[0] = 1;
4303 if (0 <= b_idx_0 - 1) {
4304 memcpy(&b_data[0], &b_b_data[0], (unsigned int)(b_idx_0 * (int)sizeof(double)));
4305 }
4306
4307 /* Actual Frequency Response Computation */
4308 /* if fvflag, */
4309 /* Frequency vector specified. Use Horner's method of polynomial */
4310 /* evaluation at the frequency points and divide the numerator */
4311 /* by the denominator. */
4312 /* */
4313 /* Note: we use positive i here because of the relationship */
4314 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
4315 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
4316 /* */
4317 /* Fs was specified, freq. vector is in Hz */
4318 /* Convert from Hz to rad/sample for computational purposes */
4319 /* Digital frequency must be used for this calculation */
4320 for (b_idx_0 = 0; b_idx_0 < 2048; b_idx_0++) {
4321 w[b_idx_0] = options->w[b_idx_0];
4322 digw = 6.2831853071795862 * options->w[b_idx_0] / options->Fs;
4323 dcv4[b_idx_0].re = digw * 0.0;
4324 dcv4[b_idx_0].im = digw;
4325 b_digw[b_idx_0] = digw;
4326 }
4327
4328 b_exp(dcv4);
4329 c_polyval(b_data, b_size, dcv4, h);
4330 for (b_idx_0 = 0; b_idx_0 < 2048; b_idx_0++) {
4331 dcv4[b_idx_0].re = ((double)b_size[1] - 1.0) * (b_digw[b_idx_0] * 0.0);
4332 dcv4[b_idx_0].im = ((double)b_size[1] - 1.0) * b_digw[b_idx_0];
4333 }
4334
4335 b_exp(dcv4);
4336 for (b_idx_0 = 0; b_idx_0 < 2048; b_idx_0++) {
4337 h_re = h[b_idx_0].re;
4338 if (dcv4[b_idx_0].im == 0.0) {
4339 if (h[b_idx_0].im == 0.0) {
4340 h[b_idx_0].re /= dcv4[b_idx_0].re;
4341 h[b_idx_0].im = 0.0;
4342 } else if (h[b_idx_0].re == 0.0) {
4343 h[b_idx_0].re = 0.0;
4344 h[b_idx_0].im /= dcv4[b_idx_0].re;
4345 } else {
4346 h[b_idx_0].re /= dcv4[b_idx_0].re;
4347 h[b_idx_0].im /= dcv4[b_idx_0].re;
4348 }
4349 } else if (dcv4[b_idx_0].re == 0.0) {
4350 if (h[b_idx_0].re == 0.0) {
4351 h[b_idx_0].re = h[b_idx_0].im / dcv4[b_idx_0].im;
4352 h[b_idx_0].im = 0.0;
4353 } else if (h[b_idx_0].im == 0.0) {
4354 h[b_idx_0].re = 0.0;
4355 h[b_idx_0].im = -(h_re / dcv4[b_idx_0].im);
4356 } else {
4357 h[b_idx_0].re = h[b_idx_0].im / dcv4[b_idx_0].im;
4358 h[b_idx_0].im = -(h_re / dcv4[b_idx_0].im);
4359 }
4360 } else {
4361 brm = fabs(dcv4[b_idx_0].re);
4362 digw = fabs(dcv4[b_idx_0].im);
4363 if (brm > digw) {
4364 digw = dcv4[b_idx_0].im / dcv4[b_idx_0].re;
4365 d = dcv4[b_idx_0].re + digw * dcv4[b_idx_0].im;
4366 h[b_idx_0].re = (h[b_idx_0].re + digw * h[b_idx_0].im) / d;
4367 h[b_idx_0].im = (h[b_idx_0].im - digw * h_re) / d;
4368 } else if (digw == brm) {
4369 if (dcv4[b_idx_0].re > 0.0) {
4370 digw = 0.5;
4371 } else {
4372 digw = -0.5;
4373 }
4374
4375 if (dcv4[b_idx_0].im > 0.0) {
4376 d = 0.5;
4377 } else {
4378 d = -0.5;
4379 }
4380
4381 h[b_idx_0].re = (h[b_idx_0].re * digw + h[b_idx_0].im * d) / brm;
4382 h[b_idx_0].im = (h[b_idx_0].im * digw - h_re * d) / brm;
4383 } else {
4384 digw = dcv4[b_idx_0].re / dcv4[b_idx_0].im;
4385 d = dcv4[b_idx_0].im + digw * dcv4[b_idx_0].re;
4386 h[b_idx_0].re = (digw * h[b_idx_0].re + h[b_idx_0].im) / d;
4387 h[b_idx_0].im = (digw * h[b_idx_0].im - h_re) / d;
4388 }
4389 }
4390 }
4391 }
4392
4393 /*
4394 * FREQZ_CG Frequency response of digital filter with codegen support
4395 *
4396 * This function is based on 'freqz' by The MathWorks Inc.
4397 * Arguments : const double b_data[]
4398 * const int b_size[2]
4399 * const double w[2048]
4400 * double Fs
4401 * creal_T hh[2048]
4402 * Return Type : void
4403 */
d_freqz_cg(const double b_data[],const int b_size[2],const double w[2048],double Fs,creal_T hh[2048])4404 static void d_freqz_cg(const double b_data[], const int b_size[2], const double
4405 w[2048], double Fs, creal_T hh[2048])
4406 {
4407 static struct_T options;
4408 int loop_ub;
4409 static const char cv18[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
4410
4411 int b_b_size[2];
4412 static const char cv19[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
4413
4414 double b_b_data[29];
4415 double b_w[2048];
4416
4417 /* Cast to enforce precision rules */
4418 options.Fs = Fs;
4419 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
4420 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
4421
4422 /* Remaining are default or for advanced use */
4423 options.fvflag = 1.0;
4424 for (loop_ub = 0; loop_ub < 8; loop_ub++) {
4425 options.range[loop_ub] = cv18[loop_ub];
4426 }
4427
4428 options.centerdc = 0.0;
4429 for (loop_ub = 0; loop_ub < 7; loop_ub++) {
4430 options.configlevel[loop_ub] = cv19[loop_ub];
4431 }
4432
4433 b_b_size[0] = 1;
4434 b_b_size[1] = b_size[1];
4435 loop_ub = b_size[0] * b_size[1];
4436 if (0 <= loop_ub - 1) {
4437 memcpy(&b_b_data[0], &b_data[0], (unsigned int)(loop_ub * (int)sizeof(double)));
4438 }
4439
4440 d_firfreqz(b_b_data, b_b_size, &options, hh, b_w);
4441
4442 /* Generate the default structure to pass to freqzplot */
4443 /* If rad/sample, Fs is empty */
4444 }
4445
4446 /*
4447 * Arguments : const double p[29]
4448 * const creal_T x[2048]
4449 * creal_T y[2048]
4450 * Return Type : void
4451 */
d_polyval(const double p[29],const creal_T x[2048],creal_T y[2048])4452 static void d_polyval(const double p[29], const creal_T x[2048],
4453 creal_T y[2048])
4454 {
4455 int i14;
4456 int k;
4457 double x_im;
4458 for (i14 = 0; i14 < 2048; i14++) {
4459 y[i14].re = p[0];
4460 y[i14].im = 0.0;
4461 }
4462
4463 for (k = 0; k < 28; k++) {
4464 for (i14 = 0; i14 < 2048; i14++) {
4465 x_im = x[i14].re * y[i14].im + x[i14].im * y[i14].re;
4466 y[i14].re = (x[i14].re * y[i14].re - x[i14].im * y[i14].im) + p[k + 1];
4467 y[i14].im = x_im;
4468 }
4469 }
4470 }
4471
4472 /*
4473 * Arguments : const double o[15]
4474 * double u[29]
4475 * Return Type : void
4476 */
d_us(const double o[15],double u[29])4477 static void d_us(const double o[15], double u[29])
4478 {
4479 double b_u[30];
4480 int ix;
4481 int iy;
4482 int k;
4483 memset(&b_u[0], 0, 30U * sizeof(double));
4484 ix = 0;
4485 iy = 0;
4486 for (k = 0; k < 15; k++) {
4487 b_u[iy] = o[ix];
4488 ix++;
4489 iy += 2;
4490 }
4491
4492 memcpy(&u[0], &b_u[0], 29U * sizeof(double));
4493 }
4494
4495 /*
4496 * Arguments : double ydb
4497 * Return Type : double
4498 */
db2mag(double ydb)4499 static double db2mag(double ydb)
4500 {
4501 return rt_powd_snf(10.0, ydb / 20.0);
4502 }
4503
4504 /*
4505 * Codegen workaround for fixed fi call requirements
4506 * Arguments : const emxArray_real_T *tap_store
4507 * double i
4508 * double M
4509 * emxArray_real_T *taps
4510 * Return Type : void
4511 */
determineBestFractionLength(const emxArray_real_T * tap_store,double i,double M,emxArray_real_T * taps)4512 static void determineBestFractionLength(const emxArray_real_T *tap_store, double
4513 i, double M, emxArray_real_T *taps)
4514 {
4515 int ixstart;
4516 emxArray_real_T *org;
4517 int ix;
4518 emxArray_real_T *r;
4519 double u;
4520 double v;
4521 emxArray_real_T *b_r;
4522 short i55;
4523 emxArray_real_T *r24;
4524 double e[16];
4525 int itmp;
4526 boolean_T exitg1;
4527 if (1.0 > M) {
4528 ixstart = 0;
4529 } else {
4530 ixstart = (int)M;
4531 }
4532
4533 emxInit_real_T(&org, 2);
4534 ix = org->size[0] * org->size[1];
4535 org->size[0] = 1;
4536 org->size[1] = ixstart;
4537 emxEnsureCapacity_real_T(org, ix);
4538 for (ix = 0; ix < ixstart; ix++) {
4539 org->data[org->size[0] * ix] = tap_store->data[((int)i + tap_store->size[0] *
4540 ix) - 1];
4541 }
4542
4543 emxInit_real_T(&r, 2);
4544 ix = r->size[0] * r->size[1];
4545 r->size[0] = 16;
4546 r->size[1] = (int)M;
4547 emxEnsureCapacity_real_T(r, ix);
4548 ixstart = (int)M << 4;
4549 for (ix = 0; ix < ixstart; ix++) {
4550 r->data[ix] = 0.0;
4551 }
4552
4553 if (1.0 > M) {
4554 ixstart = -1;
4555 } else {
4556 ixstart = (int)M - 1;
4557 }
4558
4559 for (ix = 0; ix <= ixstart; ix++) {
4560 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 2.0;
4561 v = fabs(u);
4562 if (v < 4.503599627370496E+15) {
4563 if (v >= 0.5) {
4564 u = floor(u + 0.5);
4565 } else {
4566 u *= 0.0;
4567 }
4568 }
4569
4570 if (u < 32768.0) {
4571 if (u >= -32768.0) {
4572 i55 = (short)u;
4573 } else {
4574 i55 = MIN_int16_T;
4575 }
4576 } else if (u >= 32768.0) {
4577 i55 = MAX_int16_T;
4578 } else {
4579 i55 = 0;
4580 }
4581
4582 r->data[r->size[0] * ix] = (double)i55 * 0.5;
4583 }
4584
4585 if (1.0 > M) {
4586 ixstart = 0;
4587 } else {
4588 ixstart = (int)M;
4589 }
4590
4591 emxInit_real_T(&b_r, 2);
4592 ix = b_r->size[0] * b_r->size[1];
4593 b_r->size[0] = 1;
4594 b_r->size[1] = ixstart;
4595 emxEnsureCapacity_real_T(b_r, ix);
4596 for (ix = 0; ix < ixstart; ix++) {
4597 b_r->data[b_r->size[0] * ix] = r->data[r->size[0] * ix] - org->data
4598 [org->size[0] * ix];
4599 }
4600
4601 emxInit_real_T(&r24, 2);
4602 c_abs(b_r, r24);
4603 e[0] = b_sum(r24);
4604 if (1.0 > M) {
4605 ixstart = -1;
4606 } else {
4607 ixstart = (int)M - 1;
4608 }
4609
4610 for (ix = 0; ix <= ixstart; ix++) {
4611 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 4.0;
4612 v = fabs(u);
4613 if (v < 4.503599627370496E+15) {
4614 if (v >= 0.5) {
4615 u = floor(u + 0.5);
4616 } else {
4617 u *= 0.0;
4618 }
4619 }
4620
4621 if (u < 32768.0) {
4622 if (u >= -32768.0) {
4623 i55 = (short)u;
4624 } else {
4625 i55 = MIN_int16_T;
4626 }
4627 } else if (u >= 32768.0) {
4628 i55 = MAX_int16_T;
4629 } else {
4630 i55 = 0;
4631 }
4632
4633 r->data[1 + r->size[0] * ix] = (double)i55 * 0.25;
4634 }
4635
4636 if (1.0 > M) {
4637 ixstart = 0;
4638 } else {
4639 ixstart = (int)M;
4640 }
4641
4642 ix = b_r->size[0] * b_r->size[1];
4643 b_r->size[0] = 1;
4644 b_r->size[1] = ixstart;
4645 emxEnsureCapacity_real_T(b_r, ix);
4646 for (ix = 0; ix < ixstart; ix++) {
4647 b_r->data[b_r->size[0] * ix] = r->data[1 + r->size[0] * ix] - org->data
4648 [org->size[0] * ix];
4649 }
4650
4651 c_abs(b_r, r24);
4652 e[1] = b_sum(r24);
4653 if (1.0 > M) {
4654 ixstart = -1;
4655 } else {
4656 ixstart = (int)M - 1;
4657 }
4658
4659 for (ix = 0; ix <= ixstart; ix++) {
4660 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 8.0;
4661 v = fabs(u);
4662 if (v < 4.503599627370496E+15) {
4663 if (v >= 0.5) {
4664 u = floor(u + 0.5);
4665 } else {
4666 u *= 0.0;
4667 }
4668 }
4669
4670 if (u < 32768.0) {
4671 if (u >= -32768.0) {
4672 i55 = (short)u;
4673 } else {
4674 i55 = MIN_int16_T;
4675 }
4676 } else if (u >= 32768.0) {
4677 i55 = MAX_int16_T;
4678 } else {
4679 i55 = 0;
4680 }
4681
4682 r->data[2 + r->size[0] * ix] = (double)i55 * 0.125;
4683 }
4684
4685 if (1.0 > M) {
4686 ixstart = 0;
4687 } else {
4688 ixstart = (int)M;
4689 }
4690
4691 ix = b_r->size[0] * b_r->size[1];
4692 b_r->size[0] = 1;
4693 b_r->size[1] = ixstart;
4694 emxEnsureCapacity_real_T(b_r, ix);
4695 for (ix = 0; ix < ixstart; ix++) {
4696 b_r->data[b_r->size[0] * ix] = r->data[2 + r->size[0] * ix] - org->data
4697 [org->size[0] * ix];
4698 }
4699
4700 c_abs(b_r, r24);
4701 e[2] = b_sum(r24);
4702 if (1.0 > M) {
4703 ixstart = -1;
4704 } else {
4705 ixstart = (int)M - 1;
4706 }
4707
4708 for (ix = 0; ix <= ixstart; ix++) {
4709 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 16.0;
4710 v = fabs(u);
4711 if (v < 4.503599627370496E+15) {
4712 if (v >= 0.5) {
4713 u = floor(u + 0.5);
4714 } else {
4715 u *= 0.0;
4716 }
4717 }
4718
4719 if (u < 32768.0) {
4720 if (u >= -32768.0) {
4721 i55 = (short)u;
4722 } else {
4723 i55 = MIN_int16_T;
4724 }
4725 } else if (u >= 32768.0) {
4726 i55 = MAX_int16_T;
4727 } else {
4728 i55 = 0;
4729 }
4730
4731 r->data[3 + r->size[0] * ix] = (double)i55 * 0.0625;
4732 }
4733
4734 if (1.0 > M) {
4735 ixstart = 0;
4736 } else {
4737 ixstart = (int)M;
4738 }
4739
4740 ix = b_r->size[0] * b_r->size[1];
4741 b_r->size[0] = 1;
4742 b_r->size[1] = ixstart;
4743 emxEnsureCapacity_real_T(b_r, ix);
4744 for (ix = 0; ix < ixstart; ix++) {
4745 b_r->data[b_r->size[0] * ix] = r->data[3 + r->size[0] * ix] - org->data
4746 [org->size[0] * ix];
4747 }
4748
4749 c_abs(b_r, r24);
4750 e[3] = b_sum(r24);
4751 if (1.0 > M) {
4752 ixstart = -1;
4753 } else {
4754 ixstart = (int)M - 1;
4755 }
4756
4757 for (ix = 0; ix <= ixstart; ix++) {
4758 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 32.0;
4759 v = fabs(u);
4760 if (v < 4.503599627370496E+15) {
4761 if (v >= 0.5) {
4762 u = floor(u + 0.5);
4763 } else {
4764 u *= 0.0;
4765 }
4766 }
4767
4768 if (u < 32768.0) {
4769 if (u >= -32768.0) {
4770 i55 = (short)u;
4771 } else {
4772 i55 = MIN_int16_T;
4773 }
4774 } else if (u >= 32768.0) {
4775 i55 = MAX_int16_T;
4776 } else {
4777 i55 = 0;
4778 }
4779
4780 r->data[4 + r->size[0] * ix] = (double)i55 * 0.03125;
4781 }
4782
4783 if (1.0 > M) {
4784 ixstart = 0;
4785 } else {
4786 ixstart = (int)M;
4787 }
4788
4789 ix = b_r->size[0] * b_r->size[1];
4790 b_r->size[0] = 1;
4791 b_r->size[1] = ixstart;
4792 emxEnsureCapacity_real_T(b_r, ix);
4793 for (ix = 0; ix < ixstart; ix++) {
4794 b_r->data[b_r->size[0] * ix] = r->data[4 + r->size[0] * ix] - org->data
4795 [org->size[0] * ix];
4796 }
4797
4798 c_abs(b_r, r24);
4799 e[4] = b_sum(r24);
4800 if (1.0 > M) {
4801 ixstart = -1;
4802 } else {
4803 ixstart = (int)M - 1;
4804 }
4805
4806 for (ix = 0; ix <= ixstart; ix++) {
4807 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 64.0;
4808 v = fabs(u);
4809 if (v < 4.503599627370496E+15) {
4810 if (v >= 0.5) {
4811 u = floor(u + 0.5);
4812 } else {
4813 u *= 0.0;
4814 }
4815 }
4816
4817 if (u < 32768.0) {
4818 if (u >= -32768.0) {
4819 i55 = (short)u;
4820 } else {
4821 i55 = MIN_int16_T;
4822 }
4823 } else if (u >= 32768.0) {
4824 i55 = MAX_int16_T;
4825 } else {
4826 i55 = 0;
4827 }
4828
4829 r->data[5 + r->size[0] * ix] = (double)i55 * 0.015625;
4830 }
4831
4832 if (1.0 > M) {
4833 ixstart = 0;
4834 } else {
4835 ixstart = (int)M;
4836 }
4837
4838 ix = b_r->size[0] * b_r->size[1];
4839 b_r->size[0] = 1;
4840 b_r->size[1] = ixstart;
4841 emxEnsureCapacity_real_T(b_r, ix);
4842 for (ix = 0; ix < ixstart; ix++) {
4843 b_r->data[b_r->size[0] * ix] = r->data[5 + r->size[0] * ix] - org->data
4844 [org->size[0] * ix];
4845 }
4846
4847 c_abs(b_r, r24);
4848 e[5] = b_sum(r24);
4849 if (1.0 > M) {
4850 ixstart = -1;
4851 } else {
4852 ixstart = (int)M - 1;
4853 }
4854
4855 for (ix = 0; ix <= ixstart; ix++) {
4856 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 128.0;
4857 v = fabs(u);
4858 if (v < 4.503599627370496E+15) {
4859 if (v >= 0.5) {
4860 u = floor(u + 0.5);
4861 } else {
4862 u *= 0.0;
4863 }
4864 }
4865
4866 if (u < 32768.0) {
4867 if (u >= -32768.0) {
4868 i55 = (short)u;
4869 } else {
4870 i55 = MIN_int16_T;
4871 }
4872 } else if (u >= 32768.0) {
4873 i55 = MAX_int16_T;
4874 } else {
4875 i55 = 0;
4876 }
4877
4878 r->data[6 + r->size[0] * ix] = (double)i55 * 0.0078125;
4879 }
4880
4881 if (1.0 > M) {
4882 ixstart = 0;
4883 } else {
4884 ixstart = (int)M;
4885 }
4886
4887 ix = b_r->size[0] * b_r->size[1];
4888 b_r->size[0] = 1;
4889 b_r->size[1] = ixstart;
4890 emxEnsureCapacity_real_T(b_r, ix);
4891 for (ix = 0; ix < ixstart; ix++) {
4892 b_r->data[b_r->size[0] * ix] = r->data[6 + r->size[0] * ix] - org->data
4893 [org->size[0] * ix];
4894 }
4895
4896 c_abs(b_r, r24);
4897 e[6] = b_sum(r24);
4898 if (1.0 > M) {
4899 ixstart = -1;
4900 } else {
4901 ixstart = (int)M - 1;
4902 }
4903
4904 for (ix = 0; ix <= ixstart; ix++) {
4905 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 256.0;
4906 v = fabs(u);
4907 if (v < 4.503599627370496E+15) {
4908 if (v >= 0.5) {
4909 u = floor(u + 0.5);
4910 } else {
4911 u *= 0.0;
4912 }
4913 }
4914
4915 if (u < 32768.0) {
4916 if (u >= -32768.0) {
4917 i55 = (short)u;
4918 } else {
4919 i55 = MIN_int16_T;
4920 }
4921 } else if (u >= 32768.0) {
4922 i55 = MAX_int16_T;
4923 } else {
4924 i55 = 0;
4925 }
4926
4927 r->data[7 + r->size[0] * ix] = (double)i55 * 0.00390625;
4928 }
4929
4930 if (1.0 > M) {
4931 ixstart = 0;
4932 } else {
4933 ixstart = (int)M;
4934 }
4935
4936 ix = b_r->size[0] * b_r->size[1];
4937 b_r->size[0] = 1;
4938 b_r->size[1] = ixstart;
4939 emxEnsureCapacity_real_T(b_r, ix);
4940 for (ix = 0; ix < ixstart; ix++) {
4941 b_r->data[b_r->size[0] * ix] = r->data[7 + r->size[0] * ix] - org->data
4942 [org->size[0] * ix];
4943 }
4944
4945 c_abs(b_r, r24);
4946 e[7] = b_sum(r24);
4947 if (1.0 > M) {
4948 ixstart = -1;
4949 } else {
4950 ixstart = (int)M - 1;
4951 }
4952
4953 for (ix = 0; ix <= ixstart; ix++) {
4954 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 512.0;
4955 v = fabs(u);
4956 if (v < 4.503599627370496E+15) {
4957 if (v >= 0.5) {
4958 u = floor(u + 0.5);
4959 } else {
4960 u *= 0.0;
4961 }
4962 }
4963
4964 if (u < 32768.0) {
4965 if (u >= -32768.0) {
4966 i55 = (short)u;
4967 } else {
4968 i55 = MIN_int16_T;
4969 }
4970 } else if (u >= 32768.0) {
4971 i55 = MAX_int16_T;
4972 } else {
4973 i55 = 0;
4974 }
4975
4976 r->data[8 + r->size[0] * ix] = (double)i55 * 0.001953125;
4977 }
4978
4979 if (1.0 > M) {
4980 ixstart = 0;
4981 } else {
4982 ixstart = (int)M;
4983 }
4984
4985 ix = b_r->size[0] * b_r->size[1];
4986 b_r->size[0] = 1;
4987 b_r->size[1] = ixstart;
4988 emxEnsureCapacity_real_T(b_r, ix);
4989 for (ix = 0; ix < ixstart; ix++) {
4990 b_r->data[b_r->size[0] * ix] = r->data[8 + r->size[0] * ix] - org->data
4991 [org->size[0] * ix];
4992 }
4993
4994 c_abs(b_r, r24);
4995 e[8] = b_sum(r24);
4996 if (1.0 > M) {
4997 ixstart = -1;
4998 } else {
4999 ixstart = (int)M - 1;
5000 }
5001
5002 for (ix = 0; ix <= ixstart; ix++) {
5003 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 1024.0;
5004 v = fabs(u);
5005 if (v < 4.503599627370496E+15) {
5006 if (v >= 0.5) {
5007 u = floor(u + 0.5);
5008 } else {
5009 u *= 0.0;
5010 }
5011 }
5012
5013 if (u < 32768.0) {
5014 if (u >= -32768.0) {
5015 i55 = (short)u;
5016 } else {
5017 i55 = MIN_int16_T;
5018 }
5019 } else if (u >= 32768.0) {
5020 i55 = MAX_int16_T;
5021 } else {
5022 i55 = 0;
5023 }
5024
5025 r->data[9 + r->size[0] * ix] = (double)i55 * 0.0009765625;
5026 }
5027
5028 if (1.0 > M) {
5029 ixstart = 0;
5030 } else {
5031 ixstart = (int)M;
5032 }
5033
5034 ix = b_r->size[0] * b_r->size[1];
5035 b_r->size[0] = 1;
5036 b_r->size[1] = ixstart;
5037 emxEnsureCapacity_real_T(b_r, ix);
5038 for (ix = 0; ix < ixstart; ix++) {
5039 b_r->data[b_r->size[0] * ix] = r->data[9 + r->size[0] * ix] - org->data
5040 [org->size[0] * ix];
5041 }
5042
5043 c_abs(b_r, r24);
5044 e[9] = b_sum(r24);
5045 if (1.0 > M) {
5046 ixstart = -1;
5047 } else {
5048 ixstart = (int)M - 1;
5049 }
5050
5051 for (ix = 0; ix <= ixstart; ix++) {
5052 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 2048.0;
5053 v = fabs(u);
5054 if (v < 4.503599627370496E+15) {
5055 if (v >= 0.5) {
5056 u = floor(u + 0.5);
5057 } else {
5058 u *= 0.0;
5059 }
5060 }
5061
5062 if (u < 32768.0) {
5063 if (u >= -32768.0) {
5064 i55 = (short)u;
5065 } else {
5066 i55 = MIN_int16_T;
5067 }
5068 } else if (u >= 32768.0) {
5069 i55 = MAX_int16_T;
5070 } else {
5071 i55 = 0;
5072 }
5073
5074 r->data[10 + r->size[0] * ix] = (double)i55 * 0.00048828125;
5075 }
5076
5077 if (1.0 > M) {
5078 ixstart = 0;
5079 } else {
5080 ixstart = (int)M;
5081 }
5082
5083 ix = b_r->size[0] * b_r->size[1];
5084 b_r->size[0] = 1;
5085 b_r->size[1] = ixstart;
5086 emxEnsureCapacity_real_T(b_r, ix);
5087 for (ix = 0; ix < ixstart; ix++) {
5088 b_r->data[b_r->size[0] * ix] = r->data[10 + r->size[0] * ix] - org->data
5089 [org->size[0] * ix];
5090 }
5091
5092 c_abs(b_r, r24);
5093 e[10] = b_sum(r24);
5094 if (1.0 > M) {
5095 ixstart = -1;
5096 } else {
5097 ixstart = (int)M - 1;
5098 }
5099
5100 for (ix = 0; ix <= ixstart; ix++) {
5101 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 4096.0;
5102 v = fabs(u);
5103 if (v < 4.503599627370496E+15) {
5104 if (v >= 0.5) {
5105 u = floor(u + 0.5);
5106 } else {
5107 u *= 0.0;
5108 }
5109 }
5110
5111 if (u < 32768.0) {
5112 if (u >= -32768.0) {
5113 i55 = (short)u;
5114 } else {
5115 i55 = MIN_int16_T;
5116 }
5117 } else if (u >= 32768.0) {
5118 i55 = MAX_int16_T;
5119 } else {
5120 i55 = 0;
5121 }
5122
5123 r->data[11 + r->size[0] * ix] = (double)i55 * 0.000244140625;
5124 }
5125
5126 if (1.0 > M) {
5127 ixstart = 0;
5128 } else {
5129 ixstart = (int)M;
5130 }
5131
5132 ix = b_r->size[0] * b_r->size[1];
5133 b_r->size[0] = 1;
5134 b_r->size[1] = ixstart;
5135 emxEnsureCapacity_real_T(b_r, ix);
5136 for (ix = 0; ix < ixstart; ix++) {
5137 b_r->data[b_r->size[0] * ix] = r->data[11 + r->size[0] * ix] - org->data
5138 [org->size[0] * ix];
5139 }
5140
5141 c_abs(b_r, r24);
5142 e[11] = b_sum(r24);
5143 if (1.0 > M) {
5144 ixstart = -1;
5145 } else {
5146 ixstart = (int)M - 1;
5147 }
5148
5149 for (ix = 0; ix <= ixstart; ix++) {
5150 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 8192.0;
5151 v = fabs(u);
5152 if (v < 4.503599627370496E+15) {
5153 if (v >= 0.5) {
5154 u = floor(u + 0.5);
5155 } else {
5156 u *= 0.0;
5157 }
5158 }
5159
5160 if (u < 32768.0) {
5161 if (u >= -32768.0) {
5162 i55 = (short)u;
5163 } else {
5164 i55 = MIN_int16_T;
5165 }
5166 } else if (u >= 32768.0) {
5167 i55 = MAX_int16_T;
5168 } else {
5169 i55 = 0;
5170 }
5171
5172 r->data[12 + r->size[0] * ix] = (double)i55 * 0.0001220703125;
5173 }
5174
5175 if (1.0 > M) {
5176 ixstart = 0;
5177 } else {
5178 ixstart = (int)M;
5179 }
5180
5181 ix = b_r->size[0] * b_r->size[1];
5182 b_r->size[0] = 1;
5183 b_r->size[1] = ixstart;
5184 emxEnsureCapacity_real_T(b_r, ix);
5185 for (ix = 0; ix < ixstart; ix++) {
5186 b_r->data[b_r->size[0] * ix] = r->data[12 + r->size[0] * ix] - org->data
5187 [org->size[0] * ix];
5188 }
5189
5190 c_abs(b_r, r24);
5191 e[12] = b_sum(r24);
5192 if (1.0 > M) {
5193 ixstart = -1;
5194 } else {
5195 ixstart = (int)M - 1;
5196 }
5197
5198 for (ix = 0; ix <= ixstart; ix++) {
5199 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 16384.0;
5200 v = fabs(u);
5201 if (v < 4.503599627370496E+15) {
5202 if (v >= 0.5) {
5203 u = floor(u + 0.5);
5204 } else {
5205 u *= 0.0;
5206 }
5207 }
5208
5209 if (u < 32768.0) {
5210 if (u >= -32768.0) {
5211 i55 = (short)u;
5212 } else {
5213 i55 = MIN_int16_T;
5214 }
5215 } else if (u >= 32768.0) {
5216 i55 = MAX_int16_T;
5217 } else {
5218 i55 = 0;
5219 }
5220
5221 r->data[13 + r->size[0] * ix] = (double)i55 * 6.103515625E-5;
5222 }
5223
5224 if (1.0 > M) {
5225 ixstart = 0;
5226 } else {
5227 ixstart = (int)M;
5228 }
5229
5230 ix = b_r->size[0] * b_r->size[1];
5231 b_r->size[0] = 1;
5232 b_r->size[1] = ixstart;
5233 emxEnsureCapacity_real_T(b_r, ix);
5234 for (ix = 0; ix < ixstart; ix++) {
5235 b_r->data[b_r->size[0] * ix] = r->data[13 + r->size[0] * ix] - org->data
5236 [org->size[0] * ix];
5237 }
5238
5239 c_abs(b_r, r24);
5240 e[13] = b_sum(r24);
5241 if (1.0 > M) {
5242 ixstart = -1;
5243 } else {
5244 ixstart = (int)M - 1;
5245 }
5246
5247 for (ix = 0; ix <= ixstart; ix++) {
5248 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 32768.0;
5249 v = fabs(u);
5250 if (v < 4.503599627370496E+15) {
5251 if (v >= 0.5) {
5252 u = floor(u + 0.5);
5253 } else {
5254 u *= 0.0;
5255 }
5256 }
5257
5258 if (u < 32768.0) {
5259 if (u >= -32768.0) {
5260 i55 = (short)u;
5261 } else {
5262 i55 = MIN_int16_T;
5263 }
5264 } else if (u >= 32768.0) {
5265 i55 = MAX_int16_T;
5266 } else {
5267 i55 = 0;
5268 }
5269
5270 r->data[14 + r->size[0] * ix] = (double)i55 * 3.0517578125E-5;
5271 }
5272
5273 if (1.0 > M) {
5274 ixstart = 0;
5275 } else {
5276 ixstart = (int)M;
5277 }
5278
5279 ix = b_r->size[0] * b_r->size[1];
5280 b_r->size[0] = 1;
5281 b_r->size[1] = ixstart;
5282 emxEnsureCapacity_real_T(b_r, ix);
5283 for (ix = 0; ix < ixstart; ix++) {
5284 b_r->data[b_r->size[0] * ix] = r->data[14 + r->size[0] * ix] - org->data
5285 [org->size[0] * ix];
5286 }
5287
5288 c_abs(b_r, r24);
5289 e[14] = b_sum(r24);
5290 if (1.0 > M) {
5291 ixstart = -1;
5292 } else {
5293 ixstart = (int)M - 1;
5294 }
5295
5296 for (ix = 0; ix <= ixstart; ix++) {
5297 u = tap_store->data[((int)i + tap_store->size[0] * ix) - 1] * 65536.0;
5298 v = fabs(u);
5299 if (v < 4.503599627370496E+15) {
5300 if (v >= 0.5) {
5301 u = floor(u + 0.5);
5302 } else {
5303 u *= 0.0;
5304 }
5305 }
5306
5307 if (u < 32768.0) {
5308 if (u >= -32768.0) {
5309 i55 = (short)u;
5310 } else {
5311 i55 = MIN_int16_T;
5312 }
5313 } else if (u >= 32768.0) {
5314 i55 = MAX_int16_T;
5315 } else {
5316 i55 = 0;
5317 }
5318
5319 r->data[15 + r->size[0] * ix] = (double)i55 * 1.52587890625E-5;
5320 }
5321
5322 if (1.0 > M) {
5323 ixstart = 0;
5324 } else {
5325 ixstart = (int)M;
5326 }
5327
5328 ix = b_r->size[0] * b_r->size[1];
5329 b_r->size[0] = 1;
5330 b_r->size[1] = ixstart;
5331 emxEnsureCapacity_real_T(b_r, ix);
5332 for (ix = 0; ix < ixstart; ix++) {
5333 b_r->data[b_r->size[0] * ix] = r->data[15 + r->size[0] * ix] - org->data
5334 [org->size[0] * ix];
5335 }
5336
5337 emxFree_real_T(&org);
5338 c_abs(b_r, r24);
5339 e[15] = b_sum(r24);
5340 ixstart = 1;
5341 u = e[0];
5342 itmp = 0;
5343 emxFree_real_T(&b_r);
5344 emxFree_real_T(&r24);
5345 if (rtIsNaN(e[0])) {
5346 ix = 2;
5347 exitg1 = false;
5348 while ((!exitg1) && (ix < 17)) {
5349 ixstart = ix;
5350 if (!rtIsNaN(e[ix - 1])) {
5351 u = e[ix - 1];
5352 itmp = ix - 1;
5353 exitg1 = true;
5354 } else {
5355 ix++;
5356 }
5357 }
5358 }
5359
5360 if (ixstart < 16) {
5361 while (ixstart + 1 < 17) {
5362 if (e[ixstart] < u) {
5363 u = e[ixstart];
5364 itmp = ixstart;
5365 }
5366
5367 ixstart++;
5368 }
5369 }
5370
5371 if (1.0 > M) {
5372 ixstart = 0;
5373 } else {
5374 ixstart = (int)M;
5375 }
5376
5377 ix = taps->size[0] * taps->size[1];
5378 taps->size[0] = 1;
5379 taps->size[1] = ixstart;
5380 emxEnsureCapacity_real_T(taps, ix);
5381 for (ix = 0; ix < ixstart; ix++) {
5382 taps->data[taps->size[0] * ix] = r->data[itmp + r->size[0] * ix];
5383 }
5384
5385 emxFree_real_T(&r);
5386 }
5387
5388 /*
5389 * Arguments : int numerator
5390 * int denominator
5391 * Return Type : int
5392 */
div_s32_floor(int numerator,int denominator)5393 static int div_s32_floor(int numerator, int denominator)
5394 {
5395 int quotient;
5396 unsigned int absNumerator;
5397 unsigned int absDenominator;
5398 boolean_T quotientNeedsNegation;
5399 unsigned int tempAbsQuotient;
5400 if (denominator == 0) {
5401 if (numerator >= 0) {
5402 quotient = MAX_int32_T;
5403 } else {
5404 quotient = MIN_int32_T;
5405 }
5406 } else {
5407 if (numerator < 0) {
5408 absNumerator = ~(unsigned int)numerator + 1U;
5409 } else {
5410 absNumerator = (unsigned int)numerator;
5411 }
5412
5413 if (denominator < 0) {
5414 absDenominator = ~(unsigned int)denominator + 1U;
5415 } else {
5416 absDenominator = (unsigned int)denominator;
5417 }
5418
5419 quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
5420 tempAbsQuotient = absNumerator / absDenominator;
5421 if (quotientNeedsNegation) {
5422 absNumerator %= absDenominator;
5423 if (absNumerator > 0U) {
5424 tempAbsQuotient++;
5425 }
5426
5427 quotient = -(int)tempAbsQuotient;
5428 } else {
5429 quotient = (int)tempAbsQuotient;
5430 }
5431 }
5432
5433 return quotient;
5434 }
5435
5436 /*
5437 * Make b a row
5438 * Arguments : const double b[29]
5439 * const struct_T *options
5440 * creal_T h[2048]
5441 * double w[2048]
5442 * Return Type : void
5443 */
e_firfreqz(const double b[29],const struct_T * options,creal_T h[2048],double w[2048])5444 static void e_firfreqz(const double b[29], const struct_T *options, creal_T h
5445 [2048], double w[2048])
5446 {
5447 int i66;
5448 creal_T dcv5[2048];
5449 double digw;
5450 double b_digw[2048];
5451 double h_re;
5452 double brm;
5453 double d;
5454
5455 /* -------------------------------------------------------------------------- */
5456 /* Actual Frequency Response Computation */
5457 /* if fvflag, */
5458 /* Frequency vector specified. Use Horner's method of polynomial */
5459 /* evaluation at the frequency points and divide the numerator */
5460 /* by the denominator. */
5461 /* */
5462 /* Note: we use positive i here because of the relationship */
5463 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
5464 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
5465 /* */
5466 /* Fs was specified, freq. vector is in Hz */
5467 /* Convert from Hz to rad/sample for computational purposes */
5468 /* Digital frequency must be used for this calculation */
5469 for (i66 = 0; i66 < 2048; i66++) {
5470 w[i66] = options->w[i66];
5471 digw = 6.2831853071795862 * options->w[i66] / options->Fs;
5472 dcv5[i66].re = digw * 0.0;
5473 dcv5[i66].im = digw;
5474 b_digw[i66] = digw;
5475 }
5476
5477 b_exp(dcv5);
5478 d_polyval(b, dcv5, h);
5479 for (i66 = 0; i66 < 2048; i66++) {
5480 dcv5[i66].re = 28.0 * (b_digw[i66] * 0.0);
5481 dcv5[i66].im = 28.0 * b_digw[i66];
5482 }
5483
5484 b_exp(dcv5);
5485 for (i66 = 0; i66 < 2048; i66++) {
5486 h_re = h[i66].re;
5487 if (dcv5[i66].im == 0.0) {
5488 if (h[i66].im == 0.0) {
5489 h[i66].re /= dcv5[i66].re;
5490 h[i66].im = 0.0;
5491 } else if (h[i66].re == 0.0) {
5492 h[i66].re = 0.0;
5493 h[i66].im /= dcv5[i66].re;
5494 } else {
5495 h[i66].re /= dcv5[i66].re;
5496 h[i66].im /= dcv5[i66].re;
5497 }
5498 } else if (dcv5[i66].re == 0.0) {
5499 if (h[i66].re == 0.0) {
5500 h[i66].re = h[i66].im / dcv5[i66].im;
5501 h[i66].im = 0.0;
5502 } else if (h[i66].im == 0.0) {
5503 h[i66].re = 0.0;
5504 h[i66].im = -(h_re / dcv5[i66].im);
5505 } else {
5506 h[i66].re = h[i66].im / dcv5[i66].im;
5507 h[i66].im = -(h_re / dcv5[i66].im);
5508 }
5509 } else {
5510 brm = fabs(dcv5[i66].re);
5511 digw = fabs(dcv5[i66].im);
5512 if (brm > digw) {
5513 digw = dcv5[i66].im / dcv5[i66].re;
5514 d = dcv5[i66].re + digw * dcv5[i66].im;
5515 h[i66].re = (h[i66].re + digw * h[i66].im) / d;
5516 h[i66].im = (h[i66].im - digw * h_re) / d;
5517 } else if (digw == brm) {
5518 if (dcv5[i66].re > 0.0) {
5519 digw = 0.5;
5520 } else {
5521 digw = -0.5;
5522 }
5523
5524 if (dcv5[i66].im > 0.0) {
5525 d = 0.5;
5526 } else {
5527 d = -0.5;
5528 }
5529
5530 h[i66].re = (h[i66].re * digw + h[i66].im * d) / brm;
5531 h[i66].im = (h[i66].im * digw - h_re * d) / brm;
5532 } else {
5533 digw = dcv5[i66].re / dcv5[i66].im;
5534 d = dcv5[i66].im + digw * dcv5[i66].re;
5535 h[i66].re = (digw * h[i66].re + h[i66].im) / d;
5536 h[i66].im = (digw * h[i66].im - h_re) / d;
5537 }
5538 }
5539 }
5540 }
5541
5542 /*
5543 * FREQZ_CG Frequency response of digital filter with codegen support
5544 *
5545 * This function is based on 'freqz' by The MathWorks Inc.
5546 * Arguments : const double b[29]
5547 * const double w[2048]
5548 * double Fs
5549 * creal_T hh[2048]
5550 * Return Type : void
5551 */
e_freqz_cg(const double b[29],const double w[2048],double Fs,creal_T hh[2048])5552 static void e_freqz_cg(const double b[29], const double w[2048], double Fs,
5553 creal_T hh[2048])
5554 {
5555 static struct_T options;
5556 int i15;
5557 static const char cv20[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
5558
5559 double b_w[2048];
5560 static const char cv21[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
5561
5562 /* Cast to enforce precision rules */
5563 options.Fs = Fs;
5564 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
5565 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
5566
5567 /* Remaining are default or for advanced use */
5568 options.fvflag = 1.0;
5569 for (i15 = 0; i15 < 8; i15++) {
5570 options.range[i15] = cv20[i15];
5571 }
5572
5573 options.centerdc = 0.0;
5574 for (i15 = 0; i15 < 7; i15++) {
5575 options.configlevel[i15] = cv21[i15];
5576 }
5577
5578 e_firfreqz(b, &options, hh, b_w);
5579
5580 /* Generate the default structure to pass to freqzplot */
5581 /* If rad/sample, Fs is empty */
5582 }
5583
5584 /*
5585 * Arguments : const double p[13]
5586 * const creal_T x[2048]
5587 * creal_T y[2048]
5588 * Return Type : void
5589 */
e_polyval(const double p[13],const creal_T x[2048],creal_T y[2048])5590 static void e_polyval(const double p[13], const creal_T x[2048],
5591 creal_T y[2048])
5592 {
5593 int i16;
5594 int k;
5595 double x_im;
5596 for (i16 = 0; i16 < 2048; i16++) {
5597 y[i16].re = p[0];
5598 y[i16].im = 0.0;
5599 }
5600
5601 for (k = 0; k < 12; k++) {
5602 for (i16 = 0; i16 < 2048; i16++) {
5603 x_im = x[i16].re * y[i16].im + x[i16].im * y[i16].re;
5604 y[i16].re = (x[i16].re * y[i16].re - x[i16].im * y[i16].im) + p[k + 1];
5605 y[i16].im = x_im;
5606 }
5607 }
5608 }
5609
5610 /*
5611 * Arguments : const double o[7]
5612 * double u[13]
5613 * Return Type : void
5614 */
e_us(const double o[7],double u[13])5615 static void e_us(const double o[7], double u[13])
5616 {
5617 double b_u[14];
5618 int ix;
5619 int iy;
5620 int k;
5621 memset(&b_u[0], 0, 14U * sizeof(double));
5622 ix = 0;
5623 iy = 0;
5624 for (k = 0; k < 7; k++) {
5625 b_u[iy] = o[ix];
5626 ix++;
5627 iy += 2;
5628 }
5629
5630 memcpy(&u[0], &b_u[0], 13U * sizeof(double));
5631 }
5632
5633 /*
5634 * Arguments : const emxArray_creal_T *A
5635 * emxArray_creal_T *V
5636 * Return Type : void
5637 */
eig(const emxArray_creal_T * A,emxArray_creal_T * V)5638 static void eig(const emxArray_creal_T *A, emxArray_creal_T *V)
5639 {
5640 int info;
5641 boolean_T p;
5642 int i;
5643 boolean_T exitg2;
5644 emxArray_creal_T *T;
5645 unsigned int uv0[2];
5646 int exitg1;
5647 double A_re;
5648 double V_re;
5649 double A_im;
5650 double V_im;
5651 boolean_T b_A;
5652 double beta1_re;
5653 double beta1_im;
5654 int m;
5655 double brm;
5656 int istart;
5657 int jend;
5658 if ((A->size[0] == 0) || (A->size[1] == 0)) {
5659 info = V->size[0];
5660 V->size[0] = A->size[0];
5661 emxEnsureCapacity_creal_T1(V, info);
5662 i = A->size[0];
5663 for (info = 0; info < i; info++) {
5664 V->data[info].re = 0.0;
5665 V->data[info].im = 0.0;
5666 }
5667 } else if (anyNonFinite(A)) {
5668 if ((A->size[0] == 1) && (A->size[1] == 1)) {
5669 info = V->size[0];
5670 V->size[0] = 1;
5671 emxEnsureCapacity_creal_T1(V, info);
5672 V->data[0].re = rtNaN;
5673 V->data[0].im = 0.0;
5674 } else {
5675 info = V->size[0];
5676 V->size[0] = A->size[0];
5677 emxEnsureCapacity_creal_T1(V, info);
5678 i = A->size[0];
5679 for (info = 0; info < i; info++) {
5680 V->data[info].re = rtNaN;
5681 V->data[info].im = 0.0;
5682 }
5683 }
5684 } else if ((A->size[0] == 1) && (A->size[1] == 1)) {
5685 info = V->size[0];
5686 V->size[0] = 1;
5687 emxEnsureCapacity_creal_T1(V, info);
5688 V->data[0] = A->data[0];
5689 } else {
5690 p = (A->size[0] == A->size[1]);
5691 if (p) {
5692 info = 0;
5693 exitg2 = false;
5694 while ((!exitg2) && (info <= A->size[1] - 1)) {
5695 i = 0;
5696 do {
5697 exitg1 = 0;
5698 if (i <= info) {
5699 A_re = A->data[info + A->size[0] * i].re;
5700 A_im = -A->data[info + A->size[0] * i].im;
5701 b_A = ((A->data[i + A->size[0] * info].re == A_re) && (A->data[i +
5702 A->size[0] * info].im == A_im));
5703 if (!b_A) {
5704 p = false;
5705 exitg1 = 1;
5706 } else {
5707 i++;
5708 }
5709 } else {
5710 info++;
5711 exitg1 = 2;
5712 }
5713 } while (exitg1 == 0);
5714
5715 if (exitg1 == 1) {
5716 exitg2 = true;
5717 }
5718 }
5719 }
5720
5721 if (p) {
5722 emxInit_creal_T(&T, 2);
5723 if (anyNonFinite(A)) {
5724 for (info = 0; info < 2; info++) {
5725 uv0[info] = (unsigned int)A->size[info];
5726 }
5727
5728 info = T->size[0] * T->size[1];
5729 T->size[0] = (int)uv0[0];
5730 T->size[1] = (int)uv0[1];
5731 emxEnsureCapacity_creal_T(T, info);
5732 i = (int)uv0[0] * (int)uv0[1];
5733 for (info = 0; info < i; info++) {
5734 T->data[info].re = rtNaN;
5735 T->data[info].im = 0.0;
5736 }
5737
5738 m = T->size[0];
5739 if (!(1 >= T->size[0])) {
5740 istart = 2;
5741 if (T->size[0] - 2 < T->size[1] - 1) {
5742 jend = T->size[0] - 1;
5743 } else {
5744 jend = T->size[1];
5745 }
5746
5747 for (info = 1; info <= jend; info++) {
5748 for (i = istart; i <= m; i++) {
5749 T->data[(i + T->size[0] * (info - 1)) - 1].re = 0.0;
5750 T->data[(i + T->size[0] * (info - 1)) - 1].im = 0.0;
5751 }
5752
5753 istart++;
5754 }
5755 }
5756 } else {
5757 info = T->size[0] * T->size[1];
5758 T->size[0] = A->size[0];
5759 T->size[1] = A->size[1];
5760 emxEnsureCapacity_creal_T(T, info);
5761 i = A->size[0] * A->size[1];
5762 for (info = 0; info < i; info++) {
5763 T->data[info] = A->data[info];
5764 }
5765
5766 xgehrd(T);
5767 eml_zlahqr(T);
5768 m = T->size[0];
5769 if ((T->size[0] == 0) || (T->size[1] == 0) || (3 >= T->size[0])) {
5770 } else {
5771 istart = 4;
5772 if (T->size[0] - 4 < T->size[1] - 1) {
5773 jend = T->size[0] - 3;
5774 } else {
5775 jend = T->size[1];
5776 }
5777
5778 for (info = 1; info <= jend; info++) {
5779 for (i = istart; i <= m; i++) {
5780 T->data[(i + T->size[0] * (info - 1)) - 1].re = 0.0;
5781 T->data[(i + T->size[0] * (info - 1)) - 1].im = 0.0;
5782 }
5783
5784 istart++;
5785 }
5786 }
5787 }
5788
5789 info = V->size[0];
5790 V->size[0] = T->size[0];
5791 emxEnsureCapacity_creal_T1(V, info);
5792 for (info = 0; info + 1 <= T->size[0]; info++) {
5793 V->data[info] = T->data[info + T->size[0] * info];
5794 }
5795
5796 emxFree_creal_T(&T);
5797 } else {
5798 emxInit_creal_T1(&T, 1);
5799 xzgeev(A, &info, V, T);
5800 info = V->size[0];
5801 emxEnsureCapacity_creal_T1(V, info);
5802 i = V->size[0];
5803 for (info = 0; info < i; info++) {
5804 V_re = V->data[info].re;
5805 V_im = V->data[info].im;
5806 beta1_re = T->data[info].re;
5807 beta1_im = T->data[info].im;
5808 if (beta1_im == 0.0) {
5809 if (V_im == 0.0) {
5810 V->data[info].re = V_re / beta1_re;
5811 V->data[info].im = 0.0;
5812 } else if (V_re == 0.0) {
5813 V->data[info].re = 0.0;
5814 V->data[info].im = V_im / beta1_re;
5815 } else {
5816 V->data[info].re = V_re / beta1_re;
5817 V->data[info].im = V_im / beta1_re;
5818 }
5819 } else if (beta1_re == 0.0) {
5820 if (V_re == 0.0) {
5821 V->data[info].re = V_im / beta1_im;
5822 V->data[info].im = 0.0;
5823 } else if (V_im == 0.0) {
5824 V->data[info].re = 0.0;
5825 V->data[info].im = -(V_re / beta1_im);
5826 } else {
5827 V->data[info].re = V_im / beta1_im;
5828 V->data[info].im = -(V_re / beta1_im);
5829 }
5830 } else {
5831 brm = fabs(beta1_re);
5832 A_re = fabs(beta1_im);
5833 if (brm > A_re) {
5834 A_im = beta1_im / beta1_re;
5835 A_re = beta1_re + A_im * beta1_im;
5836 V->data[info].re = (V_re + A_im * V_im) / A_re;
5837 V->data[info].im = (V_im - A_im * V_re) / A_re;
5838 } else if (A_re == brm) {
5839 if (beta1_re > 0.0) {
5840 A_im = 0.5;
5841 } else {
5842 A_im = -0.5;
5843 }
5844
5845 if (beta1_im > 0.0) {
5846 A_re = 0.5;
5847 } else {
5848 A_re = -0.5;
5849 }
5850
5851 V->data[info].re = (V_re * A_im + V_im * A_re) / brm;
5852 V->data[info].im = (V_im * A_im - V_re * A_re) / brm;
5853 } else {
5854 A_im = beta1_re / beta1_im;
5855 A_re = beta1_im + A_im * beta1_re;
5856 V->data[info].re = (A_im * V_re + V_im) / A_re;
5857 V->data[info].im = (A_im * V_im - V_re) / A_re;
5858 }
5859 }
5860 }
5861
5862 emxFree_creal_T(&T);
5863 }
5864 }
5865 }
5866
5867 /*
5868 * Arguments : emxArray_creal_T *h
5869 * Return Type : int
5870 */
eml_zlahqr(emxArray_creal_T * h)5871 static int eml_zlahqr(emxArray_creal_T *h)
5872 {
5873 int info;
5874 int n;
5875 int maxval;
5876 int itmax;
5877 int ldh;
5878 int i;
5879 double SMLNUM;
5880 double tst;
5881 boolean_T exitg1;
5882 double aa;
5883 double ba;
5884 int L;
5885 creal_T u2;
5886 boolean_T goto140;
5887 int its;
5888 boolean_T exitg2;
5889 int k;
5890 boolean_T exitg3;
5891 double htmp1;
5892 creal_T y;
5893 double ab;
5894 boolean_T goto70;
5895 int m;
5896 double x_re;
5897 double u_re;
5898 double x_im;
5899 double u_im;
5900 double s;
5901 int b_k;
5902 creal_T v[2];
5903 double b_SMLNUM;
5904 int i61;
5905 n = h->size[0];
5906 if (h->size[0] < 10) {
5907 maxval = 10;
5908 } else {
5909 maxval = h->size[0];
5910 }
5911
5912 itmax = 30 * maxval;
5913 ldh = h->size[0];
5914 info = 0;
5915 if ((h->size[0] != 0) && (1 != h->size[0])) {
5916 for (maxval = 0; maxval + 1 <= n - 3; maxval++) {
5917 h->data[(maxval + h->size[0] * maxval) + 2].re = 0.0;
5918 h->data[(maxval + h->size[0] * maxval) + 2].im = 0.0;
5919 h->data[(maxval + h->size[0] * maxval) + 3].re = 0.0;
5920 h->data[(maxval + h->size[0] * maxval) + 3].im = 0.0;
5921 }
5922
5923 if (1 <= n - 2) {
5924 h->data[(n + h->size[0] * (n - 3)) - 1].re = 0.0;
5925 h->data[(n + h->size[0] * (n - 3)) - 1].im = 0.0;
5926 }
5927
5928 for (i = 1; i + 1 <= n; i++) {
5929 if (h->data[i + h->size[0] * (i - 1)].im != 0.0) {
5930 tst = h->data[i + h->size[0] * (i - 1)].re;
5931 aa = h->data[i + h->size[0] * (i - 1)].im;
5932 ba = fabs(h->data[i + h->size[0] * (i - 1)].re) + fabs(h->data[i +
5933 h->size[0] * (i - 1)].im);
5934 if (aa == 0.0) {
5935 u2.re = tst / ba;
5936 u2.im = 0.0;
5937 } else if (tst == 0.0) {
5938 u2.re = 0.0;
5939 u2.im = aa / ba;
5940 } else {
5941 u2.re = tst / ba;
5942 u2.im = aa / ba;
5943 }
5944
5945 ba = rt_hypotd_snf(u2.re, u2.im);
5946 if (-u2.im == 0.0) {
5947 u2.re /= ba;
5948 u2.im = 0.0;
5949 } else if (u2.re == 0.0) {
5950 u2.re = 0.0;
5951 u2.im = -u2.im / ba;
5952 } else {
5953 u2.re /= ba;
5954 u2.im = -u2.im / ba;
5955 }
5956
5957 tst = h->data[i + h->size[0] * (i - 1)].re;
5958 htmp1 = h->data[i + h->size[0] * (i - 1)].im;
5959 h->data[i + h->size[0] * (i - 1)].re = rt_hypotd_snf(tst, htmp1);
5960 h->data[i + h->size[0] * (i - 1)].im = 0.0;
5961 b_xscal(n - i, u2, h, (i + i * ldh) + 1, ldh);
5962 y.re = u2.re;
5963 y.im = -u2.im;
5964 maxval = i + 2;
5965 if (n < maxval) {
5966 maxval = n;
5967 }
5968
5969 xscal(maxval, y, h, 1 + i * ldh);
5970 }
5971 }
5972
5973 SMLNUM = 2.2250738585072014E-308 * ((double)n / 2.2204460492503131E-16);
5974 i = n - 1;
5975 exitg1 = false;
5976 while ((!exitg1) && (i + 1 >= 1)) {
5977 L = -1;
5978 goto140 = false;
5979 its = 0;
5980 exitg2 = false;
5981 while ((!exitg2) && (its <= itmax)) {
5982 k = i;
5983 exitg3 = false;
5984 while ((!exitg3) && ((k + 1 > L + 2) && (!(fabs(h->data[k + h->size[0] *
5985 (k - 1)].re) + fabs(h->data[k + h->size[0] * (k - 1)].im) <=
5986 SMLNUM)))) {
5987 tst = (fabs(h->data[(k + h->size[0] * (k - 1)) - 1].re) + fabs(h->
5988 data[(k + h->size[0] * (k - 1)) - 1].im)) + (fabs(h->data[k +
5989 h->size[0] * k].re) + fabs(h->data[k + h->size[0] * k].im));
5990 if (tst == 0.0) {
5991 if (k - 1 >= 1) {
5992 tst = fabs(h->data[(k + h->size[0] * (k - 2)) - 1].re);
5993 }
5994
5995 if (k + 2 <= n) {
5996 tst += fabs(h->data[(k + h->size[0] * k) + 1].re);
5997 }
5998 }
5999
6000 if (fabs(h->data[k + h->size[0] * (k - 1)].re) <=
6001 2.2204460492503131E-16 * tst) {
6002 htmp1 = fabs(h->data[k + h->size[0] * (k - 1)].re) + fabs(h->data[k
6003 + h->size[0] * (k - 1)].im);
6004 tst = fabs(h->data[(k + h->size[0] * k) - 1].re) + fabs(h->data[(k +
6005 h->size[0] * k) - 1].im);
6006 if (htmp1 > tst) {
6007 ab = htmp1;
6008 ba = tst;
6009 } else {
6010 ab = tst;
6011 ba = htmp1;
6012 }
6013
6014 htmp1 = fabs(h->data[k + h->size[0] * k].re) + fabs(h->data[k +
6015 h->size[0] * k].im);
6016 x_re = h->data[(k + h->size[0] * (k - 1)) - 1].re - h->data[k +
6017 h->size[0] * k].re;
6018 x_im = h->data[(k + h->size[0] * (k - 1)) - 1].im - h->data[k +
6019 h->size[0] * k].im;
6020 tst = fabs(x_re) + fabs(x_im);
6021 if (htmp1 > tst) {
6022 aa = htmp1;
6023 htmp1 = tst;
6024 } else {
6025 aa = tst;
6026 }
6027
6028 s = aa + ab;
6029 tst = 2.2204460492503131E-16 * (htmp1 * (aa / s));
6030 if ((SMLNUM > tst) || rtIsNaN(tst)) {
6031 b_SMLNUM = SMLNUM;
6032 } else {
6033 b_SMLNUM = tst;
6034 }
6035
6036 if (ba * (ab / s) <= b_SMLNUM) {
6037 exitg3 = true;
6038 } else {
6039 k--;
6040 }
6041 } else {
6042 k--;
6043 }
6044 }
6045
6046 L = k - 1;
6047 if (k + 1 > 1) {
6048 h->data[k + h->size[0] * (k - 1)].re = 0.0;
6049 h->data[k + h->size[0] * (k - 1)].im = 0.0;
6050 }
6051
6052 if (k + 1 >= i + 1) {
6053 goto140 = true;
6054 exitg2 = true;
6055 } else {
6056 if (its == 10) {
6057 ba = 0.75 * fabs(h->data[(k + h->size[0] * k) + 1].re) + h->data[k +
6058 h->size[0] * k].re;
6059 ab = h->data[k + h->size[0] * k].im;
6060 } else if (its == 20) {
6061 ba = 0.75 * fabs(h->data[i + h->size[0] * (i - 1)].re) + h->data[i +
6062 h->size[0] * i].re;
6063 ab = h->data[i + h->size[0] * i].im;
6064 } else {
6065 ba = h->data[i + h->size[0] * i].re;
6066 ab = h->data[i + h->size[0] * i].im;
6067 y = h->data[(i + h->size[0] * i) - 1];
6068 c_sqrt(&y);
6069 u2 = h->data[i + h->size[0] * (i - 1)];
6070 c_sqrt(&u2);
6071 u_re = y.re * u2.re - y.im * u2.im;
6072 u_im = y.re * u2.im + y.im * u2.re;
6073 s = fabs(u_re) + fabs(u_im);
6074 if (s != 0.0) {
6075 tst = h->data[(i + h->size[0] * (i - 1)) - 1].re - h->data[i +
6076 h->size[0] * i].re;
6077 htmp1 = h->data[(i + h->size[0] * (i - 1)) - 1].im - h->data[i +
6078 h->size[0] * i].im;
6079 x_re = 0.5 * tst;
6080 x_im = 0.5 * htmp1;
6081 aa = fabs(x_re) + fabs(x_im);
6082 tst = fabs(x_re) + fabs(x_im);
6083 if (!((s > tst) || rtIsNaN(tst))) {
6084 s = tst;
6085 }
6086
6087 if (x_im == 0.0) {
6088 ba = x_re / s;
6089 ab = 0.0;
6090 } else if (x_re == 0.0) {
6091 ba = 0.0;
6092 ab = x_im / s;
6093 } else {
6094 ba = x_re / s;
6095 ab = x_im / s;
6096 }
6097
6098 tst = ba;
6099 ba = ba * ba - ab * ab;
6100 ab = tst * ab + ab * tst;
6101 if (u_im == 0.0) {
6102 u2.re = u_re / s;
6103 u2.im = 0.0;
6104 } else if (u_re == 0.0) {
6105 u2.re = 0.0;
6106 u2.im = u_im / s;
6107 } else {
6108 u2.re = u_re / s;
6109 u2.im = u_im / s;
6110 }
6111
6112 y.re = ba + (u2.re * u2.re - u2.im * u2.im);
6113 y.im = ab + (u2.re * u2.im + u2.im * u2.re);
6114 c_sqrt(&y);
6115 y.re *= s;
6116 y.im *= s;
6117 if (aa > 0.0) {
6118 if (x_im == 0.0) {
6119 ba = x_re / aa;
6120 ab = 0.0;
6121 } else if (x_re == 0.0) {
6122 ba = 0.0;
6123 ab = x_im / aa;
6124 } else {
6125 ba = x_re / aa;
6126 ab = x_im / aa;
6127 }
6128
6129 if (ba * y.re + ab * y.im < 0.0) {
6130 y.re = -y.re;
6131 y.im = -y.im;
6132 }
6133 }
6134
6135 ba = x_re + y.re;
6136 htmp1 = x_im + y.im;
6137 if (htmp1 == 0.0) {
6138 if (u_im == 0.0) {
6139 x_re = u_re / ba;
6140 tst = 0.0;
6141 } else if (u_re == 0.0) {
6142 x_re = 0.0;
6143 tst = u_im / ba;
6144 } else {
6145 x_re = u_re / ba;
6146 tst = u_im / ba;
6147 }
6148 } else if (ba == 0.0) {
6149 if (u_re == 0.0) {
6150 x_re = u_im / htmp1;
6151 tst = 0.0;
6152 } else if (u_im == 0.0) {
6153 x_re = 0.0;
6154 tst = -(u_re / htmp1);
6155 } else {
6156 x_re = u_im / htmp1;
6157 tst = -(u_re / htmp1);
6158 }
6159 } else {
6160 ab = fabs(ba);
6161 tst = fabs(htmp1);
6162 if (ab > tst) {
6163 s = htmp1 / ba;
6164 tst = ba + s * htmp1;
6165 x_re = (u_re + s * u_im) / tst;
6166 tst = (u_im - s * u_re) / tst;
6167 } else if (tst == ab) {
6168 if (ba > 0.0) {
6169 aa = 0.5;
6170 } else {
6171 aa = -0.5;
6172 }
6173
6174 if (htmp1 > 0.0) {
6175 tst = 0.5;
6176 } else {
6177 tst = -0.5;
6178 }
6179
6180 x_re = (u_re * aa + u_im * tst) / ab;
6181 tst = (u_im * aa - u_re * tst) / ab;
6182 } else {
6183 s = ba / htmp1;
6184 tst = htmp1 + s * ba;
6185 x_re = (s * u_re + u_im) / tst;
6186 tst = (s * u_im - u_re) / tst;
6187 }
6188 }
6189
6190 ba = h->data[i + h->size[0] * i].re - (u_re * x_re - u_im * tst);
6191 ab = h->data[i + h->size[0] * i].im - (u_re * tst + u_im * x_re);
6192 }
6193 }
6194
6195 goto70 = false;
6196 m = i;
6197 exitg3 = false;
6198 while ((!exitg3) && (m > k + 1)) {
6199 u2.re = h->data[(m + h->size[0] * (m - 1)) - 1].re - ba;
6200 u2.im = h->data[(m + h->size[0] * (m - 1)) - 1].im - ab;
6201 tst = h->data[m + h->size[0] * (m - 1)].re;
6202 s = (fabs(u2.re) + fabs(u2.im)) + fabs(tst);
6203 if (u2.im == 0.0) {
6204 u2.re /= s;
6205 u2.im = 0.0;
6206 } else if (u2.re == 0.0) {
6207 u2.re = 0.0;
6208 u2.im /= s;
6209 } else {
6210 u2.re /= s;
6211 u2.im /= s;
6212 }
6213
6214 tst /= s;
6215 v[0] = u2;
6216 v[1].re = tst;
6217 v[1].im = 0.0;
6218 if (fabs(h->data[(m + h->size[0] * (m - 2)) - 1].re) * fabs(tst) <=
6219 2.2204460492503131E-16 * ((fabs(u2.re) + fabs(u2.im)) * ((fabs
6220 (h->data[(m + h->size[0] * (m - 1)) - 1].re) + fabs(h->data
6221 [(m + h->size[0] * (m - 1)) - 1].im)) + (fabs(h->data[m +
6222 h->size[0] * m].re) + fabs(h->data[m + h->size[0] * m].im))))) {
6223 goto70 = true;
6224 exitg3 = true;
6225 } else {
6226 m--;
6227 }
6228 }
6229
6230 if (!goto70) {
6231 u2.re = h->data[k + h->size[0] * k].re - ba;
6232 u2.im = h->data[k + h->size[0] * k].im - ab;
6233 tst = h->data[(k + h->size[0] * k) + 1].re;
6234 s = (fabs(u2.re) + fabs(u2.im)) + fabs(tst);
6235 if (u2.im == 0.0) {
6236 u2.re /= s;
6237 u2.im = 0.0;
6238 } else if (u2.re == 0.0) {
6239 u2.re = 0.0;
6240 u2.im /= s;
6241 } else {
6242 u2.re /= s;
6243 u2.im /= s;
6244 }
6245
6246 tst /= s;
6247 v[0] = u2;
6248 v[1].re = tst;
6249 v[1].im = 0.0;
6250 }
6251
6252 for (b_k = m; b_k <= i; b_k++) {
6253 if (b_k > m) {
6254 v[0] = h->data[(b_k + h->size[0] * (b_k - 2)) - 1];
6255 v[1] = h->data[b_k + h->size[0] * (b_k - 2)];
6256 }
6257
6258 u2 = xzlarfg(&v[0], &v[1]);
6259 if (b_k > m) {
6260 h->data[(b_k + h->size[0] * (b_k - 2)) - 1] = v[0];
6261 h->data[b_k + h->size[0] * (b_k - 2)].re = 0.0;
6262 h->data[b_k + h->size[0] * (b_k - 2)].im = 0.0;
6263 }
6264
6265 htmp1 = u2.re * v[1].re - u2.im * v[1].im;
6266 for (maxval = b_k - 1; maxval + 1 <= n; maxval++) {
6267 tst = u2.re * h->data[(b_k + h->size[0] * maxval) - 1].re - -u2.im
6268 * h->data[(b_k + h->size[0] * maxval) - 1].im;
6269 aa = u2.re * h->data[(b_k + h->size[0] * maxval) - 1].im + -u2.im *
6270 h->data[(b_k + h->size[0] * maxval) - 1].re;
6271 ba = tst + htmp1 * h->data[b_k + h->size[0] * maxval].re;
6272 ab = aa + htmp1 * h->data[b_k + h->size[0] * maxval].im;
6273 h->data[(b_k + h->size[0] * maxval) - 1].re -= ba;
6274 h->data[(b_k + h->size[0] * maxval) - 1].im -= ab;
6275 h->data[b_k + h->size[0] * maxval].re -= ba * v[1].re - ab * v[1].
6276 im;
6277 h->data[b_k + h->size[0] * maxval].im -= ba * v[1].im + ab * v[1].
6278 re;
6279 }
6280
6281 if (b_k + 2 < i + 1) {
6282 i61 = b_k;
6283 } else {
6284 i61 = i - 1;
6285 }
6286
6287 for (maxval = 0; maxval + 1 <= i61 + 2; maxval++) {
6288 tst = u2.re * h->data[maxval + h->size[0] * (b_k - 1)].re - u2.im *
6289 h->data[maxval + h->size[0] * (b_k - 1)].im;
6290 aa = u2.re * h->data[maxval + h->size[0] * (b_k - 1)].im + u2.im *
6291 h->data[maxval + h->size[0] * (b_k - 1)].re;
6292 ba = tst + htmp1 * h->data[maxval + h->size[0] * b_k].re;
6293 ab = aa + htmp1 * h->data[maxval + h->size[0] * b_k].im;
6294 h->data[maxval + h->size[0] * (b_k - 1)].re -= ba;
6295 h->data[maxval + h->size[0] * (b_k - 1)].im -= ab;
6296 h->data[maxval + h->size[0] * b_k].re -= ba * v[1].re - ab * -v[1]
6297 .im;
6298 h->data[maxval + h->size[0] * b_k].im -= ba * -v[1].im + ab * v[1]
6299 .re;
6300 }
6301
6302 if ((b_k == m) && (m > k + 1)) {
6303 u2.re = 1.0 - u2.re;
6304 u2.im = 0.0 - u2.im;
6305 ba = rt_hypotd_snf(u2.re, u2.im);
6306 if (u2.im == 0.0) {
6307 u2.re /= ba;
6308 u2.im = 0.0;
6309 } else if (u2.re == 0.0) {
6310 u2.re = 0.0;
6311 u2.im /= ba;
6312 } else {
6313 u2.re /= ba;
6314 u2.im /= ba;
6315 }
6316
6317 tst = h->data[m + h->size[0] * (m - 1)].re;
6318 htmp1 = h->data[m + h->size[0] * (m - 1)].im;
6319 h->data[m + h->size[0] * (m - 1)].re = tst * u2.re - htmp1 *
6320 -u2.im;
6321 h->data[m + h->size[0] * (m - 1)].im = tst * -u2.im + htmp1 *
6322 u2.re;
6323 if (m + 2 <= i + 1) {
6324 tst = h->data[(m + h->size[0] * m) + 1].re;
6325 htmp1 = h->data[(m + h->size[0] * m) + 1].im;
6326 h->data[(m + h->size[0] * m) + 1].re = tst * u2.re - htmp1 *
6327 u2.im;
6328 h->data[(m + h->size[0] * m) + 1].im = tst * u2.im + htmp1 *
6329 u2.re;
6330 }
6331
6332 for (maxval = m; maxval <= i + 1; maxval++) {
6333 if (maxval != m + 1) {
6334 if (n > maxval) {
6335 b_xscal(n - maxval, u2, h, maxval + maxval * ldh, ldh);
6336 }
6337
6338 y.re = u2.re;
6339 y.im = -u2.im;
6340 xscal(maxval - 1, y, h, 1 + (maxval - 1) * ldh);
6341 }
6342 }
6343 }
6344 }
6345
6346 u2 = h->data[i + h->size[0] * (i - 1)];
6347 if (h->data[i + h->size[0] * (i - 1)].im != 0.0) {
6348 tst = rt_hypotd_snf(h->data[i + h->size[0] * (i - 1)].re, h->data[i
6349 + h->size[0] * (i - 1)].im);
6350 h->data[i + h->size[0] * (i - 1)].re = tst;
6351 h->data[i + h->size[0] * (i - 1)].im = 0.0;
6352 if (u2.im == 0.0) {
6353 u2.re /= tst;
6354 u2.im = 0.0;
6355 } else if (u2.re == 0.0) {
6356 u2.re = 0.0;
6357 u2.im /= tst;
6358 } else {
6359 u2.re /= tst;
6360 u2.im /= tst;
6361 }
6362
6363 if (n > i + 1) {
6364 y.re = u2.re;
6365 y.im = -u2.im;
6366 b_xscal((n - i) - 1, y, h, (i + (i + 1) * ldh) + 1, ldh);
6367 }
6368
6369 xscal(i, u2, h, 1 + i * ldh);
6370 }
6371
6372 its++;
6373 }
6374 }
6375
6376 if (!goto140) {
6377 info = i + 1;
6378 exitg1 = true;
6379 } else {
6380 i = L;
6381 }
6382 }
6383 }
6384
6385 return info;
6386 }
6387
6388 /*
6389 * Make b a row
6390 * Arguments : const double b[13]
6391 * const struct_T *options
6392 * creal_T h[2048]
6393 * double w[2048]
6394 * Return Type : void
6395 */
f_firfreqz(const double b[13],const struct_T * options,creal_T h[2048],double w[2048])6396 static void f_firfreqz(const double b[13], const struct_T *options, creal_T h
6397 [2048], double w[2048])
6398 {
6399 int i67;
6400 creal_T dcv6[2048];
6401 double digw;
6402 double b_digw[2048];
6403 double h_re;
6404 double brm;
6405 double d;
6406
6407 /* -------------------------------------------------------------------------- */
6408 /* Actual Frequency Response Computation */
6409 /* if fvflag, */
6410 /* Frequency vector specified. Use Horner's method of polynomial */
6411 /* evaluation at the frequency points and divide the numerator */
6412 /* by the denominator. */
6413 /* */
6414 /* Note: we use positive i here because of the relationship */
6415 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
6416 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
6417 /* */
6418 /* Fs was specified, freq. vector is in Hz */
6419 /* Convert from Hz to rad/sample for computational purposes */
6420 /* Digital frequency must be used for this calculation */
6421 for (i67 = 0; i67 < 2048; i67++) {
6422 w[i67] = options->w[i67];
6423 digw = 6.2831853071795862 * options->w[i67] / options->Fs;
6424 dcv6[i67].re = digw * 0.0;
6425 dcv6[i67].im = digw;
6426 b_digw[i67] = digw;
6427 }
6428
6429 b_exp(dcv6);
6430 e_polyval(b, dcv6, h);
6431 for (i67 = 0; i67 < 2048; i67++) {
6432 dcv6[i67].re = 12.0 * (b_digw[i67] * 0.0);
6433 dcv6[i67].im = 12.0 * b_digw[i67];
6434 }
6435
6436 b_exp(dcv6);
6437 for (i67 = 0; i67 < 2048; i67++) {
6438 h_re = h[i67].re;
6439 if (dcv6[i67].im == 0.0) {
6440 if (h[i67].im == 0.0) {
6441 h[i67].re /= dcv6[i67].re;
6442 h[i67].im = 0.0;
6443 } else if (h[i67].re == 0.0) {
6444 h[i67].re = 0.0;
6445 h[i67].im /= dcv6[i67].re;
6446 } else {
6447 h[i67].re /= dcv6[i67].re;
6448 h[i67].im /= dcv6[i67].re;
6449 }
6450 } else if (dcv6[i67].re == 0.0) {
6451 if (h[i67].re == 0.0) {
6452 h[i67].re = h[i67].im / dcv6[i67].im;
6453 h[i67].im = 0.0;
6454 } else if (h[i67].im == 0.0) {
6455 h[i67].re = 0.0;
6456 h[i67].im = -(h_re / dcv6[i67].im);
6457 } else {
6458 h[i67].re = h[i67].im / dcv6[i67].im;
6459 h[i67].im = -(h_re / dcv6[i67].im);
6460 }
6461 } else {
6462 brm = fabs(dcv6[i67].re);
6463 digw = fabs(dcv6[i67].im);
6464 if (brm > digw) {
6465 digw = dcv6[i67].im / dcv6[i67].re;
6466 d = dcv6[i67].re + digw * dcv6[i67].im;
6467 h[i67].re = (h[i67].re + digw * h[i67].im) / d;
6468 h[i67].im = (h[i67].im - digw * h_re) / d;
6469 } else if (digw == brm) {
6470 if (dcv6[i67].re > 0.0) {
6471 digw = 0.5;
6472 } else {
6473 digw = -0.5;
6474 }
6475
6476 if (dcv6[i67].im > 0.0) {
6477 d = 0.5;
6478 } else {
6479 d = -0.5;
6480 }
6481
6482 h[i67].re = (h[i67].re * digw + h[i67].im * d) / brm;
6483 h[i67].im = (h[i67].im * digw - h_re * d) / brm;
6484 } else {
6485 digw = dcv6[i67].re / dcv6[i67].im;
6486 d = dcv6[i67].im + digw * dcv6[i67].re;
6487 h[i67].re = (digw * h[i67].re + h[i67].im) / d;
6488 h[i67].im = (digw * h[i67].im - h_re) / d;
6489 }
6490 }
6491 }
6492 }
6493
6494 /*
6495 * FREQZ_CG Frequency response of digital filter with codegen support
6496 *
6497 * This function is based on 'freqz' by The MathWorks Inc.
6498 * Arguments : const double b[13]
6499 * const double w[2048]
6500 * double Fs
6501 * creal_T hh[2048]
6502 * Return Type : void
6503 */
f_freqz_cg(const double b[13],const double w[2048],double Fs,creal_T hh[2048])6504 static void f_freqz_cg(const double b[13], const double w[2048], double Fs,
6505 creal_T hh[2048])
6506 {
6507 static struct_T options;
6508 int i17;
6509 static const char cv22[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
6510
6511 double b_w[2048];
6512 static const char cv23[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
6513
6514 /* Cast to enforce precision rules */
6515 options.Fs = Fs;
6516 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
6517 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
6518
6519 /* Remaining are default or for advanced use */
6520 options.fvflag = 1.0;
6521 for (i17 = 0; i17 < 8; i17++) {
6522 options.range[i17] = cv22[i17];
6523 }
6524
6525 options.centerdc = 0.0;
6526 for (i17 = 0; i17 < 7; i17++) {
6527 options.configlevel[i17] = cv23[i17];
6528 }
6529
6530 f_firfreqz(b, &options, hh, b_w);
6531
6532 /* Generate the default structure to pass to freqzplot */
6533 /* If rad/sample, Fs is empty */
6534 }
6535
6536 /*
6537 * Arguments : const double p[57]
6538 * const creal_T x[2048]
6539 * creal_T y[2048]
6540 * Return Type : void
6541 */
f_polyval(const double p[57],const creal_T x[2048],creal_T y[2048])6542 static void f_polyval(const double p[57], const creal_T x[2048],
6543 creal_T y[2048])
6544 {
6545 int i18;
6546 int k;
6547 double x_im;
6548 for (i18 = 0; i18 < 2048; i18++) {
6549 y[i18].re = p[0];
6550 y[i18].im = 0.0;
6551 }
6552
6553 for (k = 0; k < 56; k++) {
6554 for (i18 = 0; i18 < 2048; i18++) {
6555 x_im = x[i18].re * y[i18].im + x[i18].im * y[i18].re;
6556 y[i18].re = (x[i18].re * y[i18].re - x[i18].im * y[i18].im) + p[k + 1];
6557 y[i18].im = x_im;
6558 }
6559 }
6560 }
6561
6562 /*
6563 * Arguments : const double o[15]
6564 * double u[57]
6565 * Return Type : void
6566 */
f_us(const double o[15],double u[57])6567 static void f_us(const double o[15], double u[57])
6568 {
6569 double b_u[60];
6570 int ix;
6571 int iy;
6572 int k;
6573 memset(&b_u[0], 0, 60U * sizeof(double));
6574 ix = 0;
6575 iy = 0;
6576 for (k = 0; k < 15; k++) {
6577 b_u[iy] = o[ix];
6578 ix++;
6579 iy += 4;
6580 }
6581
6582 memcpy(&u[0], &b_u[0], 57U * sizeof(double));
6583 }
6584
6585 /*
6586 * Make b a row
6587 * Arguments : const struct_T *options
6588 * creal_T h[2048]
6589 * double w[2048]
6590 * Return Type : void
6591 */
firfreqz(const struct_T * options,creal_T h[2048],double w[2048])6592 static void firfreqz(const struct_T *options, creal_T h[2048], double w[2048])
6593 {
6594 int i63;
6595 double brm;
6596 double bim;
6597 double d;
6598
6599 /* -------------------------------------------------------------------------- */
6600 /* Actual Frequency Response Computation */
6601 /* if fvflag, */
6602 /* Frequency vector specified. Use Horner's method of polynomial */
6603 /* evaluation at the frequency points and divide the numerator */
6604 /* by the denominator. */
6605 /* */
6606 /* Note: we use positive i here because of the relationship */
6607 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
6608 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
6609 /* */
6610 /* Fs was specified, freq. vector is in Hz */
6611 /* Convert from Hz to rad/sample for computational purposes */
6612 /* Digital frequency must be used for this calculation */
6613 for (i63 = 0; i63 < 2048; i63++) {
6614 w[i63] = options->w[i63];
6615 h[i63].re = 0.0 * (6.2831853071795862 * options->w[i63] / options->Fs * 0.0);
6616 h[i63].im = 0.0 * (6.2831853071795862 * options->w[i63] / options->Fs);
6617 }
6618
6619 b_exp(h);
6620 for (i63 = 0; i63 < 2048; i63++) {
6621 if (h[i63].im == 0.0) {
6622 h[i63].re = 1.0 / h[i63].re;
6623 h[i63].im = 0.0;
6624 } else if (h[i63].re == 0.0) {
6625 h[i63].re = 0.0;
6626 h[i63].im = -(1.0 / h[i63].im);
6627 } else {
6628 brm = fabs(h[i63].re);
6629 bim = fabs(h[i63].im);
6630 if (brm > bim) {
6631 bim = h[i63].im / h[i63].re;
6632 d = h[i63].re + bim * h[i63].im;
6633 h[i63].re = (1.0 + bim * 0.0) / d;
6634 h[i63].im = (0.0 - bim) / d;
6635 } else if (bim == brm) {
6636 if (h[i63].re > 0.0) {
6637 bim = 0.5;
6638 } else {
6639 bim = -0.5;
6640 }
6641
6642 if (h[i63].im > 0.0) {
6643 d = 0.5;
6644 } else {
6645 d = -0.5;
6646 }
6647
6648 h[i63].re = (bim + 0.0 * d) / brm;
6649 h[i63].im = (0.0 * bim - d) / brm;
6650 } else {
6651 bim = h[i63].re / h[i63].im;
6652 d = h[i63].im + bim * h[i63].re;
6653 h[i63].re = bim / d;
6654 h[i63].im = (bim * 0.0 - 1.0) / d;
6655 }
6656 }
6657 }
6658 }
6659
6660 /*
6661 * FIRPM Parks-McClellan optimal equiripple FIR filter design.
6662 *
6663 * This function is based on 'firpm' by The MathWorks Inc.
6664 * Arguments : double order
6665 * const double ff[4]
6666 * const emxArray_real_T *amplitudes
6667 * const emxArray_real_T *frequencies
6668 * const emxArray_real_T *weights
6669 * emxArray_real_T *h
6670 * Return Type : void
6671 */
firpm_cg(double order,const double ff[4],const emxArray_real_T * amplitudes,const emxArray_real_T * frequencies,const emxArray_real_T * weights,emxArray_real_T * h)6672 static void firpm_cg(double order, const double ff[4], const emxArray_real_T
6673 *amplitudes, const emxArray_real_T *frequencies, const
6674 emxArray_real_T *weights, emxArray_real_T *h)
6675 {
6676 emxArray_real_T *grid;
6677 emxArray_real_T *des;
6678 emxArray_real_T *wt;
6679 emxArray_real_T *b_h;
6680 int i41;
6681 emxArray_real_T *c_h;
6682 double b_ff[4];
6683 double err;
6684 boolean_T valid;
6685 int h_idx_0;
6686 int i42;
6687 int i43;
6688 int loop_ub;
6689 emxInit_real_T(&grid, 2);
6690 emxInit_real_T(&des, 2);
6691 emxInit_real_T(&wt, 2);
6692 emxInit_real_T(&b_h, 2);
6693
6694 /* */
6695 firpmgrid_cg(order + 1.0, ff, grid);
6696
6697 /* orgFreqIndx = frequencies; */
6698 /* */
6699 /* positionsOfNewFreqIndx = zeros(size(grid)); */
6700 /* for ind = 1:length(positionsOfNewFreqIndx) */
6701 /* */
6702 /* [~,indx] = min( abs(orgFreqIndx-grid(ind)) ); */
6703 /* */
6704 /* positionsOfNewFreqIndx(ind) = indx; */
6705 /* end */
6706 /* */
6707 /* wt = weights(positionsOfNewFreqIndx); */
6708 /* des = amplitudes(positionsOfNewFreqIndx); */
6709 interp1(frequencies, amplitudes, grid, des);
6710 interp1(frequencies, weights, grid, wt);
6711
6712 /* Workaround */
6713 /* ftype = 2; */
6714 /* sign_val = 1; */
6715 /* Always bandpass designs */
6716 /* cast to enforce precision rules */
6717 /* Call actual design algorithm */
6718 rdivide(grid, 2.0, b_h);
6719 emxFree_real_T(&grid);
6720 for (i41 = 0; i41 < 4; i41++) {
6721 b_ff[i41] = ff[i41] / 2.0;
6722 }
6723
6724 emxInit_real_T(&c_h, 2);
6725 remezm(order + 1.0, b_ff, b_h, des, wt, c_h, &err, &valid);
6726 h_idx_0 = c_h->size[0] * c_h->size[1];
6727 i41 = h->size[0] * h->size[1];
6728 h->size[0] = 1;
6729 h->size[1] = h_idx_0;
6730 emxEnsureCapacity_real_T(h, i41);
6731 emxFree_real_T(&wt);
6732 emxFree_real_T(&des);
6733 for (i41 = 0; i41 < h_idx_0; i41++) {
6734 h->data[h->size[0] * i41] = c_h->data[i41];
6735 }
6736
6737 emxFree_real_T(&c_h);
6738
6739 /* make it a row */
6740 err = (double)h->size[1] - rt_remd_snf(order + 1.0, 2.0);
6741 if (1.0 > err) {
6742 i41 = 1;
6743 h_idx_0 = 1;
6744 i42 = 0;
6745 } else {
6746 i41 = (int)err;
6747 h_idx_0 = -1;
6748 i42 = 1;
6749 }
6750
6751 i43 = b_h->size[0] * b_h->size[1];
6752 b_h->size[0] = 1;
6753 b_h->size[1] = (h->size[1] + div_s32_floor(i42 - i41, h_idx_0)) + 1;
6754 emxEnsureCapacity_real_T(b_h, i43);
6755 loop_ub = h->size[1];
6756 for (i43 = 0; i43 < loop_ub; i43++) {
6757 b_h->data[b_h->size[0] * i43] = h->data[h->size[0] * i43];
6758 }
6759
6760 loop_ub = div_s32_floor(i42 - i41, h_idx_0);
6761 for (i42 = 0; i42 <= loop_ub; i42++) {
6762 b_h->data[b_h->size[0] * (i42 + h->size[1])] = h->data[(i41 + h_idx_0 * i42)
6763 - 1];
6764 }
6765
6766 i41 = h->size[0] * h->size[1];
6767 h->size[0] = 1;
6768 h->size[1] = b_h->size[1];
6769 emxEnsureCapacity_real_T(h, i41);
6770 loop_ub = b_h->size[1];
6771 for (i41 = 0; i41 < loop_ub; i41++) {
6772 h->data[h->size[0] * i41] = b_h->data[b_h->size[0] * i41];
6773 }
6774
6775 if (1 > h->size[1]) {
6776 i41 = 1;
6777 h_idx_0 = 1;
6778 i42 = 0;
6779 } else {
6780 i41 = h->size[1];
6781 h_idx_0 = -1;
6782 i42 = 1;
6783 }
6784
6785 i43 = b_h->size[0] * b_h->size[1];
6786 b_h->size[0] = 1;
6787 b_h->size[1] = div_s32_floor(i42 - i41, h_idx_0) + 1;
6788 emxEnsureCapacity_real_T(b_h, i43);
6789 loop_ub = div_s32_floor(i42 - i41, h_idx_0);
6790 for (i42 = 0; i42 <= loop_ub; i42++) {
6791 b_h->data[b_h->size[0] * i42] = h->data[(i41 + h_idx_0 * i42) - 1];
6792 }
6793
6794 i41 = h->size[0] * h->size[1];
6795 h->size[0] = 1;
6796 h->size[1] = b_h->size[1];
6797 emxEnsureCapacity_real_T(h, i41);
6798 loop_ub = b_h->size[1];
6799 for (i41 = 0; i41 < loop_ub; i41++) {
6800 h->data[h->size[0] * i41] = b_h->data[b_h->size[0] * i41];
6801 }
6802
6803 emxFree_real_T(&b_h);
6804 }
6805
6806 /*
6807 * firpmgrid
6808 * Arguments : double nfilt
6809 * const double ff[4]
6810 * emxArray_real_T *gridactual
6811 * Return Type : void
6812 */
firpmgrid_cg(double nfilt,const double ff[4],emxArray_real_T * gridactual)6813 static void firpmgrid_cg(double nfilt, const double ff[4], emxArray_real_T
6814 *gridactual)
6815 {
6816 static double grid[100000];
6817 double ngrid;
6818 double b_ngrid;
6819 double delf;
6820 double j;
6821 double l;
6822 double gridSize;
6823 emxArray_real_T *newgrid;
6824 emxArray_int32_T *r20;
6825 double a;
6826 int k;
6827 int nm1d2;
6828 double ndbl;
6829 double apnd;
6830 double cdiff;
6831 double delf1;
6832 double absa;
6833 double absb;
6834 int n;
6835
6836 /* -------------------------------------------------------------------------- */
6837 memset(&grid[0], 0, 100000U * sizeof(double));
6838
6839 /* Make large initial memory */
6840 /* Generate frequency grid */
6841 ngrid = nfilt / 2.0;
6842 grid[0] = ff[0];
6843 if (ngrid < 0.0) {
6844 b_ngrid = ceil(ngrid);
6845 } else {
6846 b_ngrid = floor(ngrid);
6847 }
6848
6849 delf = 1.0 / (16.0 * b_ngrid);
6850
6851 /* If value at frequency 0 is constrained, make sure first grid point */
6852 /* is not too small: */
6853 j = 1.0;
6854 l = 1.0;
6855 gridSize = 1.0;
6856 emxInit_real_T(&newgrid, 2);
6857 emxInit_int32_T(&r20, 2);
6858 while (l + 1.0 <= 4.0) {
6859 a = grid[(int)j - 1] + delf;
6860 ngrid = ff[(int)(l + 1.0) - 1] + delf;
6861 if (rtIsNaN(a) || rtIsNaN(delf) || rtIsNaN(ngrid)) {
6862 k = newgrid->size[0] * newgrid->size[1];
6863 newgrid->size[0] = 1;
6864 newgrid->size[1] = 1;
6865 emxEnsureCapacity_real_T(newgrid, k);
6866 newgrid->data[0] = rtNaN;
6867 } else if ((delf == 0.0) || ((a < ngrid) && (delf < 0.0)) || ((ngrid < a) &&
6868 (delf > 0.0))) {
6869 k = newgrid->size[0] * newgrid->size[1];
6870 newgrid->size[0] = 1;
6871 newgrid->size[1] = 0;
6872 emxEnsureCapacity_real_T(newgrid, k);
6873 } else if ((rtIsInf(a) || rtIsInf(ngrid)) && (rtIsInf(delf) || (a == ngrid))) {
6874 k = newgrid->size[0] * newgrid->size[1];
6875 newgrid->size[0] = 1;
6876 newgrid->size[1] = 1;
6877 emxEnsureCapacity_real_T(newgrid, k);
6878 newgrid->data[0] = rtNaN;
6879 } else if (rtIsInf(delf)) {
6880 k = newgrid->size[0] * newgrid->size[1];
6881 newgrid->size[0] = 1;
6882 newgrid->size[1] = 1;
6883 emxEnsureCapacity_real_T(newgrid, k);
6884 newgrid->data[0] = a;
6885 } else if ((floor(a) == a) && (floor(delf) == delf)) {
6886 k = newgrid->size[0] * newgrid->size[1];
6887 newgrid->size[0] = 1;
6888 newgrid->size[1] = (int)floor((ngrid - a) / delf) + 1;
6889 emxEnsureCapacity_real_T(newgrid, k);
6890 nm1d2 = (int)floor((ngrid - a) / delf);
6891 for (k = 0; k <= nm1d2; k++) {
6892 newgrid->data[newgrid->size[0] * k] = a + delf * (double)k;
6893 }
6894 } else {
6895 ndbl = floor((ngrid - a) / delf + 0.5);
6896 apnd = a + ndbl * delf;
6897 if (delf > 0.0) {
6898 cdiff = apnd - ngrid;
6899 } else {
6900 cdiff = ngrid - apnd;
6901 }
6902
6903 absa = fabs(a);
6904 absb = fabs(ngrid);
6905 if ((absa > absb) || rtIsNaN(absb)) {
6906 absb = absa;
6907 }
6908
6909 if (fabs(cdiff) < 4.4408920985006262E-16 * absb) {
6910 ndbl++;
6911 apnd = ngrid;
6912 } else if (cdiff > 0.0) {
6913 apnd = a + (ndbl - 1.0) * delf;
6914 } else {
6915 ndbl++;
6916 }
6917
6918 if (ndbl >= 0.0) {
6919 n = (int)ndbl;
6920 } else {
6921 n = 0;
6922 }
6923
6924 k = newgrid->size[0] * newgrid->size[1];
6925 newgrid->size[0] = 1;
6926 newgrid->size[1] = n;
6927 emxEnsureCapacity_real_T(newgrid, k);
6928 if (n > 0) {
6929 newgrid->data[0] = a;
6930 if (n > 1) {
6931 newgrid->data[n - 1] = apnd;
6932 nm1d2 = (n - 1) / 2;
6933 for (k = 1; k < nm1d2; k++) {
6934 ngrid = (double)k * delf;
6935 newgrid->data[k] = a + ngrid;
6936 newgrid->data[(n - k) - 1] = apnd - ngrid;
6937 }
6938
6939 if (nm1d2 << 1 == n - 1) {
6940 newgrid->data[nm1d2] = (a + apnd) / 2.0;
6941 } else {
6942 ngrid = (double)nm1d2 * delf;
6943 newgrid->data[nm1d2] = a + ngrid;
6944 newgrid->data[nm1d2 + 1] = apnd - ngrid;
6945 }
6946 }
6947 }
6948 }
6949
6950 if (newgrid->size[1] < 11) {
6951 delf1 = ((ff[(int)(l + 1.0) - 1] + delf) - (grid[(int)j - 1] + delf)) /
6952 10.0;
6953 a = grid[(int)j - 1] + delf1;
6954 ngrid = ff[(int)(l + 1.0) - 1] + delf1;
6955 if (rtIsNaN(a) || rtIsNaN(delf1) || rtIsNaN(ngrid)) {
6956 k = newgrid->size[0] * newgrid->size[1];
6957 newgrid->size[0] = 1;
6958 newgrid->size[1] = 1;
6959 emxEnsureCapacity_real_T(newgrid, k);
6960 newgrid->data[0] = rtNaN;
6961 } else if ((delf1 == 0.0) || ((a < ngrid) && (delf1 < 0.0)) || ((ngrid < a)
6962 && (delf1 > 0.0))) {
6963 k = newgrid->size[0] * newgrid->size[1];
6964 newgrid->size[0] = 1;
6965 newgrid->size[1] = 0;
6966 emxEnsureCapacity_real_T(newgrid, k);
6967 } else if ((rtIsInf(a) || rtIsInf(ngrid)) && (rtIsInf(delf1) || (a ==
6968 ngrid))) {
6969 k = newgrid->size[0] * newgrid->size[1];
6970 newgrid->size[0] = 1;
6971 newgrid->size[1] = 1;
6972 emxEnsureCapacity_real_T(newgrid, k);
6973 newgrid->data[0] = rtNaN;
6974 } else if (rtIsInf(delf1)) {
6975 k = newgrid->size[0] * newgrid->size[1];
6976 newgrid->size[0] = 1;
6977 newgrid->size[1] = 1;
6978 emxEnsureCapacity_real_T(newgrid, k);
6979 newgrid->data[0] = a;
6980 } else if ((floor(a) == a) && (floor(delf1) == delf1)) {
6981 k = newgrid->size[0] * newgrid->size[1];
6982 newgrid->size[0] = 1;
6983 newgrid->size[1] = (int)floor((ngrid - a) / delf1) + 1;
6984 emxEnsureCapacity_real_T(newgrid, k);
6985 nm1d2 = (int)floor((ngrid - a) / delf1);
6986 for (k = 0; k <= nm1d2; k++) {
6987 newgrid->data[newgrid->size[0] * k] = a + delf1 * (double)k;
6988 }
6989 } else {
6990 ndbl = floor((ngrid - a) / delf1 + 0.5);
6991 apnd = a + ndbl * delf1;
6992 if (delf1 > 0.0) {
6993 cdiff = apnd - ngrid;
6994 } else {
6995 cdiff = ngrid - apnd;
6996 }
6997
6998 absa = fabs(a);
6999 absb = fabs(ngrid);
7000 if ((absa > absb) || rtIsNaN(absb)) {
7001 absb = absa;
7002 }
7003
7004 if (fabs(cdiff) < 4.4408920985006262E-16 * absb) {
7005 ndbl++;
7006 apnd = ngrid;
7007 } else if (cdiff > 0.0) {
7008 apnd = a + (ndbl - 1.0) * delf1;
7009 } else {
7010 ndbl++;
7011 }
7012
7013 if (ndbl >= 0.0) {
7014 n = (int)ndbl;
7015 } else {
7016 n = 0;
7017 }
7018
7019 k = newgrid->size[0] * newgrid->size[1];
7020 newgrid->size[0] = 1;
7021 newgrid->size[1] = n;
7022 emxEnsureCapacity_real_T(newgrid, k);
7023 if (n > 0) {
7024 newgrid->data[0] = a;
7025 if (n > 1) {
7026 newgrid->data[n - 1] = apnd;
7027 nm1d2 = (n - 1) / 2;
7028 for (k = 1; k < nm1d2; k++) {
7029 ngrid = (double)k * delf1;
7030 newgrid->data[k] = a + ngrid;
7031 newgrid->data[(n - k) - 1] = apnd - ngrid;
7032 }
7033
7034 if (nm1d2 << 1 == n - 1) {
7035 newgrid->data[nm1d2] = (a + apnd) / 2.0;
7036 } else {
7037 ngrid = (double)nm1d2 * delf1;
7038 newgrid->data[nm1d2] = a + ngrid;
7039 newgrid->data[nm1d2 + 1] = apnd - ngrid;
7040 }
7041 }
7042 }
7043 }
7044 }
7045
7046 /* grid = [grid newgrid]; */
7047 k = newgrid->size[1];
7048 nm1d2 = r20->size[0] * r20->size[1];
7049 r20->size[0] = 1;
7050 r20->size[1] = (int)((double)k - 1.0) + 1;
7051 emxEnsureCapacity_int32_T(r20, nm1d2);
7052 nm1d2 = (int)((double)k - 1.0);
7053 for (k = 0; k <= nm1d2; k++) {
7054 r20->data[r20->size[0] * k] = (int)(gridSize + (1.0 + (double)k));
7055 }
7056
7057 nm1d2 = newgrid->size[0] * newgrid->size[1];
7058 for (k = 0; k < nm1d2; k++) {
7059 grid[r20->data[k] - 1] = newgrid->data[k];
7060 }
7061
7062 gridSize += (double)newgrid->size[1];
7063
7064 /* jend = length(grid); */
7065 /* length(grid); */
7066 if (gridSize > 1.0) {
7067 grid[(int)(gridSize - 1.0) - 1] = ff[(int)(l + 1.0) - 1];
7068 j = gridSize;
7069 } else {
7070 j = 2.0;
7071 }
7072
7073 l += 2.0;
7074 if (l + 1.0 <= 4.0) {
7075 grid[(int)j - 1] = ff[(int)l - 1];
7076 }
7077 }
7078
7079 emxFree_int32_T(&r20);
7080 emxFree_real_T(&newgrid);
7081 ngrid = j - 1.0;
7082
7083 /* If value at frequency 1 is constrained, remove that grid point: */
7084 if (grid[(int)(j - 1.0) - 1] > 1.0 - delf) {
7085 if (ff[2] < 1.0 - delf) {
7086 ngrid = (j - 1.0) - 1.0;
7087 } else {
7088 grid[(int)(j - 1.0) - 1] = ff[2];
7089 }
7090 }
7091
7092 if (1.0 > ngrid) {
7093 nm1d2 = 0;
7094 } else {
7095 nm1d2 = (int)ngrid;
7096 }
7097
7098 k = gridactual->size[0] * gridactual->size[1];
7099 gridactual->size[0] = 1;
7100 gridactual->size[1] = nm1d2;
7101 emxEnsureCapacity_real_T(gridactual, k);
7102 for (k = 0; k < nm1d2; k++) {
7103 gridactual->data[gridactual->size[0] * k] = grid[k];
7104 }
7105 }
7106
7107 /*
7108 * FREQS Laplace-transform (s-domain) frequency response with codegen support
7109 *
7110 * This function is based on 'freqs' by The MathWorks Inc.
7111 * Arguments : const double b_data[]
7112 * const int b_size[2]
7113 * const emxArray_creal_T *a
7114 * const double w[2048]
7115 * creal_T h[2048]
7116 * Return Type : void
7117 */
freqs_cg(const double b_data[],const int b_size[2],const emxArray_creal_T * a,const double w[2048],creal_T h[2048])7118 static void freqs_cg(const double b_data[], const int b_size[2], const
7119 emxArray_creal_T *a, const double w[2048], creal_T h[2048])
7120 {
7121 emxArray_creal_T *b_a;
7122 double b_b_data[4];
7123 int b_b_size[2];
7124 int i26;
7125 boolean_T b0;
7126 static creal_T s[2048];
7127 int k;
7128 creal_T y[2048];
7129 double h_re;
7130 double bim;
7131 double d;
7132 double brm;
7133 emxInit_creal_T(&b_a, 2);
7134 removeTrailingZero(b_data, b_size, a, b_b_data, b_b_size, b_a);
7135 for (i26 = 0; i26 < 2048; i26++) {
7136 s[i26].re = w[i26] * 0.0;
7137 s[i26].im = w[i26];
7138 }
7139
7140 b0 = (b_a->size[1] == 0);
7141 if (!b0) {
7142 for (i26 = 0; i26 < 2048; i26++) {
7143 y[i26] = b_a->data[0];
7144 }
7145
7146 for (k = 0; k <= b_a->size[1] - 2; k++) {
7147 bim = b_a->data[k + 1].re;
7148 d = b_a->data[k + 1].im;
7149 for (i26 = 0; i26 < 2048; i26++) {
7150 brm = s[i26].re * y[i26].im + s[i26].im * y[i26].re;
7151 y[i26].re = (s[i26].re * y[i26].re - s[i26].im * y[i26].im) + bim;
7152 y[i26].im = brm + d;
7153 }
7154 }
7155 }
7156
7157 emxFree_creal_T(&b_a);
7158 c_polyval(b_b_data, b_b_size, s, h);
7159 for (i26 = 0; i26 < 2048; i26++) {
7160 h_re = h[i26].re;
7161 if (y[i26].im == 0.0) {
7162 if (h[i26].im == 0.0) {
7163 h[i26].re /= y[i26].re;
7164 h[i26].im = 0.0;
7165 } else if (h[i26].re == 0.0) {
7166 h[i26].re = 0.0;
7167 h[i26].im /= y[i26].re;
7168 } else {
7169 h[i26].re /= y[i26].re;
7170 h[i26].im /= y[i26].re;
7171 }
7172 } else if (y[i26].re == 0.0) {
7173 if (h[i26].re == 0.0) {
7174 h[i26].re = h[i26].im / y[i26].im;
7175 h[i26].im = 0.0;
7176 } else if (h[i26].im == 0.0) {
7177 h[i26].re = 0.0;
7178 h[i26].im = -(h_re / y[i26].im);
7179 } else {
7180 h[i26].re = h[i26].im / y[i26].im;
7181 h[i26].im = -(h_re / y[i26].im);
7182 }
7183 } else {
7184 brm = fabs(y[i26].re);
7185 bim = fabs(y[i26].im);
7186 if (brm > bim) {
7187 bim = y[i26].im / y[i26].re;
7188 d = y[i26].re + bim * y[i26].im;
7189 h[i26].re = (h[i26].re + bim * h[i26].im) / d;
7190 h[i26].im = (h[i26].im - bim * h_re) / d;
7191 } else if (bim == brm) {
7192 if (y[i26].re > 0.0) {
7193 bim = 0.5;
7194 } else {
7195 bim = -0.5;
7196 }
7197
7198 if (y[i26].im > 0.0) {
7199 d = 0.5;
7200 } else {
7201 d = -0.5;
7202 }
7203
7204 h[i26].re = (h[i26].re * bim + h[i26].im * d) / brm;
7205 h[i26].im = (h[i26].im * bim - h_re * d) / brm;
7206 } else {
7207 bim = y[i26].re / y[i26].im;
7208 d = y[i26].im + bim * y[i26].re;
7209 h[i26].re = (bim * h[i26].re + h[i26].im) / d;
7210 h[i26].im = (bim * h[i26].im - h_re) / d;
7211 }
7212 }
7213 }
7214 }
7215
7216 /*
7217 * FREQZ_CG Frequency response of digital filter with codegen support
7218 *
7219 * This function is based on 'freqz' by The MathWorks Inc.
7220 * Arguments : const double w[2048]
7221 * double Fs
7222 * creal_T hh[2048]
7223 * Return Type : void
7224 */
freqz_cg(const double w[2048],double Fs,creal_T hh[2048])7225 static void freqz_cg(const double w[2048], double Fs, creal_T hh[2048])
7226 {
7227 struct_T options;
7228 int i8;
7229 static const char cv12[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
7230
7231 double b_w[2048];
7232 static const char cv13[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
7233
7234 /* Cast to enforce precision rules */
7235 options.Fs = Fs;
7236 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
7237 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
7238
7239 /* Remaining are default or for advanced use */
7240 options.fvflag = 1.0;
7241 for (i8 = 0; i8 < 8; i8++) {
7242 options.range[i8] = cv12[i8];
7243 }
7244
7245 options.centerdc = 0.0;
7246 for (i8 = 0; i8 < 7; i8++) {
7247 options.configlevel[i8] = cv13[i8];
7248 }
7249
7250 firfreqz(&options, hh, b_w);
7251
7252 /* Generate the default structure to pass to freqzplot */
7253 /* If rad/sample, Fs is empty */
7254 }
7255
7256 /*
7257 * Make b a row
7258 * Arguments : const double b[57]
7259 * const struct_T *options
7260 * creal_T h[2048]
7261 * double w[2048]
7262 * Return Type : void
7263 */
g_firfreqz(const double b[57],const struct_T * options,creal_T h[2048],double w[2048])7264 static void g_firfreqz(const double b[57], const struct_T *options, creal_T h
7265 [2048], double w[2048])
7266 {
7267 int i68;
7268 creal_T dcv7[2048];
7269 double digw;
7270 double b_digw[2048];
7271 double h_re;
7272 double brm;
7273 double d;
7274
7275 /* -------------------------------------------------------------------------- */
7276 /* Actual Frequency Response Computation */
7277 /* if fvflag, */
7278 /* Frequency vector specified. Use Horner's method of polynomial */
7279 /* evaluation at the frequency points and divide the numerator */
7280 /* by the denominator. */
7281 /* */
7282 /* Note: we use positive i here because of the relationship */
7283 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
7284 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
7285 /* */
7286 /* Fs was specified, freq. vector is in Hz */
7287 /* Convert from Hz to rad/sample for computational purposes */
7288 /* Digital frequency must be used for this calculation */
7289 for (i68 = 0; i68 < 2048; i68++) {
7290 w[i68] = options->w[i68];
7291 digw = 6.2831853071795862 * options->w[i68] / options->Fs;
7292 dcv7[i68].re = digw * 0.0;
7293 dcv7[i68].im = digw;
7294 b_digw[i68] = digw;
7295 }
7296
7297 b_exp(dcv7);
7298 f_polyval(b, dcv7, h);
7299 for (i68 = 0; i68 < 2048; i68++) {
7300 dcv7[i68].re = 56.0 * (b_digw[i68] * 0.0);
7301 dcv7[i68].im = 56.0 * b_digw[i68];
7302 }
7303
7304 b_exp(dcv7);
7305 for (i68 = 0; i68 < 2048; i68++) {
7306 h_re = h[i68].re;
7307 if (dcv7[i68].im == 0.0) {
7308 if (h[i68].im == 0.0) {
7309 h[i68].re /= dcv7[i68].re;
7310 h[i68].im = 0.0;
7311 } else if (h[i68].re == 0.0) {
7312 h[i68].re = 0.0;
7313 h[i68].im /= dcv7[i68].re;
7314 } else {
7315 h[i68].re /= dcv7[i68].re;
7316 h[i68].im /= dcv7[i68].re;
7317 }
7318 } else if (dcv7[i68].re == 0.0) {
7319 if (h[i68].re == 0.0) {
7320 h[i68].re = h[i68].im / dcv7[i68].im;
7321 h[i68].im = 0.0;
7322 } else if (h[i68].im == 0.0) {
7323 h[i68].re = 0.0;
7324 h[i68].im = -(h_re / dcv7[i68].im);
7325 } else {
7326 h[i68].re = h[i68].im / dcv7[i68].im;
7327 h[i68].im = -(h_re / dcv7[i68].im);
7328 }
7329 } else {
7330 brm = fabs(dcv7[i68].re);
7331 digw = fabs(dcv7[i68].im);
7332 if (brm > digw) {
7333 digw = dcv7[i68].im / dcv7[i68].re;
7334 d = dcv7[i68].re + digw * dcv7[i68].im;
7335 h[i68].re = (h[i68].re + digw * h[i68].im) / d;
7336 h[i68].im = (h[i68].im - digw * h_re) / d;
7337 } else if (digw == brm) {
7338 if (dcv7[i68].re > 0.0) {
7339 digw = 0.5;
7340 } else {
7341 digw = -0.5;
7342 }
7343
7344 if (dcv7[i68].im > 0.0) {
7345 d = 0.5;
7346 } else {
7347 d = -0.5;
7348 }
7349
7350 h[i68].re = (h[i68].re * digw + h[i68].im * d) / brm;
7351 h[i68].im = (h[i68].im * digw - h_re * d) / brm;
7352 } else {
7353 digw = dcv7[i68].re / dcv7[i68].im;
7354 d = dcv7[i68].im + digw * dcv7[i68].re;
7355 h[i68].re = (digw * h[i68].re + h[i68].im) / d;
7356 h[i68].im = (digw * h[i68].im - h_re) / d;
7357 }
7358 }
7359 }
7360 }
7361
7362 /*
7363 * FREQZ_CG Frequency response of digital filter with codegen support
7364 *
7365 * This function is based on 'freqz' by The MathWorks Inc.
7366 * Arguments : const double b[57]
7367 * const double w[2048]
7368 * double Fs
7369 * creal_T hh[2048]
7370 * Return Type : void
7371 */
g_freqz_cg(const double b[57],const double w[2048],double Fs,creal_T hh[2048])7372 static void g_freqz_cg(const double b[57], const double w[2048], double Fs,
7373 creal_T hh[2048])
7374 {
7375 static struct_T options;
7376 int i19;
7377 static const char cv24[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
7378
7379 double b_w[2048];
7380 static const char cv25[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
7381
7382 /* Cast to enforce precision rules */
7383 options.Fs = Fs;
7384 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
7385 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
7386
7387 /* Remaining are default or for advanced use */
7388 options.fvflag = 1.0;
7389 for (i19 = 0; i19 < 8; i19++) {
7390 options.range[i19] = cv24[i19];
7391 }
7392
7393 options.centerdc = 0.0;
7394 for (i19 = 0; i19 < 7; i19++) {
7395 options.configlevel[i19] = cv25[i19];
7396 }
7397
7398 g_firfreqz(b, &options, hh, b_w);
7399
7400 /* Generate the default structure to pass to freqzplot */
7401 /* If rad/sample, Fs is empty */
7402 }
7403
7404 /*
7405 * Arguments : const double p[43]
7406 * const creal_T x[2048]
7407 * creal_T y[2048]
7408 * Return Type : void
7409 */
g_polyval(const double p[43],const creal_T x[2048],creal_T y[2048])7410 static void g_polyval(const double p[43], const creal_T x[2048],
7411 creal_T y[2048])
7412 {
7413 int i20;
7414 int k;
7415 double x_im;
7416 for (i20 = 0; i20 < 2048; i20++) {
7417 y[i20].re = p[0];
7418 y[i20].im = 0.0;
7419 }
7420
7421 for (k = 0; k < 42; k++) {
7422 for (i20 = 0; i20 < 2048; i20++) {
7423 x_im = x[i20].re * y[i20].im + x[i20].im * y[i20].re;
7424 y[i20].re = (x[i20].re * y[i20].re - x[i20].im * y[i20].im) + p[k + 1];
7425 y[i20].im = x_im;
7426 }
7427 }
7428 }
7429
7430 /*
7431 * Arguments : const double o[15]
7432 * double u[43]
7433 * Return Type : void
7434 */
g_us(const double o[15],double u[43])7435 static void g_us(const double o[15], double u[43])
7436 {
7437 double b_u[45];
7438 int ix;
7439 int iy;
7440 int k;
7441 memset(&b_u[0], 0, 45U * sizeof(double));
7442 ix = 0;
7443 iy = 0;
7444 for (k = 0; k < 15; k++) {
7445 b_u[iy] = o[ix];
7446 ix++;
7447 iy += 3;
7448 }
7449
7450 memcpy(&u[0], &b_u[0], 43U * sizeof(double));
7451 }
7452
7453 /*
7454 * Arguments : const char enables[4]
7455 * const double w[2048]
7456 * double Fs
7457 * const double hb1_coeff[15]
7458 * const double hb2_coeff[7]
7459 * const double hb3_coeff_data[]
7460 * const int hb3_coeff_size[2]
7461 * const double dec_int3_coeff_data[]
7462 * const int dec_int3_coeff_size[2]
7463 * creal_T combinedResponse[2048]
7464 * Return Type : void
7465 */
generateCascadedResponseRx(const char enables[4],const double w[2048],double Fs,const double hb1_coeff[15],const double hb2_coeff[7],const double hb3_coeff_data[],const int hb3_coeff_size[2],const double dec_int3_coeff_data[],const int dec_int3_coeff_size[2],creal_T combinedResponse[2048])7466 static void generateCascadedResponseRx(const char enables[4], const double w
7467 [2048], double Fs, const double hb1_coeff[15], const double hb2_coeff[7],
7468 const double hb3_coeff_data[], const int hb3_coeff_size[2], const double
7469 dec_int3_coeff_data[], const int dec_int3_coeff_size[2], creal_T
7470 combinedResponse[2048])
7471 {
7472 boolean_T b_bool;
7473 int kstr;
7474 int exitg1;
7475 double dv2[15];
7476 double dv3[7];
7477 double tmp_data[29];
7478 int tmp_size[2];
7479 double dv4[29];
7480 double dv5[29];
7481 double dv6[13];
7482 double dv7[57];
7483 double dv8[43];
7484 double dv9[19];
7485 double dv10[85];
7486 static const char cv1[4] = { '2', '1', '1', '1' };
7487
7488 double dv11[7];
7489 double dv12[13];
7490 double dv13[19];
7491 static creal_T d2[2048];
7492 static creal_T d3[2048];
7493 double combinedResponse_re;
7494 double combinedResponse_im;
7495 static const char cv2[4] = { '1', '2', '1', '1' };
7496
7497 static const char cv3[4] = { '1', '1', '2', '1' };
7498
7499 static const char cv4[4] = { '2', '2', '1', '1' };
7500
7501 static const char cv5[4] = { '2', '1', '2', '1' };
7502
7503 static const char cv6[4] = { '1', '2', '2', '1' };
7504
7505 static const char cv7[4] = { '2', '2', '2', '1' };
7506
7507 static const char cv8[4] = { '1', '1', '1', '3' };
7508
7509 static const char cv9[4] = { '2', '1', '1', '3' };
7510
7511 static const char cv10[4] = { '1', '2', '1', '3' };
7512
7513 static const char cv11[4] = { '2', '2', '1', '3' };
7514
7515 /* Cast */
7516 b_bool = false;
7517 kstr = 1;
7518 do {
7519 exitg1 = 0;
7520 if (kstr < 5) {
7521 if (enables[kstr - 1] != '1') {
7522 exitg1 = 1;
7523 } else {
7524 kstr++;
7525 }
7526 } else {
7527 b_bool = true;
7528 exitg1 = 1;
7529 }
7530 } while (exitg1 == 0);
7531
7532 if (b_bool) {
7533 kstr = 0;
7534 } else {
7535 b_bool = false;
7536 kstr = 0;
7537 do {
7538 exitg1 = 0;
7539 if (kstr + 1 < 5) {
7540 if (enables[kstr] != cv1[kstr]) {
7541 exitg1 = 1;
7542 } else {
7543 kstr++;
7544 }
7545 } else {
7546 b_bool = true;
7547 exitg1 = 1;
7548 }
7549 } while (exitg1 == 0);
7550
7551 if (b_bool) {
7552 kstr = 1;
7553 } else {
7554 b_bool = false;
7555 kstr = 0;
7556 do {
7557 exitg1 = 0;
7558 if (kstr + 1 < 5) {
7559 if (enables[kstr] != cv2[kstr]) {
7560 exitg1 = 1;
7561 } else {
7562 kstr++;
7563 }
7564 } else {
7565 b_bool = true;
7566 exitg1 = 1;
7567 }
7568 } while (exitg1 == 0);
7569
7570 if (b_bool) {
7571 kstr = 2;
7572 } else {
7573 b_bool = false;
7574 kstr = 0;
7575 do {
7576 exitg1 = 0;
7577 if (kstr + 1 < 5) {
7578 if (enables[kstr] != cv3[kstr]) {
7579 exitg1 = 1;
7580 } else {
7581 kstr++;
7582 }
7583 } else {
7584 b_bool = true;
7585 exitg1 = 1;
7586 }
7587 } while (exitg1 == 0);
7588
7589 if (b_bool) {
7590 kstr = 3;
7591 } else {
7592 b_bool = false;
7593 kstr = 0;
7594 do {
7595 exitg1 = 0;
7596 if (kstr + 1 < 5) {
7597 if (enables[kstr] != cv4[kstr]) {
7598 exitg1 = 1;
7599 } else {
7600 kstr++;
7601 }
7602 } else {
7603 b_bool = true;
7604 exitg1 = 1;
7605 }
7606 } while (exitg1 == 0);
7607
7608 if (b_bool) {
7609 kstr = 4;
7610 } else {
7611 b_bool = false;
7612 kstr = 0;
7613 do {
7614 exitg1 = 0;
7615 if (kstr + 1 < 5) {
7616 if (enables[kstr] != cv5[kstr]) {
7617 exitg1 = 1;
7618 } else {
7619 kstr++;
7620 }
7621 } else {
7622 b_bool = true;
7623 exitg1 = 1;
7624 }
7625 } while (exitg1 == 0);
7626
7627 if (b_bool) {
7628 kstr = 5;
7629 } else {
7630 b_bool = false;
7631 kstr = 0;
7632 do {
7633 exitg1 = 0;
7634 if (kstr + 1 < 5) {
7635 if (enables[kstr] != cv6[kstr]) {
7636 exitg1 = 1;
7637 } else {
7638 kstr++;
7639 }
7640 } else {
7641 b_bool = true;
7642 exitg1 = 1;
7643 }
7644 } while (exitg1 == 0);
7645
7646 if (b_bool) {
7647 kstr = 6;
7648 } else {
7649 b_bool = false;
7650 kstr = 0;
7651 do {
7652 exitg1 = 0;
7653 if (kstr + 1 < 5) {
7654 if (enables[kstr] != cv7[kstr]) {
7655 exitg1 = 1;
7656 } else {
7657 kstr++;
7658 }
7659 } else {
7660 b_bool = true;
7661 exitg1 = 1;
7662 }
7663 } while (exitg1 == 0);
7664
7665 if (b_bool) {
7666 kstr = 7;
7667 } else {
7668 b_bool = false;
7669 kstr = 0;
7670 do {
7671 exitg1 = 0;
7672 if (kstr + 1 < 5) {
7673 if (enables[kstr] != cv8[kstr]) {
7674 exitg1 = 1;
7675 } else {
7676 kstr++;
7677 }
7678 } else {
7679 b_bool = true;
7680 exitg1 = 1;
7681 }
7682 } while (exitg1 == 0);
7683
7684 if (b_bool) {
7685 kstr = 8;
7686 } else {
7687 b_bool = false;
7688 kstr = 0;
7689 do {
7690 exitg1 = 0;
7691 if (kstr + 1 < 5) {
7692 if (enables[kstr] != cv9[kstr]) {
7693 exitg1 = 1;
7694 } else {
7695 kstr++;
7696 }
7697 } else {
7698 b_bool = true;
7699 exitg1 = 1;
7700 }
7701 } while (exitg1 == 0);
7702
7703 if (b_bool) {
7704 kstr = 9;
7705 } else {
7706 b_bool = false;
7707 kstr = 0;
7708 do {
7709 exitg1 = 0;
7710 if (kstr + 1 < 5) {
7711 if (enables[kstr] != cv10[kstr]) {
7712 exitg1 = 1;
7713 } else {
7714 kstr++;
7715 }
7716 } else {
7717 b_bool = true;
7718 exitg1 = 1;
7719 }
7720 } while (exitg1 == 0);
7721
7722 if (b_bool) {
7723 kstr = 10;
7724 } else {
7725 b_bool = false;
7726 kstr = 0;
7727 do {
7728 exitg1 = 0;
7729 if (kstr + 1 < 5) {
7730 if (enables[kstr] != cv11[kstr]) {
7731 exitg1 = 1;
7732 } else {
7733 kstr++;
7734 }
7735 } else {
7736 b_bool = true;
7737 exitg1 = 1;
7738 }
7739 } while (exitg1 == 0);
7740
7741 if (b_bool) {
7742 kstr = 11;
7743 } else {
7744 kstr = -1;
7745 }
7746 }
7747 }
7748 }
7749 }
7750 }
7751 }
7752 }
7753 }
7754 }
7755 }
7756 }
7757
7758 switch (kstr) {
7759 case 0:
7760 /* only FIR */
7761 freqz_cg(w, Fs, combinedResponse);
7762 break;
7763
7764 case 1:
7765 /* Hb1 */
7766 us(hb1_coeff, dv2);
7767 b_freqz_cg(dv2, w, Fs, combinedResponse);
7768 break;
7769
7770 case 2:
7771 /* Hb2 */
7772 b_us(hb2_coeff, dv3);
7773 c_freqz_cg(dv3, w, Fs, combinedResponse);
7774 break;
7775
7776 case 3:
7777 /* Hb3 */
7778 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
7779 d_freqz_cg(tmp_data, tmp_size, w, Fs, combinedResponse);
7780 break;
7781
7782 case 4:
7783 /* Hb2,Hb1 */
7784 d_us(hb1_coeff, dv4);
7785 e_freqz_cg(dv4, w, Fs, combinedResponse);
7786 b_us(hb2_coeff, dv11);
7787 c_freqz_cg(dv11, w, Fs, d2);
7788 for (kstr = 0; kstr < 2048; kstr++) {
7789 combinedResponse_re = combinedResponse[kstr].re;
7790 combinedResponse[kstr].re = combinedResponse[kstr].re * d2[kstr].re -
7791 combinedResponse[kstr].im * d2[kstr].im;
7792 combinedResponse[kstr].im = combinedResponse_re * d2[kstr].im +
7793 combinedResponse[kstr].im * d2[kstr].re;
7794 }
7795 break;
7796
7797 case 5:
7798 /* Hb3,Hb1 */
7799 d_us(hb1_coeff, dv5);
7800 e_freqz_cg(dv5, w, Fs, combinedResponse);
7801 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
7802 d_freqz_cg(tmp_data, tmp_size, w, Fs, d2);
7803 for (kstr = 0; kstr < 2048; kstr++) {
7804 combinedResponse_re = combinedResponse[kstr].re;
7805 combinedResponse[kstr].re = combinedResponse[kstr].re * d2[kstr].re -
7806 combinedResponse[kstr].im * d2[kstr].im;
7807 combinedResponse[kstr].im = combinedResponse_re * d2[kstr].im +
7808 combinedResponse[kstr].im * d2[kstr].re;
7809 }
7810 break;
7811
7812 case 6:
7813 /* Hb3,Hb2 */
7814 e_us(hb2_coeff, dv6);
7815 f_freqz_cg(dv6, w, Fs, combinedResponse);
7816 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
7817 d_freqz_cg(tmp_data, tmp_size, w, Fs, d2);
7818 for (kstr = 0; kstr < 2048; kstr++) {
7819 combinedResponse_re = combinedResponse[kstr].re;
7820 combinedResponse[kstr].re = combinedResponse[kstr].re * d2[kstr].re -
7821 combinedResponse[kstr].im * d2[kstr].im;
7822 combinedResponse[kstr].im = combinedResponse_re * d2[kstr].im +
7823 combinedResponse[kstr].im * d2[kstr].re;
7824 }
7825 break;
7826
7827 case 7:
7828 /* Hb3,Hb2,Hb1 */
7829 f_us(hb1_coeff, dv7);
7830 g_freqz_cg(dv7, w, Fs, combinedResponse);
7831 e_us(hb2_coeff, dv12);
7832 f_freqz_cg(dv12, w, Fs, d2);
7833 c_us(hb3_coeff_data, hb3_coeff_size, tmp_data, tmp_size);
7834 d_freqz_cg(tmp_data, tmp_size, w, Fs, d3);
7835 for (kstr = 0; kstr < 2048; kstr++) {
7836 combinedResponse_re = combinedResponse[kstr].re * d2[kstr].re -
7837 combinedResponse[kstr].im * d2[kstr].im;
7838 combinedResponse_im = combinedResponse[kstr].re * d2[kstr].im +
7839 combinedResponse[kstr].im * d2[kstr].re;
7840 combinedResponse[kstr].re = combinedResponse_re * d3[kstr].re -
7841 combinedResponse_im * d3[kstr].im;
7842 combinedResponse[kstr].im = combinedResponse_re * d3[kstr].im +
7843 combinedResponse_im * d3[kstr].re;
7844 }
7845 break;
7846
7847 case 8:
7848 /* Dec/Int3 */
7849 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
7850 d_freqz_cg(tmp_data, tmp_size, w, Fs, combinedResponse);
7851
7852 /* RECHECK ALL DEC BY 3 */
7853 break;
7854
7855 case 9:
7856 /* Dec/Int3,Hb1 */
7857 g_us(hb1_coeff, dv8);
7858 h_freqz_cg(dv8, w, Fs, combinedResponse);
7859 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
7860 d_freqz_cg(tmp_data, tmp_size, w, Fs, d2);
7861 for (kstr = 0; kstr < 2048; kstr++) {
7862 combinedResponse_re = combinedResponse[kstr].re;
7863 combinedResponse[kstr].re = combinedResponse[kstr].re * d2[kstr].re -
7864 combinedResponse[kstr].im * d2[kstr].im;
7865 combinedResponse[kstr].im = combinedResponse_re * d2[kstr].im +
7866 combinedResponse[kstr].im * d2[kstr].re;
7867 }
7868 break;
7869
7870 case 10:
7871 /* Dec/Int3,Hb2 */
7872 h_us(hb2_coeff, dv9);
7873 i_freqz_cg(dv9, w, Fs, combinedResponse);
7874 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
7875 d_freqz_cg(tmp_data, tmp_size, w, Fs, d2);
7876 for (kstr = 0; kstr < 2048; kstr++) {
7877 combinedResponse_re = combinedResponse[kstr].re;
7878 combinedResponse[kstr].re = combinedResponse[kstr].re * d2[kstr].re -
7879 combinedResponse[kstr].im * d2[kstr].im;
7880 combinedResponse[kstr].im = combinedResponse_re * d2[kstr].im +
7881 combinedResponse[kstr].im * d2[kstr].re;
7882 }
7883 break;
7884
7885 case 11:
7886 /* Dec/Int3,Hb2,Hb1 {Hm4,Hm2c34,Hm1} */
7887 i_us(hb1_coeff, dv10);
7888 j_freqz_cg(dv10, w, Fs, combinedResponse);
7889 h_us(hb2_coeff, dv13);
7890 i_freqz_cg(dv13, w, Fs, d2);
7891 c_us(dec_int3_coeff_data, dec_int3_coeff_size, tmp_data, tmp_size);
7892 d_freqz_cg(tmp_data, tmp_size, w, Fs, d3);
7893 for (kstr = 0; kstr < 2048; kstr++) {
7894 combinedResponse_re = combinedResponse[kstr].re * d2[kstr].re -
7895 combinedResponse[kstr].im * d2[kstr].im;
7896 combinedResponse_im = combinedResponse[kstr].re * d2[kstr].im +
7897 combinedResponse[kstr].im * d2[kstr].re;
7898 combinedResponse[kstr].re = combinedResponse_re * d3[kstr].re -
7899 combinedResponse_im * d3[kstr].im;
7900 combinedResponse[kstr].im = combinedResponse_re * d3[kstr].im +
7901 combinedResponse_im * d3[kstr].re;
7902 }
7903 break;
7904 }
7905
7906 /* Add filter extra filter to end of cascade */
7907 }
7908
7909 /*
7910 * Make b a row
7911 * Arguments : const double b[43]
7912 * const struct_T *options
7913 * creal_T h[2048]
7914 * double w[2048]
7915 * Return Type : void
7916 */
h_firfreqz(const double b[43],const struct_T * options,creal_T h[2048],double w[2048])7917 static void h_firfreqz(const double b[43], const struct_T *options, creal_T h
7918 [2048], double w[2048])
7919 {
7920 int i69;
7921 creal_T dcv8[2048];
7922 double digw;
7923 double b_digw[2048];
7924 double h_re;
7925 double brm;
7926 double d;
7927
7928 /* -------------------------------------------------------------------------- */
7929 /* Actual Frequency Response Computation */
7930 /* if fvflag, */
7931 /* Frequency vector specified. Use Horner's method of polynomial */
7932 /* evaluation at the frequency points and divide the numerator */
7933 /* by the denominator. */
7934 /* */
7935 /* Note: we use positive i here because of the relationship */
7936 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
7937 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
7938 /* */
7939 /* Fs was specified, freq. vector is in Hz */
7940 /* Convert from Hz to rad/sample for computational purposes */
7941 /* Digital frequency must be used for this calculation */
7942 for (i69 = 0; i69 < 2048; i69++) {
7943 w[i69] = options->w[i69];
7944 digw = 6.2831853071795862 * options->w[i69] / options->Fs;
7945 dcv8[i69].re = digw * 0.0;
7946 dcv8[i69].im = digw;
7947 b_digw[i69] = digw;
7948 }
7949
7950 b_exp(dcv8);
7951 g_polyval(b, dcv8, h);
7952 for (i69 = 0; i69 < 2048; i69++) {
7953 dcv8[i69].re = 42.0 * (b_digw[i69] * 0.0);
7954 dcv8[i69].im = 42.0 * b_digw[i69];
7955 }
7956
7957 b_exp(dcv8);
7958 for (i69 = 0; i69 < 2048; i69++) {
7959 h_re = h[i69].re;
7960 if (dcv8[i69].im == 0.0) {
7961 if (h[i69].im == 0.0) {
7962 h[i69].re /= dcv8[i69].re;
7963 h[i69].im = 0.0;
7964 } else if (h[i69].re == 0.0) {
7965 h[i69].re = 0.0;
7966 h[i69].im /= dcv8[i69].re;
7967 } else {
7968 h[i69].re /= dcv8[i69].re;
7969 h[i69].im /= dcv8[i69].re;
7970 }
7971 } else if (dcv8[i69].re == 0.0) {
7972 if (h[i69].re == 0.0) {
7973 h[i69].re = h[i69].im / dcv8[i69].im;
7974 h[i69].im = 0.0;
7975 } else if (h[i69].im == 0.0) {
7976 h[i69].re = 0.0;
7977 h[i69].im = -(h_re / dcv8[i69].im);
7978 } else {
7979 h[i69].re = h[i69].im / dcv8[i69].im;
7980 h[i69].im = -(h_re / dcv8[i69].im);
7981 }
7982 } else {
7983 brm = fabs(dcv8[i69].re);
7984 digw = fabs(dcv8[i69].im);
7985 if (brm > digw) {
7986 digw = dcv8[i69].im / dcv8[i69].re;
7987 d = dcv8[i69].re + digw * dcv8[i69].im;
7988 h[i69].re = (h[i69].re + digw * h[i69].im) / d;
7989 h[i69].im = (h[i69].im - digw * h_re) / d;
7990 } else if (digw == brm) {
7991 if (dcv8[i69].re > 0.0) {
7992 digw = 0.5;
7993 } else {
7994 digw = -0.5;
7995 }
7996
7997 if (dcv8[i69].im > 0.0) {
7998 d = 0.5;
7999 } else {
8000 d = -0.5;
8001 }
8002
8003 h[i69].re = (h[i69].re * digw + h[i69].im * d) / brm;
8004 h[i69].im = (h[i69].im * digw - h_re * d) / brm;
8005 } else {
8006 digw = dcv8[i69].re / dcv8[i69].im;
8007 d = dcv8[i69].im + digw * dcv8[i69].re;
8008 h[i69].re = (digw * h[i69].re + h[i69].im) / d;
8009 h[i69].im = (digw * h[i69].im - h_re) / d;
8010 }
8011 }
8012 }
8013 }
8014
8015 /*
8016 * FREQZ_CG Frequency response of digital filter with codegen support
8017 *
8018 * This function is based on 'freqz' by The MathWorks Inc.
8019 * Arguments : const double b[43]
8020 * const double w[2048]
8021 * double Fs
8022 * creal_T hh[2048]
8023 * Return Type : void
8024 */
h_freqz_cg(const double b[43],const double w[2048],double Fs,creal_T hh[2048])8025 static void h_freqz_cg(const double b[43], const double w[2048], double Fs,
8026 creal_T hh[2048])
8027 {
8028 static struct_T options;
8029 int i21;
8030 static const char cv26[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
8031
8032 double b_w[2048];
8033 static const char cv27[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
8034
8035 /* Cast to enforce precision rules */
8036 options.Fs = Fs;
8037 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
8038 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
8039
8040 /* Remaining are default or for advanced use */
8041 options.fvflag = 1.0;
8042 for (i21 = 0; i21 < 8; i21++) {
8043 options.range[i21] = cv26[i21];
8044 }
8045
8046 options.centerdc = 0.0;
8047 for (i21 = 0; i21 < 7; i21++) {
8048 options.configlevel[i21] = cv27[i21];
8049 }
8050
8051 h_firfreqz(b, &options, hh, b_w);
8052
8053 /* Generate the default structure to pass to freqzplot */
8054 /* If rad/sample, Fs is empty */
8055 }
8056
8057 /*
8058 * Arguments : const double p[19]
8059 * const creal_T x[2048]
8060 * creal_T y[2048]
8061 * Return Type : void
8062 */
h_polyval(const double p[19],const creal_T x[2048],creal_T y[2048])8063 static void h_polyval(const double p[19], const creal_T x[2048],
8064 creal_T y[2048])
8065 {
8066 int i22;
8067 int k;
8068 double x_im;
8069 for (i22 = 0; i22 < 2048; i22++) {
8070 y[i22].re = p[0];
8071 y[i22].im = 0.0;
8072 }
8073
8074 for (k = 0; k < 18; k++) {
8075 for (i22 = 0; i22 < 2048; i22++) {
8076 x_im = x[i22].re * y[i22].im + x[i22].im * y[i22].re;
8077 y[i22].re = (x[i22].re * y[i22].re - x[i22].im * y[i22].im) + p[k + 1];
8078 y[i22].im = x_im;
8079 }
8080 }
8081 }
8082
8083 /*
8084 * Arguments : const double o[7]
8085 * double u[19]
8086 * Return Type : void
8087 */
h_us(const double o[7],double u[19])8088 static void h_us(const double o[7], double u[19])
8089 {
8090 double b_u[21];
8091 int ix;
8092 int iy;
8093 int k;
8094 memset(&b_u[0], 0, 21U * sizeof(double));
8095 ix = 0;
8096 iy = 0;
8097 for (k = 0; k < 7; k++) {
8098 b_u[iy] = o[ix];
8099 ix++;
8100 iy += 3;
8101 }
8102
8103 memcpy(&u[0], &b_u[0], 19U * sizeof(double));
8104 }
8105
8106 /*
8107 * Make b a row
8108 * Arguments : const double b[19]
8109 * const struct_T *options
8110 * creal_T h[2048]
8111 * double w[2048]
8112 * Return Type : void
8113 */
i_firfreqz(const double b[19],const struct_T * options,creal_T h[2048],double w[2048])8114 static void i_firfreqz(const double b[19], const struct_T *options, creal_T h
8115 [2048], double w[2048])
8116 {
8117 int i70;
8118 creal_T dcv9[2048];
8119 double digw;
8120 double b_digw[2048];
8121 double h_re;
8122 double brm;
8123 double d;
8124
8125 /* -------------------------------------------------------------------------- */
8126 /* Actual Frequency Response Computation */
8127 /* if fvflag, */
8128 /* Frequency vector specified. Use Horner's method of polynomial */
8129 /* evaluation at the frequency points and divide the numerator */
8130 /* by the denominator. */
8131 /* */
8132 /* Note: we use positive i here because of the relationship */
8133 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
8134 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
8135 /* */
8136 /* Fs was specified, freq. vector is in Hz */
8137 /* Convert from Hz to rad/sample for computational purposes */
8138 /* Digital frequency must be used for this calculation */
8139 for (i70 = 0; i70 < 2048; i70++) {
8140 w[i70] = options->w[i70];
8141 digw = 6.2831853071795862 * options->w[i70] / options->Fs;
8142 dcv9[i70].re = digw * 0.0;
8143 dcv9[i70].im = digw;
8144 b_digw[i70] = digw;
8145 }
8146
8147 b_exp(dcv9);
8148 h_polyval(b, dcv9, h);
8149 for (i70 = 0; i70 < 2048; i70++) {
8150 dcv9[i70].re = 18.0 * (b_digw[i70] * 0.0);
8151 dcv9[i70].im = 18.0 * b_digw[i70];
8152 }
8153
8154 b_exp(dcv9);
8155 for (i70 = 0; i70 < 2048; i70++) {
8156 h_re = h[i70].re;
8157 if (dcv9[i70].im == 0.0) {
8158 if (h[i70].im == 0.0) {
8159 h[i70].re /= dcv9[i70].re;
8160 h[i70].im = 0.0;
8161 } else if (h[i70].re == 0.0) {
8162 h[i70].re = 0.0;
8163 h[i70].im /= dcv9[i70].re;
8164 } else {
8165 h[i70].re /= dcv9[i70].re;
8166 h[i70].im /= dcv9[i70].re;
8167 }
8168 } else if (dcv9[i70].re == 0.0) {
8169 if (h[i70].re == 0.0) {
8170 h[i70].re = h[i70].im / dcv9[i70].im;
8171 h[i70].im = 0.0;
8172 } else if (h[i70].im == 0.0) {
8173 h[i70].re = 0.0;
8174 h[i70].im = -(h_re / dcv9[i70].im);
8175 } else {
8176 h[i70].re = h[i70].im / dcv9[i70].im;
8177 h[i70].im = -(h_re / dcv9[i70].im);
8178 }
8179 } else {
8180 brm = fabs(dcv9[i70].re);
8181 digw = fabs(dcv9[i70].im);
8182 if (brm > digw) {
8183 digw = dcv9[i70].im / dcv9[i70].re;
8184 d = dcv9[i70].re + digw * dcv9[i70].im;
8185 h[i70].re = (h[i70].re + digw * h[i70].im) / d;
8186 h[i70].im = (h[i70].im - digw * h_re) / d;
8187 } else if (digw == brm) {
8188 if (dcv9[i70].re > 0.0) {
8189 digw = 0.5;
8190 } else {
8191 digw = -0.5;
8192 }
8193
8194 if (dcv9[i70].im > 0.0) {
8195 d = 0.5;
8196 } else {
8197 d = -0.5;
8198 }
8199
8200 h[i70].re = (h[i70].re * digw + h[i70].im * d) / brm;
8201 h[i70].im = (h[i70].im * digw - h_re * d) / brm;
8202 } else {
8203 digw = dcv9[i70].re / dcv9[i70].im;
8204 d = dcv9[i70].im + digw * dcv9[i70].re;
8205 h[i70].re = (digw * h[i70].re + h[i70].im) / d;
8206 h[i70].im = (digw * h[i70].im - h_re) / d;
8207 }
8208 }
8209 }
8210 }
8211
8212 /*
8213 * FREQZ_CG Frequency response of digital filter with codegen support
8214 *
8215 * This function is based on 'freqz' by The MathWorks Inc.
8216 * Arguments : const double b[19]
8217 * const double w[2048]
8218 * double Fs
8219 * creal_T hh[2048]
8220 * Return Type : void
8221 */
i_freqz_cg(const double b[19],const double w[2048],double Fs,creal_T hh[2048])8222 static void i_freqz_cg(const double b[19], const double w[2048], double Fs,
8223 creal_T hh[2048])
8224 {
8225 static struct_T options;
8226 int i23;
8227 static const char cv28[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
8228
8229 double b_w[2048];
8230 static const char cv29[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
8231
8232 /* Cast to enforce precision rules */
8233 options.Fs = Fs;
8234 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
8235 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
8236
8237 /* Remaining are default or for advanced use */
8238 options.fvflag = 1.0;
8239 for (i23 = 0; i23 < 8; i23++) {
8240 options.range[i23] = cv28[i23];
8241 }
8242
8243 options.centerdc = 0.0;
8244 for (i23 = 0; i23 < 7; i23++) {
8245 options.configlevel[i23] = cv29[i23];
8246 }
8247
8248 i_firfreqz(b, &options, hh, b_w);
8249
8250 /* Generate the default structure to pass to freqzplot */
8251 /* If rad/sample, Fs is empty */
8252 }
8253
8254 /*
8255 * Arguments : const double p[85]
8256 * const creal_T x[2048]
8257 * creal_T y[2048]
8258 * Return Type : void
8259 */
i_polyval(const double p[85],const creal_T x[2048],creal_T y[2048])8260 static void i_polyval(const double p[85], const creal_T x[2048],
8261 creal_T y[2048])
8262 {
8263 int i24;
8264 int k;
8265 double x_im;
8266 for (i24 = 0; i24 < 2048; i24++) {
8267 y[i24].re = p[0];
8268 y[i24].im = 0.0;
8269 }
8270
8271 for (k = 0; k < 84; k++) {
8272 for (i24 = 0; i24 < 2048; i24++) {
8273 x_im = x[i24].re * y[i24].im + x[i24].im * y[i24].re;
8274 y[i24].re = (x[i24].re * y[i24].re - x[i24].im * y[i24].im) + p[k + 1];
8275 y[i24].im = x_im;
8276 }
8277 }
8278 }
8279
8280 /*
8281 * Arguments : const double o[15]
8282 * double u[85]
8283 * Return Type : void
8284 */
i_us(const double o[15],double u[85])8285 static void i_us(const double o[15], double u[85])
8286 {
8287 double b_u[90];
8288 int ix;
8289 int iy;
8290 int k;
8291 memset(&b_u[0], 0, 90U * sizeof(double));
8292 ix = 0;
8293 iy = 0;
8294 for (k = 0; k < 15; k++) {
8295 b_u[iy] = o[ix];
8296 ix++;
8297 iy += 6;
8298 }
8299
8300 memcpy(&u[0], &b_u[0], 85U * sizeof(double));
8301 }
8302
8303 /*
8304 * Arguments : const emxArray_real_T *varargin_1
8305 * const emxArray_real_T *varargin_2
8306 * const emxArray_real_T *varargin_3
8307 * emxArray_real_T *Vq
8308 * Return Type : void
8309 */
interp1(const emxArray_real_T * varargin_1,const emxArray_real_T * varargin_2,const emxArray_real_T * varargin_3,emxArray_real_T * Vq)8310 static void interp1(const emxArray_real_T *varargin_1, const emxArray_real_T
8311 *varargin_2, const emxArray_real_T *varargin_3,
8312 emxArray_real_T *Vq)
8313 {
8314 emxArray_real_T *y;
8315 int low_ip1;
8316 int nd2;
8317 emxArray_real_T *x;
8318 int nx;
8319 int outsize[2];
8320 int k;
8321 int exitg1;
8322 int mid_i;
8323 double r;
8324 emxInit_real_T(&y, 2);
8325 low_ip1 = y->size[0] * y->size[1];
8326 y->size[0] = 1;
8327 y->size[1] = varargin_2->size[1];
8328 emxEnsureCapacity_real_T(y, low_ip1);
8329 nd2 = varargin_2->size[0] * varargin_2->size[1];
8330 for (low_ip1 = 0; low_ip1 < nd2; low_ip1++) {
8331 y->data[low_ip1] = varargin_2->data[low_ip1];
8332 }
8333
8334 emxInit_real_T(&x, 2);
8335 low_ip1 = x->size[0] * x->size[1];
8336 x->size[0] = 1;
8337 x->size[1] = varargin_1->size[1];
8338 emxEnsureCapacity_real_T(x, low_ip1);
8339 nd2 = varargin_1->size[0] * varargin_1->size[1];
8340 for (low_ip1 = 0; low_ip1 < nd2; low_ip1++) {
8341 x->data[low_ip1] = varargin_1->data[low_ip1];
8342 }
8343
8344 nx = varargin_1->size[1];
8345 for (low_ip1 = 0; low_ip1 < 2; low_ip1++) {
8346 outsize[low_ip1] = varargin_3->size[low_ip1];
8347 }
8348
8349 low_ip1 = Vq->size[0] * Vq->size[1];
8350 Vq->size[0] = 1;
8351 Vq->size[1] = outsize[1];
8352 emxEnsureCapacity_real_T(Vq, low_ip1);
8353 nd2 = outsize[1];
8354 for (low_ip1 = 0; low_ip1 < nd2; low_ip1++) {
8355 Vq->data[low_ip1] = rtNaN;
8356 }
8357
8358 if (varargin_3->size[1] != 0) {
8359 k = 1;
8360 do {
8361 exitg1 = 0;
8362 if (k <= nx) {
8363 if (rtIsNaN(varargin_1->data[k - 1])) {
8364 exitg1 = 1;
8365 } else {
8366 k++;
8367 }
8368 } else {
8369 if (varargin_1->data[1] < varargin_1->data[0]) {
8370 low_ip1 = nx >> 1;
8371 for (mid_i = 1; mid_i <= low_ip1; mid_i++) {
8372 r = x->data[mid_i - 1];
8373 x->data[mid_i - 1] = x->data[nx - mid_i];
8374 x->data[nx - mid_i] = r;
8375 }
8376
8377 nd2 = varargin_2->size[1] >> 1;
8378 for (mid_i = 1; mid_i <= nd2; mid_i++) {
8379 low_ip1 = varargin_2->size[1] - mid_i;
8380 r = y->data[mid_i - 1];
8381 y->data[mid_i - 1] = y->data[low_ip1];
8382 y->data[low_ip1] = r;
8383 }
8384 }
8385
8386 for (k = 0; k + 1 <= varargin_3->size[1]; k++) {
8387 if (rtIsNaN(varargin_3->data[k])) {
8388 Vq->data[k] = rtNaN;
8389 } else {
8390 if ((!(varargin_3->data[k] > x->data[x->size[1] - 1])) &&
8391 (!(varargin_3->data[k] < x->data[0]))) {
8392 nd2 = 1;
8393 low_ip1 = 2;
8394 nx = x->size[1];
8395 while (nx > low_ip1) {
8396 mid_i = (nd2 >> 1) + (nx >> 1);
8397 if (((nd2 & 1) == 1) && ((nx & 1) == 1)) {
8398 mid_i++;
8399 }
8400
8401 if (varargin_3->data[k] >= x->data[mid_i - 1]) {
8402 nd2 = mid_i;
8403 low_ip1 = mid_i + 1;
8404 } else {
8405 nx = mid_i;
8406 }
8407 }
8408
8409 r = (varargin_3->data[k] - x->data[nd2 - 1]) / (x->data[nd2] -
8410 x->data[nd2 - 1]);
8411 if (r == 0.0) {
8412 Vq->data[k] = y->data[nd2 - 1];
8413 } else if (r == 1.0) {
8414 Vq->data[k] = y->data[nd2];
8415 } else if (y->data[nd2 - 1] == y->data[nd2]) {
8416 Vq->data[k] = y->data[nd2 - 1];
8417 } else {
8418 Vq->data[k] = (1.0 - r) * y->data[nd2 - 1] + r * y->data[nd2];
8419 }
8420 }
8421 }
8422 }
8423
8424 exitg1 = 1;
8425 }
8426 } while (exitg1 == 0);
8427 }
8428
8429 emxFree_real_T(&x);
8430 emxFree_real_T(&y);
8431 }
8432
8433 /*
8434 * Make b a row
8435 * Arguments : const double b[85]
8436 * const struct_T *options
8437 * creal_T h[2048]
8438 * double w[2048]
8439 * Return Type : void
8440 */
j_firfreqz(const double b[85],const struct_T * options,creal_T h[2048],double w[2048])8441 static void j_firfreqz(const double b[85], const struct_T *options, creal_T h
8442 [2048], double w[2048])
8443 {
8444 int i71;
8445 creal_T dcv10[2048];
8446 double digw;
8447 double b_digw[2048];
8448 double h_re;
8449 double brm;
8450 double d;
8451
8452 /* -------------------------------------------------------------------------- */
8453 /* Actual Frequency Response Computation */
8454 /* if fvflag, */
8455 /* Frequency vector specified. Use Horner's method of polynomial */
8456 /* evaluation at the frequency points and divide the numerator */
8457 /* by the denominator. */
8458 /* */
8459 /* Note: we use positive i here because of the relationship */
8460 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
8461 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
8462 /* */
8463 /* Fs was specified, freq. vector is in Hz */
8464 /* Convert from Hz to rad/sample for computational purposes */
8465 /* Digital frequency must be used for this calculation */
8466 for (i71 = 0; i71 < 2048; i71++) {
8467 w[i71] = options->w[i71];
8468 digw = 6.2831853071795862 * options->w[i71] / options->Fs;
8469 dcv10[i71].re = digw * 0.0;
8470 dcv10[i71].im = digw;
8471 b_digw[i71] = digw;
8472 }
8473
8474 b_exp(dcv10);
8475 i_polyval(b, dcv10, h);
8476 for (i71 = 0; i71 < 2048; i71++) {
8477 dcv10[i71].re = 84.0 * (b_digw[i71] * 0.0);
8478 dcv10[i71].im = 84.0 * b_digw[i71];
8479 }
8480
8481 b_exp(dcv10);
8482 for (i71 = 0; i71 < 2048; i71++) {
8483 h_re = h[i71].re;
8484 if (dcv10[i71].im == 0.0) {
8485 if (h[i71].im == 0.0) {
8486 h[i71].re /= dcv10[i71].re;
8487 h[i71].im = 0.0;
8488 } else if (h[i71].re == 0.0) {
8489 h[i71].re = 0.0;
8490 h[i71].im /= dcv10[i71].re;
8491 } else {
8492 h[i71].re /= dcv10[i71].re;
8493 h[i71].im /= dcv10[i71].re;
8494 }
8495 } else if (dcv10[i71].re == 0.0) {
8496 if (h[i71].re == 0.0) {
8497 h[i71].re = h[i71].im / dcv10[i71].im;
8498 h[i71].im = 0.0;
8499 } else if (h[i71].im == 0.0) {
8500 h[i71].re = 0.0;
8501 h[i71].im = -(h_re / dcv10[i71].im);
8502 } else {
8503 h[i71].re = h[i71].im / dcv10[i71].im;
8504 h[i71].im = -(h_re / dcv10[i71].im);
8505 }
8506 } else {
8507 brm = fabs(dcv10[i71].re);
8508 digw = fabs(dcv10[i71].im);
8509 if (brm > digw) {
8510 digw = dcv10[i71].im / dcv10[i71].re;
8511 d = dcv10[i71].re + digw * dcv10[i71].im;
8512 h[i71].re = (h[i71].re + digw * h[i71].im) / d;
8513 h[i71].im = (h[i71].im - digw * h_re) / d;
8514 } else if (digw == brm) {
8515 if (dcv10[i71].re > 0.0) {
8516 digw = 0.5;
8517 } else {
8518 digw = -0.5;
8519 }
8520
8521 if (dcv10[i71].im > 0.0) {
8522 d = 0.5;
8523 } else {
8524 d = -0.5;
8525 }
8526
8527 h[i71].re = (h[i71].re * digw + h[i71].im * d) / brm;
8528 h[i71].im = (h[i71].im * digw - h_re * d) / brm;
8529 } else {
8530 digw = dcv10[i71].re / dcv10[i71].im;
8531 d = dcv10[i71].im + digw * dcv10[i71].re;
8532 h[i71].re = (digw * h[i71].re + h[i71].im) / d;
8533 h[i71].im = (digw * h[i71].im - h_re) / d;
8534 }
8535 }
8536 }
8537 }
8538
8539 /*
8540 * FREQZ_CG Frequency response of digital filter with codegen support
8541 *
8542 * This function is based on 'freqz' by The MathWorks Inc.
8543 * Arguments : const double b[85]
8544 * const double w[2048]
8545 * double Fs
8546 * creal_T hh[2048]
8547 * Return Type : void
8548 */
j_freqz_cg(const double b[85],const double w[2048],double Fs,creal_T hh[2048])8549 static void j_freqz_cg(const double b[85], const double w[2048], double Fs,
8550 creal_T hh[2048])
8551 {
8552 static struct_T options;
8553 int i25;
8554 static const char cv30[8] = { 'o', 'n', 'e', 's', 'i', 'd', 'e', 'd' };
8555
8556 double b_w[2048];
8557 static const char cv31[7] = { 'o', 'm', 'i', 't', 't', 'e', 'd' };
8558
8559 /* Cast to enforce precision rules */
8560 options.Fs = Fs;
8561 memcpy(&options.nfft[0], &w[0], sizeof(double) << 11);
8562 memcpy(&options.w[0], &w[0], sizeof(double) << 11);
8563
8564 /* Remaining are default or for advanced use */
8565 options.fvflag = 1.0;
8566 for (i25 = 0; i25 < 8; i25++) {
8567 options.range[i25] = cv30[i25];
8568 }
8569
8570 options.centerdc = 0.0;
8571 for (i25 = 0; i25 < 7; i25++) {
8572 options.configlevel[i25] = cv31[i25];
8573 }
8574
8575 j_firfreqz(b, &options, hh, b_w);
8576
8577 /* Generate the default structure to pass to freqzplot */
8578 /* If rad/sample, Fs is empty */
8579 }
8580
8581 /*
8582 * Arguments : const emxArray_real_T *p
8583 * const emxArray_creal_T *x
8584 * emxArray_creal_T *y
8585 * Return Type : void
8586 */
j_polyval(const emxArray_real_T * p,const emxArray_creal_T * x,emxArray_creal_T * y)8587 static void j_polyval(const emxArray_real_T *p, const emxArray_creal_T *x,
8588 emxArray_creal_T *y)
8589 {
8590 int i33;
8591 boolean_T b4;
8592 int loop_ub;
8593 int k;
8594 double p_re;
8595 double x_re;
8596 double x_im;
8597 i33 = y->size[0] * y->size[1];
8598 y->size[0] = 1;
8599 y->size[1] = x->size[1];
8600 emxEnsureCapacity_creal_T(y, i33);
8601 if ((y->size[1] == 0) || (p->size[1] == 0)) {
8602 b4 = true;
8603 } else {
8604 b4 = false;
8605 }
8606
8607 if (!b4) {
8608 i33 = y->size[0] * y->size[1];
8609 y->size[0] = 1;
8610 emxEnsureCapacity_creal_T(y, i33);
8611 loop_ub = y->size[1];
8612 for (i33 = 0; i33 < loop_ub; i33++) {
8613 y->data[y->size[0] * i33].re = p->data[0];
8614 y->data[y->size[0] * i33].im = 0.0;
8615 }
8616
8617 for (k = 0; k <= p->size[1] - 2; k++) {
8618 i33 = y->size[0] * y->size[1];
8619 y->size[0] = 1;
8620 y->size[1] = x->size[1];
8621 emxEnsureCapacity_creal_T(y, i33);
8622 p_re = p->data[k + 1];
8623 loop_ub = x->size[0] * x->size[1];
8624 for (i33 = 0; i33 < loop_ub; i33++) {
8625 x_re = x->data[i33].re * y->data[i33].re - x->data[i33].im * y->data[i33]
8626 .im;
8627 x_im = x->data[i33].re * y->data[i33].im + x->data[i33].im * y->data[i33]
8628 .re;
8629 y->data[i33].re = x_re + p_re;
8630 y->data[i33].im = x_im;
8631 }
8632 }
8633 }
8634 }
8635
8636 /*
8637 * FREQZ_CG Frequency response of digital filter with codegen support
8638 *
8639 * This function is based on 'freqz' by The MathWorks Inc.
8640 * Arguments : const emxArray_real_T *w
8641 * double Fs
8642 * emxArray_creal_T *hh
8643 * Return Type : void
8644 */
k_freqz_cg(const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)8645 static void k_freqz_cg(const emxArray_real_T *w, double Fs,
8646 emxArray_creal_T *hh)
8647 {
8648 emxArray_real_T *r4;
8649 int i28;
8650 int loop_ub;
8651 emxArray_real_T *digw;
8652 emxArray_creal_T *y;
8653 emxArray_creal_T *r5;
8654 boolean_T b1;
8655 double re;
8656 double im;
8657 emxInit_real_T(&r4, 2);
8658
8659 /* Cast to enforce precision rules */
8660 /* Remaining are default or for advanced use */
8661 /* Make b a row */
8662 /* -------------------------------------------------------------------------- */
8663 /* Actual Frequency Response Computation */
8664 /* if fvflag, */
8665 /* Frequency vector specified. Use Horner's method of polynomial */
8666 /* evaluation at the frequency points and divide the numerator */
8667 /* by the denominator. */
8668 /* */
8669 /* Note: we use positive i here because of the relationship */
8670 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
8671 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
8672 /* */
8673 /* Fs was specified, freq. vector is in Hz */
8674 i28 = r4->size[0] * r4->size[1];
8675 r4->size[0] = 1;
8676 r4->size[1] = w->size[1];
8677 emxEnsureCapacity_real_T(r4, i28);
8678 loop_ub = w->size[0] * w->size[1];
8679 for (i28 = 0; i28 < loop_ub; i28++) {
8680 r4->data[i28] = 6.2831853071795862 * w->data[i28];
8681 }
8682
8683 emxInit_real_T(&digw, 2);
8684 emxInit_creal_T(&y, 2);
8685 emxInit_creal_T(&r5, 2);
8686 rdivide(r4, Fs, digw);
8687
8688 /* Convert from Hz to rad/sample for computational purposes */
8689 /* Digital frequency must be used for this calculation */
8690 i28 = r5->size[0] * r5->size[1];
8691 r5->size[0] = 1;
8692 r5->size[1] = digw->size[1];
8693 emxEnsureCapacity_creal_T(r5, i28);
8694 loop_ub = digw->size[0] * digw->size[1];
8695 emxFree_real_T(&r4);
8696 for (i28 = 0; i28 < loop_ub; i28++) {
8697 r5->data[i28].re = digw->data[i28] * 0.0;
8698 r5->data[i28].im = digw->data[i28];
8699 }
8700
8701 c_exp(r5);
8702 i28 = y->size[0] * y->size[1];
8703 y->size[0] = 1;
8704 y->size[1] = r5->size[1];
8705 emxEnsureCapacity_creal_T(y, i28);
8706 b1 = (y->size[1] == 0);
8707 if (!b1) {
8708 i28 = y->size[0] * y->size[1];
8709 y->size[0] = 1;
8710 emxEnsureCapacity_creal_T(y, i28);
8711 loop_ub = y->size[1];
8712 for (i28 = 0; i28 < loop_ub; i28++) {
8713 y->data[y->size[0] * i28].re = 1.0;
8714 y->data[y->size[0] * i28].im = 0.0;
8715 }
8716 }
8717
8718 i28 = r5->size[0] * r5->size[1];
8719 r5->size[0] = 1;
8720 r5->size[1] = digw->size[1];
8721 emxEnsureCapacity_creal_T(r5, i28);
8722 loop_ub = digw->size[0] * digw->size[1];
8723 for (i28 = 0; i28 < loop_ub; i28++) {
8724 re = digw->data[i28] * 0.0;
8725 im = digw->data[i28];
8726 r5->data[i28].re = 0.0 * re;
8727 r5->data[i28].im = 0.0 * im;
8728 }
8729
8730 emxFree_real_T(&digw);
8731 c_exp(r5);
8732 b_rdivide(y, r5, hh);
8733
8734 /* Generate the default structure to pass to freqzplot */
8735 /* If rad/sample, Fs is empty */
8736 emxFree_creal_T(&r5);
8737 emxFree_creal_T(&y);
8738 }
8739
8740 /*
8741 * FREQZ_CG Frequency response of digital filter with codegen support
8742 *
8743 * This function is based on 'freqz' by The MathWorks Inc.
8744 * Arguments : const double b[15]
8745 * const emxArray_real_T *w
8746 * double Fs
8747 * emxArray_creal_T *hh
8748 * Return Type : void
8749 */
l_freqz_cg(const double b[15],const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)8750 static void l_freqz_cg(const double b[15], const emxArray_real_T *w, double Fs,
8751 emxArray_creal_T *hh)
8752 {
8753 emxArray_real_T *r6;
8754 int i30;
8755 int loop_ub;
8756 emxArray_real_T *digw;
8757 emxArray_creal_T *s;
8758 emxArray_creal_T *y;
8759 boolean_T b2;
8760 int k;
8761 double s_re;
8762 double s_im;
8763 emxInit_real_T(&r6, 2);
8764
8765 /* Cast to enforce precision rules */
8766 /* Remaining are default or for advanced use */
8767 /* Make b a row */
8768 /* -------------------------------------------------------------------------- */
8769 /* Actual Frequency Response Computation */
8770 /* if fvflag, */
8771 /* Frequency vector specified. Use Horner's method of polynomial */
8772 /* evaluation at the frequency points and divide the numerator */
8773 /* by the denominator. */
8774 /* */
8775 /* Note: we use positive i here because of the relationship */
8776 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
8777 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
8778 /* */
8779 /* Fs was specified, freq. vector is in Hz */
8780 i30 = r6->size[0] * r6->size[1];
8781 r6->size[0] = 1;
8782 r6->size[1] = w->size[1];
8783 emxEnsureCapacity_real_T(r6, i30);
8784 loop_ub = w->size[0] * w->size[1];
8785 for (i30 = 0; i30 < loop_ub; i30++) {
8786 r6->data[i30] = 6.2831853071795862 * w->data[i30];
8787 }
8788
8789 emxInit_real_T(&digw, 2);
8790 emxInit_creal_T(&s, 2);
8791 rdivide(r6, Fs, digw);
8792
8793 /* Convert from Hz to rad/sample for computational purposes */
8794 i30 = s->size[0] * s->size[1];
8795 s->size[0] = 1;
8796 s->size[1] = digw->size[1];
8797 emxEnsureCapacity_creal_T(s, i30);
8798 loop_ub = digw->size[0] * digw->size[1];
8799 emxFree_real_T(&r6);
8800 for (i30 = 0; i30 < loop_ub; i30++) {
8801 s->data[i30].re = digw->data[i30] * 0.0;
8802 s->data[i30].im = digw->data[i30];
8803 }
8804
8805 emxInit_creal_T(&y, 2);
8806 c_exp(s);
8807
8808 /* Digital frequency must be used for this calculation */
8809 i30 = y->size[0] * y->size[1];
8810 y->size[0] = 1;
8811 y->size[1] = s->size[1];
8812 emxEnsureCapacity_creal_T(y, i30);
8813 b2 = (y->size[1] == 0);
8814 if (!b2) {
8815 i30 = y->size[0] * y->size[1];
8816 y->size[0] = 1;
8817 emxEnsureCapacity_creal_T(y, i30);
8818 loop_ub = y->size[1];
8819 for (i30 = 0; i30 < loop_ub; i30++) {
8820 y->data[y->size[0] * i30].re = b[0];
8821 y->data[y->size[0] * i30].im = 0.0;
8822 }
8823
8824 for (k = 0; k < 14; k++) {
8825 i30 = y->size[0] * y->size[1];
8826 y->size[0] = 1;
8827 y->size[1] = s->size[1];
8828 emxEnsureCapacity_creal_T(y, i30);
8829 loop_ub = s->size[0] * s->size[1];
8830 for (i30 = 0; i30 < loop_ub; i30++) {
8831 s_re = s->data[i30].re * y->data[i30].re - s->data[i30].im * y->data[i30]
8832 .im;
8833 s_im = s->data[i30].re * y->data[i30].im + s->data[i30].im * y->data[i30]
8834 .re;
8835 y->data[i30].re = s_re + b[k + 1];
8836 y->data[i30].im = s_im;
8837 }
8838 }
8839 }
8840
8841 i30 = s->size[0] * s->size[1];
8842 s->size[0] = 1;
8843 s->size[1] = digw->size[1];
8844 emxEnsureCapacity_creal_T(s, i30);
8845 loop_ub = digw->size[0] * digw->size[1];
8846 for (i30 = 0; i30 < loop_ub; i30++) {
8847 s_re = digw->data[i30] * 0.0;
8848 s_im = digw->data[i30];
8849 s->data[i30].re = 14.0 * s_re;
8850 s->data[i30].im = 14.0 * s_im;
8851 }
8852
8853 emxFree_real_T(&digw);
8854 c_exp(s);
8855 b_rdivide(y, s, hh);
8856
8857 /* Generate the default structure to pass to freqzplot */
8858 /* If rad/sample, Fs is empty */
8859 emxFree_creal_T(&y);
8860 emxFree_creal_T(&s);
8861 }
8862
8863 /*
8864 * LP2LP Lowpass to lowpass analog filter transformation. Codegen support
8865 *
8866 * This function is based on 'lp2lp' by The MathWorks Inc.
8867 * Arguments : const emxArray_creal_T *a
8868 * const emxArray_real_T *b
8869 * double wo
8870 * emxArray_creal_T *at
8871 * emxArray_real_T *bt
8872 * double *dt
8873 * Return Type : void
8874 */
lp2lp_cg(const emxArray_creal_T * a,const emxArray_real_T * b,double wo,emxArray_creal_T * at,emxArray_real_T * bt,double * dt)8875 static void lp2lp_cg(const emxArray_creal_T *a, const emxArray_real_T *b, double
8876 wo, emxArray_creal_T *at, emxArray_real_T *bt, double *dt)
8877 {
8878 int i58;
8879 int loop_ub;
8880
8881 /* Transform lowpass to lowpass */
8882 i58 = at->size[0] * at->size[1];
8883 at->size[0] = a->size[0];
8884 at->size[1] = a->size[1];
8885 emxEnsureCapacity_creal_T(at, i58);
8886 loop_ub = a->size[0] * a->size[1];
8887 for (i58 = 0; i58 < loop_ub; i58++) {
8888 at->data[i58].re = wo * a->data[i58].re;
8889 at->data[i58].im = wo * a->data[i58].im;
8890 }
8891
8892 i58 = bt->size[0];
8893 bt->size[0] = b->size[0];
8894 emxEnsureCapacity_real_T1(bt, i58);
8895 loop_ub = b->size[0];
8896 for (i58 = 0; i58 < loop_ub; i58++) {
8897 bt->data[i58] = wo * b->data[i58];
8898 }
8899
8900 *dt = 0.0;
8901 }
8902
8903 /*
8904 * FREQZ_CG Frequency response of digital filter with codegen support
8905 *
8906 * This function is based on 'freqz' by The MathWorks Inc.
8907 * Arguments : const double b[7]
8908 * const emxArray_real_T *w
8909 * double Fs
8910 * emxArray_creal_T *hh
8911 * Return Type : void
8912 */
m_freqz_cg(const double b[7],const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)8913 static void m_freqz_cg(const double b[7], const emxArray_real_T *w, double Fs,
8914 emxArray_creal_T *hh)
8915 {
8916 emxArray_real_T *r7;
8917 int i31;
8918 int loop_ub;
8919 emxArray_real_T *digw;
8920 emxArray_creal_T *s;
8921 emxArray_creal_T *y;
8922 boolean_T b3;
8923 int k;
8924 double s_re;
8925 double s_im;
8926 emxInit_real_T(&r7, 2);
8927
8928 /* Cast to enforce precision rules */
8929 /* Remaining are default or for advanced use */
8930 /* Make b a row */
8931 /* -------------------------------------------------------------------------- */
8932 /* Actual Frequency Response Computation */
8933 /* if fvflag, */
8934 /* Frequency vector specified. Use Horner's method of polynomial */
8935 /* evaluation at the frequency points and divide the numerator */
8936 /* by the denominator. */
8937 /* */
8938 /* Note: we use positive i here because of the relationship */
8939 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
8940 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
8941 /* */
8942 /* Fs was specified, freq. vector is in Hz */
8943 i31 = r7->size[0] * r7->size[1];
8944 r7->size[0] = 1;
8945 r7->size[1] = w->size[1];
8946 emxEnsureCapacity_real_T(r7, i31);
8947 loop_ub = w->size[0] * w->size[1];
8948 for (i31 = 0; i31 < loop_ub; i31++) {
8949 r7->data[i31] = 6.2831853071795862 * w->data[i31];
8950 }
8951
8952 emxInit_real_T(&digw, 2);
8953 emxInit_creal_T(&s, 2);
8954 rdivide(r7, Fs, digw);
8955
8956 /* Convert from Hz to rad/sample for computational purposes */
8957 i31 = s->size[0] * s->size[1];
8958 s->size[0] = 1;
8959 s->size[1] = digw->size[1];
8960 emxEnsureCapacity_creal_T(s, i31);
8961 loop_ub = digw->size[0] * digw->size[1];
8962 emxFree_real_T(&r7);
8963 for (i31 = 0; i31 < loop_ub; i31++) {
8964 s->data[i31].re = digw->data[i31] * 0.0;
8965 s->data[i31].im = digw->data[i31];
8966 }
8967
8968 emxInit_creal_T(&y, 2);
8969 c_exp(s);
8970
8971 /* Digital frequency must be used for this calculation */
8972 i31 = y->size[0] * y->size[1];
8973 y->size[0] = 1;
8974 y->size[1] = s->size[1];
8975 emxEnsureCapacity_creal_T(y, i31);
8976 b3 = (y->size[1] == 0);
8977 if (!b3) {
8978 i31 = y->size[0] * y->size[1];
8979 y->size[0] = 1;
8980 emxEnsureCapacity_creal_T(y, i31);
8981 loop_ub = y->size[1];
8982 for (i31 = 0; i31 < loop_ub; i31++) {
8983 y->data[y->size[0] * i31].re = b[0];
8984 y->data[y->size[0] * i31].im = 0.0;
8985 }
8986
8987 for (k = 0; k < 6; k++) {
8988 i31 = y->size[0] * y->size[1];
8989 y->size[0] = 1;
8990 y->size[1] = s->size[1];
8991 emxEnsureCapacity_creal_T(y, i31);
8992 loop_ub = s->size[0] * s->size[1];
8993 for (i31 = 0; i31 < loop_ub; i31++) {
8994 s_re = s->data[i31].re * y->data[i31].re - s->data[i31].im * y->data[i31]
8995 .im;
8996 s_im = s->data[i31].re * y->data[i31].im + s->data[i31].im * y->data[i31]
8997 .re;
8998 y->data[i31].re = s_re + b[k + 1];
8999 y->data[i31].im = s_im;
9000 }
9001 }
9002 }
9003
9004 i31 = s->size[0] * s->size[1];
9005 s->size[0] = 1;
9006 s->size[1] = digw->size[1];
9007 emxEnsureCapacity_creal_T(s, i31);
9008 loop_ub = digw->size[0] * digw->size[1];
9009 for (i31 = 0; i31 < loop_ub; i31++) {
9010 s_re = digw->data[i31] * 0.0;
9011 s_im = digw->data[i31];
9012 s->data[i31].re = 6.0 * s_re;
9013 s->data[i31].im = 6.0 * s_im;
9014 }
9015
9016 emxFree_real_T(&digw);
9017 c_exp(s);
9018 b_rdivide(y, s, hh);
9019
9020 /* Generate the default structure to pass to freqzplot */
9021 /* If rad/sample, Fs is empty */
9022 emxFree_creal_T(&y);
9023 emxFree_creal_T(&s);
9024 }
9025
9026 /*
9027 * Arguments : double y
9028 * Return Type : double
9029 */
mag2db(double y)9030 static double mag2db(double y)
9031 {
9032 return 20.0 * log10(y);
9033 }
9034
9035 /*
9036 * Arguments : double a
9037 * double b
9038 * Return Type : double
9039 */
mpower(double a,double b)9040 static double mpower(double a, double b)
9041 {
9042 return rt_powd_snf(a, b);
9043 }
9044
9045 /*
9046 * FREQZ_CG Frequency response of digital filter with codegen support
9047 *
9048 * This function is based on 'freqz' by The MathWorks Inc.
9049 * Arguments : const emxArray_real_T *b
9050 * const emxArray_real_T *w
9051 * double Fs
9052 * emxArray_creal_T *hh
9053 * Return Type : void
9054 */
n_freqz_cg(const emxArray_real_T * b,const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)9055 static void n_freqz_cg(const emxArray_real_T *b, const emxArray_real_T *w,
9056 double Fs, emxArray_creal_T *hh)
9057 {
9058 emxArray_real_T *y;
9059 int b_idx_0;
9060 int i32;
9061 emxArray_real_T *r8;
9062 emxArray_real_T *digw;
9063 emxArray_creal_T *r9;
9064 emxArray_creal_T *r10;
9065 double b_y;
9066 double re;
9067 double im;
9068 emxInit_real_T(&y, 2);
9069
9070 /* Cast to enforce precision rules */
9071 /* Remaining are default or for advanced use */
9072 /* Make b a row */
9073 /* -------------------------------------------------------------------------- */
9074 b_idx_0 = b->size[1];
9075 i32 = y->size[0] * y->size[1];
9076 y->size[0] = 1;
9077 y->size[1] = b_idx_0;
9078 emxEnsureCapacity_real_T(y, i32);
9079 for (i32 = 0; i32 < b_idx_0; i32++) {
9080 y->data[y->size[0] * i32] = b->data[i32];
9081 }
9082
9083 emxInit_real_T(&r8, 2);
9084
9085 /* Actual Frequency Response Computation */
9086 /* if fvflag, */
9087 /* Frequency vector specified. Use Horner's method of polynomial */
9088 /* evaluation at the frequency points and divide the numerator */
9089 /* by the denominator. */
9090 /* */
9091 /* Note: we use positive i here because of the relationship */
9092 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
9093 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
9094 /* */
9095 /* Fs was specified, freq. vector is in Hz */
9096 i32 = r8->size[0] * r8->size[1];
9097 r8->size[0] = 1;
9098 r8->size[1] = w->size[1];
9099 emxEnsureCapacity_real_T(r8, i32);
9100 b_idx_0 = w->size[0] * w->size[1];
9101 for (i32 = 0; i32 < b_idx_0; i32++) {
9102 r8->data[i32] = 6.2831853071795862 * w->data[i32];
9103 }
9104
9105 emxInit_real_T(&digw, 2);
9106 emxInit_creal_T(&r9, 2);
9107 rdivide(r8, Fs, digw);
9108
9109 /* Convert from Hz to rad/sample for computational purposes */
9110 /* Digital frequency must be used for this calculation */
9111 i32 = r9->size[0] * r9->size[1];
9112 r9->size[0] = 1;
9113 r9->size[1] = digw->size[1];
9114 emxEnsureCapacity_creal_T(r9, i32);
9115 b_idx_0 = digw->size[0] * digw->size[1];
9116 emxFree_real_T(&r8);
9117 for (i32 = 0; i32 < b_idx_0; i32++) {
9118 r9->data[i32].re = digw->data[i32] * 0.0;
9119 r9->data[i32].im = digw->data[i32];
9120 }
9121
9122 emxInit_creal_T(&r10, 2);
9123 c_exp(r9);
9124 j_polyval(y, r9, r10);
9125 i32 = r9->size[0] * r9->size[1];
9126 r9->size[0] = 1;
9127 r9->size[1] = digw->size[1];
9128 emxEnsureCapacity_creal_T(r9, i32);
9129 b_y = (double)y->size[1] - 1.0;
9130 b_idx_0 = digw->size[0] * digw->size[1];
9131 emxFree_real_T(&y);
9132 for (i32 = 0; i32 < b_idx_0; i32++) {
9133 re = digw->data[i32] * 0.0;
9134 im = digw->data[i32];
9135 r9->data[i32].re = b_y * re;
9136 r9->data[i32].im = b_y * im;
9137 }
9138
9139 emxFree_real_T(&digw);
9140 c_exp(r9);
9141 b_rdivide(r10, r9, hh);
9142
9143 /* Generate the default structure to pass to freqzplot */
9144 /* If rad/sample, Fs is empty */
9145 emxFree_creal_T(&r10);
9146 emxFree_creal_T(&r9);
9147 }
9148
9149 /*
9150 * FREQZ_CG Frequency response of digital filter with codegen support
9151 *
9152 * This function is based on 'freqz' by The MathWorks Inc.
9153 * Arguments : const double b[29]
9154 * const emxArray_real_T *w
9155 * double Fs
9156 * emxArray_creal_T *hh
9157 * Return Type : void
9158 */
o_freqz_cg(const double b[29],const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)9159 static void o_freqz_cg(const double b[29], const emxArray_real_T *w, double Fs,
9160 emxArray_creal_T *hh)
9161 {
9162 emxArray_real_T *r11;
9163 int i34;
9164 int loop_ub;
9165 emxArray_real_T *digw;
9166 emxArray_creal_T *s;
9167 emxArray_creal_T *y;
9168 boolean_T b5;
9169 int k;
9170 double s_re;
9171 double s_im;
9172 emxInit_real_T(&r11, 2);
9173
9174 /* Cast to enforce precision rules */
9175 /* Remaining are default or for advanced use */
9176 /* Make b a row */
9177 /* -------------------------------------------------------------------------- */
9178 /* Actual Frequency Response Computation */
9179 /* if fvflag, */
9180 /* Frequency vector specified. Use Horner's method of polynomial */
9181 /* evaluation at the frequency points and divide the numerator */
9182 /* by the denominator. */
9183 /* */
9184 /* Note: we use positive i here because of the relationship */
9185 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
9186 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
9187 /* */
9188 /* Fs was specified, freq. vector is in Hz */
9189 i34 = r11->size[0] * r11->size[1];
9190 r11->size[0] = 1;
9191 r11->size[1] = w->size[1];
9192 emxEnsureCapacity_real_T(r11, i34);
9193 loop_ub = w->size[0] * w->size[1];
9194 for (i34 = 0; i34 < loop_ub; i34++) {
9195 r11->data[i34] = 6.2831853071795862 * w->data[i34];
9196 }
9197
9198 emxInit_real_T(&digw, 2);
9199 emxInit_creal_T(&s, 2);
9200 rdivide(r11, Fs, digw);
9201
9202 /* Convert from Hz to rad/sample for computational purposes */
9203 i34 = s->size[0] * s->size[1];
9204 s->size[0] = 1;
9205 s->size[1] = digw->size[1];
9206 emxEnsureCapacity_creal_T(s, i34);
9207 loop_ub = digw->size[0] * digw->size[1];
9208 emxFree_real_T(&r11);
9209 for (i34 = 0; i34 < loop_ub; i34++) {
9210 s->data[i34].re = digw->data[i34] * 0.0;
9211 s->data[i34].im = digw->data[i34];
9212 }
9213
9214 emxInit_creal_T(&y, 2);
9215 c_exp(s);
9216
9217 /* Digital frequency must be used for this calculation */
9218 i34 = y->size[0] * y->size[1];
9219 y->size[0] = 1;
9220 y->size[1] = s->size[1];
9221 emxEnsureCapacity_creal_T(y, i34);
9222 b5 = (y->size[1] == 0);
9223 if (!b5) {
9224 i34 = y->size[0] * y->size[1];
9225 y->size[0] = 1;
9226 emxEnsureCapacity_creal_T(y, i34);
9227 loop_ub = y->size[1];
9228 for (i34 = 0; i34 < loop_ub; i34++) {
9229 y->data[y->size[0] * i34].re = b[0];
9230 y->data[y->size[0] * i34].im = 0.0;
9231 }
9232
9233 for (k = 0; k < 28; k++) {
9234 i34 = y->size[0] * y->size[1];
9235 y->size[0] = 1;
9236 y->size[1] = s->size[1];
9237 emxEnsureCapacity_creal_T(y, i34);
9238 loop_ub = s->size[0] * s->size[1];
9239 for (i34 = 0; i34 < loop_ub; i34++) {
9240 s_re = s->data[i34].re * y->data[i34].re - s->data[i34].im * y->data[i34]
9241 .im;
9242 s_im = s->data[i34].re * y->data[i34].im + s->data[i34].im * y->data[i34]
9243 .re;
9244 y->data[i34].re = s_re + b[k + 1];
9245 y->data[i34].im = s_im;
9246 }
9247 }
9248 }
9249
9250 i34 = s->size[0] * s->size[1];
9251 s->size[0] = 1;
9252 s->size[1] = digw->size[1];
9253 emxEnsureCapacity_creal_T(s, i34);
9254 loop_ub = digw->size[0] * digw->size[1];
9255 for (i34 = 0; i34 < loop_ub; i34++) {
9256 s_re = digw->data[i34] * 0.0;
9257 s_im = digw->data[i34];
9258 s->data[i34].re = 28.0 * s_re;
9259 s->data[i34].im = 28.0 * s_im;
9260 }
9261
9262 emxFree_real_T(&digw);
9263 c_exp(s);
9264 b_rdivide(y, s, hh);
9265
9266 /* Generate the default structure to pass to freqzplot */
9267 /* If rad/sample, Fs is empty */
9268 emxFree_creal_T(&y);
9269 emxFree_creal_T(&s);
9270 }
9271
9272 /*
9273 * FREQZ_CG Frequency response of digital filter with codegen support
9274 *
9275 * This function is based on 'freqz' by The MathWorks Inc.
9276 * Arguments : const double b[13]
9277 * const emxArray_real_T *w
9278 * double Fs
9279 * emxArray_creal_T *hh
9280 * Return Type : void
9281 */
p_freqz_cg(const double b[13],const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)9282 static void p_freqz_cg(const double b[13], const emxArray_real_T *w, double Fs,
9283 emxArray_creal_T *hh)
9284 {
9285 emxArray_real_T *r12;
9286 int i35;
9287 int loop_ub;
9288 emxArray_real_T *digw;
9289 emxArray_creal_T *s;
9290 emxArray_creal_T *y;
9291 boolean_T b6;
9292 int k;
9293 double s_re;
9294 double s_im;
9295 emxInit_real_T(&r12, 2);
9296
9297 /* Cast to enforce precision rules */
9298 /* Remaining are default or for advanced use */
9299 /* Make b a row */
9300 /* -------------------------------------------------------------------------- */
9301 /* Actual Frequency Response Computation */
9302 /* if fvflag, */
9303 /* Frequency vector specified. Use Horner's method of polynomial */
9304 /* evaluation at the frequency points and divide the numerator */
9305 /* by the denominator. */
9306 /* */
9307 /* Note: we use positive i here because of the relationship */
9308 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
9309 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
9310 /* */
9311 /* Fs was specified, freq. vector is in Hz */
9312 i35 = r12->size[0] * r12->size[1];
9313 r12->size[0] = 1;
9314 r12->size[1] = w->size[1];
9315 emxEnsureCapacity_real_T(r12, i35);
9316 loop_ub = w->size[0] * w->size[1];
9317 for (i35 = 0; i35 < loop_ub; i35++) {
9318 r12->data[i35] = 6.2831853071795862 * w->data[i35];
9319 }
9320
9321 emxInit_real_T(&digw, 2);
9322 emxInit_creal_T(&s, 2);
9323 rdivide(r12, Fs, digw);
9324
9325 /* Convert from Hz to rad/sample for computational purposes */
9326 i35 = s->size[0] * s->size[1];
9327 s->size[0] = 1;
9328 s->size[1] = digw->size[1];
9329 emxEnsureCapacity_creal_T(s, i35);
9330 loop_ub = digw->size[0] * digw->size[1];
9331 emxFree_real_T(&r12);
9332 for (i35 = 0; i35 < loop_ub; i35++) {
9333 s->data[i35].re = digw->data[i35] * 0.0;
9334 s->data[i35].im = digw->data[i35];
9335 }
9336
9337 emxInit_creal_T(&y, 2);
9338 c_exp(s);
9339
9340 /* Digital frequency must be used for this calculation */
9341 i35 = y->size[0] * y->size[1];
9342 y->size[0] = 1;
9343 y->size[1] = s->size[1];
9344 emxEnsureCapacity_creal_T(y, i35);
9345 b6 = (y->size[1] == 0);
9346 if (!b6) {
9347 i35 = y->size[0] * y->size[1];
9348 y->size[0] = 1;
9349 emxEnsureCapacity_creal_T(y, i35);
9350 loop_ub = y->size[1];
9351 for (i35 = 0; i35 < loop_ub; i35++) {
9352 y->data[y->size[0] * i35].re = b[0];
9353 y->data[y->size[0] * i35].im = 0.0;
9354 }
9355
9356 for (k = 0; k < 12; k++) {
9357 i35 = y->size[0] * y->size[1];
9358 y->size[0] = 1;
9359 y->size[1] = s->size[1];
9360 emxEnsureCapacity_creal_T(y, i35);
9361 loop_ub = s->size[0] * s->size[1];
9362 for (i35 = 0; i35 < loop_ub; i35++) {
9363 s_re = s->data[i35].re * y->data[i35].re - s->data[i35].im * y->data[i35]
9364 .im;
9365 s_im = s->data[i35].re * y->data[i35].im + s->data[i35].im * y->data[i35]
9366 .re;
9367 y->data[i35].re = s_re + b[k + 1];
9368 y->data[i35].im = s_im;
9369 }
9370 }
9371 }
9372
9373 i35 = s->size[0] * s->size[1];
9374 s->size[0] = 1;
9375 s->size[1] = digw->size[1];
9376 emxEnsureCapacity_creal_T(s, i35);
9377 loop_ub = digw->size[0] * digw->size[1];
9378 for (i35 = 0; i35 < loop_ub; i35++) {
9379 s_re = digw->data[i35] * 0.0;
9380 s_im = digw->data[i35];
9381 s->data[i35].re = 12.0 * s_re;
9382 s->data[i35].im = 12.0 * s_im;
9383 }
9384
9385 emxFree_real_T(&digw);
9386 c_exp(s);
9387 b_rdivide(y, s, hh);
9388
9389 /* Generate the default structure to pass to freqzplot */
9390 /* If rad/sample, Fs is empty */
9391 emxFree_creal_T(&y);
9392 emxFree_creal_T(&s);
9393 }
9394
9395 /*
9396 * Arguments : const emxArray_creal_T *x
9397 * emxArray_creal_T *c
9398 * Return Type : void
9399 */
poly(const emxArray_creal_T * x,emxArray_creal_T * c)9400 static void poly(const emxArray_creal_T *x, emxArray_creal_T *c)
9401 {
9402 emxArray_creal_T *r2;
9403 emxInit_creal_T1(&r2, 1);
9404 eig(x, r2);
9405 vector_poly(r2, c);
9406 emxFree_creal_T(&r2);
9407 }
9408
9409 /*
9410 * Arguments : const double p[15]
9411 * const creal_T x[2048]
9412 * creal_T y[2048]
9413 * Return Type : void
9414 */
polyval(const double p[15],const creal_T x[2048],creal_T y[2048])9415 static void polyval(const double p[15], const creal_T x[2048], creal_T y[2048])
9416 {
9417 int i9;
9418 int k;
9419 double x_im;
9420 for (i9 = 0; i9 < 2048; i9++) {
9421 y[i9].re = p[0];
9422 y[i9].im = 0.0;
9423 }
9424
9425 for (k = 0; k < 14; k++) {
9426 for (i9 = 0; i9 < 2048; i9++) {
9427 x_im = x[i9].re * y[i9].im + x[i9].im * y[i9].re;
9428 y[i9].re = (x[i9].re * y[i9].re - x[i9].im * y[i9].im) + p[k + 1];
9429 y[i9].im = x_im;
9430 }
9431 }
9432 }
9433
9434 /*
9435 * Arguments : const double a[2048]
9436 * double y[2048]
9437 * Return Type : void
9438 */
power(const double a[2048],double y[2048])9439 static void power(const double a[2048], double y[2048])
9440 {
9441 int k;
9442 for (k = 0; k < 2048; k++) {
9443 y[k] = rt_powd_snf(a[k], 3.0);
9444 }
9445 }
9446
9447 /*
9448 * FREQZ_CG Frequency response of digital filter with codegen support
9449 *
9450 * This function is based on 'freqz' by The MathWorks Inc.
9451 * Arguments : const double b[57]
9452 * const emxArray_real_T *w
9453 * double Fs
9454 * emxArray_creal_T *hh
9455 * Return Type : void
9456 */
q_freqz_cg(const double b[57],const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)9457 static void q_freqz_cg(const double b[57], const emxArray_real_T *w, double Fs,
9458 emxArray_creal_T *hh)
9459 {
9460 emxArray_real_T *r13;
9461 int i36;
9462 int loop_ub;
9463 emxArray_real_T *digw;
9464 emxArray_creal_T *s;
9465 emxArray_creal_T *y;
9466 boolean_T b7;
9467 int k;
9468 double s_re;
9469 double s_im;
9470 emxInit_real_T(&r13, 2);
9471
9472 /* Cast to enforce precision rules */
9473 /* Remaining are default or for advanced use */
9474 /* Make b a row */
9475 /* -------------------------------------------------------------------------- */
9476 /* Actual Frequency Response Computation */
9477 /* if fvflag, */
9478 /* Frequency vector specified. Use Horner's method of polynomial */
9479 /* evaluation at the frequency points and divide the numerator */
9480 /* by the denominator. */
9481 /* */
9482 /* Note: we use positive i here because of the relationship */
9483 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
9484 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
9485 /* */
9486 /* Fs was specified, freq. vector is in Hz */
9487 i36 = r13->size[0] * r13->size[1];
9488 r13->size[0] = 1;
9489 r13->size[1] = w->size[1];
9490 emxEnsureCapacity_real_T(r13, i36);
9491 loop_ub = w->size[0] * w->size[1];
9492 for (i36 = 0; i36 < loop_ub; i36++) {
9493 r13->data[i36] = 6.2831853071795862 * w->data[i36];
9494 }
9495
9496 emxInit_real_T(&digw, 2);
9497 emxInit_creal_T(&s, 2);
9498 rdivide(r13, Fs, digw);
9499
9500 /* Convert from Hz to rad/sample for computational purposes */
9501 i36 = s->size[0] * s->size[1];
9502 s->size[0] = 1;
9503 s->size[1] = digw->size[1];
9504 emxEnsureCapacity_creal_T(s, i36);
9505 loop_ub = digw->size[0] * digw->size[1];
9506 emxFree_real_T(&r13);
9507 for (i36 = 0; i36 < loop_ub; i36++) {
9508 s->data[i36].re = digw->data[i36] * 0.0;
9509 s->data[i36].im = digw->data[i36];
9510 }
9511
9512 emxInit_creal_T(&y, 2);
9513 c_exp(s);
9514
9515 /* Digital frequency must be used for this calculation */
9516 i36 = y->size[0] * y->size[1];
9517 y->size[0] = 1;
9518 y->size[1] = s->size[1];
9519 emxEnsureCapacity_creal_T(y, i36);
9520 b7 = (y->size[1] == 0);
9521 if (!b7) {
9522 i36 = y->size[0] * y->size[1];
9523 y->size[0] = 1;
9524 emxEnsureCapacity_creal_T(y, i36);
9525 loop_ub = y->size[1];
9526 for (i36 = 0; i36 < loop_ub; i36++) {
9527 y->data[y->size[0] * i36].re = b[0];
9528 y->data[y->size[0] * i36].im = 0.0;
9529 }
9530
9531 for (k = 0; k < 56; k++) {
9532 i36 = y->size[0] * y->size[1];
9533 y->size[0] = 1;
9534 y->size[1] = s->size[1];
9535 emxEnsureCapacity_creal_T(y, i36);
9536 loop_ub = s->size[0] * s->size[1];
9537 for (i36 = 0; i36 < loop_ub; i36++) {
9538 s_re = s->data[i36].re * y->data[i36].re - s->data[i36].im * y->data[i36]
9539 .im;
9540 s_im = s->data[i36].re * y->data[i36].im + s->data[i36].im * y->data[i36]
9541 .re;
9542 y->data[i36].re = s_re + b[k + 1];
9543 y->data[i36].im = s_im;
9544 }
9545 }
9546 }
9547
9548 i36 = s->size[0] * s->size[1];
9549 s->size[0] = 1;
9550 s->size[1] = digw->size[1];
9551 emxEnsureCapacity_creal_T(s, i36);
9552 loop_ub = digw->size[0] * digw->size[1];
9553 for (i36 = 0; i36 < loop_ub; i36++) {
9554 s_re = digw->data[i36] * 0.0;
9555 s_im = digw->data[i36];
9556 s->data[i36].re = 56.0 * s_re;
9557 s->data[i36].im = 56.0 * s_im;
9558 }
9559
9560 emxFree_real_T(&digw);
9561 c_exp(s);
9562 b_rdivide(y, s, hh);
9563
9564 /* Generate the default structure to pass to freqzplot */
9565 /* If rad/sample, Fs is empty */
9566 emxFree_creal_T(&y);
9567 emxFree_creal_T(&s);
9568 }
9569
9570 /*
9571 * FREQZ_CG Frequency response of digital filter with codegen support
9572 *
9573 * This function is based on 'freqz' by The MathWorks Inc.
9574 * Arguments : const double b[43]
9575 * const emxArray_real_T *w
9576 * double Fs
9577 * emxArray_creal_T *hh
9578 * Return Type : void
9579 */
r_freqz_cg(const double b[43],const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)9580 static void r_freqz_cg(const double b[43], const emxArray_real_T *w, double Fs,
9581 emxArray_creal_T *hh)
9582 {
9583 emxArray_real_T *r14;
9584 int i37;
9585 int loop_ub;
9586 emxArray_real_T *digw;
9587 emxArray_creal_T *s;
9588 emxArray_creal_T *y;
9589 boolean_T b8;
9590 int k;
9591 double s_re;
9592 double s_im;
9593 emxInit_real_T(&r14, 2);
9594
9595 /* Cast to enforce precision rules */
9596 /* Remaining are default or for advanced use */
9597 /* Make b a row */
9598 /* -------------------------------------------------------------------------- */
9599 /* Actual Frequency Response Computation */
9600 /* if fvflag, */
9601 /* Frequency vector specified. Use Horner's method of polynomial */
9602 /* evaluation at the frequency points and divide the numerator */
9603 /* by the denominator. */
9604 /* */
9605 /* Note: we use positive i here because of the relationship */
9606 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
9607 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
9608 /* */
9609 /* Fs was specified, freq. vector is in Hz */
9610 i37 = r14->size[0] * r14->size[1];
9611 r14->size[0] = 1;
9612 r14->size[1] = w->size[1];
9613 emxEnsureCapacity_real_T(r14, i37);
9614 loop_ub = w->size[0] * w->size[1];
9615 for (i37 = 0; i37 < loop_ub; i37++) {
9616 r14->data[i37] = 6.2831853071795862 * w->data[i37];
9617 }
9618
9619 emxInit_real_T(&digw, 2);
9620 emxInit_creal_T(&s, 2);
9621 rdivide(r14, Fs, digw);
9622
9623 /* Convert from Hz to rad/sample for computational purposes */
9624 i37 = s->size[0] * s->size[1];
9625 s->size[0] = 1;
9626 s->size[1] = digw->size[1];
9627 emxEnsureCapacity_creal_T(s, i37);
9628 loop_ub = digw->size[0] * digw->size[1];
9629 emxFree_real_T(&r14);
9630 for (i37 = 0; i37 < loop_ub; i37++) {
9631 s->data[i37].re = digw->data[i37] * 0.0;
9632 s->data[i37].im = digw->data[i37];
9633 }
9634
9635 emxInit_creal_T(&y, 2);
9636 c_exp(s);
9637
9638 /* Digital frequency must be used for this calculation */
9639 i37 = y->size[0] * y->size[1];
9640 y->size[0] = 1;
9641 y->size[1] = s->size[1];
9642 emxEnsureCapacity_creal_T(y, i37);
9643 b8 = (y->size[1] == 0);
9644 if (!b8) {
9645 i37 = y->size[0] * y->size[1];
9646 y->size[0] = 1;
9647 emxEnsureCapacity_creal_T(y, i37);
9648 loop_ub = y->size[1];
9649 for (i37 = 0; i37 < loop_ub; i37++) {
9650 y->data[y->size[0] * i37].re = b[0];
9651 y->data[y->size[0] * i37].im = 0.0;
9652 }
9653
9654 for (k = 0; k < 42; k++) {
9655 i37 = y->size[0] * y->size[1];
9656 y->size[0] = 1;
9657 y->size[1] = s->size[1];
9658 emxEnsureCapacity_creal_T(y, i37);
9659 loop_ub = s->size[0] * s->size[1];
9660 for (i37 = 0; i37 < loop_ub; i37++) {
9661 s_re = s->data[i37].re * y->data[i37].re - s->data[i37].im * y->data[i37]
9662 .im;
9663 s_im = s->data[i37].re * y->data[i37].im + s->data[i37].im * y->data[i37]
9664 .re;
9665 y->data[i37].re = s_re + b[k + 1];
9666 y->data[i37].im = s_im;
9667 }
9668 }
9669 }
9670
9671 i37 = s->size[0] * s->size[1];
9672 s->size[0] = 1;
9673 s->size[1] = digw->size[1];
9674 emxEnsureCapacity_creal_T(s, i37);
9675 loop_ub = digw->size[0] * digw->size[1];
9676 for (i37 = 0; i37 < loop_ub; i37++) {
9677 s_re = digw->data[i37] * 0.0;
9678 s_im = digw->data[i37];
9679 s->data[i37].re = 42.0 * s_re;
9680 s->data[i37].im = 42.0 * s_im;
9681 }
9682
9683 emxFree_real_T(&digw);
9684 c_exp(s);
9685 b_rdivide(y, s, hh);
9686
9687 /* Generate the default structure to pass to freqzplot */
9688 /* If rad/sample, Fs is empty */
9689 emxFree_creal_T(&y);
9690 emxFree_creal_T(&s);
9691 }
9692
9693 /*
9694 * Arguments : const emxArray_real_T *x
9695 * double y
9696 * emxArray_real_T *z
9697 * Return Type : void
9698 */
rdivide(const emxArray_real_T * x,double y,emxArray_real_T * z)9699 static void rdivide(const emxArray_real_T *x, double y, emxArray_real_T *z)
9700 {
9701 int i27;
9702 int loop_ub;
9703 i27 = z->size[0] * z->size[1];
9704 z->size[0] = 1;
9705 z->size[1] = x->size[1];
9706 emxEnsureCapacity_real_T(z, i27);
9707 loop_ub = x->size[0] * x->size[1];
9708 for (i27 = 0; i27 < loop_ub; i27++) {
9709 z->data[i27] = x->data[i27] / y;
9710 }
9711 }
9712
9713 /*
9714 * Arguments : const creal_T y
9715 * Return Type : creal_T
9716 */
recip(const creal_T y)9717 static creal_T recip(const creal_T y)
9718 {
9719 creal_T z;
9720 double brm;
9721 double bim;
9722 double d;
9723 brm = fabs(y.re);
9724 bim = fabs(y.im);
9725 if (y.im == 0.0) {
9726 z.re = 1.0 / y.re;
9727 z.im = 0.0;
9728 } else if (y.re == 0.0) {
9729 z.re = 0.0;
9730 z.im = -1.0 / y.im;
9731 } else if (brm > bim) {
9732 bim = y.im / y.re;
9733 d = y.re + bim * y.im;
9734 z.re = 1.0 / d;
9735 z.im = -bim / d;
9736 } else if (brm == bim) {
9737 bim = 0.5;
9738 if (y.re < 0.0) {
9739 bim = -0.5;
9740 }
9741
9742 d = 0.5;
9743 if (y.im < 0.0) {
9744 d = -0.5;
9745 }
9746
9747 z.re = bim / brm;
9748 z.im = -d / brm;
9749 } else {
9750 bim = y.re / y.im;
9751 d = y.im + bim * y.re;
9752 z.re = bim / d;
9753 z.im = -1.0 / d;
9754 }
9755
9756 return z;
9757 }
9758
9759 /*
9760 * REMEZDD Lagrange interpolation coefficients.
9761 * Arguments : double k
9762 * double n
9763 * double m
9764 * const emxArray_real_T *x
9765 * Return Type : double
9766 */
remezdd(double k,double n,double m,const emxArray_real_T * x)9767 static double remezdd(double k, double n, double m, const emxArray_real_T *x)
9768 {
9769 double y;
9770 int l;
9771 emxArray_real_T *xx;
9772 emxArray_int32_T *r23;
9773 int i51;
9774 int i;
9775 int end;
9776 double b_x;
9777 int vlen;
9778
9779 /* */
9780 /* Author: T. Krauss 1993 */
9781 /* Was Revision: 1.4, Date: 1994/01/25 17:59:44 */
9782 y = 1.0;
9783 l = 0;
9784 emxInit_real_T(&xx, 2);
9785 emxInit_int32_T(&r23, 2);
9786 while (l <= (int)m - 1) {
9787 if ((m == 0.0) || (((m > 0.0) && (1.0 + (double)l > n)) || ((0.0 > m) && (n >
9788 1.0 + (double)l)))) {
9789 i51 = 1;
9790 i = 1;
9791 end = 0;
9792 } else {
9793 i51 = l + 1;
9794 i = (int)m;
9795 end = (int)n;
9796 }
9797
9798 b_x = x->data[(int)k - 1];
9799 vlen = xx->size[0] * xx->size[1];
9800 xx->size[0] = 1;
9801 xx->size[1] = div_s32_floor(end - i51, i) + 1;
9802 emxEnsureCapacity_real_T(xx, vlen);
9803 vlen = div_s32_floor(end - i51, i);
9804 for (end = 0; end <= vlen; end++) {
9805 xx->data[xx->size[0] * end] = 2.0 * (b_x - x->data[(i51 + i * end) - 1]);
9806 }
9807
9808 end = xx->size[1] - 1;
9809 vlen = 0;
9810 for (i = 0; i <= end; i++) {
9811 if (xx->data[i] != 0.0) {
9812 vlen++;
9813 }
9814 }
9815
9816 i51 = r23->size[0] * r23->size[1];
9817 r23->size[0] = 1;
9818 r23->size[1] = vlen;
9819 emxEnsureCapacity_int32_T(r23, i51);
9820 vlen = 0;
9821 for (i = 0; i <= end; i++) {
9822 if (xx->data[i] != 0.0) {
9823 r23->data[vlen] = i + 1;
9824 vlen++;
9825 }
9826 }
9827
9828 vlen = r23->size[1];
9829 if (r23->size[1] == 0) {
9830 b_x = 1.0;
9831 } else {
9832 b_x = xx->data[r23->data[0] - 1];
9833 for (i = 2; i <= vlen; i++) {
9834 b_x *= xx->data[r23->data[r23->size[0] * (i - 1)] - 1];
9835 }
9836 }
9837
9838 y *= b_x;
9839 l++;
9840 }
9841
9842 emxFree_int32_T(&r23);
9843 emxFree_real_T(&xx);
9844 y = 1.0 / y;
9845
9846 /* EOF */
9847 return y;
9848 }
9849
9850 /*
9851 * remezm function
9852 * Inputs
9853 * nfilt - filter length
9854 * edge - vector of band edges (between 0 and .5)
9855 * grid - frequency grid (between 0 and .5)
9856 * des - desired function on frequency grid
9857 * wt - weight function on frequency grid
9858 * neg == 1 ==> antisymmetric imp resp,
9859 * == 0 ==> symmetric imp resp
9860 * Outputs
9861 * h - coefficients of basis functions
9862 * dev - computed error
9863 * iext - indices of extremal frequencies
9864 * Arguments : double nfilt
9865 * const double edge[4]
9866 * const emxArray_real_T *grid
9867 * emxArray_real_T *des
9868 * emxArray_real_T *wt
9869 * emxArray_real_T *h
9870 * double *dev
9871 * boolean_T *valid
9872 * Return Type : void
9873 */
remezm(double nfilt,const double edge[4],const emxArray_real_T * grid,emxArray_real_T * des,emxArray_real_T * wt,emxArray_real_T * h,double * dev,boolean_T * valid)9874 static void remezm(double nfilt, const double edge[4], const emxArray_real_T
9875 *grid, emxArray_real_T *des, emxArray_real_T *wt,
9876 emxArray_real_T *h, double *dev, boolean_T *valid)
9877 {
9878 boolean_T b_valid;
9879 double nodd;
9880 double nfcns;
9881 int varargin_2;
9882 int ngrid;
9883 emxArray_real_T *j;
9884 emxArray_real_T *x2;
9885 int i44;
9886 double temp;
9887 int loop_ub;
9888 emxArray_real_T *iext;
9889 emxArray_real_T *y;
9890 emxArray_real_T *x;
9891 int ixstart;
9892 int flag;
9893 double comp;
9894 double dtemp;
9895 double b_y1;
9896 int luck;
9897 int nut1;
9898 double err;
9899 double b_dev;
9900 double devl;
9901 emxArray_real_T *ad;
9902 int niter;
9903 double jchnge;
9904 double d1;
9905 emxArray_real_T *l;
9906 emxArray_int32_T *r21;
9907 emxArray_real_T *b;
9908 emxArray_real_T *a;
9909 emxArray_real_T *b_wt;
9910 emxArray_int8_T *r22;
9911 emxArray_int32_T *b_iext;
9912 emxArray_real_T *mtmp;
9913 boolean_T guard1 = false;
9914 int exitg1;
9915 double k1;
9916 int nut;
9917 double b_j;
9918 double b_l;
9919 int nu;
9920 double varargin_1[2];
9921 double dnum;
9922 double kup;
9923 int i45;
9924 int i46;
9925 int i47;
9926 int i48;
9927 int i49;
9928 boolean_T guard2 = false;
9929 double b_y;
9930 int flag34;
9931 int exitg3;
9932 boolean_T exitg2;
9933 boolean_T exitg4;
9934
9935 /* */
9936 b_valid = true;
9937 nodd = rt_remd_snf(nfilt, 2.0);
9938
9939 /* nodd == 1 ==> filter length is odd */
9940 /* nodd == 0 ==> filter length is even */
9941 nfcns = nfilt / 2.0;
9942 b_fix(&nfcns);
9943 if (nodd == 1.0) {
9944 nfcns++;
9945 }
9946
9947 varargin_2 = grid->size[1];
9948 ngrid = grid->size[1];
9949 emxInit_real_T(&j, 2);
9950 emxInit_real_T(&x2, 2);
9951 if (nodd != 1.0) {
9952 i44 = x2->size[0] * x2->size[1];
9953 x2->size[0] = 1;
9954 x2->size[1] = grid->size[1];
9955 emxEnsureCapacity_real_T(x2, i44);
9956 loop_ub = grid->size[0] * grid->size[1];
9957 for (i44 = 0; i44 < loop_ub; i44++) {
9958 x2->data[i44] = 3.1415926535897931 * grid->data[i44];
9959 }
9960
9961 b_cos(x2);
9962 c_rdivide(des, x2, j);
9963 i44 = des->size[0] * des->size[1];
9964 des->size[0] = 1;
9965 des->size[1] = j->size[1];
9966 emxEnsureCapacity_real_T(des, i44);
9967 loop_ub = j->size[0] * j->size[1];
9968 for (i44 = 0; i44 < loop_ub; i44++) {
9969 des->data[i44] = j->data[i44];
9970 }
9971
9972 i44 = x2->size[0] * x2->size[1];
9973 x2->size[0] = 1;
9974 x2->size[1] = grid->size[1];
9975 emxEnsureCapacity_real_T(x2, i44);
9976 loop_ub = grid->size[0] * grid->size[1];
9977 for (i44 = 0; i44 < loop_ub; i44++) {
9978 x2->data[i44] = 3.1415926535897931 * grid->data[i44];
9979 }
9980
9981 b_cos(x2);
9982 i44 = wt->size[0] * wt->size[1];
9983 wt->size[0] = 1;
9984 emxEnsureCapacity_real_T(wt, i44);
9985 ixstart = wt->size[0];
9986 flag = wt->size[1];
9987 loop_ub = ixstart * flag;
9988 for (i44 = 0; i44 < loop_ub; i44++) {
9989 wt->data[i44] *= x2->data[i44];
9990 }
9991 }
9992
9993 temp = ((double)grid->size[1] - 1.0) / nfcns;
9994 if (rtIsNaN(nfcns)) {
9995 i44 = j->size[0] * j->size[1];
9996 j->size[0] = 1;
9997 j->size[1] = 1;
9998 emxEnsureCapacity_real_T(j, i44);
9999 j->data[0] = rtNaN;
10000 } else if (nfcns < 1.0) {
10001 i44 = j->size[0] * j->size[1];
10002 j->size[0] = 1;
10003 j->size[1] = 0;
10004 emxEnsureCapacity_real_T(j, i44);
10005 } else if (rtIsInf(nfcns) && (1.0 == nfcns)) {
10006 i44 = j->size[0] * j->size[1];
10007 j->size[0] = 1;
10008 j->size[1] = 1;
10009 emxEnsureCapacity_real_T(j, i44);
10010 j->data[0] = rtNaN;
10011 } else {
10012 i44 = j->size[0] * j->size[1];
10013 j->size[0] = 1;
10014 j->size[1] = (int)floor(nfcns - 1.0) + 1;
10015 emxEnsureCapacity_real_T(j, i44);
10016 loop_ub = (int)floor(nfcns - 1.0);
10017 for (i44 = 0; i44 <= loop_ub; i44++) {
10018 j->data[j->size[0] * i44] = 1.0 + (double)i44;
10019 }
10020 }
10021
10022 i44 = x2->size[0] * x2->size[1];
10023 x2->size[0] = 1;
10024 x2->size[1] = j->size[1] + 1;
10025 emxEnsureCapacity_real_T(x2, i44);
10026 loop_ub = j->size[1];
10027 for (i44 = 0; i44 < loop_ub; i44++) {
10028 x2->data[x2->size[0] * i44] = temp * (j->data[j->size[0] * i44] - 1.0) + 1.0;
10029 }
10030
10031 emxInit_real_T1(&iext, 1);
10032 x2->data[x2->size[0] * j->size[1]] = grid->size[1];
10033 c_fix(x2);
10034 i44 = iext->size[0];
10035 iext->size[0] = x2->size[1] + 1;
10036 emxEnsureCapacity_real_T1(iext, i44);
10037 loop_ub = x2->size[1];
10038 for (i44 = 0; i44 < loop_ub; i44++) {
10039 iext->data[i44] = x2->data[x2->size[0] * i44];
10040 }
10041
10042 emxInit_real_T(&y, 2);
10043 emxInit_real_T(&x, 2);
10044 iext->data[x2->size[1]] = 0.0;
10045
10046 /* Remez exchange loop */
10047 comp = -1.0;
10048 dtemp = -1.0;
10049 b_y1 = -1.0;
10050 luck = -1;
10051 nut1 = -1;
10052 err = -1.0;
10053 i44 = y->size[0] * y->size[1];
10054 y->size[0] = 1;
10055 y->size[1] = 1;
10056 emxEnsureCapacity_real_T(y, i44);
10057 y->data[0] = -1.0;
10058 b_dev = -1.0;
10059 devl = -1.0;
10060 i44 = x->size[0] * x->size[1];
10061 x->size[0] = 1;
10062 x->size[1] = (int)(nfcns + 1.0);
10063 emxEnsureCapacity_real_T(x, i44);
10064 loop_ub = (int)(nfcns + 1.0);
10065 for (i44 = 0; i44 < loop_ub; i44++) {
10066 x->data[i44] = 0.0;
10067 }
10068
10069 emxInit_real_T(&ad, 2);
10070 niter = 0;
10071 jchnge = 1.0;
10072 d1 = (nfcns - 1.0) / 15.0;
10073 b_fix(&d1);
10074 i44 = ad->size[0] * ad->size[1];
10075 ad->size[0] = 1;
10076 ad->size[1] = (int)(nfcns + 1.0);
10077 emxEnsureCapacity_real_T(ad, i44);
10078 loop_ub = (int)(nfcns + 1.0);
10079 for (i44 = 0; i44 < loop_ub; i44++) {
10080 ad->data[i44] = 0.0;
10081 }
10082
10083 /* index manager(s) */
10084 emxInit_real_T(&l, 2);
10085 emxInit_int32_T(&r21, 2);
10086 emxInit_real_T1(&b, 1);
10087 emxInit_real_T(&a, 2);
10088 emxInit_real_T(&b_wt, 2);
10089 emxInit_int8_T(&r22, 2);
10090 emxInit_int32_T1(&b_iext, 1);
10091 emxInit_real_T1(&mtmp, 1);
10092 guard1 = false;
10093 do {
10094 exitg1 = 0;
10095 if (jchnge > 0.0) {
10096 iext->data[(int)((nfcns + 1.0) + 1.0) - 1] = (double)varargin_2 + 1.0;
10097 niter++;
10098 if (niter > 250) {
10099 guard1 = true;
10100 exitg1 = 1;
10101 } else {
10102 if (1.0 > nfcns + 1.0) {
10103 loop_ub = 0;
10104 } else {
10105 loop_ub = (int)(nfcns + 1.0);
10106 }
10107
10108 i44 = l->size[0] * l->size[1];
10109 l->size[0] = 1;
10110 l->size[1] = loop_ub;
10111 emxEnsureCapacity_real_T(l, i44);
10112 for (i44 = 0; i44 < loop_ub; i44++) {
10113 l->data[l->size[0] * i44] = iext->data[i44];
10114 }
10115
10116 i44 = x->size[0] * x->size[1];
10117 x->size[0] = 1;
10118 x->size[1] = l->size[1];
10119 emxEnsureCapacity_real_T(x, i44);
10120 nut = l->size[0] * l->size[1];
10121 for (i44 = 0; i44 < nut; i44++) {
10122 x->data[i44] = 6.2831853071795862 * grid->data[(int)l->data[i44] - 1];
10123 }
10124
10125 b_cos(x);
10126 for (ixstart = 0; ixstart < (int)(nfcns + 1.0); ixstart++) {
10127 ad->data[ixstart] = remezdd(1.0 + (double)ixstart, nfcns + 1.0, d1 +
10128 1.0, x);
10129 }
10130
10131 for (i44 = 0; i44 < 2; i44++) {
10132 varargin_1[i44] = ad->size[i44];
10133 }
10134
10135 i44 = j->size[0] * j->size[1];
10136 j->size[0] = 1;
10137 j->size[1] = (int)varargin_1[1];
10138 emxEnsureCapacity_real_T(j, i44);
10139 nut = (int)varargin_1[1];
10140 for (i44 = 0; i44 < nut; i44++) {
10141 j->data[i44] = 1.0;
10142 }
10143
10144 if (2.0 > nfcns + 1.0) {
10145 i44 = 0;
10146 i45 = 1;
10147 i46 = 0;
10148 i47 = 0;
10149 i48 = 1;
10150 } else {
10151 i44 = 1;
10152 i45 = 2;
10153 i46 = (int)(nfcns + 1.0);
10154 i47 = 1;
10155 i48 = 2;
10156 }
10157
10158 i49 = r22->size[0] * r22->size[1];
10159 r22->size[0] = 1;
10160 r22->size[1] = (int)varargin_1[1];
10161 emxEnsureCapacity_int8_T(r22, i49);
10162 nut = (int)varargin_1[1];
10163 for (i49 = 0; i49 < nut; i49++) {
10164 r22->data[r22->size[0] * i49] = 1;
10165 }
10166
10167 nut = div_s32_floor((i46 - i44) - 1, i45);
10168 for (i46 = 0; i46 <= nut; i46++) {
10169 j->data[i47 + i48 * i46] = -(double)r22->data[i44 + i45 * i46];
10170 }
10171
10172 i44 = b->size[0];
10173 b->size[0] = l->size[1];
10174 emxEnsureCapacity_real_T1(b, i44);
10175 nut = l->size[1];
10176 for (i44 = 0; i44 < nut; i44++) {
10177 b->data[i44] = des->data[(int)l->data[l->size[0] * i44] - 1];
10178 }
10179
10180 guard2 = false;
10181 if (ad->size[1] == 1) {
10182 guard2 = true;
10183 } else {
10184 i44 = b_iext->size[0];
10185 b_iext->size[0] = loop_ub;
10186 emxEnsureCapacity_int32_T1(b_iext, i44);
10187 for (i44 = 0; i44 < loop_ub; i44++) {
10188 b_iext->data[i44] = (int)iext->data[i44];
10189 }
10190
10191 if (b_iext->size[0] == 1) {
10192 guard2 = true;
10193 } else {
10194 dnum = 0.0;
10195 for (i44 = 0; i44 < ad->size[1]; i44++) {
10196 dnum += ad->data[ad->size[0] * i44] * b->data[i44];
10197 }
10198 }
10199 }
10200
10201 if (guard2) {
10202 dnum = 0.0;
10203 for (i44 = 0; i44 < ad->size[1]; i44++) {
10204 dnum += ad->data[ad->size[0] * i44] * b->data[i44];
10205 }
10206 }
10207
10208 i44 = b_wt->size[0] * b_wt->size[1];
10209 b_wt->size[0] = 1;
10210 b_wt->size[1] = l->size[1];
10211 emxEnsureCapacity_real_T(b_wt, i44);
10212 loop_ub = l->size[0] * l->size[1];
10213 for (i44 = 0; i44 < loop_ub; i44++) {
10214 b_wt->data[i44] = wt->data[(int)l->data[i44] - 1];
10215 }
10216
10217 c_rdivide(ad, b_wt, x2);
10218 i44 = b->size[0];
10219 b->size[0] = x2->size[1];
10220 emxEnsureCapacity_real_T1(b, i44);
10221 loop_ub = x2->size[1];
10222 for (i44 = 0; i44 < loop_ub; i44++) {
10223 b->data[i44] = x2->data[x2->size[0] * i44];
10224 }
10225
10226 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10227 temp = 0.0;
10228 for (i44 = 0; i44 < j->size[1]; i44++) {
10229 temp += j->data[j->size[0] * i44] * b->data[i44];
10230 }
10231 } else {
10232 temp = 0.0;
10233 for (i44 = 0; i44 < j->size[1]; i44++) {
10234 temp += j->data[j->size[0] * i44] * b->data[i44];
10235 }
10236 }
10237
10238 b_dev = dnum / temp;
10239 nu = 1;
10240 if (b_dev > 0.0) {
10241 nu = -1;
10242 }
10243
10244 b_dev *= -(double)nu;
10245 temp = (double)nu * b_dev;
10246 i44 = a->size[0] * a->size[1];
10247 a->size[0] = 1;
10248 a->size[1] = j->size[1];
10249 emxEnsureCapacity_real_T(a, i44);
10250 loop_ub = j->size[0] * j->size[1];
10251 for (i44 = 0; i44 < loop_ub; i44++) {
10252 a->data[i44] = temp * j->data[i44];
10253 }
10254
10255 i44 = b_wt->size[0] * b_wt->size[1];
10256 b_wt->size[0] = 1;
10257 b_wt->size[1] = l->size[1];
10258 emxEnsureCapacity_real_T(b_wt, i44);
10259 loop_ub = l->size[0] * l->size[1];
10260 for (i44 = 0; i44 < loop_ub; i44++) {
10261 b_wt->data[i44] = wt->data[(int)l->data[i44] - 1];
10262 }
10263
10264 c_rdivide(a, b_wt, x2);
10265 i44 = y->size[0] * y->size[1];
10266 y->size[0] = 1;
10267 y->size[1] = l->size[1];
10268 emxEnsureCapacity_real_T(y, i44);
10269 loop_ub = l->size[0] * l->size[1];
10270 for (i44 = 0; i44 < loop_ub; i44++) {
10271 y->data[i44] = des->data[(int)l->data[i44] - 1] + x2->data[i44];
10272 }
10273
10274 if (b_dev <= devl) {
10275 /* warning(message('signal:firpm:DidNotConverge',niter)) */
10276 printf("%s\n", "DidNotConverge");
10277 fflush(stdout);
10278 i44 = h->size[0] * h->size[1];
10279 h->size[0] = (int)nfilt;
10280 h->size[1] = 1;
10281 emxEnsureCapacity_real_T(h, i44);
10282 loop_ub = (int)nfilt;
10283 for (i44 = 0; i44 < loop_ub; i44++) {
10284 h->data[i44] = 0.0;
10285 }
10286
10287 b_dev = -1.0;
10288
10289 /* iext */
10290 b_valid = false;
10291 exitg1 = 1;
10292 } else {
10293 devl = b_dev;
10294 jchnge = 0.0;
10295 k1 = iext->data[0];
10296 dnum = iext->data[(int)(nfcns + 1.0) - 1];
10297 temp = 0.0;
10298 nut = -nu;
10299 b_j = 1.0;
10300 flag34 = 1;
10301 while (b_j < (nfcns + 1.0) + 1.0) {
10302 kup = iext->data[(int)(unsigned int)b_j];
10303 b_l = iext->data[(int)b_j - 1] + 1.0;
10304 nut = -nut;
10305 if (b_j == 2.0) {
10306 b_y1 = comp;
10307 }
10308
10309 comp = b_dev;
10310 flag = 1;
10311 if (iext->data[(int)b_j - 1] + 1.0 < iext->data[(int)(b_j + 1.0) - 1]) {
10312 /* gee */
10313 dtemp = cos(6.2831853071795862 * grid->data[(int)(iext->data[(int)
10314 b_j - 1] + 1.0) - 1]);
10315 i44 = b_wt->size[0] * b_wt->size[1];
10316 b_wt->size[0] = 1;
10317 b_wt->size[1] = x->size[1];
10318 emxEnsureCapacity_real_T(b_wt, i44);
10319 loop_ub = x->size[0] * x->size[1];
10320 for (i44 = 0; i44 < loop_ub; i44++) {
10321 b_wt->data[i44] = dtemp - x->data[i44];
10322 }
10323
10324 c_rdivide(ad, b_wt, j);
10325 i44 = b->size[0];
10326 b->size[0] = y->size[1];
10327 emxEnsureCapacity_real_T1(b, i44);
10328 loop_ub = y->size[1];
10329 for (i44 = 0; i44 < loop_ub; i44++) {
10330 b->data[i44] = y->data[y->size[0] * i44];
10331 }
10332
10333 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10334 b_y = 0.0;
10335 for (i44 = 0; i44 < j->size[1]; i44++) {
10336 b_y += j->data[j->size[0] * i44] * b->data[i44];
10337 }
10338 } else {
10339 b_y = 0.0;
10340 for (i44 = 0; i44 < j->size[1]; i44++) {
10341 b_y += j->data[j->size[0] * i44] * b->data[i44];
10342 }
10343 }
10344
10345 dtemp = b_sum(j);
10346 err = (b_y / dtemp - des->data[(int)(iext->data[(int)b_j - 1] +
10347 1.0) - 1]) * wt->data[(int)(iext->data[(int)b_j - 1] + 1.0)
10348 - 1];
10349 dtemp = (double)nut * err - b_dev;
10350 if (dtemp > 0.0) {
10351 comp = (double)nut * err;
10352 b_l = (iext->data[(int)b_j - 1] + 1.0) + 1.0;
10353 exitg2 = false;
10354 while ((!exitg2) && (b_l < kup)) {
10355 /* gee */
10356 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10357 i44 = b_wt->size[0] * b_wt->size[1];
10358 b_wt->size[0] = 1;
10359 b_wt->size[1] = x->size[1];
10360 emxEnsureCapacity_real_T(b_wt, i44);
10361 loop_ub = x->size[0] * x->size[1];
10362 for (i44 = 0; i44 < loop_ub; i44++) {
10363 b_wt->data[i44] = dtemp - x->data[i44];
10364 }
10365
10366 c_rdivide(ad, b_wt, j);
10367 i44 = b->size[0];
10368 b->size[0] = y->size[1];
10369 emxEnsureCapacity_real_T1(b, i44);
10370 loop_ub = y->size[1];
10371 for (i44 = 0; i44 < loop_ub; i44++) {
10372 b->data[i44] = y->data[y->size[0] * i44];
10373 }
10374
10375 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10376 b_y = 0.0;
10377 for (i44 = 0; i44 < j->size[1]; i44++) {
10378 b_y += j->data[j->size[0] * i44] * b->data[i44];
10379 }
10380 } else {
10381 b_y = 0.0;
10382 for (i44 = 0; i44 < j->size[1]; i44++) {
10383 b_y += j->data[j->size[0] * i44] * b->data[i44];
10384 }
10385 }
10386
10387 dtemp = b_sum(j);
10388 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data[(int)
10389 b_l - 1];
10390 dtemp = (double)nut * err - comp;
10391 if (dtemp > 0.0) {
10392 comp = (double)nut * err;
10393 b_l++;
10394 } else {
10395 exitg2 = true;
10396 }
10397 }
10398
10399 iext->data[(int)b_j - 1] = b_l - 1.0;
10400 b_j++;
10401 temp = b_l - 1.0;
10402 jchnge++;
10403 flag = 0;
10404 }
10405 }
10406
10407 if (flag != 0) {
10408 b_l -= 2.0;
10409 exitg2 = false;
10410 while ((!exitg2) && (b_l > temp)) {
10411 /* gee */
10412 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10413 i44 = b_wt->size[0] * b_wt->size[1];
10414 b_wt->size[0] = 1;
10415 b_wt->size[1] = x->size[1];
10416 emxEnsureCapacity_real_T(b_wt, i44);
10417 loop_ub = x->size[0] * x->size[1];
10418 for (i44 = 0; i44 < loop_ub; i44++) {
10419 b_wt->data[i44] = dtemp - x->data[i44];
10420 }
10421
10422 c_rdivide(ad, b_wt, j);
10423 i44 = b->size[0];
10424 b->size[0] = y->size[1];
10425 emxEnsureCapacity_real_T1(b, i44);
10426 loop_ub = y->size[1];
10427 for (i44 = 0; i44 < loop_ub; i44++) {
10428 b->data[i44] = y->data[y->size[0] * i44];
10429 }
10430
10431 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10432 b_y = 0.0;
10433 for (i44 = 0; i44 < j->size[1]; i44++) {
10434 b_y += j->data[j->size[0] * i44] * b->data[i44];
10435 }
10436 } else {
10437 b_y = 0.0;
10438 for (i44 = 0; i44 < j->size[1]; i44++) {
10439 b_y += j->data[j->size[0] * i44] * b->data[i44];
10440 }
10441 }
10442
10443 dtemp = b_sum(j);
10444 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data[(int)
10445 b_l - 1];
10446 dtemp = (double)nut * err - comp;
10447 if ((dtemp > 0.0) || (jchnge > 0.0)) {
10448 exitg2 = true;
10449 } else {
10450 b_l--;
10451 }
10452 }
10453
10454 if (b_l <= temp) {
10455 b_l = iext->data[(int)b_j - 1] + 1.0;
10456 if (jchnge > 0.0) {
10457 iext->data[(int)b_j - 1] = (iext->data[(int)b_j - 1] + 1.0) -
10458 1.0;
10459 b_j++;
10460 temp = b_l - 1.0;
10461 jchnge++;
10462 } else {
10463 b_l = (iext->data[(int)b_j - 1] + 1.0) + 1.0;
10464 exitg2 = false;
10465 while ((!exitg2) && (b_l < kup)) {
10466 /* gee */
10467 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10468 i44 = b_wt->size[0] * b_wt->size[1];
10469 b_wt->size[0] = 1;
10470 b_wt->size[1] = x->size[1];
10471 emxEnsureCapacity_real_T(b_wt, i44);
10472 loop_ub = x->size[0] * x->size[1];
10473 for (i44 = 0; i44 < loop_ub; i44++) {
10474 b_wt->data[i44] = dtemp - x->data[i44];
10475 }
10476
10477 c_rdivide(ad, b_wt, j);
10478 i44 = b->size[0];
10479 b->size[0] = y->size[1];
10480 emxEnsureCapacity_real_T1(b, i44);
10481 loop_ub = y->size[1];
10482 for (i44 = 0; i44 < loop_ub; i44++) {
10483 b->data[i44] = y->data[y->size[0] * i44];
10484 }
10485
10486 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10487 b_y = 0.0;
10488 for (i44 = 0; i44 < j->size[1]; i44++) {
10489 b_y += j->data[j->size[0] * i44] * b->data[i44];
10490 }
10491 } else {
10492 b_y = 0.0;
10493 for (i44 = 0; i44 < j->size[1]; i44++) {
10494 b_y += j->data[j->size[0] * i44] * b->data[i44];
10495 }
10496 }
10497
10498 dtemp = b_sum(j);
10499 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data
10500 [(int)b_l - 1];
10501 dtemp = (double)nut * err - comp;
10502 if (dtemp > 0.0) {
10503 exitg2 = true;
10504 } else {
10505 b_l++;
10506 }
10507 }
10508
10509 if ((b_l < kup) && (dtemp > 0.0)) {
10510 comp = (double)nut * err;
10511 b_l++;
10512 exitg2 = false;
10513 while ((!exitg2) && (b_l < kup)) {
10514 /* gee */
10515 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10516 i44 = b_wt->size[0] * b_wt->size[1];
10517 b_wt->size[0] = 1;
10518 b_wt->size[1] = x->size[1];
10519 emxEnsureCapacity_real_T(b_wt, i44);
10520 loop_ub = x->size[0] * x->size[1];
10521 for (i44 = 0; i44 < loop_ub; i44++) {
10522 b_wt->data[i44] = dtemp - x->data[i44];
10523 }
10524
10525 c_rdivide(ad, b_wt, j);
10526 i44 = b->size[0];
10527 b->size[0] = y->size[1];
10528 emxEnsureCapacity_real_T1(b, i44);
10529 loop_ub = y->size[1];
10530 for (i44 = 0; i44 < loop_ub; i44++) {
10531 b->data[i44] = y->data[y->size[0] * i44];
10532 }
10533
10534 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10535 b_y = 0.0;
10536 for (i44 = 0; i44 < j->size[1]; i44++) {
10537 b_y += j->data[j->size[0] * i44] * b->data[i44];
10538 }
10539 } else {
10540 b_y = 0.0;
10541 for (i44 = 0; i44 < j->size[1]; i44++) {
10542 b_y += j->data[j->size[0] * i44] * b->data[i44];
10543 }
10544 }
10545
10546 dtemp = b_sum(j);
10547 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data
10548 [(int)b_l - 1];
10549 dtemp = (double)nut * err - comp;
10550 if (dtemp > 0.0) {
10551 comp = (double)nut * err;
10552 b_l++;
10553 } else {
10554 exitg2 = true;
10555 }
10556 }
10557
10558 iext->data[(int)b_j - 1] = b_l - 1.0;
10559 b_j++;
10560 temp = b_l - 1.0;
10561 jchnge = 1.0;
10562 } else {
10563 temp = iext->data[(int)b_j - 1];
10564 b_j++;
10565 }
10566 }
10567 } else if (dtemp > 0.0) {
10568 comp = (double)nut * err;
10569 b_l--;
10570 exitg2 = false;
10571 while ((!exitg2) && (b_l > temp)) {
10572 /* gee */
10573 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10574 i44 = b_wt->size[0] * b_wt->size[1];
10575 b_wt->size[0] = 1;
10576 b_wt->size[1] = x->size[1];
10577 emxEnsureCapacity_real_T(b_wt, i44);
10578 loop_ub = x->size[0] * x->size[1];
10579 for (i44 = 0; i44 < loop_ub; i44++) {
10580 b_wt->data[i44] = dtemp - x->data[i44];
10581 }
10582
10583 c_rdivide(ad, b_wt, j);
10584 i44 = b->size[0];
10585 b->size[0] = y->size[1];
10586 emxEnsureCapacity_real_T1(b, i44);
10587 loop_ub = y->size[1];
10588 for (i44 = 0; i44 < loop_ub; i44++) {
10589 b->data[i44] = y->data[y->size[0] * i44];
10590 }
10591
10592 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10593 b_y = 0.0;
10594 for (i44 = 0; i44 < j->size[1]; i44++) {
10595 b_y += j->data[j->size[0] * i44] * b->data[i44];
10596 }
10597 } else {
10598 b_y = 0.0;
10599 for (i44 = 0; i44 < j->size[1]; i44++) {
10600 b_y += j->data[j->size[0] * i44] * b->data[i44];
10601 }
10602 }
10603
10604 dtemp = b_sum(j);
10605 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data[(int)
10606 b_l - 1];
10607 dtemp = (double)nut * err - comp;
10608 if (dtemp > 0.0) {
10609 comp = (double)nut * err;
10610 b_l--;
10611 } else {
10612 exitg2 = true;
10613 }
10614 }
10615
10616 temp = iext->data[(int)b_j - 1];
10617 iext->data[(int)b_j - 1] = b_l + 1.0;
10618 b_j++;
10619 jchnge++;
10620 } else {
10621 temp = iext->data[(int)b_j - 1];
10622 b_j++;
10623 }
10624 }
10625 }
10626
10627 do {
10628 exitg3 = 0;
10629 if (b_j == (nfcns + 1.0) + 1.0) {
10630 varargin_1[1] = iext->data[0];
10631 ixstart = 1;
10632 if (rtIsNaN(k1)) {
10633 flag = 2;
10634 exitg2 = false;
10635 while ((!exitg2) && (flag < 3)) {
10636 ixstart = 2;
10637 if (!rtIsNaN(varargin_1[1])) {
10638 k1 = varargin_1[1];
10639 exitg2 = true;
10640 } else {
10641 flag = 3;
10642 }
10643 }
10644 }
10645
10646 if ((ixstart < 2) && (varargin_1[1] < k1)) {
10647 k1 = varargin_1[1];
10648 }
10649
10650 varargin_1[1] = iext->data[(int)(nfcns + 1.0) - 1];
10651 ixstart = 1;
10652 if (rtIsNaN(dnum)) {
10653 flag = 2;
10654 exitg2 = false;
10655 while ((!exitg2) && (flag < 3)) {
10656 ixstart = 2;
10657 if (!rtIsNaN(varargin_1[1])) {
10658 dnum = varargin_1[1];
10659 exitg2 = true;
10660 } else {
10661 flag = 3;
10662 }
10663 }
10664 }
10665
10666 if ((ixstart < 2) && (varargin_1[1] > dnum)) {
10667 dnum = varargin_1[1];
10668 }
10669
10670 nut1 = nut;
10671 nut = -nu;
10672 comp *= 1.00001;
10673 luck = 1;
10674 flag = 1;
10675 b_l = 1.0;
10676 exitg2 = false;
10677 while ((!exitg2) && (b_l < k1)) {
10678 /* gee */
10679 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10680 i44 = b_wt->size[0] * b_wt->size[1];
10681 b_wt->size[0] = 1;
10682 b_wt->size[1] = x->size[1];
10683 emxEnsureCapacity_real_T(b_wt, i44);
10684 loop_ub = x->size[0] * x->size[1];
10685 for (i44 = 0; i44 < loop_ub; i44++) {
10686 b_wt->data[i44] = dtemp - x->data[i44];
10687 }
10688
10689 c_rdivide(ad, b_wt, j);
10690 i44 = b->size[0];
10691 b->size[0] = y->size[1];
10692 emxEnsureCapacity_real_T1(b, i44);
10693 loop_ub = y->size[1];
10694 for (i44 = 0; i44 < loop_ub; i44++) {
10695 b->data[i44] = y->data[y->size[0] * i44];
10696 }
10697
10698 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10699 b_y = 0.0;
10700 for (i44 = 0; i44 < j->size[1]; i44++) {
10701 b_y += j->data[j->size[0] * i44] * b->data[i44];
10702 }
10703 } else {
10704 b_y = 0.0;
10705 for (i44 = 0; i44 < j->size[1]; i44++) {
10706 b_y += j->data[j->size[0] * i44] * b->data[i44];
10707 }
10708 }
10709
10710 dtemp = b_sum(j);
10711 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data[(int)
10712 b_l - 1];
10713 dtemp = err * -(double)nu - comp;
10714 if (dtemp > 0.0) {
10715 comp = -(double)nu * err;
10716 b_l++;
10717 exitg4 = false;
10718 while ((!exitg4) && (b_l < k1)) {
10719 /* gee */
10720 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10721 i44 = b_wt->size[0] * b_wt->size[1];
10722 b_wt->size[0] = 1;
10723 b_wt->size[1] = x->size[1];
10724 emxEnsureCapacity_real_T(b_wt, i44);
10725 loop_ub = x->size[0] * x->size[1];
10726 for (i44 = 0; i44 < loop_ub; i44++) {
10727 b_wt->data[i44] = dtemp - x->data[i44];
10728 }
10729
10730 c_rdivide(ad, b_wt, j);
10731 i44 = b->size[0];
10732 b->size[0] = y->size[1];
10733 emxEnsureCapacity_real_T1(b, i44);
10734 loop_ub = y->size[1];
10735 for (i44 = 0; i44 < loop_ub; i44++) {
10736 b->data[i44] = y->data[y->size[0] * i44];
10737 }
10738
10739 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10740 b_y = 0.0;
10741 for (i44 = 0; i44 < j->size[1]; i44++) {
10742 b_y += j->data[j->size[0] * i44] * b->data[i44];
10743 }
10744 } else {
10745 b_y = 0.0;
10746 for (i44 = 0; i44 < j->size[1]; i44++) {
10747 b_y += j->data[j->size[0] * i44] * b->data[i44];
10748 }
10749 }
10750
10751 dtemp = b_sum(j);
10752 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data
10753 [(int)b_l - 1];
10754 dtemp = -(double)nu * err - comp;
10755 if (dtemp > 0.0) {
10756 comp = -(double)nu * err;
10757 b_l++;
10758 } else {
10759 exitg4 = true;
10760 }
10761 }
10762
10763 iext->data[(int)((nfcns + 1.0) + 1.0) - 1] = b_l - 1.0;
10764 b_j = ((nfcns + 1.0) + 1.0) + 1.0;
10765 jchnge++;
10766 flag = 0;
10767 exitg2 = true;
10768 } else {
10769 b_l++;
10770 }
10771 }
10772
10773 if (flag != 0) {
10774 luck = 6;
10775 nut = -nut1;
10776 comp = b_y1 * 1.00001;
10777 b_l = ((double)ngrid + 1.0) - 1.0;
10778 exitg2 = false;
10779 while ((!exitg2) && (b_l > dnum)) {
10780 /* gee */
10781 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10782 i44 = b_wt->size[0] * b_wt->size[1];
10783 b_wt->size[0] = 1;
10784 b_wt->size[1] = x->size[1];
10785 emxEnsureCapacity_real_T(b_wt, i44);
10786 loop_ub = x->size[0] * x->size[1];
10787 for (i44 = 0; i44 < loop_ub; i44++) {
10788 b_wt->data[i44] = dtemp - x->data[i44];
10789 }
10790
10791 c_rdivide(ad, b_wt, j);
10792 i44 = b->size[0];
10793 b->size[0] = y->size[1];
10794 emxEnsureCapacity_real_T1(b, i44);
10795 loop_ub = y->size[1];
10796 for (i44 = 0; i44 < loop_ub; i44++) {
10797 b->data[i44] = y->data[y->size[0] * i44];
10798 }
10799
10800 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10801 b_y = 0.0;
10802 for (i44 = 0; i44 < j->size[1]; i44++) {
10803 b_y += j->data[j->size[0] * i44] * b->data[i44];
10804 }
10805 } else {
10806 b_y = 0.0;
10807 for (i44 = 0; i44 < j->size[1]; i44++) {
10808 b_y += j->data[j->size[0] * i44] * b->data[i44];
10809 }
10810 }
10811
10812 dtemp = b_sum(j);
10813 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data[(int)
10814 b_l - 1];
10815 dtemp = err * -(double)nut1 - comp;
10816 if (dtemp > 0.0) {
10817 comp = -(double)nut1 * err;
10818 luck = 16;
10819 b_l--;
10820 exitg4 = false;
10821 while ((!exitg4) && (b_l > dnum)) {
10822 /* gee */
10823 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
10824 i44 = b_wt->size[0] * b_wt->size[1];
10825 b_wt->size[0] = 1;
10826 b_wt->size[1] = x->size[1];
10827 emxEnsureCapacity_real_T(b_wt, i44);
10828 loop_ub = x->size[0] * x->size[1];
10829 for (i44 = 0; i44 < loop_ub; i44++) {
10830 b_wt->data[i44] = dtemp - x->data[i44];
10831 }
10832
10833 c_rdivide(ad, b_wt, j);
10834 i44 = b->size[0];
10835 b->size[0] = y->size[1];
10836 emxEnsureCapacity_real_T1(b, i44);
10837 loop_ub = y->size[1];
10838 for (i44 = 0; i44 < loop_ub; i44++) {
10839 b->data[i44] = y->data[y->size[0] * i44];
10840 }
10841
10842 if ((j->size[1] == 1) || (b->size[0] == 1)) {
10843 b_y = 0.0;
10844 for (i44 = 0; i44 < j->size[1]; i44++) {
10845 b_y += j->data[j->size[0] * i44] * b->data[i44];
10846 }
10847 } else {
10848 b_y = 0.0;
10849 for (i44 = 0; i44 < j->size[1]; i44++) {
10850 b_y += j->data[j->size[0] * i44] * b->data[i44];
10851 }
10852 }
10853
10854 dtemp = b_sum(j);
10855 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data
10856 [(int)b_l - 1];
10857 dtemp = -(double)nut1 * err - comp;
10858 if (dtemp > 0.0) {
10859 comp = -(double)nut1 * err;
10860 b_l--;
10861 } else {
10862 exitg4 = true;
10863 }
10864 }
10865
10866 iext->data[(int)((nfcns + 1.0) + 1.0) - 1] = b_l + 1.0;
10867 b_j = ((nfcns + 1.0) + 1.0) + 1.0;
10868 jchnge++;
10869 flag = 0;
10870 exitg2 = true;
10871 } else {
10872 b_l--;
10873 }
10874 }
10875
10876 if (flag != 0) {
10877 flag34 = 0;
10878 if (luck != 6) {
10879 temp = (nfcns + 1.0) - nfcns;
10880 if (2.0 > temp) {
10881 i44 = -2;
10882 i45 = 0;
10883 } else {
10884 i44 = -1;
10885 i45 = (int)temp;
10886 }
10887
10888 temp = (nfcns + 1.0) - nfcns;
10889 if (temp > (nfcns + 1.0) - 1.0) {
10890 i46 = 1;
10891 i47 = 0;
10892 } else {
10893 i46 = (int)temp;
10894 i47 = (int)((nfcns + 1.0) - 1.0);
10895 }
10896
10897 /* Update index */
10898 temp = (nfcns + 1.0) - nfcns;
10899 if (2.0 > temp) {
10900 i48 = -2;
10901 i49 = 0;
10902 } else {
10903 i48 = -1;
10904 i49 = (int)temp;
10905 }
10906
10907 temp = (nfcns + 1.0) - nfcns;
10908 if (temp > (nfcns + 1.0) - 1.0) {
10909 ixstart = 1;
10910 flag = 0;
10911 } else {
10912 ixstart = (int)temp;
10913 flag = (int)((nfcns + 1.0) - 1.0);
10914 }
10915
10916 nu = b_wt->size[0] * b_wt->size[1];
10917 b_wt->size[0] = 1;
10918 b_wt->size[1] = ((i45 - i44) + i47) - i46;
10919 emxEnsureCapacity_real_T(b_wt, nu);
10920 loop_ub = i45 - i44;
10921 for (nu = 0; nu <= loop_ub - 3; nu++) {
10922 b_wt->data[b_wt->size[0] * (nu + 1)] = iext->data[(i44 +
10923 nu) + 2];
10924 }
10925
10926 loop_ub = i47 - i46;
10927 for (i47 = 0; i47 <= loop_ub; i47++) {
10928 b_wt->data[b_wt->size[0] * (((i47 + i45) - i44) - 1)] =
10929 iext->data[(i46 + i47) - 1];
10930 }
10931
10932 loop_ub = b_wt->size[1];
10933 i44 = r21->size[0] * r21->size[1];
10934 r21->size[0] = 1;
10935 r21->size[1] = loop_ub;
10936 emxEnsureCapacity_int32_T(r21, i44);
10937 for (i44 = 0; i44 < loop_ub; i44++) {
10938 r21->data[r21->size[0] * i44] = i44;
10939 }
10940
10941 i44 = mtmp->size[0];
10942 mtmp->size[0] = ((i49 - i48) + flag) - ixstart;
10943 emxEnsureCapacity_real_T1(mtmp, i44);
10944 mtmp->data[0] = k1;
10945 loop_ub = i49 - i48;
10946 for (i44 = 0; i44 <= loop_ub - 3; i44++) {
10947 mtmp->data[i44 + 1] = iext->data[(i48 + i44) + 2];
10948 }
10949
10950 loop_ub = flag - ixstart;
10951 for (i44 = 0; i44 <= loop_ub; i44++) {
10952 mtmp->data[((i44 + i49) - i48) - 1] = iext->data[(ixstart
10953 + i44) - 1];
10954 }
10955
10956 loop_ub = r21->size[1];
10957 for (i44 = 0; i44 < loop_ub; i44++) {
10958 iext->data[r21->data[r21->size[0] * i44]] = mtmp->data
10959 [(*(int (*)[2])r21->size)[0] * i44];
10960 }
10961
10962 jchnge++;
10963 }
10964
10965 exitg3 = 1;
10966 }
10967 }
10968 } else {
10969 exitg3 = 1;
10970 }
10971 } while (exitg3 == 0);
10972
10973 if ((flag34 != 0) && (b_j > (nfcns + 1.0) + 1.0)) {
10974 if (luck > 9) {
10975 if (2.0 > nfcns + 1.0) {
10976 i44 = 0;
10977 i45 = 0;
10978 } else {
10979 i44 = 1;
10980 i45 = (int)(nfcns + 1.0);
10981 }
10982
10983 if (nfcns + 1.0 > (nfcns + 1.0) - 1.0) {
10984 i46 = 0;
10985 i47 = 0;
10986 } else {
10987 i46 = (int)(nfcns + 1.0) - 1;
10988 i47 = (int)((nfcns + 1.0) - 1.0);
10989 }
10990
10991 /* Update index */
10992 if (2.0 > nfcns + 1.0) {
10993 i48 = 0;
10994 i49 = 0;
10995 } else {
10996 i48 = 1;
10997 i49 = (int)(nfcns + 1.0);
10998 }
10999
11000 if (nfcns + 1.0 > (nfcns + 1.0) - 1.0) {
11001 ixstart = 0;
11002 flag = 0;
11003 } else {
11004 ixstart = (int)(nfcns + 1.0) - 1;
11005 flag = (int)((nfcns + 1.0) - 1.0);
11006 }
11007
11008 nu = b_wt->size[0] * b_wt->size[1];
11009 b_wt->size[0] = 1;
11010 b_wt->size[1] = (((i45 - i44) + i47) - i46) + 2;
11011 emxEnsureCapacity_real_T(b_wt, nu);
11012 loop_ub = i45 - i44;
11013 for (nu = 0; nu < loop_ub; nu++) {
11014 b_wt->data[b_wt->size[0] * nu] = iext->data[i44 + nu];
11015 }
11016
11017 loop_ub = i47 - i46;
11018 for (nu = 0; nu < loop_ub; nu++) {
11019 b_wt->data[b_wt->size[0] * ((nu + i45) - i44)] = iext->data[i46
11020 + nu];
11021 }
11022
11023 b_wt->data[b_wt->size[0] * (((i45 - i44) + i47) - i46)] =
11024 iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11025 b_wt->data[b_wt->size[0] * ((((i45 - i44) + i47) - i46) + 1)] =
11026 iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11027 loop_ub = b_wt->size[1];
11028 i44 = r21->size[0] * r21->size[1];
11029 r21->size[0] = 1;
11030 r21->size[1] = loop_ub;
11031 emxEnsureCapacity_int32_T(r21, i44);
11032 for (i44 = 0; i44 < loop_ub; i44++) {
11033 r21->data[r21->size[0] * i44] = i44;
11034 }
11035
11036 temp = iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11037 dnum = iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11038 i44 = mtmp->size[0];
11039 mtmp->size[0] = (((i49 - i48) + flag) - ixstart) + 2;
11040 emxEnsureCapacity_real_T1(mtmp, i44);
11041 loop_ub = i49 - i48;
11042 for (i44 = 0; i44 < loop_ub; i44++) {
11043 mtmp->data[i44] = iext->data[i48 + i44];
11044 }
11045
11046 loop_ub = flag - ixstart;
11047 for (i44 = 0; i44 < loop_ub; i44++) {
11048 mtmp->data[(i44 + i49) - i48] = iext->data[ixstart + i44];
11049 }
11050
11051 mtmp->data[((i49 - i48) + flag) - ixstart] = temp;
11052 mtmp->data[(((i49 - i48) + flag) - ixstart) + 1] = dnum;
11053 loop_ub = r21->size[1];
11054 for (i44 = 0; i44 < loop_ub; i44++) {
11055 iext->data[r21->data[r21->size[0] * i44]] = mtmp->data[(*(int (*)
11056 [2])r21->size)[0] * i44];
11057 }
11058
11059 jchnge++;
11060 } else {
11061 ixstart = 1;
11062 if (rtIsNaN(b_y1)) {
11063 flag = 2;
11064 exitg2 = false;
11065 while ((!exitg2) && (flag < 3)) {
11066 ixstart = 2;
11067 if (!rtIsNaN(comp)) {
11068 b_y1 = comp;
11069 exitg2 = true;
11070 } else {
11071 flag = 3;
11072 }
11073 }
11074 }
11075
11076 if ((ixstart < 2) && (comp > b_y1)) {
11077 b_y1 = comp;
11078 }
11079
11080 k1 = iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11081 comp = b_y1 * 1.00001;
11082 b_l = ((double)ngrid + 1.0) - 1.0;
11083 exitg2 = false;
11084 while ((!exitg2) && (b_l > dnum)) {
11085 /* gee */
11086 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
11087 i44 = b_wt->size[0] * b_wt->size[1];
11088 b_wt->size[0] = 1;
11089 b_wt->size[1] = x->size[1];
11090 emxEnsureCapacity_real_T(b_wt, i44);
11091 loop_ub = x->size[0] * x->size[1];
11092 for (i44 = 0; i44 < loop_ub; i44++) {
11093 b_wt->data[i44] = dtemp - x->data[i44];
11094 }
11095
11096 c_rdivide(ad, b_wt, j);
11097 i44 = b->size[0];
11098 b->size[0] = y->size[1];
11099 emxEnsureCapacity_real_T1(b, i44);
11100 loop_ub = y->size[1];
11101 for (i44 = 0; i44 < loop_ub; i44++) {
11102 b->data[i44] = y->data[y->size[0] * i44];
11103 }
11104
11105 if ((j->size[1] == 1) || (b->size[0] == 1)) {
11106 b_y = 0.0;
11107 for (i44 = 0; i44 < j->size[1]; i44++) {
11108 b_y += j->data[j->size[0] * i44] * b->data[i44];
11109 }
11110 } else {
11111 b_y = 0.0;
11112 for (i44 = 0; i44 < j->size[1]; i44++) {
11113 b_y += j->data[j->size[0] * i44] * b->data[i44];
11114 }
11115 }
11116
11117 dtemp = b_sum(j);
11118 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data[(int)
11119 b_l - 1];
11120 dtemp = err * -(double)nut1 - comp;
11121 if (dtemp > 0.0) {
11122 comp = -(double)nut1 * err;
11123 luck += 10;
11124 b_l--;
11125 exitg4 = false;
11126 while ((!exitg4) && (b_l > dnum)) {
11127 /* gee */
11128 dtemp = cos(6.2831853071795862 * grid->data[(int)b_l - 1]);
11129 i44 = b_wt->size[0] * b_wt->size[1];
11130 b_wt->size[0] = 1;
11131 b_wt->size[1] = x->size[1];
11132 emxEnsureCapacity_real_T(b_wt, i44);
11133 loop_ub = x->size[0] * x->size[1];
11134 for (i44 = 0; i44 < loop_ub; i44++) {
11135 b_wt->data[i44] = dtemp - x->data[i44];
11136 }
11137
11138 c_rdivide(ad, b_wt, j);
11139 i44 = b->size[0];
11140 b->size[0] = y->size[1];
11141 emxEnsureCapacity_real_T1(b, i44);
11142 loop_ub = y->size[1];
11143 for (i44 = 0; i44 < loop_ub; i44++) {
11144 b->data[i44] = y->data[y->size[0] * i44];
11145 }
11146
11147 if ((j->size[1] == 1) || (b->size[0] == 1)) {
11148 b_y = 0.0;
11149 for (i44 = 0; i44 < j->size[1]; i44++) {
11150 b_y += j->data[j->size[0] * i44] * b->data[i44];
11151 }
11152 } else {
11153 b_y = 0.0;
11154 for (i44 = 0; i44 < j->size[1]; i44++) {
11155 b_y += j->data[j->size[0] * i44] * b->data[i44];
11156 }
11157 }
11158
11159 dtemp = b_sum(j);
11160 err = (b_y / dtemp - des->data[(int)b_l - 1]) * wt->data
11161 [(int)b_l - 1];
11162 dtemp = -(double)nut1 * err - comp;
11163 if (dtemp > 0.0) {
11164 comp = -(double)nut1 * err;
11165 b_l--;
11166 } else {
11167 exitg4 = true;
11168 }
11169 }
11170
11171 iext->data[(int)((nfcns + 1.0) + 1.0) - 1] = b_l + 1.0;
11172 jchnge++;
11173 if (2.0 > nfcns + 1.0) {
11174 i44 = 0;
11175 i45 = 0;
11176 } else {
11177 i44 = 1;
11178 i45 = (int)(nfcns + 1.0);
11179 }
11180
11181 if (nfcns + 1.0 > (nfcns + 1.0) - 1.0) {
11182 i46 = 0;
11183 i47 = 0;
11184 } else {
11185 i46 = (int)(nfcns + 1.0) - 1;
11186 i47 = (int)((nfcns + 1.0) - 1.0);
11187 }
11188
11189 /* Update index */
11190 if (2.0 > nfcns + 1.0) {
11191 i48 = 0;
11192 i49 = 0;
11193 } else {
11194 i48 = 1;
11195 i49 = (int)(nfcns + 1.0);
11196 }
11197
11198 if (nfcns + 1.0 > (nfcns + 1.0) - 1.0) {
11199 ixstart = 0;
11200 flag = 0;
11201 } else {
11202 ixstart = (int)(nfcns + 1.0) - 1;
11203 flag = (int)((nfcns + 1.0) - 1.0);
11204 }
11205
11206 nu = b_wt->size[0] * b_wt->size[1];
11207 b_wt->size[0] = 1;
11208 b_wt->size[1] = (((i45 - i44) + i47) - i46) + 2;
11209 emxEnsureCapacity_real_T(b_wt, nu);
11210 loop_ub = i45 - i44;
11211 for (nu = 0; nu < loop_ub; nu++) {
11212 b_wt->data[b_wt->size[0] * nu] = iext->data[i44 + nu];
11213 }
11214
11215 loop_ub = i47 - i46;
11216 for (nu = 0; nu < loop_ub; nu++) {
11217 b_wt->data[b_wt->size[0] * ((nu + i45) - i44)] = iext->
11218 data[i46 + nu];
11219 }
11220
11221 b_wt->data[b_wt->size[0] * (((i45 - i44) + i47) - i46)] =
11222 iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11223 b_wt->data[b_wt->size[0] * ((((i45 - i44) + i47) - i46) + 1)] =
11224 iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11225 loop_ub = b_wt->size[1];
11226 i44 = r21->size[0] * r21->size[1];
11227 r21->size[0] = 1;
11228 r21->size[1] = loop_ub;
11229 emxEnsureCapacity_int32_T(r21, i44);
11230 for (i44 = 0; i44 < loop_ub; i44++) {
11231 r21->data[r21->size[0] * i44] = i44;
11232 }
11233
11234 temp = iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11235 dnum = iext->data[(int)((nfcns + 1.0) + 1.0) - 1];
11236 i44 = mtmp->size[0];
11237 mtmp->size[0] = (((i49 - i48) + flag) - ixstart) + 2;
11238 emxEnsureCapacity_real_T1(mtmp, i44);
11239 loop_ub = i49 - i48;
11240 for (i44 = 0; i44 < loop_ub; i44++) {
11241 mtmp->data[i44] = iext->data[i48 + i44];
11242 }
11243
11244 loop_ub = flag - ixstart;
11245 for (i44 = 0; i44 < loop_ub; i44++) {
11246 mtmp->data[(i44 + i49) - i48] = iext->data[ixstart + i44];
11247 }
11248
11249 mtmp->data[((i49 - i48) + flag) - ixstart] = temp;
11250 mtmp->data[(((i49 - i48) + flag) - ixstart) + 1] = dnum;
11251 loop_ub = r21->size[1];
11252 for (i44 = 0; i44 < loop_ub; i44++) {
11253 iext->data[r21->data[r21->size[0] * i44]] = mtmp->data
11254 [(*(int (*)[2])r21->size)[0] * i44];
11255 }
11256
11257 exitg2 = true;
11258 } else {
11259 b_l--;
11260 }
11261 }
11262
11263 if (luck != 6) {
11264 temp = (nfcns + 1.0) - nfcns;
11265 if (2.0 > temp) {
11266 i44 = -2;
11267 i45 = 0;
11268 } else {
11269 i44 = -1;
11270 i45 = (int)temp;
11271 }
11272
11273 temp = (nfcns + 1.0) - nfcns;
11274 if (temp > (nfcns + 1.0) - 1.0) {
11275 i46 = 1;
11276 i47 = 0;
11277 } else {
11278 i46 = (int)temp;
11279 i47 = (int)((nfcns + 1.0) - 1.0);
11280 }
11281
11282 /* Update index */
11283 temp = (nfcns + 1.0) - nfcns;
11284 if (2.0 > temp) {
11285 i48 = -2;
11286 i49 = 0;
11287 } else {
11288 i48 = -1;
11289 i49 = (int)temp;
11290 }
11291
11292 temp = (nfcns + 1.0) - nfcns;
11293 if (temp > (nfcns + 1.0) - 1.0) {
11294 ixstart = 1;
11295 flag = 0;
11296 } else {
11297 ixstart = (int)temp;
11298 flag = (int)((nfcns + 1.0) - 1.0);
11299 }
11300
11301 nu = b_wt->size[0] * b_wt->size[1];
11302 b_wt->size[0] = 1;
11303 b_wt->size[1] = ((i45 - i44) + i47) - i46;
11304 emxEnsureCapacity_real_T(b_wt, nu);
11305 loop_ub = i45 - i44;
11306 for (nu = 0; nu <= loop_ub - 3; nu++) {
11307 b_wt->data[b_wt->size[0] * (nu + 1)] = iext->data[(i44 + nu) +
11308 2];
11309 }
11310
11311 loop_ub = i47 - i46;
11312 for (i47 = 0; i47 <= loop_ub; i47++) {
11313 b_wt->data[b_wt->size[0] * (((i47 + i45) - i44) - 1)] =
11314 iext->data[(i46 + i47) - 1];
11315 }
11316
11317 loop_ub = b_wt->size[1];
11318 i44 = r21->size[0] * r21->size[1];
11319 r21->size[0] = 1;
11320 r21->size[1] = loop_ub;
11321 emxEnsureCapacity_int32_T(r21, i44);
11322 for (i44 = 0; i44 < loop_ub; i44++) {
11323 r21->data[r21->size[0] * i44] = i44;
11324 }
11325
11326 i44 = mtmp->size[0];
11327 mtmp->size[0] = ((i49 - i48) + flag) - ixstart;
11328 emxEnsureCapacity_real_T1(mtmp, i44);
11329 mtmp->data[0] = k1;
11330 loop_ub = i49 - i48;
11331 for (i44 = 0; i44 <= loop_ub - 3; i44++) {
11332 mtmp->data[i44 + 1] = iext->data[(i48 + i44) + 2];
11333 }
11334
11335 loop_ub = flag - ixstart;
11336 for (i44 = 0; i44 <= loop_ub; i44++) {
11337 mtmp->data[((i44 + i49) - i48) - 1] = iext->data[(ixstart +
11338 i44) - 1];
11339 }
11340
11341 loop_ub = r21->size[1];
11342 for (i44 = 0; i44 < loop_ub; i44++) {
11343 iext->data[r21->data[r21->size[0] * i44]] = mtmp->data[(*(int
11344 (*)[2])r21->size)[0] * i44];
11345 }
11346
11347 jchnge++;
11348 }
11349 }
11350 }
11351
11352 guard1 = false;
11353 }
11354 }
11355 } else {
11356 guard1 = true;
11357 exitg1 = 1;
11358 }
11359 } while (exitg1 == 0);
11360
11361 if (guard1) {
11362 /* Inverse Fourier transformation */
11363 err = -1.0;
11364 k1 = -1.0;
11365
11366 /* initialize memory */
11367 /* x(nzz) = -2; */
11368 i44 = x2->size[0] * x2->size[1];
11369 x2->size[0] = 1;
11370 x2->size[1] = x->size[1] + 1;
11371 emxEnsureCapacity_real_T(x2, i44);
11372 loop_ub = x->size[1];
11373 for (i44 = 0; i44 < loop_ub; i44++) {
11374 x2->data[x2->size[0] * i44] = x->data[x->size[0] * i44];
11375 }
11376
11377 x2->data[x2->size[0] * x->size[1]] = -2.0;
11378 jchnge = 2.0 * nfcns - 1.0;
11379 b_j = 1.0 / jchnge;
11380 b_l = 1.0;
11381 nu = 0;
11382 if (((edge[0] == 0.0) && (edge[3] == 0.5)) || (nfcns <= 3.0)) {
11383 nu = 1;
11384 }
11385
11386 if (nu != 1) {
11387 dtemp = cos(6.2831853071795862 * grid->data[0]);
11388 dnum = cos(6.2831853071795862 * grid->data[varargin_2 - 1]);
11389 k1 = 2.0 / (dtemp - dnum);
11390 err = -(dtemp + dnum) / (dtemp - dnum);
11391 }
11392
11393 i44 = a->size[0] * a->size[1];
11394 a->size[0] = 1;
11395 a->size[1] = (int)nfcns;
11396 emxEnsureCapacity_real_T(a, i44);
11397 for (nut = 0; nut < (int)nfcns; nut++) {
11398 temp = ((1.0 + (double)nut) - 1.0) * b_j;
11399 kup = cos(6.2831853071795862 * temp);
11400 if (nu != 1) {
11401 kup = (kup - err) / k1;
11402 dtemp = kup;
11403 b_acos(&dtemp);
11404 temp = dtemp / 6.2831853071795862;
11405 }
11406
11407 dnum = x2->data[(int)b_l - 1];
11408 while ((kup <= dnum) && (dnum - kup >= 1.0E-6)) {
11409 b_l++;
11410 dnum = x2->data[(int)b_l - 1];
11411 }
11412
11413 if (fabs(kup - dnum) < 1.0E-6) {
11414 a->data[nut] = y->data[(int)b_l - 1];
11415 } else {
11416 /* gee */
11417 loop_ub = (int)(nfcns + 1.0);
11418 dtemp = cos(6.2831853071795862 * temp);
11419 i44 = b_wt->size[0] * b_wt->size[1];
11420 b_wt->size[0] = 1;
11421 b_wt->size[1] = (int)(nfcns + 1.0);
11422 emxEnsureCapacity_real_T(b_wt, i44);
11423 for (i44 = 0; i44 < loop_ub; i44++) {
11424 b_wt->data[b_wt->size[0] * i44] = dtemp - x2->data[i44];
11425 }
11426
11427 c_rdivide(ad, b_wt, j);
11428 i44 = b->size[0];
11429 b->size[0] = y->size[1];
11430 emxEnsureCapacity_real_T1(b, i44);
11431 loop_ub = y->size[1];
11432 for (i44 = 0; i44 < loop_ub; i44++) {
11433 b->data[i44] = y->data[y->size[0] * i44];
11434 }
11435
11436 if ((j->size[1] == 1) || (b->size[0] == 1)) {
11437 b_y = 0.0;
11438 for (i44 = 0; i44 < j->size[1]; i44++) {
11439 b_y += j->data[j->size[0] * i44] * b->data[i44];
11440 }
11441 } else {
11442 b_y = 0.0;
11443 for (i44 = 0; i44 < j->size[1]; i44++) {
11444 b_y += j->data[j->size[0] * i44] * b->data[i44];
11445 }
11446 }
11447
11448 dtemp = b_sum(j);
11449 a->data[nut] = b_y / dtemp;
11450 }
11451
11452 ixstart = 1;
11453 if ((int)(b_l - 1.0) > 1) {
11454 ixstart = (int)(b_l - 1.0);
11455 }
11456
11457 b_l = ixstart;
11458 }
11459
11460 temp = 6.2831853071795862 / jchnge;
11461 i44 = j->size[0] * j->size[1];
11462 j->size[0] = 1;
11463 j->size[1] = (int)nfcns;
11464 emxEnsureCapacity_real_T(j, i44);
11465 for (nut = 0; nut < (int)nfcns; nut++) {
11466 dnum = ((1.0 + (double)nut) - 1.0) * temp;
11467 if (nfcns - 1.0 < 1.0) {
11468 j->data[nut] = a->data[0];
11469 } else {
11470 if (2.0 > nfcns) {
11471 i44 = 1;
11472 i45 = 1;
11473 } else {
11474 i44 = 2;
11475 i45 = (int)nfcns + 1;
11476 }
11477
11478 if (nfcns - 1.0 < 1.0) {
11479 i46 = y->size[0] * y->size[1];
11480 y->size[0] = 1;
11481 y->size[1] = 0;
11482 emxEnsureCapacity_real_T(y, i46);
11483 } else {
11484 i46 = y->size[0] * y->size[1];
11485 y->size[0] = 1;
11486 y->size[1] = (int)floor((nfcns - 1.0) - 1.0) + 1;
11487 emxEnsureCapacity_real_T(y, i46);
11488 loop_ub = (int)floor((nfcns - 1.0) - 1.0);
11489 for (i46 = 0; i46 <= loop_ub; i46++) {
11490 y->data[y->size[0] * i46] = 1.0 + (double)i46;
11491 }
11492 }
11493
11494 i46 = y->size[0] * y->size[1];
11495 y->size[0] = 1;
11496 emxEnsureCapacity_real_T(y, i46);
11497 ixstart = y->size[0];
11498 flag = y->size[1];
11499 loop_ub = ixstart * flag;
11500 for (i46 = 0; i46 < loop_ub; i46++) {
11501 y->data[i46] *= dnum;
11502 }
11503
11504 b_cos(y);
11505 i46 = y->size[0] * y->size[1];
11506 y->size[0] = 1;
11507 emxEnsureCapacity_real_T(y, i46);
11508 ixstart = y->size[0];
11509 flag = y->size[1];
11510 loop_ub = ixstart * flag;
11511 for (i46 = 0; i46 < loop_ub; i46++) {
11512 y->data[i46] *= 2.0;
11513 }
11514
11515 i46 = b->size[0];
11516 b->size[0] = i45 - i44;
11517 emxEnsureCapacity_real_T1(b, i46);
11518 loop_ub = i45 - i44;
11519 for (i46 = 0; i46 < loop_ub; i46++) {
11520 b->data[i46] = a->data[(i44 + i46) - 1];
11521 }
11522
11523 if ((y->size[1] == 1) || (i45 - i44 == 1)) {
11524 b_y = 0.0;
11525 for (i44 = 0; i44 < y->size[1]; i44++) {
11526 b_y += y->data[y->size[0] * i44] * b->data[i44];
11527 }
11528 } else {
11529 b_y = 0.0;
11530 for (i44 = 0; i44 < y->size[1]; i44++) {
11531 b_y += y->data[y->size[0] * i44] * b->data[i44];
11532 }
11533 }
11534
11535 j->data[nut] = a->data[0] + b_y;
11536 }
11537 }
11538
11539 if (2.0 > nfcns) {
11540 i44 = -1;
11541 i45 = 0;
11542 } else {
11543 i44 = 0;
11544 i45 = (int)nfcns;
11545 }
11546
11547 i46 = iext->size[0];
11548 iext->size[0] = i45 - i44;
11549 emxEnsureCapacity_real_T1(iext, i46);
11550 iext->data[0] = j->data[0] / jchnge;
11551 loop_ub = i45 - i44;
11552 for (i45 = 0; i45 <= loop_ub - 2; i45++) {
11553 iext->data[i45 + 1] = 2.0 * j->data[(i44 + i45) + 1] / jchnge;
11554 }
11555
11556 i44 = j->size[0] * j->size[1];
11557 j->size[0] = 1;
11558 j->size[1] = (int)nfcns;
11559 emxEnsureCapacity_real_T(j, i44);
11560 loop_ub = (int)nfcns;
11561 for (i44 = 0; i44 < loop_ub; i44++) {
11562 j->data[i44] = 0.0;
11563 }
11564
11565 i44 = l->size[0] * l->size[1];
11566 l->size[0] = 1;
11567 l->size[1] = (int)nfcns - 1;
11568 emxEnsureCapacity_real_T(l, i44);
11569 loop_ub = (int)nfcns - 1;
11570 for (i44 = 0; i44 < loop_ub; i44++) {
11571 l->data[i44] = 0.0;
11572 }
11573
11574 if (nu != 1) {
11575 j->data[0] = 2.0 * iext->data[(int)nfcns - 1] * err + iext->data[(int)
11576 nfcns - 2];
11577 j->data[1] = 2.0 * k1 * iext->data[(int)nfcns - 1];
11578 l->data[0] = iext->data[(int)nfcns - 3] - iext->data[(int)nfcns - 1];
11579 for (nut = 0; nut <= (int)nfcns - 3; nut++) {
11580 if (2 + nut == (int)nfcns - 1) {
11581 k1 /= 2.0;
11582 err /= 2.0;
11583 }
11584
11585 j->data[nut + 2] = 0.0;
11586 i44 = x2->size[0] * x2->size[1];
11587 x2->size[0] = 1;
11588 x2->size[1] = (int)((2.0 + (double)nut) - 1.0) + 1;
11589 emxEnsureCapacity_real_T(x2, i44);
11590 loop_ub = (int)((2.0 + (double)nut) - 1.0);
11591 for (i44 = 0; i44 <= loop_ub; i44++) {
11592 x2->data[x2->size[0] * i44] = 1.0 + (double)i44;
11593 }
11594
11595 loop_ub = x2->size[0] * x2->size[1];
11596 for (i44 = 0; i44 < loop_ub; i44++) {
11597 a->data[(int)x2->data[i44] - 1] = j->data[(int)x2->data[i44] - 1];
11598 }
11599
11600 temp = 2.0 * err;
11601 loop_ub = x2->size[0] * x2->size[1];
11602 for (i44 = 0; i44 < loop_ub; i44++) {
11603 j->data[(int)x2->data[i44] - 1] = temp * a->data[(int)x2->data[i44] -
11604 1];
11605 }
11606
11607 j->data[1] += 2.0 * a->data[0] * k1;
11608 i44 = x2->size[0] * x2->size[1];
11609 x2->size[0] = 1;
11610 x2->size[1] = (int)(((2.0 + (double)nut) - 1.0) - 1.0) + 1;
11611 emxEnsureCapacity_real_T(x2, i44);
11612 loop_ub = (int)(((2.0 + (double)nut) - 1.0) - 1.0);
11613 for (i44 = 0; i44 <= loop_ub; i44++) {
11614 x2->data[x2->size[0] * i44] = 1.0 + (double)i44;
11615 }
11616
11617 i44 = x->size[0] * x->size[1];
11618 x->size[0] = 1;
11619 x->size[1] = x2->size[1];
11620 emxEnsureCapacity_real_T(x, i44);
11621 loop_ub = x2->size[0] * x2->size[1];
11622 for (i44 = 0; i44 < loop_ub; i44++) {
11623 x->data[i44] = a->data[(int)x2->data[i44]];
11624 }
11625
11626 i44 = mtmp->size[0];
11627 mtmp->size[0] = x2->size[0] * x2->size[1];
11628 emxEnsureCapacity_real_T1(mtmp, i44);
11629 loop_ub = x2->size[0] * x2->size[1];
11630 for (i44 = 0; i44 < loop_ub; i44++) {
11631 mtmp->data[i44] = (j->data[(int)x2->data[i44] - 1] + l->data[(int)
11632 x2->data[i44] - 1]) + k1 * x->data[i44];
11633 }
11634
11635 loop_ub = mtmp->size[0];
11636 for (i44 = 0; i44 < loop_ub; i44++) {
11637 j->data[(int)x2->data[i44] - 1] = mtmp->data[i44];
11638 }
11639
11640 i44 = x2->size[0] * x2->size[1];
11641 x2->size[0] = 1;
11642 x2->size[1] = (int)(((2.0 + (double)nut) + 1.0) - 3.0) + 1;
11643 emxEnsureCapacity_real_T(x2, i44);
11644 loop_ub = (int)(((2.0 + (double)nut) + 1.0) - 3.0);
11645 for (i44 = 0; i44 <= loop_ub; i44++) {
11646 x2->data[x2->size[0] * i44] = 3.0 + (double)i44;
11647 }
11648
11649 i44 = x->size[0] * x->size[1];
11650 x->size[0] = 1;
11651 x->size[1] = x2->size[1];
11652 emxEnsureCapacity_real_T(x, i44);
11653 loop_ub = x2->size[0] * x2->size[1];
11654 for (i44 = 0; i44 < loop_ub; i44++) {
11655 x->data[i44] = a->data[(int)x2->data[i44] - 2];
11656 }
11657
11658 i44 = mtmp->size[0];
11659 mtmp->size[0] = x2->size[0] * x2->size[1];
11660 emxEnsureCapacity_real_T1(mtmp, i44);
11661 loop_ub = x2->size[0] * x2->size[1];
11662 for (i44 = 0; i44 < loop_ub; i44++) {
11663 mtmp->data[i44] = j->data[(int)x2->data[i44] - 1] + k1 * x->data[i44];
11664 }
11665
11666 loop_ub = mtmp->size[0];
11667 for (i44 = 0; i44 < loop_ub; i44++) {
11668 j->data[(int)x2->data[i44] - 1] = mtmp->data[i44];
11669 }
11670
11671 if (2 + nut != (int)nfcns - 1) {
11672 i44 = x2->size[0] * x2->size[1];
11673 x2->size[0] = 1;
11674 x2->size[1] = (int)((2.0 + (double)nut) - 1.0) + 1;
11675 emxEnsureCapacity_real_T(x2, i44);
11676 loop_ub = (int)((2.0 + (double)nut) - 1.0);
11677 for (i44 = 0; i44 <= loop_ub; i44++) {
11678 x2->data[x2->size[0] * i44] = 1.0 + (double)i44;
11679 }
11680
11681 loop_ub = x2->size[0] * x2->size[1];
11682 for (i44 = 0; i44 < loop_ub; i44++) {
11683 l->data[(int)x2->data[i44] - 1] = -a->data[(int)x2->data[i44] - 1];
11684 }
11685
11686 l->data[0] += iext->data[((int)nfcns - nut) - 4];
11687 }
11688 }
11689
11690 loop_ub = (int)nfcns;
11691 for (i44 = 0; i44 < loop_ub; i44++) {
11692 iext->data[i44] = j->data[i44];
11693 }
11694 }
11695
11696 /* alpha must be at lease >=3 */
11697 if (nfcns <= 3.0) {
11698 /* alpha(nfcns + 1) = 0; */
11699 /* alpha(nfcns + 2) = 0; */
11700 i44 = j->size[0] * j->size[1];
11701 j->size[0] = 1;
11702 j->size[1] = iext->size[0] + 2;
11703 emxEnsureCapacity_real_T(j, i44);
11704 loop_ub = iext->size[0];
11705 for (i44 = 0; i44 < loop_ub; i44++) {
11706 j->data[j->size[0] * i44] = iext->data[i44];
11707 }
11708
11709 j->data[j->size[0] * iext->size[0]] = 0.0;
11710 j->data[j->size[0] * (iext->size[0] + 1)] = 0.0;
11711 } else {
11712 i44 = j->size[0] * j->size[1];
11713 j->size[0] = 1;
11714 j->size[1] = iext->size[0];
11715 emxEnsureCapacity_real_T(j, i44);
11716 loop_ub = iext->size[0];
11717 for (i44 = 0; i44 < loop_ub; i44++) {
11718 j->data[j->size[0] * i44] = iext->data[i44];
11719 }
11720 }
11721
11722 /* alpha=alpha'; */
11723 /* now that's done! */
11724 if (nodd != 0.0) {
11725 i44 = x->size[0] * x->size[1];
11726 x->size[0] = 1;
11727 x->size[1] = (int)floor(-((0.0 - (nfcns - 1.0)) - -1.0)) + 1;
11728 emxEnsureCapacity_real_T(x, i44);
11729 loop_ub = (int)floor(-((0.0 - (nfcns - 1.0)) - -1.0));
11730 for (i44 = 0; i44 <= loop_ub; i44++) {
11731 x->data[x->size[0] * i44] = j->data[(int)((nfcns + 1.0) + (-1.0 -
11732 (double)i44)) - 1];
11733 }
11734
11735 i44 = h->size[0] * h->size[1];
11736 h->size[0] = 1;
11737 h->size[1] = x->size[1] + 1;
11738 emxEnsureCapacity_real_T(h, i44);
11739 loop_ub = x->size[1];
11740 for (i44 = 0; i44 < loop_ub; i44++) {
11741 h->data[h->size[0] * i44] = 0.5 * x->data[x->size[0] * i44];
11742 }
11743
11744 h->data[h->size[0] * x->size[1]] = j->data[0];
11745 } else {
11746 if ((nfcns - (nfcns - 1.0)) + 2.0 > nfcns) {
11747 i44 = 0;
11748 i45 = 1;
11749 } else {
11750 i44 = (int)nfcns - 1;
11751 i45 = -1;
11752 }
11753
11754 i46 = x2->size[0] * x2->size[1];
11755 x2->size[0] = 1;
11756 x2->size[1] = (int)floor(-((0.0 - (nfcns - 1.0)) - -2.0)) + 1;
11757 emxEnsureCapacity_real_T(x2, i46);
11758 loop_ub = (int)floor(-((0.0 - (nfcns - 1.0)) - -2.0));
11759 for (i46 = 0; i46 <= loop_ub; i46++) {
11760 x2->data[x2->size[0] * i46] = j->data[(int)((nfcns + 1.0) + (double)(int)
11761 (-2.0 - (double)i46)) - 1];
11762 }
11763
11764 i46 = h->size[0] * h->size[1];
11765 h->size[0] = 1;
11766 h->size[1] = 2 + x2->size[1];
11767 emxEnsureCapacity_real_T(h, i46);
11768 h->data[0] = 0.25 * j->data[(int)nfcns - 1];
11769 loop_ub = x2->size[1];
11770 for (i46 = 0; i46 < loop_ub; i46++) {
11771 h->data[h->size[0] * (i46 + 1)] = 0.25 * (x2->data[x2->size[0] * i46] +
11772 j->data[i44 + i45 * i46]);
11773 }
11774
11775 h->data[h->size[0] * (1 + x2->size[1])] = 0.25 * (2.0 * j->data[0] +
11776 j->data[1]);
11777 }
11778 }
11779
11780 emxFree_real_T(&mtmp);
11781 emxFree_int32_T(&b_iext);
11782 emxFree_int8_T(&r22);
11783 emxFree_real_T(&b_wt);
11784 emxFree_real_T(&a);
11785 emxFree_real_T(&b);
11786 emxFree_int32_T(&r21);
11787 emxFree_real_T(&x2);
11788 emxFree_real_T(&l);
11789 emxFree_real_T(&ad);
11790 emxFree_real_T(&x);
11791 emxFree_real_T(&y);
11792 emxFree_real_T(&iext);
11793 emxFree_real_T(&j);
11794 *dev = b_dev;
11795 *valid = b_valid;
11796 }
11797
11798 /*
11799 * Arguments : const double b_data[]
11800 * const int b_size[2]
11801 * const emxArray_creal_T *a
11802 * double bR_data[]
11803 * int bR_size[2]
11804 * emxArray_creal_T *aR
11805 * Return Type : void
11806 */
removeTrailingZero(const double b_data[],const int b_size[2],const emxArray_creal_T * a,double bR_data[],int bR_size[2],emxArray_creal_T * aR)11807 static void removeTrailingZero(const double b_data[], const int b_size[2], const
11808 emxArray_creal_T *a, double bR_data[], int bR_size[2], emxArray_creal_T *aR)
11809 {
11810 int idx;
11811 int ii_size_idx_1;
11812 int ii;
11813 boolean_T exitg1;
11814 int ii_data[1];
11815 int b_ii_size_idx_1;
11816 boolean_T b_a;
11817 int b_ii_data[1];
11818 signed char varargin_1_data[1];
11819 int varargin_2_data[1];
11820 int c_ii_size_idx_1;
11821 int trz_len_data_idx_0;
11822 double d0;
11823
11824 /* end freqs */
11825 idx = 0;
11826 ii_size_idx_1 = 1;
11827 ii = b_size[1];
11828 exitg1 = false;
11829 while ((!exitg1) && (ii > 0)) {
11830 if (b_data[ii - 1] != 0.0) {
11831 idx = 1;
11832 ii_data[0] = ii;
11833 exitg1 = true;
11834 } else {
11835 ii--;
11836 }
11837 }
11838
11839 if (idx == 0) {
11840 ii_size_idx_1 = 0;
11841 }
11842
11843 idx = 0;
11844 b_ii_size_idx_1 = 1;
11845 ii = a->size[1];
11846 exitg1 = false;
11847 while ((!exitg1) && (ii > 0)) {
11848 b_a = ((a->data[ii - 1].re != 0.0) || (a->data[ii - 1].im != 0.0));
11849 if (b_a) {
11850 idx = 1;
11851 b_ii_data[0] = ii;
11852 exitg1 = true;
11853 } else {
11854 ii--;
11855 }
11856 }
11857
11858 if (idx == 0) {
11859 b_ii_size_idx_1 = 0;
11860 }
11861
11862 for (ii = 0; ii < ii_size_idx_1; ii++) {
11863 varargin_1_data[ii] = (signed char)((signed char)b_size[1] - (signed char)
11864 ii_data[ii]);
11865 }
11866
11867 idx = a->size[1];
11868 for (ii = 0; ii < b_ii_size_idx_1; ii++) {
11869 varargin_2_data[ii] = idx - b_ii_data[ii];
11870 }
11871
11872 if (ii_size_idx_1 <= b_ii_size_idx_1) {
11873 c_ii_size_idx_1 = ii_size_idx_1;
11874 } else {
11875 c_ii_size_idx_1 = 0;
11876 }
11877
11878 if (1 <= c_ii_size_idx_1) {
11879 idx = varargin_1_data[0];
11880 trz_len_data_idx_0 = varargin_2_data[0];
11881 if (idx < trz_len_data_idx_0) {
11882 trz_len_data_idx_0 = idx;
11883 }
11884 }
11885
11886 if (trz_len_data_idx_0 > 0) {
11887 d0 = (double)b_size[1] - (double)trz_len_data_idx_0;
11888 if (1.0 > d0) {
11889 idx = 0;
11890 } else {
11891 idx = (int)d0;
11892 }
11893
11894 bR_size[0] = 1;
11895 bR_size[1] = idx;
11896 for (ii = 0; ii < idx; ii++) {
11897 bR_data[bR_size[0] * ii] = b_data[ii];
11898 }
11899
11900 d0 = (double)a->size[1] - (double)trz_len_data_idx_0;
11901 if (1.0 > d0) {
11902 idx = 0;
11903 } else {
11904 idx = (int)d0;
11905 }
11906
11907 ii = aR->size[0] * aR->size[1];
11908 aR->size[0] = 1;
11909 aR->size[1] = idx;
11910 emxEnsureCapacity_creal_T(aR, ii);
11911 for (ii = 0; ii < idx; ii++) {
11912 aR->data[aR->size[0] * ii] = a->data[ii];
11913 }
11914 } else {
11915 bR_size[0] = 1;
11916 bR_size[1] = b_size[1];
11917 idx = b_size[0] * b_size[1];
11918 for (ii = 0; ii < idx; ii++) {
11919 bR_data[ii] = b_data[ii];
11920 }
11921
11922 ii = aR->size[0] * aR->size[1];
11923 aR->size[0] = 1;
11924 aR->size[1] = a->size[1];
11925 emxEnsureCapacity_creal_T(aR, ii);
11926 idx = a->size[0] * a->size[1];
11927 for (ii = 0; ii < idx; ii++) {
11928 aR->data[ii] = a->data[ii];
11929 }
11930 }
11931
11932 /* end removeTrailingZero */
11933 }
11934
11935 /*
11936 * Arguments : double u0
11937 * double u1
11938 * Return Type : double
11939 */
rt_atan2d_snf(double u0,double u1)11940 static double rt_atan2d_snf(double u0, double u1)
11941 {
11942 double y;
11943 int b_u0;
11944 int b_u1;
11945 if (rtIsNaN(u0) || rtIsNaN(u1)) {
11946 y = rtNaN;
11947 } else if (rtIsInf(u0) && rtIsInf(u1)) {
11948 if (u0 > 0.0) {
11949 b_u0 = 1;
11950 } else {
11951 b_u0 = -1;
11952 }
11953
11954 if (u1 > 0.0) {
11955 b_u1 = 1;
11956 } else {
11957 b_u1 = -1;
11958 }
11959
11960 y = atan2(b_u0, b_u1);
11961 } else if (u1 == 0.0) {
11962 if (u0 > 0.0) {
11963 y = RT_PI / 2.0;
11964 } else if (u0 < 0.0) {
11965 y = -(RT_PI / 2.0);
11966 } else {
11967 y = 0.0;
11968 }
11969 } else {
11970 y = atan2(u0, u1);
11971 }
11972
11973 return y;
11974 }
11975
11976 /*
11977 * Arguments : double u0
11978 * double u1
11979 * Return Type : double
11980 */
rt_hypotd_snf(double u0,double u1)11981 static double rt_hypotd_snf(double u0, double u1)
11982 {
11983 double y;
11984 double a;
11985 double b;
11986 a = fabs(u0);
11987 b = fabs(u1);
11988 if (a < b) {
11989 a /= b;
11990 y = b * sqrt(a * a + 1.0);
11991 } else if (a > b) {
11992 b /= a;
11993 y = a * sqrt(b * b + 1.0);
11994 } else if (rtIsNaN(b)) {
11995 y = b;
11996 } else {
11997 y = a * 1.4142135623730951;
11998 }
11999
12000 return y;
12001 }
12002
12003 /*
12004 * Arguments : double u0
12005 * double u1
12006 * Return Type : double
12007 */
rt_powd_snf(double u0,double u1)12008 static double rt_powd_snf(double u0, double u1)
12009 {
12010 double y;
12011 double d3;
12012 double d4;
12013 if (rtIsNaN(u0) || rtIsNaN(u1)) {
12014 y = rtNaN;
12015 } else {
12016 d3 = fabs(u0);
12017 d4 = fabs(u1);
12018 if (rtIsInf(u1)) {
12019 if (d3 == 1.0) {
12020 y = 1.0;
12021 } else if (d3 > 1.0) {
12022 if (u1 > 0.0) {
12023 y = rtInf;
12024 } else {
12025 y = 0.0;
12026 }
12027 } else if (u1 > 0.0) {
12028 y = 0.0;
12029 } else {
12030 y = rtInf;
12031 }
12032 } else if (d4 == 0.0) {
12033 y = 1.0;
12034 } else if (d4 == 1.0) {
12035 if (u1 > 0.0) {
12036 y = u0;
12037 } else {
12038 y = 1.0 / u0;
12039 }
12040 } else if (u1 == 2.0) {
12041 y = u0 * u0;
12042 } else if ((u1 == 0.5) && (u0 >= 0.0)) {
12043 y = sqrt(u0);
12044 } else if ((u0 < 0.0) && (u1 > floor(u1))) {
12045 y = rtNaN;
12046 } else {
12047 y = pow(u0, u1);
12048 }
12049 }
12050
12051 return y;
12052 }
12053
12054 /*
12055 * Arguments : double u0
12056 * double u1
12057 * Return Type : double
12058 */
rt_remd_snf(double u0,double u1)12059 static double rt_remd_snf(double u0, double u1)
12060 {
12061 double y;
12062 double b_u1;
12063 double q;
12064 if (!((!rtIsNaN(u0)) && (!rtIsInf(u0)) && ((!rtIsNaN(u1)) && (!rtIsInf(u1))))) {
12065 y = rtNaN;
12066 } else {
12067 if (u1 < 0.0) {
12068 b_u1 = ceil(u1);
12069 } else {
12070 b_u1 = floor(u1);
12071 }
12072
12073 if ((u1 != 0.0) && (u1 != b_u1)) {
12074 q = fabs(u0 / u1);
12075 if (fabs(q - floor(q + 0.5)) <= DBL_EPSILON * q) {
12076 y = 0.0 * u0;
12077 } else {
12078 y = fmod(u0, u1);
12079 }
12080 } else {
12081 y = fmod(u0, u1);
12082 }
12083 }
12084
12085 return y;
12086 }
12087
12088 /*
12089 * Arguments : double u
12090 * Return Type : double
12091 */
rt_roundd_snf(double u)12092 static double rt_roundd_snf(double u)
12093 {
12094 double y;
12095 if (fabs(u) < 4.503599627370496E+15) {
12096 if (u >= 0.5) {
12097 y = floor(u + 0.5);
12098 } else if (u > -0.5) {
12099 y = u * 0.0;
12100 } else {
12101 y = ceil(u - 0.5);
12102 }
12103 } else {
12104 y = u;
12105 }
12106
12107 return y;
12108 }
12109
12110 /*
12111 * FREQZ_CG Frequency response of digital filter with codegen support
12112 *
12113 * This function is based on 'freqz' by The MathWorks Inc.
12114 * Arguments : const double b[19]
12115 * const emxArray_real_T *w
12116 * double Fs
12117 * emxArray_creal_T *hh
12118 * Return Type : void
12119 */
s_freqz_cg(const double b[19],const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)12120 static void s_freqz_cg(const double b[19], const emxArray_real_T *w, double Fs,
12121 emxArray_creal_T *hh)
12122 {
12123 emxArray_real_T *r15;
12124 int i38;
12125 int loop_ub;
12126 emxArray_real_T *digw;
12127 emxArray_creal_T *s;
12128 emxArray_creal_T *y;
12129 boolean_T b9;
12130 int k;
12131 double s_re;
12132 double s_im;
12133 emxInit_real_T(&r15, 2);
12134
12135 /* Cast to enforce precision rules */
12136 /* Remaining are default or for advanced use */
12137 /* Make b a row */
12138 /* -------------------------------------------------------------------------- */
12139 /* Actual Frequency Response Computation */
12140 /* if fvflag, */
12141 /* Frequency vector specified. Use Horner's method of polynomial */
12142 /* evaluation at the frequency points and divide the numerator */
12143 /* by the denominator. */
12144 /* */
12145 /* Note: we use positive i here because of the relationship */
12146 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
12147 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
12148 /* */
12149 /* Fs was specified, freq. vector is in Hz */
12150 i38 = r15->size[0] * r15->size[1];
12151 r15->size[0] = 1;
12152 r15->size[1] = w->size[1];
12153 emxEnsureCapacity_real_T(r15, i38);
12154 loop_ub = w->size[0] * w->size[1];
12155 for (i38 = 0; i38 < loop_ub; i38++) {
12156 r15->data[i38] = 6.2831853071795862 * w->data[i38];
12157 }
12158
12159 emxInit_real_T(&digw, 2);
12160 emxInit_creal_T(&s, 2);
12161 rdivide(r15, Fs, digw);
12162
12163 /* Convert from Hz to rad/sample for computational purposes */
12164 i38 = s->size[0] * s->size[1];
12165 s->size[0] = 1;
12166 s->size[1] = digw->size[1];
12167 emxEnsureCapacity_creal_T(s, i38);
12168 loop_ub = digw->size[0] * digw->size[1];
12169 emxFree_real_T(&r15);
12170 for (i38 = 0; i38 < loop_ub; i38++) {
12171 s->data[i38].re = digw->data[i38] * 0.0;
12172 s->data[i38].im = digw->data[i38];
12173 }
12174
12175 emxInit_creal_T(&y, 2);
12176 c_exp(s);
12177
12178 /* Digital frequency must be used for this calculation */
12179 i38 = y->size[0] * y->size[1];
12180 y->size[0] = 1;
12181 y->size[1] = s->size[1];
12182 emxEnsureCapacity_creal_T(y, i38);
12183 b9 = (y->size[1] == 0);
12184 if (!b9) {
12185 i38 = y->size[0] * y->size[1];
12186 y->size[0] = 1;
12187 emxEnsureCapacity_creal_T(y, i38);
12188 loop_ub = y->size[1];
12189 for (i38 = 0; i38 < loop_ub; i38++) {
12190 y->data[y->size[0] * i38].re = b[0];
12191 y->data[y->size[0] * i38].im = 0.0;
12192 }
12193
12194 for (k = 0; k < 18; k++) {
12195 i38 = y->size[0] * y->size[1];
12196 y->size[0] = 1;
12197 y->size[1] = s->size[1];
12198 emxEnsureCapacity_creal_T(y, i38);
12199 loop_ub = s->size[0] * s->size[1];
12200 for (i38 = 0; i38 < loop_ub; i38++) {
12201 s_re = s->data[i38].re * y->data[i38].re - s->data[i38].im * y->data[i38]
12202 .im;
12203 s_im = s->data[i38].re * y->data[i38].im + s->data[i38].im * y->data[i38]
12204 .re;
12205 y->data[i38].re = s_re + b[k + 1];
12206 y->data[i38].im = s_im;
12207 }
12208 }
12209 }
12210
12211 i38 = s->size[0] * s->size[1];
12212 s->size[0] = 1;
12213 s->size[1] = digw->size[1];
12214 emxEnsureCapacity_creal_T(s, i38);
12215 loop_ub = digw->size[0] * digw->size[1];
12216 for (i38 = 0; i38 < loop_ub; i38++) {
12217 s_re = digw->data[i38] * 0.0;
12218 s_im = digw->data[i38];
12219 s->data[i38].re = 18.0 * s_re;
12220 s->data[i38].im = 18.0 * s_im;
12221 }
12222
12223 emxFree_real_T(&digw);
12224 c_exp(s);
12225 b_rdivide(y, s, hh);
12226
12227 /* Generate the default structure to pass to freqzplot */
12228 /* If rad/sample, Fs is empty */
12229 emxFree_creal_T(&y);
12230 emxFree_creal_T(&s);
12231 }
12232
12233 /*
12234 * Arguments : double x[2048]
12235 * Return Type : void
12236 */
sinc(double x[2048])12237 static void sinc(double x[2048])
12238 {
12239 int k;
12240 for (k = 0; k < 2048; k++) {
12241 if (fabs(x[k]) < 1.0020841800044864E-292) {
12242 x[k] = 1.0;
12243 } else {
12244 x[k] *= 3.1415926535897931;
12245 x[k] = sin(x[k]) / x[k];
12246 }
12247 }
12248 }
12249
12250 /*
12251 * Arguments : const double x[2048]
12252 * Return Type : double
12253 */
sum(const double x[2048])12254 static double sum(const double x[2048])
12255 {
12256 double y;
12257 int k;
12258 y = x[0];
12259 for (k = 0; k < 2047; k++) {
12260 y += x[k + 1];
12261 }
12262
12263 return y;
12264 }
12265
12266 /*
12267 * FREQZ_CG Frequency response of digital filter with codegen support
12268 *
12269 * This function is based on 'freqz' by The MathWorks Inc.
12270 * Arguments : const double b[85]
12271 * const emxArray_real_T *w
12272 * double Fs
12273 * emxArray_creal_T *hh
12274 * Return Type : void
12275 */
t_freqz_cg(const double b[85],const emxArray_real_T * w,double Fs,emxArray_creal_T * hh)12276 static void t_freqz_cg(const double b[85], const emxArray_real_T *w, double Fs,
12277 emxArray_creal_T *hh)
12278 {
12279 emxArray_real_T *r16;
12280 int i39;
12281 int loop_ub;
12282 emxArray_real_T *digw;
12283 emxArray_creal_T *s;
12284 emxArray_creal_T *y;
12285 boolean_T b10;
12286 int k;
12287 double s_re;
12288 double s_im;
12289 emxInit_real_T(&r16, 2);
12290
12291 /* Cast to enforce precision rules */
12292 /* Remaining are default or for advanced use */
12293 /* Make b a row */
12294 /* -------------------------------------------------------------------------- */
12295 /* Actual Frequency Response Computation */
12296 /* if fvflag, */
12297 /* Frequency vector specified. Use Horner's method of polynomial */
12298 /* evaluation at the frequency points and divide the numerator */
12299 /* by the denominator. */
12300 /* */
12301 /* Note: we use positive i here because of the relationship */
12302 /* polyval(a,exp(1i*w)) = fft(a).*exp(1i*w*(length(a)-1)) */
12303 /* ( assuming w = 2*pi*(0:length(a)-1)/length(a) ) */
12304 /* */
12305 /* Fs was specified, freq. vector is in Hz */
12306 i39 = r16->size[0] * r16->size[1];
12307 r16->size[0] = 1;
12308 r16->size[1] = w->size[1];
12309 emxEnsureCapacity_real_T(r16, i39);
12310 loop_ub = w->size[0] * w->size[1];
12311 for (i39 = 0; i39 < loop_ub; i39++) {
12312 r16->data[i39] = 6.2831853071795862 * w->data[i39];
12313 }
12314
12315 emxInit_real_T(&digw, 2);
12316 emxInit_creal_T(&s, 2);
12317 rdivide(r16, Fs, digw);
12318
12319 /* Convert from Hz to rad/sample for computational purposes */
12320 i39 = s->size[0] * s->size[1];
12321 s->size[0] = 1;
12322 s->size[1] = digw->size[1];
12323 emxEnsureCapacity_creal_T(s, i39);
12324 loop_ub = digw->size[0] * digw->size[1];
12325 emxFree_real_T(&r16);
12326 for (i39 = 0; i39 < loop_ub; i39++) {
12327 s->data[i39].re = digw->data[i39] * 0.0;
12328 s->data[i39].im = digw->data[i39];
12329 }
12330
12331 emxInit_creal_T(&y, 2);
12332 c_exp(s);
12333
12334 /* Digital frequency must be used for this calculation */
12335 i39 = y->size[0] * y->size[1];
12336 y->size[0] = 1;
12337 y->size[1] = s->size[1];
12338 emxEnsureCapacity_creal_T(y, i39);
12339 b10 = (y->size[1] == 0);
12340 if (!b10) {
12341 i39 = y->size[0] * y->size[1];
12342 y->size[0] = 1;
12343 emxEnsureCapacity_creal_T(y, i39);
12344 loop_ub = y->size[1];
12345 for (i39 = 0; i39 < loop_ub; i39++) {
12346 y->data[y->size[0] * i39].re = b[0];
12347 y->data[y->size[0] * i39].im = 0.0;
12348 }
12349
12350 for (k = 0; k < 84; k++) {
12351 i39 = y->size[0] * y->size[1];
12352 y->size[0] = 1;
12353 y->size[1] = s->size[1];
12354 emxEnsureCapacity_creal_T(y, i39);
12355 loop_ub = s->size[0] * s->size[1];
12356 for (i39 = 0; i39 < loop_ub; i39++) {
12357 s_re = s->data[i39].re * y->data[i39].re - s->data[i39].im * y->data[i39]
12358 .im;
12359 s_im = s->data[i39].re * y->data[i39].im + s->data[i39].im * y->data[i39]
12360 .re;
12361 y->data[i39].re = s_re + b[k + 1];
12362 y->data[i39].im = s_im;
12363 }
12364 }
12365 }
12366
12367 i39 = s->size[0] * s->size[1];
12368 s->size[0] = 1;
12369 s->size[1] = digw->size[1];
12370 emxEnsureCapacity_creal_T(s, i39);
12371 loop_ub = digw->size[0] * digw->size[1];
12372 for (i39 = 0; i39 < loop_ub; i39++) {
12373 s_re = digw->data[i39] * 0.0;
12374 s_im = digw->data[i39];
12375 s->data[i39].re = 84.0 * s_re;
12376 s->data[i39].im = 84.0 * s_im;
12377 }
12378
12379 emxFree_real_T(&digw);
12380 c_exp(s);
12381 b_rdivide(y, s, hh);
12382
12383 /* Generate the default structure to pass to freqzplot */
12384 /* If rad/sample, Fs is empty */
12385 emxFree_creal_T(&y);
12386 emxFree_creal_T(&s);
12387 }
12388
12389 /*
12390 * Arguments : const double o[15]
12391 * double u[15]
12392 * Return Type : void
12393 */
us(const double o[15],double u[15])12394 static void us(const double o[15], double u[15])
12395 {
12396 int ix;
12397 int iy;
12398 int k;
12399 memset(&u[0], 0, 15U * sizeof(double));
12400 ix = 0;
12401 iy = 0;
12402 for (k = 0; k < 15; k++) {
12403 u[iy] = o[ix];
12404 ix++;
12405 iy++;
12406 }
12407 }
12408
12409 /*
12410 * Arguments : const emxArray_creal_T *x
12411 * emxArray_creal_T *c
12412 * Return Type : void
12413 */
vector_poly(const emxArray_creal_T * x,emxArray_creal_T * c)12414 static void vector_poly(const emxArray_creal_T *x, emxArray_creal_T *c)
12415 {
12416 int n;
12417 int unnamed_idx_1;
12418 int k;
12419 double x_re;
12420 double x_im;
12421 double c_re;
12422 double c_im;
12423 n = x->size[0];
12424 unnamed_idx_1 = x->size[0] + 1;
12425 k = c->size[0] * c->size[1];
12426 c->size[0] = 1;
12427 c->size[1] = unnamed_idx_1;
12428 emxEnsureCapacity_creal_T(c, k);
12429 c->data[0].re = 1.0;
12430 c->data[0].im = 0.0;
12431 for (unnamed_idx_1 = 0; unnamed_idx_1 + 1 <= n; unnamed_idx_1++) {
12432 x_re = -x->data[unnamed_idx_1].re;
12433 x_im = -x->data[unnamed_idx_1].im;
12434 c_re = c->data[unnamed_idx_1].re;
12435 c_im = c->data[unnamed_idx_1].im;
12436 c->data[unnamed_idx_1 + 1].re = x_re * c_re - x_im * c_im;
12437 c->data[unnamed_idx_1 + 1].im = x_re * c_im + x_im * c_re;
12438 for (k = unnamed_idx_1; k + 1 > 1; k--) {
12439 x_re = x->data[unnamed_idx_1].re * c->data[k - 1].re - x->
12440 data[unnamed_idx_1].im * c->data[k - 1].im;
12441 x_im = x->data[unnamed_idx_1].re * c->data[k - 1].im + x->
12442 data[unnamed_idx_1].im * c->data[k - 1].re;
12443 c->data[k].re -= x_re;
12444 c->data[k].im -= x_im;
12445 }
12446 }
12447 }
12448
12449 /*
12450 * Arguments : double x1
12451 * double x2
12452 * double x3
12453 * Return Type : double
12454 */
xdlapy3(double x1,double x2,double x3)12455 static double xdlapy3(double x1, double x2, double x3)
12456 {
12457 double y;
12458 double a;
12459 double b;
12460 double c;
12461 a = fabs(x1);
12462 b = fabs(x2);
12463 c = fabs(x3);
12464 if ((a > b) || rtIsNaN(b)) {
12465 y = a;
12466 } else {
12467 y = b;
12468 }
12469
12470 if (c > y) {
12471 y = c;
12472 }
12473
12474 if ((y > 0.0) && (!rtIsInf(y))) {
12475 a /= y;
12476 b /= y;
12477 c /= y;
12478 y *= sqrt((a * a + c * c) + b * b);
12479 } else {
12480 y = (a + b) + c;
12481 }
12482
12483 return y;
12484 }
12485
12486 /*
12487 * Arguments : emxArray_creal_T *a
12488 * Return Type : void
12489 */
xgehrd(emxArray_creal_T * a)12490 static void xgehrd(emxArray_creal_T *a)
12491 {
12492 int n;
12493 int ntau;
12494 emxArray_creal_T *tau;
12495 emxArray_creal_T *work;
12496 int i59;
12497 int i;
12498 int im1n;
12499 int in;
12500 creal_T alpha1;
12501 int c;
12502 double alpha1_re;
12503 double alpha1_im;
12504 double xnorm;
12505 double beta1;
12506 int jy;
12507 int knt;
12508 boolean_T b_tau;
12509 double ai;
12510 int lastv;
12511 int lastc;
12512 int k;
12513 boolean_T exitg1;
12514 creal_T b_c;
12515 int ix;
12516 int exitg2;
12517 n = a->size[0];
12518 if (a->size[0] < 1) {
12519 ntau = 0;
12520 } else {
12521 ntau = a->size[0] - 1;
12522 }
12523
12524 emxInit_creal_T1(&tau, 1);
12525 emxInit_creal_T1(&work, 1);
12526 i59 = tau->size[0];
12527 tau->size[0] = ntau;
12528 emxEnsureCapacity_creal_T1(tau, i59);
12529 ntau = a->size[0];
12530 i59 = work->size[0];
12531 work->size[0] = ntau;
12532 emxEnsureCapacity_creal_T1(work, i59);
12533 for (i59 = 0; i59 < ntau; i59++) {
12534 work->data[i59].re = 0.0;
12535 work->data[i59].im = 0.0;
12536 }
12537
12538 for (i = 0; i + 1 < n; i++) {
12539 im1n = i * n + 2;
12540 in = (i + 1) * n;
12541 alpha1 = a->data[(i + a->size[0] * i) + 1];
12542 ntau = i + 3;
12543 if (!(ntau < n)) {
12544 ntau = n;
12545 }
12546
12547 ntau += i * n;
12548 c = (n - i) - 2;
12549 alpha1_re = 0.0;
12550 alpha1_im = 0.0;
12551 if (!(c + 1 <= 0)) {
12552 xnorm = xnrm2(c, a, ntau);
12553 if ((xnorm != 0.0) || (a->data[(i + a->size[0] * i) + 1].im != 0.0)) {
12554 beta1 = xdlapy3(a->data[(i + a->size[0] * i) + 1].re, a->data[(i +
12555 a->size[0] * i) + 1].im, xnorm);
12556 if (a->data[(i + a->size[0] * i) + 1].re >= 0.0) {
12557 beta1 = -beta1;
12558 }
12559
12560 if (fabs(beta1) < 1.0020841800044864E-292) {
12561 knt = 0;
12562 i59 = (ntau + c) - 1;
12563 do {
12564 knt++;
12565 for (k = ntau; k <= i59; k++) {
12566 xnorm = a->data[k - 1].re;
12567 ai = a->data[k - 1].im;
12568 a->data[k - 1].re = 9.9792015476736E+291 * xnorm - 0.0 * ai;
12569 a->data[k - 1].im = 9.9792015476736E+291 * ai + 0.0 * xnorm;
12570 }
12571
12572 beta1 *= 9.9792015476736E+291;
12573 alpha1.re *= 9.9792015476736E+291;
12574 alpha1.im *= 9.9792015476736E+291;
12575 } while (!(fabs(beta1) >= 1.0020841800044864E-292));
12576
12577 beta1 = xdlapy3(alpha1.re, alpha1.im, xnrm2(c, a, ntau));
12578 if (alpha1.re >= 0.0) {
12579 beta1 = -beta1;
12580 }
12581
12582 xnorm = beta1 - alpha1.re;
12583 if (0.0 - alpha1.im == 0.0) {
12584 alpha1_re = xnorm / beta1;
12585 alpha1_im = 0.0;
12586 } else if (xnorm == 0.0) {
12587 alpha1_re = 0.0;
12588 alpha1_im = (0.0 - alpha1.im) / beta1;
12589 } else {
12590 alpha1_re = xnorm / beta1;
12591 alpha1_im = (0.0 - alpha1.im) / beta1;
12592 }
12593
12594 b_c.re = alpha1.re - beta1;
12595 b_c.im = alpha1.im;
12596 xscal(c, recip(b_c), a, ntau);
12597 for (k = 1; k <= knt; k++) {
12598 beta1 *= 1.0020841800044864E-292;
12599 }
12600
12601 alpha1.re = beta1;
12602 alpha1.im = 0.0;
12603 } else {
12604 xnorm = beta1 - a->data[(i + a->size[0] * i) + 1].re;
12605 ai = 0.0 - a->data[(i + a->size[0] * i) + 1].im;
12606 if (ai == 0.0) {
12607 alpha1_re = xnorm / beta1;
12608 alpha1_im = 0.0;
12609 } else if (xnorm == 0.0) {
12610 alpha1_re = 0.0;
12611 alpha1_im = ai / beta1;
12612 } else {
12613 alpha1_re = xnorm / beta1;
12614 alpha1_im = ai / beta1;
12615 }
12616
12617 b_c.re = a->data[(i + a->size[0] * i) + 1].re - beta1;
12618 b_c.im = a->data[(i + a->size[0] * i) + 1].im;
12619 xscal(c, recip(b_c), a, ntau);
12620 alpha1.re = beta1;
12621 alpha1.im = 0.0;
12622 }
12623 }
12624 }
12625
12626 tau->data[i].re = alpha1_re;
12627 tau->data[i].im = alpha1_im;
12628 a->data[(i + a->size[0] * i) + 1].re = 1.0;
12629 a->data[(i + a->size[0] * i) + 1].im = 0.0;
12630 c = (n - i) - 3;
12631 jy = (i + im1n) - 1;
12632 b_tau = ((tau->data[i].re != 0.0) || (tau->data[i].im != 0.0));
12633 if (b_tau) {
12634 lastv = c + 2;
12635 ntau = jy + c;
12636 exitg1 = false;
12637 while ((!exitg1) && (lastv > 0)) {
12638 b_tau = ((a->data[ntau + 1].re == 0.0) && (a->data[ntau + 1].im == 0.0));
12639 if (b_tau) {
12640 lastv--;
12641 ntau--;
12642 } else {
12643 exitg1 = true;
12644 }
12645 }
12646
12647 lastc = n;
12648 exitg1 = false;
12649 while ((!exitg1) && (lastc > 0)) {
12650 ntau = in + lastc;
12651 c = ntau;
12652 do {
12653 exitg2 = 0;
12654 if ((n > 0) && (c <= ntau + (lastv - 1) * n)) {
12655 b_tau = ((a->data[c - 1].re != 0.0) || (a->data[c - 1].im != 0.0));
12656 if (b_tau) {
12657 exitg2 = 1;
12658 } else {
12659 c += n;
12660 }
12661 } else {
12662 lastc--;
12663 exitg2 = 2;
12664 }
12665 } while (exitg2 == 0);
12666
12667 if (exitg2 == 1) {
12668 exitg1 = true;
12669 }
12670 }
12671 } else {
12672 lastv = 0;
12673 lastc = 0;
12674 }
12675
12676 if (lastv > 0) {
12677 if (lastc != 0) {
12678 for (ntau = 1; ntau <= lastc; ntau++) {
12679 work->data[ntau - 1].re = 0.0;
12680 work->data[ntau - 1].im = 0.0;
12681 }
12682
12683 ix = jy;
12684 i59 = (in + n * (lastv - 1)) + 1;
12685 knt = in + 1;
12686 while ((n > 0) && (knt <= i59)) {
12687 b_c.re = a->data[ix].re - 0.0 * a->data[ix].im;
12688 b_c.im = a->data[ix].im + 0.0 * a->data[ix].re;
12689 ntau = 0;
12690 k = (knt + lastc) - 1;
12691 for (c = knt; c <= k; c++) {
12692 xnorm = a->data[c - 1].re * b_c.re - a->data[c - 1].im * b_c.im;
12693 ai = a->data[c - 1].re * b_c.im + a->data[c - 1].im * b_c.re;
12694 work->data[ntau].re += xnorm;
12695 work->data[ntau].im += ai;
12696 ntau++;
12697 }
12698
12699 ix++;
12700 knt += n;
12701 }
12702 }
12703
12704 alpha1_re = -tau->data[i].re;
12705 alpha1_im = -tau->data[i].im;
12706 if (!((alpha1_re == 0.0) && (alpha1_im == 0.0))) {
12707 ntau = in;
12708 for (knt = 1; knt <= lastv; knt++) {
12709 b_tau = ((a->data[jy].re != 0.0) || (a->data[jy].im != 0.0));
12710 if (b_tau) {
12711 b_c.re = a->data[jy].re * alpha1_re + a->data[jy].im * alpha1_im;
12712 b_c.im = a->data[jy].re * alpha1_im - a->data[jy].im * alpha1_re;
12713 ix = 0;
12714 i59 = lastc + ntau;
12715 for (k = ntau; k + 1 <= i59; k++) {
12716 xnorm = work->data[ix].re * b_c.re - work->data[ix].im * b_c.im;
12717 ai = work->data[ix].re * b_c.im + work->data[ix].im * b_c.re;
12718 a->data[k].re += xnorm;
12719 a->data[k].im += ai;
12720 ix++;
12721 }
12722 }
12723
12724 jy++;
12725 ntau += n;
12726 }
12727 }
12728 }
12729
12730 c = (n - i) - 3;
12731 im1n = (i + im1n) - 1;
12732 jy = (i + in) + 2;
12733 alpha1_re = tau->data[i].re;
12734 alpha1_im = -tau->data[i].im;
12735 if ((alpha1_re != 0.0) || (alpha1_im != 0.0)) {
12736 lastv = c + 2;
12737 ntau = im1n + c;
12738 exitg1 = false;
12739 while ((!exitg1) && (lastv > 0)) {
12740 b_tau = ((a->data[ntau + 1].re == 0.0) && (a->data[ntau + 1].im == 0.0));
12741 if (b_tau) {
12742 lastv--;
12743 ntau--;
12744 } else {
12745 exitg1 = true;
12746 }
12747 }
12748
12749 lastc = (n - i) - 1;
12750 exitg1 = false;
12751 while ((!exitg1) && (lastc > 0)) {
12752 ntau = jy + (lastc - 1) * n;
12753 c = ntau;
12754 do {
12755 exitg2 = 0;
12756 if (c <= (ntau + lastv) - 1) {
12757 b_tau = ((a->data[c - 1].re != 0.0) || (a->data[c - 1].im != 0.0));
12758 if (b_tau) {
12759 exitg2 = 1;
12760 } else {
12761 c++;
12762 }
12763 } else {
12764 lastc--;
12765 exitg2 = 2;
12766 }
12767 } while (exitg2 == 0);
12768
12769 if (exitg2 == 1) {
12770 exitg1 = true;
12771 }
12772 }
12773 } else {
12774 lastv = 0;
12775 lastc = 0;
12776 }
12777
12778 if (lastv > 0) {
12779 if (lastc != 0) {
12780 for (ntau = 1; ntau <= lastc; ntau++) {
12781 work->data[ntau - 1].re = 0.0;
12782 work->data[ntau - 1].im = 0.0;
12783 }
12784
12785 ntau = 0;
12786 i59 = jy + n * (lastc - 1);
12787 knt = jy;
12788 while ((n > 0) && (knt <= i59)) {
12789 ix = im1n;
12790 b_c.re = 0.0;
12791 b_c.im = 0.0;
12792 k = (knt + lastv) - 1;
12793 for (c = knt - 1; c + 1 <= k; c++) {
12794 b_c.re += a->data[c].re * a->data[ix].re + a->data[c].im * a->
12795 data[ix].im;
12796 b_c.im += a->data[c].re * a->data[ix].im - a->data[c].im * a->
12797 data[ix].re;
12798 ix++;
12799 }
12800
12801 work->data[ntau].re += b_c.re - 0.0 * b_c.im;
12802 work->data[ntau].im += b_c.im + 0.0 * b_c.re;
12803 ntau++;
12804 knt += n;
12805 }
12806 }
12807
12808 alpha1_re = -alpha1_re;
12809 alpha1_im = -alpha1_im;
12810 if (!((alpha1_re == 0.0) && (alpha1_im == 0.0))) {
12811 ntau = jy - 1;
12812 jy = 0;
12813 for (knt = 1; knt <= lastc; knt++) {
12814 b_tau = ((work->data[jy].re != 0.0) || (work->data[jy].im != 0.0));
12815 if (b_tau) {
12816 b_c.re = work->data[jy].re * alpha1_re + work->data[jy].im *
12817 alpha1_im;
12818 b_c.im = work->data[jy].re * alpha1_im - work->data[jy].im *
12819 alpha1_re;
12820 ix = im1n;
12821 i59 = lastv + ntau;
12822 for (k = ntau; k + 1 <= i59; k++) {
12823 xnorm = a->data[ix].re * b_c.re - a->data[ix].im * b_c.im;
12824 ai = a->data[ix].re * b_c.im + a->data[ix].im * b_c.re;
12825 a->data[k].re += xnorm;
12826 a->data[k].im += ai;
12827 ix++;
12828 }
12829 }
12830
12831 jy++;
12832 ntau += n;
12833 }
12834 }
12835 }
12836
12837 a->data[(i + a->size[0] * i) + 1] = alpha1;
12838 }
12839
12840 emxFree_creal_T(&work);
12841 emxFree_creal_T(&tau);
12842 }
12843
12844 /*
12845 * Arguments : int n
12846 * const emxArray_creal_T *x
12847 * int ix0
12848 * Return Type : double
12849 */
xnrm2(int n,const emxArray_creal_T * x,int ix0)12850 static double xnrm2(int n, const emxArray_creal_T *x, int ix0)
12851 {
12852 double y;
12853 double scale;
12854 int kend;
12855 int k;
12856 double absxk;
12857 double t;
12858 y = 0.0;
12859 if (!(n < 1)) {
12860 if (n == 1) {
12861 y = rt_hypotd_snf(x->data[ix0 - 1].re, x->data[ix0 - 1].im);
12862 } else {
12863 scale = 3.3121686421112381E-170;
12864 kend = (ix0 + n) - 1;
12865 for (k = ix0; k <= kend; k++) {
12866 absxk = fabs(x->data[k - 1].re);
12867 if (absxk > scale) {
12868 t = scale / absxk;
12869 y = 1.0 + y * t * t;
12870 scale = absxk;
12871 } else {
12872 t = absxk / scale;
12873 y += t * t;
12874 }
12875
12876 absxk = fabs(x->data[k - 1].im);
12877 if (absxk > scale) {
12878 t = scale / absxk;
12879 y = 1.0 + y * t * t;
12880 scale = absxk;
12881 } else {
12882 t = absxk / scale;
12883 y += t * t;
12884 }
12885 }
12886
12887 y = scale * sqrt(y);
12888 }
12889 }
12890
12891 return y;
12892 }
12893
12894 /*
12895 * Arguments : int n
12896 * const creal_T a
12897 * emxArray_creal_T *x
12898 * int ix0
12899 * Return Type : void
12900 */
xscal(int n,const creal_T a,emxArray_creal_T * x,int ix0)12901 static void xscal(int n, const creal_T a, emxArray_creal_T *x, int ix0)
12902 {
12903 int i60;
12904 int k;
12905 double x_re;
12906 double x_im;
12907 i60 = (ix0 + n) - 1;
12908 for (k = ix0; k <= i60; k++) {
12909 x_re = x->data[k - 1].re;
12910 x_im = x->data[k - 1].im;
12911 x->data[k - 1].re = a.re * x_re - a.im * x_im;
12912 x->data[k - 1].im = a.re * x_im + a.im * x_re;
12913 }
12914 }
12915
12916 /*
12917 * Arguments : const emxArray_creal_T *A
12918 * int *info
12919 * emxArray_creal_T *alpha1
12920 * emxArray_creal_T *beta1
12921 * Return Type : void
12922 */
xzgeev(const emxArray_creal_T * A,int * info,emxArray_creal_T * alpha1,emxArray_creal_T * beta1)12923 static void xzgeev(const emxArray_creal_T *A, int *info, emxArray_creal_T
12924 *alpha1, emxArray_creal_T *beta1)
12925 {
12926 emxArray_creal_T *At;
12927 int jcol;
12928 int ii;
12929 int nzcount;
12930 double anrm;
12931 boolean_T exitg1;
12932 double absxk;
12933 boolean_T ilascl;
12934 double anrmto;
12935 int ilo;
12936 double ctoc;
12937 int ihi;
12938 boolean_T notdone;
12939 int exitg3;
12940 double cfrom1;
12941 int i;
12942 double cto1;
12943 int j;
12944 double mul;
12945 creal_T b_At;
12946 creal_T c_At;
12947 double c;
12948 creal_T atmp;
12949 boolean_T exitg4;
12950 int exitg2;
12951 boolean_T d_At;
12952 double stemp_re;
12953 emxInit_creal_T(&At, 2);
12954 jcol = At->size[0] * At->size[1];
12955 At->size[0] = A->size[0];
12956 At->size[1] = A->size[1];
12957 emxEnsureCapacity_creal_T(At, jcol);
12958 ii = A->size[0] * A->size[1];
12959 for (jcol = 0; jcol < ii; jcol++) {
12960 At->data[jcol] = A->data[jcol];
12961 }
12962
12963 nzcount = 0;
12964 jcol = alpha1->size[0];
12965 alpha1->size[0] = At->size[0];
12966 emxEnsureCapacity_creal_T1(alpha1, jcol);
12967 ii = At->size[0];
12968 for (jcol = 0; jcol < ii; jcol++) {
12969 alpha1->data[jcol].re = 0.0;
12970 alpha1->data[jcol].im = 0.0;
12971 }
12972
12973 jcol = beta1->size[0];
12974 beta1->size[0] = At->size[0];
12975 emxEnsureCapacity_creal_T1(beta1, jcol);
12976 ii = At->size[0];
12977 for (jcol = 0; jcol < ii; jcol++) {
12978 beta1->data[jcol].re = 0.0;
12979 beta1->data[jcol].im = 0.0;
12980 }
12981
12982 if (!((At->size[0] == 0) || (At->size[1] == 0))) {
12983 anrm = 0.0;
12984 jcol = 0;
12985 exitg1 = false;
12986 while ((!exitg1) && (jcol <= At->size[0] * At->size[1] - 1)) {
12987 absxk = rt_hypotd_snf(At->data[jcol].re, At->data[jcol].im);
12988 if (rtIsNaN(absxk)) {
12989 anrm = rtNaN;
12990 exitg1 = true;
12991 } else {
12992 if (absxk > anrm) {
12993 anrm = absxk;
12994 }
12995
12996 jcol++;
12997 }
12998 }
12999
13000 if (!((!rtIsInf(anrm)) && (!rtIsNaN(anrm)))) {
13001 jcol = alpha1->size[0];
13002 alpha1->size[0] = At->size[0];
13003 emxEnsureCapacity_creal_T1(alpha1, jcol);
13004 ii = At->size[0];
13005 for (jcol = 0; jcol < ii; jcol++) {
13006 alpha1->data[jcol].re = rtNaN;
13007 alpha1->data[jcol].im = 0.0;
13008 }
13009
13010 jcol = beta1->size[0];
13011 beta1->size[0] = At->size[0];
13012 emxEnsureCapacity_creal_T1(beta1, jcol);
13013 ii = At->size[0];
13014 for (jcol = 0; jcol < ii; jcol++) {
13015 beta1->data[jcol].re = rtNaN;
13016 beta1->data[jcol].im = 0.0;
13017 }
13018 } else {
13019 ilascl = false;
13020 anrmto = anrm;
13021 if ((anrm > 0.0) && (anrm < 6.7178761075670888E-139)) {
13022 anrmto = 6.7178761075670888E-139;
13023 ilascl = true;
13024 } else {
13025 if (anrm > 1.4885657073574029E+138) {
13026 anrmto = 1.4885657073574029E+138;
13027 ilascl = true;
13028 }
13029 }
13030
13031 if (ilascl) {
13032 absxk = anrm;
13033 ctoc = anrmto;
13034 notdone = true;
13035 while (notdone) {
13036 cfrom1 = absxk * 2.0041683600089728E-292;
13037 cto1 = ctoc / 4.9896007738368E+291;
13038 if ((cfrom1 > ctoc) && (ctoc != 0.0)) {
13039 mul = 2.0041683600089728E-292;
13040 absxk = cfrom1;
13041 } else if (cto1 > absxk) {
13042 mul = 4.9896007738368E+291;
13043 ctoc = cto1;
13044 } else {
13045 mul = ctoc / absxk;
13046 notdone = false;
13047 }
13048
13049 jcol = At->size[0] * At->size[1];
13050 emxEnsureCapacity_creal_T(At, jcol);
13051 jcol = At->size[0];
13052 ii = At->size[1];
13053 ii *= jcol;
13054 for (jcol = 0; jcol < ii; jcol++) {
13055 At->data[jcol].re *= mul;
13056 At->data[jcol].im *= mul;
13057 }
13058 }
13059 }
13060
13061 ilo = 0;
13062 ihi = At->size[0];
13063 if (At->size[0] <= 1) {
13064 ihi = 1;
13065 } else {
13066 do {
13067 exitg3 = 0;
13068 i = 0;
13069 j = 0;
13070 notdone = false;
13071 ii = ihi;
13072 exitg1 = false;
13073 while ((!exitg1) && (ii > 0)) {
13074 nzcount = 0;
13075 i = ii;
13076 j = ihi;
13077 jcol = 1;
13078 exitg4 = false;
13079 while ((!exitg4) && (jcol <= ihi)) {
13080 d_At = ((At->data[(ii + At->size[0] * (jcol - 1)) - 1].re != 0.0) ||
13081 (At->data[(ii + At->size[0] * (jcol - 1)) - 1].im != 0.0));
13082 if (d_At || (ii == jcol)) {
13083 if (nzcount == 0) {
13084 j = jcol;
13085 nzcount = 1;
13086 jcol++;
13087 } else {
13088 nzcount = 2;
13089 exitg4 = true;
13090 }
13091 } else {
13092 jcol++;
13093 }
13094 }
13095
13096 if (nzcount < 2) {
13097 notdone = true;
13098 exitg1 = true;
13099 } else {
13100 ii--;
13101 }
13102 }
13103
13104 if (!notdone) {
13105 exitg3 = 2;
13106 } else {
13107 nzcount = At->size[0];
13108 if (i != ihi) {
13109 for (jcol = 0; jcol + 1 <= nzcount; jcol++) {
13110 atmp = At->data[(i + At->size[0] * jcol) - 1];
13111 At->data[(i + At->size[0] * jcol) - 1] = At->data[(ihi +
13112 At->size[0] * jcol) - 1];
13113 At->data[(ihi + At->size[0] * jcol) - 1] = atmp;
13114 }
13115 }
13116
13117 if (j != ihi) {
13118 for (jcol = 0; jcol + 1 <= ihi; jcol++) {
13119 atmp = At->data[jcol + At->size[0] * (j - 1)];
13120 At->data[jcol + At->size[0] * (j - 1)] = At->data[jcol +
13121 At->size[0] * (ihi - 1)];
13122 At->data[jcol + At->size[0] * (ihi - 1)] = atmp;
13123 }
13124 }
13125
13126 ihi--;
13127 if (ihi == 1) {
13128 exitg3 = 1;
13129 }
13130 }
13131 } while (exitg3 == 0);
13132
13133 if (exitg3 == 1) {
13134 } else {
13135 do {
13136 exitg2 = 0;
13137 i = 0;
13138 j = 0;
13139 notdone = false;
13140 jcol = ilo + 1;
13141 exitg1 = false;
13142 while ((!exitg1) && (jcol <= ihi)) {
13143 nzcount = 0;
13144 i = ihi;
13145 j = jcol;
13146 ii = ilo + 1;
13147 exitg4 = false;
13148 while ((!exitg4) && (ii <= ihi)) {
13149 d_At = ((At->data[(ii + At->size[0] * (jcol - 1)) - 1].re != 0.0)
13150 || (At->data[(ii + At->size[0] * (jcol - 1)) - 1].im !=
13151 0.0));
13152 if (d_At || (ii == jcol)) {
13153 if (nzcount == 0) {
13154 i = ii;
13155 nzcount = 1;
13156 ii++;
13157 } else {
13158 nzcount = 2;
13159 exitg4 = true;
13160 }
13161 } else {
13162 ii++;
13163 }
13164 }
13165
13166 if (nzcount < 2) {
13167 notdone = true;
13168 exitg1 = true;
13169 } else {
13170 jcol++;
13171 }
13172 }
13173
13174 if (!notdone) {
13175 exitg2 = 1;
13176 } else {
13177 nzcount = At->size[0];
13178 if (i != ilo + 1) {
13179 for (jcol = ilo; jcol + 1 <= nzcount; jcol++) {
13180 atmp = At->data[(i + At->size[0] * jcol) - 1];
13181 At->data[(i + At->size[0] * jcol) - 1] = At->data[ilo +
13182 At->size[0] * jcol];
13183 At->data[ilo + At->size[0] * jcol] = atmp;
13184 }
13185 }
13186
13187 if (j != ilo + 1) {
13188 for (jcol = 0; jcol + 1 <= ihi; jcol++) {
13189 atmp = At->data[jcol + At->size[0] * (j - 1)];
13190 At->data[jcol + At->size[0] * (j - 1)] = At->data[jcol +
13191 At->size[0] * ilo];
13192 At->data[jcol + At->size[0] * ilo] = atmp;
13193 }
13194 }
13195
13196 ilo++;
13197 if (ilo + 1 == ihi) {
13198 exitg2 = 1;
13199 }
13200 }
13201 } while (exitg2 == 0);
13202 }
13203 }
13204
13205 nzcount = At->size[0];
13206 if ((!(At->size[0] <= 1)) && (!(ihi < ilo + 3))) {
13207 for (jcol = ilo; jcol + 1 < ihi - 1; jcol++) {
13208 for (ii = ihi - 1; ii + 1 > jcol + 2; ii--) {
13209 b_At = At->data[(ii + At->size[0] * jcol) - 1];
13210 c_At = At->data[ii + At->size[0] * jcol];
13211 xzlartg(b_At, c_At, &c, &atmp, &At->data[(ii + At->size[0] * jcol) -
13212 1]);
13213 At->data[ii + At->size[0] * jcol].re = 0.0;
13214 At->data[ii + At->size[0] * jcol].im = 0.0;
13215 for (j = jcol + 1; j + 1 <= nzcount; j++) {
13216 absxk = atmp.re * At->data[ii + At->size[0] * j].re - atmp.im *
13217 At->data[ii + At->size[0] * j].im;
13218 ctoc = atmp.re * At->data[ii + At->size[0] * j].im + atmp.im *
13219 At->data[ii + At->size[0] * j].re;
13220 stemp_re = c * At->data[(ii + At->size[0] * j) - 1].re + absxk;
13221 absxk = c * At->data[(ii + At->size[0] * j) - 1].im + ctoc;
13222 ctoc = At->data[(ii + At->size[0] * j) - 1].re;
13223 cfrom1 = At->data[(ii + At->size[0] * j) - 1].im;
13224 cto1 = At->data[(ii + At->size[0] * j) - 1].im;
13225 mul = At->data[(ii + At->size[0] * j) - 1].re;
13226 At->data[ii + At->size[0] * j].re = c * At->data[ii + At->size[0] *
13227 j].re - (atmp.re * ctoc + atmp.im * cfrom1);
13228 At->data[ii + At->size[0] * j].im = c * At->data[ii + At->size[0] *
13229 j].im - (atmp.re * cto1 - atmp.im * mul);
13230 At->data[(ii + At->size[0] * j) - 1].re = stemp_re;
13231 At->data[(ii + At->size[0] * j) - 1].im = absxk;
13232 }
13233
13234 atmp.re = -atmp.re;
13235 atmp.im = -atmp.im;
13236 for (i = 0; i + 1 <= ihi; i++) {
13237 absxk = atmp.re * At->data[i + At->size[0] * (ii - 1)].re -
13238 atmp.im * At->data[i + At->size[0] * (ii - 1)].im;
13239 ctoc = atmp.re * At->data[i + At->size[0] * (ii - 1)].im + atmp.im
13240 * At->data[i + At->size[0] * (ii - 1)].re;
13241 stemp_re = c * At->data[i + At->size[0] * ii].re + absxk;
13242 absxk = c * At->data[i + At->size[0] * ii].im + ctoc;
13243 ctoc = At->data[i + At->size[0] * ii].re;
13244 cfrom1 = At->data[i + At->size[0] * ii].im;
13245 cto1 = At->data[i + At->size[0] * ii].im;
13246 mul = At->data[i + At->size[0] * ii].re;
13247 At->data[i + At->size[0] * (ii - 1)].re = c * At->data[i +
13248 At->size[0] * (ii - 1)].re - (atmp.re * ctoc + atmp.im * cfrom1);
13249 At->data[i + At->size[0] * (ii - 1)].im = c * At->data[i +
13250 At->size[0] * (ii - 1)].im - (atmp.re * cto1 - atmp.im * mul);
13251 At->data[i + At->size[0] * ii].re = stemp_re;
13252 At->data[i + At->size[0] * ii].im = absxk;
13253 }
13254 }
13255 }
13256 }
13257
13258 xzhgeqz(At, ilo + 1, ihi, &nzcount, alpha1, beta1);
13259 if ((nzcount == 0) && ilascl) {
13260 notdone = true;
13261 while (notdone) {
13262 cfrom1 = anrmto * 2.0041683600089728E-292;
13263 cto1 = anrm / 4.9896007738368E+291;
13264 if ((cfrom1 > anrm) && (anrm != 0.0)) {
13265 mul = 2.0041683600089728E-292;
13266 anrmto = cfrom1;
13267 } else if (cto1 > anrmto) {
13268 mul = 4.9896007738368E+291;
13269 anrm = cto1;
13270 } else {
13271 mul = anrm / anrmto;
13272 notdone = false;
13273 }
13274
13275 jcol = alpha1->size[0];
13276 emxEnsureCapacity_creal_T1(alpha1, jcol);
13277 ii = alpha1->size[0];
13278 for (jcol = 0; jcol < ii; jcol++) {
13279 alpha1->data[jcol].re *= mul;
13280 alpha1->data[jcol].im *= mul;
13281 }
13282 }
13283 }
13284 }
13285 }
13286
13287 emxFree_creal_T(&At);
13288 *info = nzcount;
13289 }
13290
13291 /*
13292 * Arguments : const emxArray_creal_T *A
13293 * int ilo
13294 * int ihi
13295 * int *info
13296 * emxArray_creal_T *alpha1
13297 * emxArray_creal_T *beta1
13298 * Return Type : void
13299 */
xzhgeqz(const emxArray_creal_T * A,int ilo,int ihi,int * info,emxArray_creal_T * alpha1,emxArray_creal_T * beta1)13300 static void xzhgeqz(const emxArray_creal_T *A, int ilo, int ihi, int *info,
13301 emxArray_creal_T *alpha1, emxArray_creal_T *beta1)
13302 {
13303 emxArray_creal_T *b_A;
13304 int jm1;
13305 int jp1;
13306 int b_info;
13307 double eshift_re;
13308 double eshift_im;
13309 creal_T ctemp;
13310 double anorm;
13311 double scale;
13312 double reAij;
13313 double sumsq;
13314 double b_atol;
13315 boolean_T firstNonZero;
13316 int j;
13317 double ascale;
13318 double bscale;
13319 int i;
13320 boolean_T failed;
13321 double imAij;
13322 boolean_T guard1 = false;
13323 boolean_T guard2 = false;
13324 int ifirst;
13325 int istart;
13326 double temp2;
13327 int ilast;
13328 int ilastm1;
13329 int ifrstm;
13330 int ilastm;
13331 int iiter;
13332 boolean_T goto60;
13333 boolean_T goto70;
13334 boolean_T goto90;
13335 int jiter;
13336 int exitg1;
13337 boolean_T exitg2;
13338 creal_T b_ascale;
13339 creal_T shift;
13340 creal_T c_A;
13341 double ad22_re;
13342 double ad22_im;
13343 double t1_im;
13344 emxInit_creal_T(&b_A, 2);
13345 jm1 = b_A->size[0] * b_A->size[1];
13346 b_A->size[0] = A->size[0];
13347 b_A->size[1] = A->size[1];
13348 emxEnsureCapacity_creal_T(b_A, jm1);
13349 jp1 = A->size[0] * A->size[1];
13350 for (jm1 = 0; jm1 < jp1; jm1++) {
13351 b_A->data[jm1] = A->data[jm1];
13352 }
13353
13354 b_info = -1;
13355 if ((A->size[0] == 1) && (A->size[1] == 1)) {
13356 ihi = 1;
13357 }
13358
13359 jm1 = alpha1->size[0];
13360 alpha1->size[0] = A->size[0];
13361 emxEnsureCapacity_creal_T1(alpha1, jm1);
13362 jp1 = A->size[0];
13363 for (jm1 = 0; jm1 < jp1; jm1++) {
13364 alpha1->data[jm1].re = 0.0;
13365 alpha1->data[jm1].im = 0.0;
13366 }
13367
13368 jm1 = beta1->size[0];
13369 beta1->size[0] = A->size[0];
13370 emxEnsureCapacity_creal_T1(beta1, jm1);
13371 jp1 = A->size[0];
13372 for (jm1 = 0; jm1 < jp1; jm1++) {
13373 beta1->data[jm1].re = 1.0;
13374 beta1->data[jm1].im = 0.0;
13375 }
13376
13377 eshift_re = 0.0;
13378 eshift_im = 0.0;
13379 ctemp.re = 0.0;
13380 ctemp.im = 0.0;
13381 anorm = 0.0;
13382 if (!(ilo > ihi)) {
13383 scale = 0.0;
13384 sumsq = 0.0;
13385 firstNonZero = true;
13386 for (j = ilo; j <= ihi; j++) {
13387 jm1 = j + 1;
13388 if (ihi < j + 1) {
13389 jm1 = ihi;
13390 }
13391
13392 for (i = ilo; i <= jm1; i++) {
13393 reAij = A->data[(i + A->size[0] * (j - 1)) - 1].re;
13394 imAij = A->data[(i + A->size[0] * (j - 1)) - 1].im;
13395 if (reAij != 0.0) {
13396 anorm = fabs(reAij);
13397 if (firstNonZero) {
13398 sumsq = 1.0;
13399 scale = anorm;
13400 firstNonZero = false;
13401 } else if (scale < anorm) {
13402 temp2 = scale / anorm;
13403 sumsq = 1.0 + sumsq * temp2 * temp2;
13404 scale = anorm;
13405 } else {
13406 temp2 = anorm / scale;
13407 sumsq += temp2 * temp2;
13408 }
13409 }
13410
13411 if (imAij != 0.0) {
13412 anorm = fabs(imAij);
13413 if (firstNonZero) {
13414 sumsq = 1.0;
13415 scale = anorm;
13416 firstNonZero = false;
13417 } else if (scale < anorm) {
13418 temp2 = scale / anorm;
13419 sumsq = 1.0 + sumsq * temp2 * temp2;
13420 scale = anorm;
13421 } else {
13422 temp2 = anorm / scale;
13423 sumsq += temp2 * temp2;
13424 }
13425 }
13426 }
13427 }
13428
13429 anorm = scale * sqrt(sumsq);
13430 }
13431
13432 reAij = 2.2204460492503131E-16 * anorm;
13433 b_atol = 2.2250738585072014E-308;
13434 if (reAij > 2.2250738585072014E-308) {
13435 b_atol = reAij;
13436 }
13437
13438 reAij = 2.2250738585072014E-308;
13439 if (anorm > 2.2250738585072014E-308) {
13440 reAij = anorm;
13441 }
13442
13443 ascale = 1.0 / reAij;
13444 bscale = 1.0 / sqrt(A->size[0]);
13445 failed = true;
13446 for (j = ihi; j + 1 <= A->size[0]; j++) {
13447 alpha1->data[j] = A->data[j + A->size[0] * j];
13448 }
13449
13450 guard1 = false;
13451 guard2 = false;
13452 if (ihi >= ilo) {
13453 ifirst = ilo;
13454 istart = ilo;
13455 ilast = ihi - 1;
13456 ilastm1 = ihi - 2;
13457 ifrstm = ilo;
13458 ilastm = ihi;
13459 iiter = 0;
13460 goto60 = false;
13461 goto70 = false;
13462 goto90 = false;
13463 jiter = 1;
13464 do {
13465 exitg1 = 0;
13466 if (jiter <= 30 * ((ihi - ilo) + 1)) {
13467 if (ilast + 1 == ilo) {
13468 goto60 = true;
13469 } else if (fabs(b_A->data[ilast + b_A->size[0] * ilastm1].re) + fabs
13470 (b_A->data[ilast + b_A->size[0] * ilastm1].im) <= b_atol) {
13471 b_A->data[ilast + b_A->size[0] * ilastm1].re = 0.0;
13472 b_A->data[ilast + b_A->size[0] * ilastm1].im = 0.0;
13473 goto60 = true;
13474 } else {
13475 j = ilastm1;
13476 exitg2 = false;
13477 while ((!exitg2) && (j + 1 >= ilo)) {
13478 if (j + 1 == ilo) {
13479 firstNonZero = true;
13480 } else if (fabs(b_A->data[j + b_A->size[0] * (j - 1)].re) + fabs
13481 (b_A->data[j + b_A->size[0] * (j - 1)].im) <= b_atol) {
13482 b_A->data[j + b_A->size[0] * (j - 1)].re = 0.0;
13483 b_A->data[j + b_A->size[0] * (j - 1)].im = 0.0;
13484 firstNonZero = true;
13485 } else {
13486 firstNonZero = false;
13487 }
13488
13489 if (firstNonZero) {
13490 ifirst = j + 1;
13491 goto70 = true;
13492 exitg2 = true;
13493 } else {
13494 j--;
13495 }
13496 }
13497 }
13498
13499 if (goto60 || goto70) {
13500 firstNonZero = true;
13501 } else {
13502 firstNonZero = false;
13503 }
13504
13505 if (!firstNonZero) {
13506 jp1 = alpha1->size[0];
13507 jm1 = alpha1->size[0];
13508 alpha1->size[0] = jp1;
13509 emxEnsureCapacity_creal_T1(alpha1, jm1);
13510 for (jm1 = 0; jm1 < jp1; jm1++) {
13511 alpha1->data[jm1].re = rtNaN;
13512 alpha1->data[jm1].im = 0.0;
13513 }
13514
13515 jp1 = beta1->size[0];
13516 jm1 = beta1->size[0];
13517 beta1->size[0] = jp1;
13518 emxEnsureCapacity_creal_T1(beta1, jm1);
13519 for (jm1 = 0; jm1 < jp1; jm1++) {
13520 beta1->data[jm1].re = rtNaN;
13521 beta1->data[jm1].im = 0.0;
13522 }
13523
13524 b_info = 0;
13525 exitg1 = 1;
13526 } else if (goto60) {
13527 goto60 = false;
13528 alpha1->data[ilast] = b_A->data[ilast + b_A->size[0] * ilast];
13529 ilast = ilastm1;
13530 ilastm1--;
13531 if (ilast + 1 < ilo) {
13532 failed = false;
13533 guard2 = true;
13534 exitg1 = 1;
13535 } else {
13536 iiter = 0;
13537 eshift_re = 0.0;
13538 eshift_im = 0.0;
13539 ilastm = ilast + 1;
13540 if (ifrstm > ilast + 1) {
13541 ifrstm = ilo;
13542 }
13543
13544 jiter++;
13545 }
13546 } else {
13547 if (goto70) {
13548 goto70 = false;
13549 iiter++;
13550 ifrstm = ifirst;
13551 if (iiter - iiter / 10 * 10 != 0) {
13552 anorm = ascale * b_A->data[ilastm1 + b_A->size[0] * ilastm1].re;
13553 reAij = ascale * b_A->data[ilastm1 + b_A->size[0] * ilastm1].im;
13554 if (reAij == 0.0) {
13555 shift.re = anorm / bscale;
13556 shift.im = 0.0;
13557 } else if (anorm == 0.0) {
13558 shift.re = 0.0;
13559 shift.im = reAij / bscale;
13560 } else {
13561 shift.re = anorm / bscale;
13562 shift.im = reAij / bscale;
13563 }
13564
13565 anorm = ascale * b_A->data[ilast + b_A->size[0] * ilast].re;
13566 reAij = ascale * b_A->data[ilast + b_A->size[0] * ilast].im;
13567 if (reAij == 0.0) {
13568 ad22_re = anorm / bscale;
13569 ad22_im = 0.0;
13570 } else if (anorm == 0.0) {
13571 ad22_re = 0.0;
13572 ad22_im = reAij / bscale;
13573 } else {
13574 ad22_re = anorm / bscale;
13575 ad22_im = reAij / bscale;
13576 }
13577
13578 temp2 = 0.5 * (shift.re + ad22_re);
13579 t1_im = 0.5 * (shift.im + ad22_im);
13580 anorm = ascale * b_A->data[ilastm1 + b_A->size[0] * ilast].re;
13581 reAij = ascale * b_A->data[ilastm1 + b_A->size[0] * ilast].im;
13582 if (reAij == 0.0) {
13583 sumsq = anorm / bscale;
13584 imAij = 0.0;
13585 } else if (anorm == 0.0) {
13586 sumsq = 0.0;
13587 imAij = reAij / bscale;
13588 } else {
13589 sumsq = anorm / bscale;
13590 imAij = reAij / bscale;
13591 }
13592
13593 anorm = ascale * b_A->data[ilast + b_A->size[0] * ilastm1].re;
13594 reAij = ascale * b_A->data[ilast + b_A->size[0] * ilastm1].im;
13595 if (reAij == 0.0) {
13596 scale = anorm / bscale;
13597 anorm = 0.0;
13598 } else if (anorm == 0.0) {
13599 scale = 0.0;
13600 anorm = reAij / bscale;
13601 } else {
13602 scale = anorm / bscale;
13603 anorm = reAij / bscale;
13604 }
13605
13606 reAij = shift.re * ad22_im + shift.im * ad22_re;
13607 shift.re = ((temp2 * temp2 - t1_im * t1_im) + (sumsq * scale -
13608 imAij * anorm)) - (shift.re * ad22_re - shift.im * ad22_im);
13609 shift.im = ((temp2 * t1_im + t1_im * temp2) + (sumsq * anorm +
13610 imAij * scale)) - reAij;
13611 c_sqrt(&shift);
13612 if ((temp2 - ad22_re) * shift.re + (t1_im - ad22_im) * shift.im <=
13613 0.0) {
13614 shift.re += temp2;
13615 shift.im += t1_im;
13616 } else {
13617 shift.re = temp2 - shift.re;
13618 shift.im = t1_im - shift.im;
13619 }
13620 } else {
13621 anorm = ascale * b_A->data[ilast + b_A->size[0] * ilastm1].re;
13622 reAij = ascale * b_A->data[ilast + b_A->size[0] * ilastm1].im;
13623 if (reAij == 0.0) {
13624 sumsq = anorm / bscale;
13625 imAij = 0.0;
13626 } else if (anorm == 0.0) {
13627 sumsq = 0.0;
13628 imAij = reAij / bscale;
13629 } else {
13630 sumsq = anorm / bscale;
13631 imAij = reAij / bscale;
13632 }
13633
13634 eshift_re += sumsq;
13635 eshift_im += imAij;
13636 shift.re = eshift_re;
13637 shift.im = eshift_im;
13638 }
13639
13640 j = ilastm1;
13641 jp1 = ilastm1 + 1;
13642 exitg2 = false;
13643 while ((!exitg2) && (j + 1 > ifirst)) {
13644 istart = j + 1;
13645 ctemp.re = ascale * b_A->data[j + b_A->size[0] * j].re - shift.re *
13646 bscale;
13647 ctemp.im = ascale * b_A->data[j + b_A->size[0] * j].im - shift.im *
13648 bscale;
13649 anorm = fabs(ctemp.re) + fabs(ctemp.im);
13650 temp2 = ascale * (fabs(b_A->data[jp1 + b_A->size[0] * j].re) +
13651 fabs(b_A->data[jp1 + b_A->size[0] * j].im));
13652 reAij = anorm;
13653 if (temp2 > anorm) {
13654 reAij = temp2;
13655 }
13656
13657 if ((reAij < 1.0) && (reAij != 0.0)) {
13658 anorm /= reAij;
13659 temp2 /= reAij;
13660 }
13661
13662 if ((fabs(b_A->data[j + b_A->size[0] * (j - 1)].re) + fabs
13663 (b_A->data[j + b_A->size[0] * (j - 1)].im)) * temp2 <= anorm *
13664 b_atol) {
13665 goto90 = true;
13666 exitg2 = true;
13667 } else {
13668 jp1 = j;
13669 j--;
13670 }
13671 }
13672
13673 if (!goto90) {
13674 istart = ifirst;
13675 ctemp.re = ascale * b_A->data[(ifirst + b_A->size[0] * (ifirst - 1))
13676 - 1].re - shift.re * bscale;
13677 ctemp.im = ascale * b_A->data[(ifirst + b_A->size[0] * (ifirst - 1))
13678 - 1].im - shift.im * bscale;
13679 goto90 = true;
13680 }
13681 }
13682
13683 if (goto90) {
13684 goto90 = false;
13685 b_ascale.re = ascale * b_A->data[istart + b_A->size[0] * (istart - 1)]
13686 .re;
13687 b_ascale.im = ascale * b_A->data[istart + b_A->size[0] * (istart - 1)]
13688 .im;
13689 b_xzlartg(ctemp, b_ascale, &imAij, &shift);
13690 j = istart;
13691 jm1 = istart - 2;
13692 while (j < ilast + 1) {
13693 if (j > istart) {
13694 b_ascale = b_A->data[(j + b_A->size[0] * jm1) - 1];
13695 c_A = b_A->data[j + b_A->size[0] * jm1];
13696 xzlartg(b_ascale, c_A, &imAij, &shift, &b_A->data[(j + b_A->
13697 size[0] * jm1) - 1]);
13698 b_A->data[j + b_A->size[0] * jm1].re = 0.0;
13699 b_A->data[j + b_A->size[0] * jm1].im = 0.0;
13700 }
13701
13702 for (jp1 = j - 1; jp1 + 1 <= ilastm; jp1++) {
13703 anorm = shift.re * b_A->data[j + b_A->size[0] * jp1].re -
13704 shift.im * b_A->data[j + b_A->size[0] * jp1].im;
13705 reAij = shift.re * b_A->data[j + b_A->size[0] * jp1].im +
13706 shift.im * b_A->data[j + b_A->size[0] * jp1].re;
13707 ad22_re = imAij * b_A->data[(j + b_A->size[0] * jp1) - 1].re +
13708 anorm;
13709 ad22_im = imAij * b_A->data[(j + b_A->size[0] * jp1) - 1].im +
13710 reAij;
13711 anorm = b_A->data[(j + b_A->size[0] * jp1) - 1].re;
13712 reAij = b_A->data[(j + b_A->size[0] * jp1) - 1].im;
13713 scale = b_A->data[(j + b_A->size[0] * jp1) - 1].im;
13714 sumsq = b_A->data[(j + b_A->size[0] * jp1) - 1].re;
13715 b_A->data[j + b_A->size[0] * jp1].re = imAij * b_A->data[j +
13716 b_A->size[0] * jp1].re - (shift.re * anorm + shift.im * reAij);
13717 b_A->data[j + b_A->size[0] * jp1].im = imAij * b_A->data[j +
13718 b_A->size[0] * jp1].im - (shift.re * scale - shift.im * sumsq);
13719 b_A->data[(j + b_A->size[0] * jp1) - 1].re = ad22_re;
13720 b_A->data[(j + b_A->size[0] * jp1) - 1].im = ad22_im;
13721 }
13722
13723 shift.re = -shift.re;
13724 shift.im = -shift.im;
13725 jp1 = j;
13726 if (ilast + 1 < j + 2) {
13727 jp1 = ilast - 1;
13728 }
13729
13730 for (i = ifrstm - 1; i + 1 <= jp1 + 2; i++) {
13731 anorm = shift.re * b_A->data[i + b_A->size[0] * (j - 1)].re -
13732 shift.im * b_A->data[i + b_A->size[0] * (j - 1)].im;
13733 reAij = shift.re * b_A->data[i + b_A->size[0] * (j - 1)].im +
13734 shift.im * b_A->data[i + b_A->size[0] * (j - 1)].re;
13735 ad22_re = imAij * b_A->data[i + b_A->size[0] * j].re + anorm;
13736 ad22_im = imAij * b_A->data[i + b_A->size[0] * j].im + reAij;
13737 anorm = b_A->data[i + b_A->size[0] * j].re;
13738 reAij = b_A->data[i + b_A->size[0] * j].im;
13739 scale = b_A->data[i + b_A->size[0] * j].im;
13740 sumsq = b_A->data[i + b_A->size[0] * j].re;
13741 b_A->data[i + b_A->size[0] * (j - 1)].re = imAij * b_A->data[i +
13742 b_A->size[0] * (j - 1)].re - (shift.re * anorm + shift.im *
13743 reAij);
13744 b_A->data[i + b_A->size[0] * (j - 1)].im = imAij * b_A->data[i +
13745 b_A->size[0] * (j - 1)].im - (shift.re * scale - shift.im *
13746 sumsq);
13747 b_A->data[i + b_A->size[0] * j].re = ad22_re;
13748 b_A->data[i + b_A->size[0] * j].im = ad22_im;
13749 }
13750
13751 jm1 = j - 1;
13752 j++;
13753 }
13754 }
13755
13756 jiter++;
13757 }
13758 } else {
13759 guard2 = true;
13760 exitg1 = 1;
13761 }
13762 } while (exitg1 == 0);
13763 } else {
13764 guard1 = true;
13765 }
13766
13767 if (guard2) {
13768 if (failed) {
13769 b_info = ilast;
13770 for (jp1 = 0; jp1 + 1 <= ilast + 1; jp1++) {
13771 alpha1->data[jp1].re = rtNaN;
13772 alpha1->data[jp1].im = 0.0;
13773 beta1->data[jp1].re = rtNaN;
13774 beta1->data[jp1].im = 0.0;
13775 }
13776 } else {
13777 guard1 = true;
13778 }
13779 }
13780
13781 if (guard1) {
13782 for (j = 0; j + 1 < ilo; j++) {
13783 alpha1->data[j] = b_A->data[j + b_A->size[0] * j];
13784 }
13785 }
13786
13787 emxFree_creal_T(&b_A);
13788 *info = b_info + 1;
13789 }
13790
13791 /*
13792 * Arguments : creal_T *alpha1
13793 * creal_T *x
13794 * Return Type : creal_T
13795 */
xzlarfg(creal_T * alpha1,creal_T * x)13796 static creal_T xzlarfg(creal_T *alpha1, creal_T *x)
13797 {
13798 creal_T tau;
13799 double xnorm;
13800 double beta1;
13801 int knt;
13802 double ai;
13803 creal_T b_alpha1;
13804 double x_re;
13805 double x_im;
13806 int k;
13807 tau.re = 0.0;
13808 tau.im = 0.0;
13809 xnorm = rt_hypotd_snf(x->re, x->im);
13810 if ((xnorm != 0.0) || (alpha1->im != 0.0)) {
13811 beta1 = xdlapy3(alpha1->re, alpha1->im, xnorm);
13812 if (alpha1->re >= 0.0) {
13813 beta1 = -beta1;
13814 }
13815
13816 if (fabs(beta1) < 1.0020841800044864E-292) {
13817 knt = 0;
13818 do {
13819 knt++;
13820 x->re *= 9.9792015476736E+291;
13821 x->im *= 9.9792015476736E+291;
13822 beta1 *= 9.9792015476736E+291;
13823 alpha1->re *= 9.9792015476736E+291;
13824 alpha1->im *= 9.9792015476736E+291;
13825 } while (!(fabs(beta1) >= 1.0020841800044864E-292));
13826
13827 beta1 = xdlapy3(alpha1->re, alpha1->im, rt_hypotd_snf(x->re, x->im));
13828 if (alpha1->re >= 0.0) {
13829 beta1 = -beta1;
13830 }
13831
13832 xnorm = beta1 - alpha1->re;
13833 ai = 0.0 - alpha1->im;
13834 if (ai == 0.0) {
13835 tau.re = xnorm / beta1;
13836 tau.im = 0.0;
13837 } else if (xnorm == 0.0) {
13838 tau.re = 0.0;
13839 tau.im = ai / beta1;
13840 } else {
13841 tau.re = xnorm / beta1;
13842 tau.im = ai / beta1;
13843 }
13844
13845 b_alpha1.re = alpha1->re - beta1;
13846 b_alpha1.im = alpha1->im;
13847 *alpha1 = recip(b_alpha1);
13848 xnorm = alpha1->re;
13849 ai = alpha1->im;
13850 x_re = x->re;
13851 x_im = x->im;
13852 x->re = xnorm * x_re - ai * x_im;
13853 x->im = xnorm * x_im + ai * x_re;
13854 for (k = 1; k <= knt; k++) {
13855 beta1 *= 1.0020841800044864E-292;
13856 }
13857
13858 alpha1->re = beta1;
13859 alpha1->im = 0.0;
13860 } else {
13861 xnorm = beta1 - alpha1->re;
13862 ai = 0.0 - alpha1->im;
13863 if (ai == 0.0) {
13864 tau.re = xnorm / beta1;
13865 tau.im = 0.0;
13866 } else if (xnorm == 0.0) {
13867 tau.re = 0.0;
13868 tau.im = ai / beta1;
13869 } else {
13870 tau.re = xnorm / beta1;
13871 tau.im = ai / beta1;
13872 }
13873
13874 b_alpha1.re = alpha1->re - beta1;
13875 b_alpha1.im = alpha1->im;
13876 *alpha1 = recip(b_alpha1);
13877 xnorm = alpha1->re;
13878 ai = alpha1->im;
13879 x_re = x->re;
13880 x_im = x->im;
13881 x->re = xnorm * x_re - ai * x_im;
13882 x->im = xnorm * x_im + ai * x_re;
13883 alpha1->re = beta1;
13884 alpha1->im = 0.0;
13885 }
13886 }
13887
13888 return tau;
13889 }
13890
13891 /*
13892 * Arguments : const creal_T f
13893 * const creal_T g
13894 * double *cs
13895 * creal_T *sn
13896 * creal_T *r
13897 * Return Type : void
13898 */
xzlartg(const creal_T f,const creal_T g,double * cs,creal_T * sn,creal_T * r)13899 static void xzlartg(const creal_T f, const creal_T g, double *cs, creal_T *sn,
13900 creal_T *r)
13901 {
13902 double scale;
13903 double f2s;
13904 double x;
13905 double fs_re;
13906 double fs_im;
13907 double gs_re;
13908 double gs_im;
13909 int count;
13910 int rescaledir;
13911 boolean_T guard1 = false;
13912 double g2;
13913 double g2s;
13914 scale = fabs(f.re);
13915 f2s = fabs(f.im);
13916 if (f2s > scale) {
13917 scale = f2s;
13918 }
13919
13920 x = fabs(g.re);
13921 f2s = fabs(g.im);
13922 if (f2s > x) {
13923 x = f2s;
13924 }
13925
13926 if (x > scale) {
13927 scale = x;
13928 }
13929
13930 fs_re = f.re;
13931 fs_im = f.im;
13932 gs_re = g.re;
13933 gs_im = g.im;
13934 count = 0;
13935 rescaledir = 0;
13936 guard1 = false;
13937 if (scale >= 7.4428285367870146E+137) {
13938 do {
13939 count++;
13940 fs_re *= 1.3435752215134178E-138;
13941 fs_im *= 1.3435752215134178E-138;
13942 gs_re *= 1.3435752215134178E-138;
13943 gs_im *= 1.3435752215134178E-138;
13944 scale *= 1.3435752215134178E-138;
13945 } while (!(scale < 7.4428285367870146E+137));
13946
13947 rescaledir = 1;
13948 guard1 = true;
13949 } else if (scale <= 1.3435752215134178E-138) {
13950 if ((g.re == 0.0) && (g.im == 0.0)) {
13951 *cs = 1.0;
13952 sn->re = 0.0;
13953 sn->im = 0.0;
13954 *r = f;
13955 } else {
13956 do {
13957 count++;
13958 fs_re *= 7.4428285367870146E+137;
13959 fs_im *= 7.4428285367870146E+137;
13960 gs_re *= 7.4428285367870146E+137;
13961 gs_im *= 7.4428285367870146E+137;
13962 scale *= 7.4428285367870146E+137;
13963 } while (!(scale > 1.3435752215134178E-138));
13964
13965 rescaledir = -1;
13966 guard1 = true;
13967 }
13968 } else {
13969 guard1 = true;
13970 }
13971
13972 if (guard1) {
13973 scale = fs_re * fs_re + fs_im * fs_im;
13974 g2 = gs_re * gs_re + gs_im * gs_im;
13975 x = g2;
13976 if (1.0 > g2) {
13977 x = 1.0;
13978 }
13979
13980 if (scale <= x * 2.0041683600089728E-292) {
13981 if ((f.re == 0.0) && (f.im == 0.0)) {
13982 *cs = 0.0;
13983 r->re = rt_hypotd_snf(g.re, g.im);
13984 r->im = 0.0;
13985 g2 = rt_hypotd_snf(gs_re, gs_im);
13986 sn->re = gs_re / g2;
13987 sn->im = -gs_im / g2;
13988 } else {
13989 g2s = sqrt(g2);
13990 *cs = rt_hypotd_snf(fs_re, fs_im) / g2s;
13991 x = fabs(f.re);
13992 f2s = fabs(f.im);
13993 if (f2s > x) {
13994 x = f2s;
13995 }
13996
13997 if (x > 1.0) {
13998 g2 = rt_hypotd_snf(f.re, f.im);
13999 fs_re = f.re / g2;
14000 fs_im = f.im / g2;
14001 } else {
14002 scale = 7.4428285367870146E+137 * f.re;
14003 f2s = 7.4428285367870146E+137 * f.im;
14004 g2 = rt_hypotd_snf(scale, f2s);
14005 fs_re = scale / g2;
14006 fs_im = f2s / g2;
14007 }
14008
14009 gs_re /= g2s;
14010 gs_im = -gs_im / g2s;
14011 sn->re = fs_re * gs_re - fs_im * gs_im;
14012 sn->im = fs_re * gs_im + fs_im * gs_re;
14013 r->re = *cs * f.re + (sn->re * g.re - sn->im * g.im);
14014 r->im = *cs * f.im + (sn->re * g.im + sn->im * g.re);
14015 }
14016 } else {
14017 f2s = sqrt(1.0 + g2 / scale);
14018 r->re = f2s * fs_re;
14019 r->im = f2s * fs_im;
14020 *cs = 1.0 / f2s;
14021 g2 += scale;
14022 scale = r->re / g2;
14023 f2s = r->im / g2;
14024 sn->re = scale * gs_re - f2s * -gs_im;
14025 sn->im = scale * -gs_im + f2s * gs_re;
14026 if (rescaledir > 0) {
14027 for (rescaledir = 1; rescaledir <= count; rescaledir++) {
14028 r->re *= 7.4428285367870146E+137;
14029 r->im *= 7.4428285367870146E+137;
14030 }
14031 } else {
14032 if (rescaledir < 0) {
14033 for (rescaledir = 1; rescaledir <= count; rescaledir++) {
14034 r->re *= 1.3435752215134178E-138;
14035 r->im *= 1.3435752215134178E-138;
14036 }
14037 }
14038 }
14039 }
14040 }
14041 }
14042
14043 /*
14044 * ZP2SS Zero-pole to state-space conversion. Codegen support
14045 *
14046 * This function is based on 'zp2ss' by The MathWorks Inc.
14047 * Arguments : emxArray_creal_T *a
14048 * emxArray_real_T *b
14049 * emxArray_real_T *c
14050 * double *d
14051 * Return Type : void
14052 */
zp2ss_cg(emxArray_creal_T * a,emxArray_real_T * b,emxArray_real_T * c,double * d)14053 static void zp2ss_cg(emxArray_creal_T *a, emxArray_real_T *b,
14054 emxArray_real_T *c,
14055 double *d)
14056 {
14057 int i6;
14058 creal_T b_c[3];
14059 int r1;
14060 double wn;
14061 double a22;
14062 int k;
14063 double den[3];
14064 double b1[2];
14065 double t[4];
14066 double B[4];
14067 int r2;
14068 double Y[4];
14069 emxArray_int8_T *reshapes_f2;
14070 emxArray_cint8_T *b_a;
14071 emxArray_cint8_T *result;
14072 int i7;
14073 signed char a_re;
14074 signed char a_im;
14075 emxArray_real_T *b_b1;
14076 emxArray_real_T *varargin_2;
14077 int result_im;
14078 emxArray_real_T *r3;
14079
14080 /* Strip infinities and throw away. */
14081 /* Group into complex pairs */
14082 /* try */
14083 /* % z and p should have real elements and exact complex conjugate pair. */
14084 /* z = cplxpair(zF,0); */
14085 /* p = cplxpair(pF,0); */
14086 /* catch */
14087 /* % If fail, revert to use the old default tolerance. */
14088 /* % The use of tolerance in checking for real entries and conjugate pairs */
14089 /* % may result in misinterpretation for edge cases. Please review the */
14090 /* % process of how z and p are generated. */
14091 /* z = cplxpair(zF,1e6*nz*norm(zF)*eps + eps); */
14092 /* p = cplxpair(pF,1e6*np*norm(pF)*eps + eps); */
14093 /* end */
14094 /* Initialize state-space matrices for running series */
14095 /* If odd number of poles AND zeros, convert the pole and zero */
14096 /* at the end into state-space. */
14097 /* H(s) = (s-z1)/(s-p1) = (s + num(2)) / (s + den(2)) */
14098 /* If odd number of poles only, convert the pole at the */
14099 /* end into state-space. */
14100 /* H(s) = 1/(s-p1) = 1/(s + den(2)) */
14101 i6 = a->size[0] * a->size[1];
14102 a->size[0] = 1;
14103 a->size[1] = 1;
14104 emxEnsureCapacity_creal_T(a, i6);
14105 a->data[0].re = -1.0;
14106 a->data[0].im = 0.0;
14107 i6 = b->size[0];
14108 b->size[0] = 1;
14109 emxEnsureCapacity_real_T1(b, i6);
14110 b->data[0] = 1.0;
14111 i6 = c->size[0] * c->size[1];
14112 c->size[0] = 1;
14113 c->size[1] = 1;
14114 emxEnsureCapacity_real_T(c, i6);
14115 c->data[0] = 1.0;
14116
14117 /* If odd number of zeros only, convert the zero at the */
14118 /* end, along with a pole-pair into state-space. */
14119 /* H(s) = (s+num(2))/(s^2+den(2)s+den(3)) */
14120 /* Now we have an even number of poles and zeros, although not */
14121 /* necessarily the same number - there may be more poles. */
14122 /* H(s) = (s^2+num(2)s+num(3))/(s^2+den(2)s+den(3)) */
14123 /* Loop through rest of pairs, connecting in series to build the model. */
14124 /* Take care of any left over unmatched pole pairs. */
14125 /* H(s) = 1/(s^2+den(2)s+den(3)) */
14126 b_c[0].re = 1.0;
14127 b_c[0].im = 0.0;
14128 for (r1 = 0; r1 < 2; r1++) {
14129 wn = -(0.86602540378443871 + -1.7320508075688774 * (double)r1);
14130 a22 = b_c[r1].re;
14131 b_c[r1 + 1].re = 0.49999999999999978 * b_c[r1].re - wn * b_c[r1].im;
14132 b_c[r1 + 1].im = 0.49999999999999978 * b_c[r1].im + wn * a22;
14133 k = r1;
14134 while (k + 1 > 1) {
14135 wn = 0.86602540378443871 + -1.7320508075688774 * (double)r1;
14136 b_c[1].re -= -0.49999999999999978 * b_c[0].re - wn * b_c[0].im;
14137 b_c[1].im -= -0.49999999999999978 * b_c[0].im + wn * b_c[0].re;
14138 k = 0;
14139 }
14140 }
14141
14142 for (i6 = 0; i6 < 3; i6++) {
14143 den[i6] = b_c[i6].re;
14144 }
14145
14146 for (k = 0; k < 2; k++) {
14147 b1[k] = rt_hypotd_snf(-0.49999999999999978, 0.86602540378443871 +
14148 -1.7320508075688774 * (double)k);
14149 }
14150
14151 wn = sqrt(b1[0] * b1[1]);
14152 b1[0] = 1.0;
14153 b1[1] = 1.0 / wn;
14154 for (i6 = 0; i6 < 4; i6++) {
14155 t[i6] = 0.0;
14156 }
14157
14158 /* Balancing transformation */
14159 B[0] = -den[1];
14160 B[2] = -den[2];
14161 for (r1 = 0; r1 < 2; r1++) {
14162 t[r1 + (r1 << 1)] = b1[r1];
14163 B[1 + (r1 << 1)] = 1.0 - (double)r1;
14164 }
14165
14166 if (t[1] > t[0]) {
14167 r1 = 1;
14168 r2 = 0;
14169 } else {
14170 r1 = 0;
14171 r2 = 1;
14172 }
14173
14174 wn = t[r2] / t[r1];
14175 a22 = t[2 + r2] - wn * t[2 + r1];
14176 for (k = 0; k < 2; k++) {
14177 Y[1 + (k << 1)] = (B[r2 + (k << 1)] - B[r1 + (k << 1)] * wn) / a22;
14178 Y[k << 1] = (B[r1 + (k << 1)] - Y[1 + (k << 1)] * t[2 + r1]) / t[r1];
14179 }
14180
14181 if (t[1] > t[0]) {
14182 r1 = 1;
14183 r2 = 0;
14184 } else {
14185 r1 = 0;
14186 r2 = 1;
14187 }
14188
14189 emxInit_int8_T(&reshapes_f2, 2);
14190 wn = t[r2] / t[r1];
14191 b1[1] = ((1.0 - (double)r2) - (1.0 - (double)r1) * wn) / (t[2 + r2] - wn * t[2
14192 + r1]);
14193 b1[0] = ((1.0 - (double)r1) - b1[1] * t[2 + r1]) / t[r1];
14194
14195 /* [a,b,c,d] = series(a,b,c,d,a1,b1,c1,d1); */
14196 /* Next lines perform series connection */
14197 i6 = reshapes_f2->size[0] * reshapes_f2->size[1];
14198 reshapes_f2->size[0] = 1;
14199 reshapes_f2->size[1] = 2;
14200 emxEnsureCapacity_int8_T(reshapes_f2, i6);
14201 for (i6 = 0; i6 < 2; i6++) {
14202 reshapes_f2->data[i6] = 0;
14203 }
14204
14205 emxInit_cint8_T(&b_a, 2);
14206 i6 = b_a->size[0] * b_a->size[1];
14207 b_a->size[0] = a->size[0];
14208 b_a->size[1] = a->size[1];
14209 emxEnsureCapacity_cint8_T(b_a, i6);
14210 r1 = a->size[1];
14211 for (i6 = 0; i6 < r1; i6++) {
14212 r2 = a->size[0];
14213 for (i7 = 0; i7 < r2; i7++) {
14214 a_re = (signed char)a->data[i7 + a->size[0] * i6].re;
14215 a_im = (signed char)a->data[i7 + a->size[0] * i6].im;
14216 b_a->data[i7 + b_a->size[0] * i6].re = a_re;
14217 b_a->data[i7 + b_a->size[0] * i6].im = a_im;
14218 }
14219 }
14220
14221 emxInit_cint8_T(&result, 2);
14222 i6 = result->size[0] * result->size[1];
14223 result->size[0] = 1;
14224 result->size[1] = 1 + reshapes_f2->size[1];
14225 emxEnsureCapacity_cint8_T(result, i6);
14226 for (i6 = 0; i6 < 1; i6++) {
14227 for (i7 = 0; i7 < 1; i7++) {
14228 result->data[0] = b_a->data[0];
14229 }
14230 }
14231
14232 emxFree_cint8_T(&b_a);
14233 r1 = reshapes_f2->size[1];
14234 for (i6 = 0; i6 < r1; i6++) {
14235 r2 = reshapes_f2->size[0];
14236 for (i7 = 0; i7 < r2; i7++) {
14237 result->data[i7 + result->size[0] * (i6 + 1)].re = reshapes_f2->data[i7 +
14238 reshapes_f2->size[0] * i6];
14239 result->data[i7 + result->size[0] * (i6 + 1)].im = 0;
14240 }
14241 }
14242
14243 emxFree_int8_T(&reshapes_f2);
14244 emxInit_real_T(&b_b1, 2);
14245 i6 = b_b1->size[0] * b_b1->size[1];
14246 b_b1->size[0] = 2;
14247 b_b1->size[1] = c->size[1];
14248 emxEnsureCapacity_real_T(b_b1, i6);
14249 for (i6 = 0; i6 < 2; i6++) {
14250 r1 = c->size[1];
14251 for (i7 = 0; i7 < r1; i7++) {
14252 b_b1->data[i6 + b_b1->size[0] * i7] = b1[i6] * c->data[c->size[0] * i7];
14253 }
14254 }
14255
14256 for (i6 = 0; i6 < 2; i6++) {
14257 for (i7 = 0; i7 < 2; i7++) {
14258 B[i6 + (i7 << 1)] = 0.0;
14259 for (r1 = 0; r1 < 2; r1++) {
14260 B[i6 + (i7 << 1)] += Y[i6 + (r1 << 1)] * t[r1 + (i7 << 1)];
14261 }
14262 }
14263 }
14264
14265 emxInit_real_T(&varargin_2, 2);
14266 i6 = varargin_2->size[0] * varargin_2->size[1];
14267 varargin_2->size[0] = 2;
14268 varargin_2->size[1] = b_b1->size[1] + 2;
14269 emxEnsureCapacity_real_T(varargin_2, i6);
14270 r1 = b_b1->size[1];
14271 for (i6 = 0; i6 < r1; i6++) {
14272 for (i7 = 0; i7 < 2; i7++) {
14273 varargin_2->data[i7 + varargin_2->size[0] * i6] = b_b1->data[i7 +
14274 b_b1->size[0] * i6];
14275 }
14276 }
14277
14278 for (i6 = 0; i6 < 2; i6++) {
14279 for (i7 = 0; i7 < 2; i7++) {
14280 varargin_2->data[i7 + varargin_2->size[0] * (i6 + b_b1->size[1])] = B[i7 +
14281 (i6 << 1)];
14282 }
14283 }
14284
14285 emxFree_real_T(&b_b1);
14286 if (!(result->size[1] == 0)) {
14287 r1 = result->size[1];
14288 } else {
14289 r1 = 3;
14290 }
14291
14292 r2 = !(result->size[1] == 0);
14293 i6 = a->size[0] * a->size[1];
14294 a->size[0] = r2 + 2;
14295 a->size[1] = r1;
14296 emxEnsureCapacity_creal_T(a, i6);
14297 for (i6 = 0; i6 < r1; i6++) {
14298 for (i7 = 0; i7 < r2; i7++) {
14299 k = result->data[i7 + r2 * i6].re;
14300 result_im = result->data[i7 + r2 * i6].im;
14301 a->data[i7 + a->size[0] * i6].re = k;
14302 a->data[i7 + a->size[0] * i6].im = result_im;
14303 }
14304 }
14305
14306 emxFree_cint8_T(&result);
14307 for (i6 = 0; i6 < r1; i6++) {
14308 for (i7 = 0; i7 < 2; i7++) {
14309 a->data[(i7 + r2) + a->size[0] * i6].re = varargin_2->data[i7 + (i6 << 1)];
14310 a->data[(i7 + r2) + a->size[0] * i6].im = 0.0;
14311 }
14312 }
14313
14314 emxFree_real_T(&varargin_2);
14315 r1 = b->size[0];
14316 i6 = b->size[0];
14317 b->size[0] = r1 + 2;
14318 emxEnsureCapacity_real_T1(b, i6);
14319 for (i6 = 0; i6 < 2; i6++) {
14320 b->data[r1 + i6] = b1[i6] * 0.0;
14321 }
14322
14323 for (i6 = 0; i6 < 2; i6++) {
14324 b1[i6] = 0.0;
14325 for (i7 = 0; i7 < 2; i7++) {
14326 b1[i6] += (double)i7 * t[i7 + (i6 << 1)];
14327 }
14328 }
14329
14330 emxInit_real_T(&r3, 2);
14331 i6 = r3->size[0] * r3->size[1];
14332 r3->size[0] = 1;
14333 r3->size[1] = c->size[1] + 2;
14334 emxEnsureCapacity_real_T(r3, i6);
14335 r1 = c->size[1];
14336 for (i6 = 0; i6 < r1; i6++) {
14337 r3->data[r3->size[0] * i6] = 0.0 * c->data[c->size[0] * i6];
14338 }
14339
14340 for (i6 = 0; i6 < 2; i6++) {
14341 r3->data[r3->size[0] * (i6 + c->size[1])] = b1[i6];
14342 }
14343
14344 i6 = c->size[0] * c->size[1];
14345 c->size[0] = 1;
14346 c->size[1] = r3->size[1];
14347 emxEnsureCapacity_real_T(c, i6);
14348 r1 = r3->size[1];
14349 for (i6 = 0; i6 < r1; i6++) {
14350 c->data[c->size[0] * i6] = r3->data[r3->size[0] * i6];
14351 }
14352
14353 emxFree_real_T(&r3);
14354
14355 /* Apply gain k: */
14356 i6 = c->size[0] * c->size[1];
14357 c->size[0] = 1;
14358 emxEnsureCapacity_real_T(c, i6);
14359 r1 = c->size[0];
14360 r2 = c->size[1];
14361 r1 *= r2;
14362 for (i6 = 0; i6 < r1; i6++) {
14363 c->data[i6] *= 0.99999999999999989;
14364 }
14365
14366 *d = 0.0;
14367 }
14368
14369 /*
14370 * %%%%%%%%%%%%%%%%%%% Perform Input Checks
14371 * Arguments : double Rdata
14372 * double Fpass
14373 * double Fstop
14374 * double caldiv
14375 * double FIR
14376 * double HB1
14377 * double PLL_mult
14378 * double Apass
14379 * double Astop
14380 * double phEQ
14381 * double HB2
14382 * double HB3
14383 * const char Type[7]
14384 * const char RxTx[2]
14385 * double RFbw
14386 * double DAC_div
14387 * double converter_rate
14388 * double PLL_rate
14389 * double Fcenter
14390 * double wnom
14391 * double FIRdBmin
14392 * double int_FIR
14393 * double maxTaps
14394 * short outputTaps[128]
14395 * double *numOutputTaps
14396 * double *filterGain
14397 * Return Type : void
14398 */
internal_design_filter_cg(double Rdata,double Fpass,double Fstop,double caldiv,double FIR,double HB1,double PLL_mult,double Apass,double Astop,double phEQ,double HB2,double HB3,const char Type[7],const char RxTx[2],double RFbw,double DAC_div,double converter_rate,double PLL_rate,double Fcenter,double wnom,double FIRdBmin,double int_FIR,double maxTaps,short outputTaps[128],double * numOutputTaps,double * filterGain)14399 void internal_design_filter_cg(double Rdata, double Fpass, double Fstop, double
14400 caldiv, double FIR, double HB1, double PLL_mult, double Apass, double Astop,
14401 double phEQ, double HB2, double HB3, const char Type[7], const char RxTx[2],
14402 double RFbw, double DAC_div, double converter_rate, double PLL_rate, double
14403 Fcenter, double wnom, double FIRdBmin, double int_FIR, double maxTaps, short
14404 outputTaps[128], double *numOutputTaps, double *filterGain)
14405 {
14406 emxArray_creal_T *a1;
14407 emxArray_creal_T *a2;
14408 double b1[4];
14409 double b2[2];
14410 creal_T a2_data[2];
14411 int a2_size[2];
14412 int b1_size[2];
14413 int i0;
14414 double b1_data[4];
14415 int b2_size[2];
14416 double b2_data[4];
14417 double hb1_coeff[15];
14418 static const double dv0[15] = { -0.00323486328125, 0.0, 0.01910400390625, 0.0,
14419 -0.07049560546875, 0.0, 0.30450439453125, 0.5, 0.30450439453125, 0.0,
14420 -0.07049560546875, 0.0, 0.01910400390625, 0.0, -0.00323486328125
14421 };
14422
14423 static const double dv1[15] = { -0.00390625, 0.0, 0.0205078125, 0.0,
14424 -0.07177734375, 0.0, 0.30224609375, 0.49462890625, 0.30224609375, 0.0,
14425 -0.07177734375, 0.0, 0.0205078125, 0.0, -0.00390625
14426 };
14427
14428 int hb3_coeff_size[2];
14429 int dec_int3_coeff_size[2];
14430 double hb3_coeff_data[5];
14431 static const double y[3] = { 0.25, 0.5, 0.25 };
14432
14433 static const double b_y[5] = { 0.0625, 0.25, 0.375, 0.25, 0.0625 };
14434
14435 double dec_int3_coeff_data[29];
14436 static const double c_y[29] = { 0.00146484375, -0.00077311197916666663, 0.0,
14437 -0.00634765625, -0.00048828125, 0.0, 0.019490559895833332,
14438 0.0090738932291666661, 0.0, -0.0494384765625, -0.0404052734375, 0.0,
14439 0.14522298177083331, 0.25541178385416663, 0.33333333333333331,
14440 0.25541178385416663, 0.14522298177083331, 0.0, -0.0404052734375,
14441 -0.0494384765625, 0.0, 0.0090738932291666661, 0.019490559895833332, 0.0,
14442 -0.00048828125, -0.00634765625, 0.0, -0.00077311197916666663, 0.00146484375
14443 };
14444
14445 static const double d_y[17] = { 0.00335693359375, 0.00506591796875, 0.0,
14446 -0.02398681640625, -0.035400390625, 0.0, 0.1168212890625, 0.24664306640625,
14447 0.3125, 0.24664306640625, 0.1168212890625, 0.0, -0.035400390625,
14448 -0.02398681640625, 0.0, 0.00506591796875, 0.00335693359375
14449 };
14450
14451 int hb3;
14452 int dec_int3;
14453 double sigma;
14454 signed char i1;
14455 char enables[4];
14456 double w[2048];
14457 double phi[2048];
14458 static const double hb2_coeff[7] = { -0.03515625, 0.0, 0.28515625, 0.5,
14459 0.28515625, 0.0, -0.03515625
14460 };
14461
14462 static creal_T combinedResponse[2048];
14463 static creal_T dcv0[2048];
14464 double b_combinedResponse[2048];
14465 double invariance[2048];
14466 double sigmax;
14467 double b_phi[2048];
14468 double b_w[2048];
14469 double clkFIR;
14470 double Gpass;
14471 double Gstop;
14472 emxArray_real_T *fg;
14473 int loop_ub;
14474 emxArray_real_T *omega;
14475 emxArray_creal_T *rg1;
14476 emxArray_creal_T *c_combinedResponse;
14477 emxArray_creal_T *r0;
14478 double apnd;
14479 double absa;
14480 double rg1_im;
14481 double re;
14482 double im;
14483 emxArray_real_T *sw;
14484 emxArray_real_T *fg2;
14485 int n;
14486 emxArray_real_T *omega2;
14487 emxArray_creal_T *rgN;
14488 emxArray_real_T *b_omega2;
14489 emxArray_creal_T *d_combinedResponse;
14490 emxArray_real_T *a;
14491 emxArray_real_T *wg;
14492 emxArray_real_T *weight;
14493 boolean_T exitg1;
14494 emxArray_real_T *F1;
14495 emxArray_real_T *F2;
14496 emxArray_real_T *A1;
14497 emxArray_real_T *A2;
14498 emxArray_real_T *W1;
14499 emxArray_real_T *W2;
14500 emxArray_real_T *tap_store;
14501 emxArray_real_T *Apass_actual_vector;
14502 emxArray_real_T *Astop_actual_vector;
14503 unsigned int i;
14504 emxArray_real_T *ccoef;
14505 emxArray_real_T *F4;
14506 emxArray_real_T *b_W1;
14507 int exitg2;
14508 int i2;
14509 int b_loop_ub;
14510 boolean_T valid;
14511 int i3;
14512 double firTapsPreScale[128];
14513 double b_firTapsPreScale[128];
14514 short i4;
14515 (void)caldiv;
14516 (void)PLL_mult;
14517 (void)Type;
14518 (void)RFbw;
14519 (void)DAC_div;
14520 (void)PLL_rate;
14521 (void)Fcenter;
14522
14523 /* internal_design_filter_cg */
14524 /* */
14525 /* This implementation of the ADI ad936x-filter-wizard supports */
14526 /* code generation from MATLAB Coder */
14527 /* */
14528 /* Edits by: Travis F. Collins <travisfcollins@gmail.com> */
14529 /* */
14530 /* When calling this function utilize the function "process_input" to make */
14531 /* sure all the necessary fields exist */
14532 /* */
14533 /* Todo: */
14534 /* - Set fractionalLength based on FSR of inputs (ML has no doc on this) */
14535 /* */
14536 /* Inputs */
14537 /* ============================================ */
14538 /* Rdata = Input/output sample data rate (in Hz) */
14539 /* Fpass = Passband frequency (in Hz) */
14540 /* Fstop = Stopband frequency (in Hz) */
14541 /* caldiv = The actual discrete register value that describes the */
14542 /* rolloff for the analog filters */
14543 /* FIR = FIR interpolation/decimation factor */
14544 /* HB1 = HB1 interpolation/decimation rates */
14545 /* PLL_mult = PLL multiplication */
14546 /* Apass = Max ripple allowed in passband (in dB) */
14547 /* Astop = Min attenuation in stopband (in dB) */
14548 /* phEQ = Phase equalization on (not -1)/off (-1) */
14549 /* HB2 = HB2 interpolation/decimation rates */
14550 /* HB3 = HB3 interpolation/decimation rates */
14551 /* Type = The type of filter required. one of: */
14552 /* 'Lowpass' (default,and only support) */
14553 /* 'Bandpass' (Not implemented) */
14554 /* 'Equalize' (Not implemented) */
14555 /* 'Root Raised Cosine' (Not implemented) */
14556 /* RxTx = Is this 'Rx' or 'Tx'?. */
14557 /* RFbw */
14558 /* DAC_div = The ADC/DAC ratio, for Rx channels, this is */
14559 /* always '1', for Tx, it is either '1' or '2' */
14560 /* converter_rate = Rate of converter */
14561 /* PLL_rate = the PLL rate in Hz */
14562 /* Fcenter = Center Frequency in Hz (only used for Bandpass), */
14563 /* otherwise 0 */
14564 /* wnom = analog cutoff frequency (in Hz) */
14565 /* FIRdBmin = min rejection that FIR is required to have (in dB) */
14566 /* int_FIR = use AD9361 FIR on (1)/off (0) */
14567 /* */
14568 /* Outputs */
14569 /* =============================================== */
14570 /* firtaps = fixed point FIR coefficients */
14571 /* Scalar checks */
14572 /* String checks */
14573 /* This is unused */
14574 /* %%%%%%%%%%%%%%%%%%% Build processing struct */
14575 /* Not used */
14576 /* Design analog filters */
14577 emxInit_creal_T(&a1, 2);
14578 emxInit_creal_T(&a2, 2);
14579 if (b_strcmp(RxTx)) {
14580 /* Define the analog filters (for design purpose) */
14581 butter_cg(6.2831853071795862 * (wnom * 1.7857142857142858), b2, a2_data,
14582 a2_size);
14583 b1_size[0] = 1;
14584 b1_size[1] = 2;
14585 for (i0 = 0; i0 < 2; i0++) {
14586 b1_data[i0] = b2[i0];
14587 }
14588
14589 i0 = a1->size[0] * a1->size[1];
14590 a1->size[0] = 1;
14591 a1->size[1] = 2;
14592 emxEnsureCapacity_creal_T(a1, i0);
14593 for (i0 = 0; i0 < 2; i0++) {
14594 a1->data[i0] = a2_data[i0];
14595 }
14596
14597 /* 1st order */
14598 b_butter_cg(6.2831853071795862 * wnom, b1, a2);
14599 b2_size[0] = 1;
14600 b2_size[1] = 4;
14601 for (i0 = 0; i0 < 4; i0++) {
14602 b2_data[i0] = b1[i0];
14603 }
14604
14605 /* 3rd order */
14606 /* Define the digital filters with fixed coefficients */
14607 memcpy(&hb1_coeff[0], &dv1[0], 15U * sizeof(double));
14608 hb3_coeff_size[0] = 1;
14609 hb3_coeff_size[1] = 5;
14610 for (i0 = 0; i0 < 5; i0++) {
14611 hb3_coeff_data[i0] = b_y[i0];
14612 }
14613
14614 dec_int3_coeff_size[0] = 1;
14615 dec_int3_coeff_size[1] = 17;
14616 memcpy(&dec_int3_coeff_data[0], &d_y[0], 17U * sizeof(double));
14617 } else {
14618 /* Define the analog filters (for design purpose) */
14619 b_butter_cg(6.2831853071795862 * wnom, b1, a1);
14620 b1_size[0] = 1;
14621 b1_size[1] = 4;
14622 for (i0 = 0; i0 < 4; i0++) {
14623 b1_data[i0] = b1[i0];
14624 }
14625
14626 /* 3rd order */
14627 butter_cg(6.2831853071795862 * (wnom * 3.125), b2, a2_data, a2_size);
14628 b2_size[0] = 1;
14629 b2_size[1] = 2;
14630 for (i0 = 0; i0 < 2; i0++) {
14631 b2_data[i0] = b2[i0];
14632 }
14633
14634 i0 = a2->size[0] * a2->size[1];
14635 a2->size[0] = 1;
14636 a2->size[1] = 2;
14637 emxEnsureCapacity_creal_T(a2, i0);
14638 for (i0 = 0; i0 < 2; i0++) {
14639 a2->data[i0] = a2_data[i0];
14640 }
14641
14642 /* 1st order */
14643 /* Define the digital filters with fixed coefficients */
14644 memcpy(&hb1_coeff[0], &dv0[0], 15U * sizeof(double));
14645 hb3_coeff_size[0] = 1;
14646 hb3_coeff_size[1] = 3;
14647 for (i0 = 0; i0 < 3; i0++) {
14648 hb3_coeff_data[i0] = y[i0];
14649 }
14650
14651 dec_int3_coeff_size[0] = 1;
14652 dec_int3_coeff_size[1] = 29;
14653 memcpy(&dec_int3_coeff_data[0], &c_y[0], 29U * sizeof(double));
14654 }
14655
14656 /* Configure staging of filters */
14657 if (HB3 == 2.0) {
14658 hb3 = 50;
14659 dec_int3 = 49;
14660 } else if (HB3 == 3.0) {
14661 hb3 = 49;
14662 dec_int3 = 51;
14663 } else {
14664 hb3 = 49;
14665 dec_int3 = 49;
14666 }
14667
14668 /* convert the enables into a string */
14669 sigma = rt_roundd_snf(HB1);
14670 if (sigma < 128.0) {
14671 if (sigma >= -128.0) {
14672 i1 = (signed char)sigma;
14673 } else {
14674 i1 = MIN_int8_T;
14675 }
14676 } else if (sigma >= 128.0) {
14677 i1 = MAX_int8_T;
14678 } else {
14679 i1 = 0;
14680 }
14681
14682 i0 = 48 + i1;
14683 if (i0 > 127) {
14684 i0 = 127;
14685 }
14686
14687 enables[0] = (signed char)i0;
14688 sigma = rt_roundd_snf(HB2);
14689 if (sigma < 128.0) {
14690 if (sigma >= -128.0) {
14691 i1 = (signed char)sigma;
14692 } else {
14693 i1 = MIN_int8_T;
14694 }
14695 } else if (sigma >= 128.0) {
14696 i1 = MAX_int8_T;
14697 } else {
14698 i1 = 0;
14699 }
14700
14701 i0 = 48 + i1;
14702 if (i0 > 127) {
14703 i0 = 127;
14704 }
14705
14706 enables[1] = (signed char)i0;
14707 enables[2] = (signed char)hb3;
14708 enables[3] = (signed char)dec_int3;
14709
14710 /* Find out the best fit delay on passband */
14711 memset(&w[0], 0, sizeof(double) << 11);
14712 memset(&phi[0], 0, sizeof(double) << 11);
14713 w[0] = -Fpass;
14714 for (hb3 = 0; hb3 < 2047; hb3++) {
14715 w[hb3 + 1] = w[0] - 2.0 * w[0] * (2.0 + (double)hb3) / 2048.0;
14716 }
14717
14718 /* Generate target responses used in filter design phase */
14719 /* Generate responses then convolve */
14720 generateCascadedResponseRx(enables, w, converter_rate, hb1_coeff, hb2_coeff,
14721 hb3_coeff_data, hb3_coeff_size, dec_int3_coeff_data, dec_int3_coeff_size,
14722 combinedResponse);
14723
14724 /* Determine overall response with analog filters inline */
14725 analogresp(RxTx, w, converter_rate, b1_data, b1_size, a1, b2_data, b2_size, a2,
14726 dcv0);
14727 for (i0 = 0; i0 < 2048; i0++) {
14728 sigmax = combinedResponse[i0].re;
14729 combinedResponse[i0].re = combinedResponse[i0].re * dcv0[i0].re -
14730 combinedResponse[i0].im * dcv0[i0].im;
14731 combinedResponse[i0].im = sigmax * dcv0[i0].im + combinedResponse[i0].im *
14732 dcv0[i0].re;
14733 b_combinedResponse[i0] = combinedResponse[i0].re;
14734 }
14735
14736 b_power(b_combinedResponse, invariance);
14737 for (i0 = 0; i0 < 2048; i0++) {
14738 b_combinedResponse[i0] = combinedResponse[i0].im;
14739 }
14740
14741 b_power(b_combinedResponse, b_phi);
14742 for (i0 = 0; i0 < 2048; i0++) {
14743 invariance[i0] += b_phi[i0];
14744 }
14745
14746 phi[0] = rt_atan2d_snf(combinedResponse[0].im, combinedResponse[0].re);
14747 for (hb3 = 0; hb3 < 2047; hb3++) {
14748 sigma = rt_atan2d_snf(combinedResponse[hb3 + 1].im, combinedResponse[hb3 + 1]
14749 .re) - phi[hb3];
14750 phi[hb3 + 1] = phi[hb3] + (sigma - 6.2831853071795862 * floor(sigma /
14751 6.2831853071795862 + 0.5));
14752 }
14753
14754 sigma = sum(invariance);
14755 for (i0 = 0; i0 < 2048; i0++) {
14756 b_combinedResponse[i0] = w[i0] * invariance[i0];
14757 }
14758
14759 sigmax = sum(b_combinedResponse);
14760 if ((phEQ == 0.0) || (phEQ == -1.0)) {
14761 for (i0 = 0; i0 < 2048; i0++) {
14762 b_combinedResponse[i0] = w[i0] * phi[i0] * invariance[i0];
14763 b_phi[i0] = phi[i0] * invariance[i0];
14764 b_w[i0] = w[i0] * w[i0] * invariance[i0];
14765 }
14766
14767 sigma = -((sigma * sum(b_combinedResponse) - sigmax * sum(b_phi)) / (sigma *
14768 sum(b_w) - sigmax * sigmax)) / 6.2831853071795862;
14769 } else {
14770 sigma = phEQ * 1.0E-9;
14771 }
14772
14773 /* Design the FIR */
14774 clkFIR = Rdata * FIR;
14775 Gpass = floor(16384.0 * Fpass / clkFIR);
14776 Gstop = ceil(16384.0 * Fstop / clkFIR);
14777 if (!((Gpass < Gstop - 1.0) || rtIsNaN(Gstop - 1.0))) {
14778 Gpass = Gstop - 1.0;
14779 }
14780
14781 emxInit_real_T(&fg, 2);
14782 i0 = fg->size[0] * fg->size[1];
14783 fg->size[0] = 1;
14784 fg->size[1] = (int)(Gpass + 1.0);
14785 emxEnsureCapacity_real_T(fg, i0);
14786 loop_ub = (int)(Gpass + 1.0);
14787 for (i0 = 0; i0 < loop_ub; i0++) {
14788 fg->data[i0] = 0.0;
14789 }
14790
14791 emxInit_real_T(&omega, 2);
14792 i0 = omega->size[0] * omega->size[1];
14793 omega->size[0] = 1;
14794 omega->size[1] = (int)(Gpass + 1.0);
14795 emxEnsureCapacity_real_T(omega, i0);
14796 loop_ub = (int)(Gpass + 1.0);
14797 for (i0 = 0; i0 < loop_ub; i0++) {
14798 omega->data[i0] = 0.0;
14799 }
14800
14801 /* passband */
14802 for (hb3 = 0; hb3 < (int)(Gpass + 1.0); hb3++) {
14803 fg->data[hb3] = ((1.0 + (double)hb3) - 1.0) / 16384.0;
14804 omega->data[hb3] = fg->data[hb3] * clkFIR;
14805 }
14806
14807 emxInit_creal_T(&rg1, 2);
14808 emxInit_creal_T(&c_combinedResponse, 2);
14809
14810 /* Generate responses then convolve */
14811 b_generateCascadedResponseRx(enables, omega, converter_rate, hb1_coeff,
14812 hb2_coeff, hb3_coeff_data, hb3_coeff_size, dec_int3_coeff_data,
14813 dec_int3_coeff_size, c_combinedResponse);
14814
14815 /* Determine overall response with analog filters inline */
14816 b_analogresp(RxTx, omega, converter_rate, b1_data, b1_size, a1, b2_data,
14817 b2_size, a2, rg1);
14818 i0 = rg1->size[0] * rg1->size[1];
14819 rg1->size[0] = 1;
14820 rg1->size[1] = c_combinedResponse->size[1];
14821 emxEnsureCapacity_creal_T(rg1, i0);
14822 loop_ub = c_combinedResponse->size[0] * c_combinedResponse->size[1];
14823 for (i0 = 0; i0 < loop_ub; i0++) {
14824 sigmax = c_combinedResponse->data[i0].re;
14825 apnd = c_combinedResponse->data[i0].im;
14826 absa = rg1->data[i0].re;
14827 rg1_im = rg1->data[i0].im;
14828 rg1->data[i0].re = sigmax * absa - apnd * rg1_im;
14829 rg1->data[i0].im = sigmax * rg1_im + apnd * absa;
14830 }
14831
14832 emxInit_creal_T(&r0, 2);
14833 i0 = r0->size[0] * r0->size[1];
14834 r0->size[0] = 1;
14835 r0->size[1] = omega->size[1];
14836 emxEnsureCapacity_creal_T(r0, i0);
14837 loop_ub = omega->size[0] * omega->size[1];
14838 for (i0 = 0; i0 < loop_ub; i0++) {
14839 re = omega->data[i0] * -0.0;
14840 im = omega->data[i0] * -6.2831853071795862;
14841 r0->data[i0].re = sigma * re;
14842 r0->data[i0].im = sigma * im;
14843 }
14844
14845 c_exp(r0);
14846 b_rdivide(r0, rg1, c_combinedResponse);
14847 sigma = Gpass + 1.0;
14848
14849 /* Expand memory correctly */
14850 emxInit_real_T(&sw, 2);
14851 if (rtIsNaN(Gstop)) {
14852 i0 = sw->size[0] * sw->size[1];
14853 sw->size[1] = 1;
14854 emxEnsureCapacity_real_T(sw, i0);
14855 } else if (8192.0 < Gstop) {
14856 i0 = sw->size[0] * sw->size[1];
14857 sw->size[1] = 0;
14858 emxEnsureCapacity_real_T(sw, i0);
14859 } else if (rtIsInf(Gstop) && (Gstop == 8192.0)) {
14860 i0 = sw->size[0] * sw->size[1];
14861 sw->size[1] = 1;
14862 emxEnsureCapacity_real_T(sw, i0);
14863 } else if (Gstop == Gstop) {
14864 i0 = sw->size[0] * sw->size[1];
14865 sw->size[0] = 1;
14866 sw->size[1] = (int)(8192.0 - Gstop) + 1;
14867 emxEnsureCapacity_real_T(sw, i0);
14868 loop_ub = (int)(8192.0 - Gstop);
14869 for (i0 = 0; i0 <= loop_ub; i0++) {
14870 sw->data[sw->size[0] * i0] = Gstop + (double)i0;
14871 }
14872 } else {
14873 sigmax = floor((8192.0 - Gstop) + 0.5);
14874 apnd = Gstop + sigmax;
14875 absa = fabs(Gstop);
14876 if (!(absa > 8192.0)) {
14877 absa = 8192.0;
14878 }
14879
14880 if (fabs(apnd - 8192.0) < 4.4408920985006262E-16 * absa) {
14881 sigmax++;
14882 apnd = 8192.0;
14883 } else if (apnd - 8192.0 > 0.0) {
14884 apnd = Gstop + (sigmax - 1.0);
14885 } else {
14886 sigmax++;
14887 }
14888
14889 if (sigmax >= 0.0) {
14890 n = (int)sigmax;
14891 } else {
14892 n = 0;
14893 }
14894
14895 i0 = sw->size[0] * sw->size[1];
14896 sw->size[0] = 1;
14897 sw->size[1] = n;
14898 emxEnsureCapacity_real_T(sw, i0);
14899 if (n > 0) {
14900 sw->data[0] = Gstop;
14901 if (n > 1) {
14902 sw->data[n - 1] = apnd;
14903 hb3 = (n - 1) / 2;
14904 for (dec_int3 = 1; dec_int3 < hb3; dec_int3++) {
14905 sw->data[dec_int3] = Gstop + (double)dec_int3;
14906 sw->data[(n - dec_int3) - 1] = apnd - (double)dec_int3;
14907 }
14908
14909 if (hb3 << 1 == n - 1) {
14910 sw->data[hb3] = (Gstop + apnd) / 2.0;
14911 } else {
14912 sw->data[hb3] = Gstop + (double)hb3;
14913 sw->data[hb3 + 1] = apnd - (double)hb3;
14914 }
14915 }
14916 }
14917 }
14918
14919 emxInit_real_T(&fg2, 2);
14920 i0 = fg2->size[0] * fg2->size[1];
14921 fg2->size[0] = 1;
14922 fg2->size[1] = (int)((unsigned int)sw->size[1] + fg->size[1]);
14923 emxEnsureCapacity_real_T(fg2, i0);
14924 loop_ub = (int)((unsigned int)sw->size[1] + fg->size[1]);
14925 for (i0 = 0; i0 < loop_ub; i0++) {
14926 fg2->data[i0] = 0.0;
14927 }
14928
14929 loop_ub = fg->size[1];
14930 for (i0 = 0; i0 < loop_ub; i0++) {
14931 fg2->data[i0] = fg->data[fg->size[0] * i0];
14932 }
14933
14934 if (rtIsNaN(Gstop)) {
14935 i0 = sw->size[0] * sw->size[1];
14936 sw->size[1] = 1;
14937 emxEnsureCapacity_real_T(sw, i0);
14938 } else if (8192.0 < Gstop) {
14939 i0 = sw->size[0] * sw->size[1];
14940 sw->size[1] = 0;
14941 emxEnsureCapacity_real_T(sw, i0);
14942 } else if (rtIsInf(Gstop) && (Gstop == 8192.0)) {
14943 i0 = sw->size[0] * sw->size[1];
14944 sw->size[1] = 1;
14945 emxEnsureCapacity_real_T(sw, i0);
14946 } else if (Gstop == Gstop) {
14947 i0 = sw->size[0] * sw->size[1];
14948 sw->size[0] = 1;
14949 sw->size[1] = (int)(8192.0 - Gstop) + 1;
14950 emxEnsureCapacity_real_T(sw, i0);
14951 loop_ub = (int)(8192.0 - Gstop);
14952 for (i0 = 0; i0 <= loop_ub; i0++) {
14953 sw->data[sw->size[0] * i0] = Gstop + (double)i0;
14954 }
14955 } else {
14956 sigmax = floor((8192.0 - Gstop) + 0.5);
14957 apnd = Gstop + sigmax;
14958 absa = fabs(Gstop);
14959 if (!(absa > 8192.0)) {
14960 absa = 8192.0;
14961 }
14962
14963 if (fabs(apnd - 8192.0) < 4.4408920985006262E-16 * absa) {
14964 sigmax++;
14965 apnd = 8192.0;
14966 } else if (apnd - 8192.0 > 0.0) {
14967 apnd = Gstop + (sigmax - 1.0);
14968 } else {
14969 sigmax++;
14970 }
14971
14972 if (sigmax >= 0.0) {
14973 n = (int)sigmax;
14974 } else {
14975 n = 0;
14976 }
14977
14978 i0 = sw->size[0] * sw->size[1];
14979 sw->size[0] = 1;
14980 sw->size[1] = n;
14981 emxEnsureCapacity_real_T(sw, i0);
14982 if (n > 0) {
14983 sw->data[0] = Gstop;
14984 if (n > 1) {
14985 sw->data[n - 1] = apnd;
14986 hb3 = (n - 1) / 2;
14987 for (dec_int3 = 1; dec_int3 < hb3; dec_int3++) {
14988 sw->data[dec_int3] = Gstop + (double)dec_int3;
14989 sw->data[(n - dec_int3) - 1] = apnd - (double)dec_int3;
14990 }
14991
14992 if (hb3 << 1 == n - 1) {
14993 sw->data[hb3] = (Gstop + apnd) / 2.0;
14994 } else {
14995 sw->data[hb3] = Gstop + (double)hb3;
14996 sw->data[hb3 + 1] = apnd - (double)hb3;
14997 }
14998 }
14999 }
15000 }
15001
15002 emxInit_real_T(&omega2, 2);
15003 i0 = omega2->size[0] * omega2->size[1];
15004 omega2->size[0] = 1;
15005 omega2->size[1] = (int)((unsigned int)sw->size[1] + omega->size[1]);
15006 emxEnsureCapacity_real_T(omega2, i0);
15007 loop_ub = (int)((unsigned int)sw->size[1] + omega->size[1]);
15008 for (i0 = 0; i0 < loop_ub; i0++) {
15009 omega2->data[i0] = 0.0;
15010 }
15011
15012 loop_ub = omega->size[1];
15013 for (i0 = 0; i0 < loop_ub; i0++) {
15014 omega2->data[i0] = omega->data[omega->size[0] * i0];
15015 }
15016
15017 if (rtIsNaN(Gstop)) {
15018 i0 = sw->size[0] * sw->size[1];
15019 sw->size[1] = 1;
15020 emxEnsureCapacity_real_T(sw, i0);
15021 } else if (8192.0 < Gstop) {
15022 i0 = sw->size[0] * sw->size[1];
15023 sw->size[1] = 0;
15024 emxEnsureCapacity_real_T(sw, i0);
15025 } else if (rtIsInf(Gstop) && (Gstop == 8192.0)) {
15026 i0 = sw->size[0] * sw->size[1];
15027 sw->size[1] = 1;
15028 emxEnsureCapacity_real_T(sw, i0);
15029 } else if (Gstop == Gstop) {
15030 i0 = sw->size[0] * sw->size[1];
15031 sw->size[0] = 1;
15032 sw->size[1] = (int)(8192.0 - Gstop) + 1;
15033 emxEnsureCapacity_real_T(sw, i0);
15034 loop_ub = (int)(8192.0 - Gstop);
15035 for (i0 = 0; i0 <= loop_ub; i0++) {
15036 sw->data[sw->size[0] * i0] = Gstop + (double)i0;
15037 }
15038 } else {
15039 sigmax = floor((8192.0 - Gstop) + 0.5);
15040 apnd = Gstop + sigmax;
15041 absa = fabs(Gstop);
15042 if (!(absa > 8192.0)) {
15043 absa = 8192.0;
15044 }
15045
15046 if (fabs(apnd - 8192.0) < 4.4408920985006262E-16 * absa) {
15047 sigmax++;
15048 apnd = 8192.0;
15049 } else if (apnd - 8192.0 > 0.0) {
15050 apnd = Gstop + (sigmax - 1.0);
15051 } else {
15052 sigmax++;
15053 }
15054
15055 if (sigmax >= 0.0) {
15056 n = (int)sigmax;
15057 } else {
15058 n = 0;
15059 }
15060
15061 i0 = sw->size[0] * sw->size[1];
15062 sw->size[0] = 1;
15063 sw->size[1] = n;
15064 emxEnsureCapacity_real_T(sw, i0);
15065 if (n > 0) {
15066 sw->data[0] = Gstop;
15067 if (n > 1) {
15068 sw->data[n - 1] = apnd;
15069 hb3 = (n - 1) / 2;
15070 for (dec_int3 = 1; dec_int3 < hb3; dec_int3++) {
15071 sw->data[dec_int3] = Gstop + (double)dec_int3;
15072 sw->data[(n - dec_int3) - 1] = apnd - (double)dec_int3;
15073 }
15074
15075 if (hb3 << 1 == n - 1) {
15076 sw->data[hb3] = (Gstop + apnd) / 2.0;
15077 } else {
15078 sw->data[hb3] = Gstop + (double)hb3;
15079 sw->data[hb3 + 1] = apnd - (double)hb3;
15080 }
15081 }
15082 }
15083 }
15084
15085 emxInit_creal_T(&rgN, 2);
15086 i0 = rgN->size[0] * rgN->size[1];
15087 rgN->size[0] = 1;
15088 rgN->size[1] = (int)((unsigned int)sw->size[1] + c_combinedResponse->size[1]);
15089 emxEnsureCapacity_creal_T(rgN, i0);
15090 loop_ub = (int)((unsigned int)sw->size[1] + c_combinedResponse->size[1]);
15091 for (i0 = 0; i0 < loop_ub; i0++) {
15092 rgN->data[i0].re = 0.0;
15093 rgN->data[i0].im = 0.0;
15094 }
15095
15096 loop_ub = c_combinedResponse->size[1];
15097 for (i0 = 0; i0 < loop_ub; i0++) {
15098 rgN->data[i0] = c_combinedResponse->data[c_combinedResponse->size[0] * i0];
15099 }
15100
15101 /* stop band */
15102 for (hb3 = 0; hb3 < (int)(8192.0 + (1.0 - Gstop)); hb3++) {
15103 sigma++;
15104 fg2->data[(int)sigma - 1] = (Gstop + (double)hb3) / 16384.0;
15105 omega2->data[(int)sigma - 1] = fg2->data[(int)sigma - 1] * clkFIR;
15106 rgN->data[(int)sigma - 1].re = 0.0;
15107 rgN->data[(int)sigma - 1].im = 0.0;
15108 }
15109
15110 /* Generate responses then convolve */
15111 if (Gpass + 2.0 > omega2->size[1]) {
15112 i0 = 0;
15113 n = 0;
15114 } else {
15115 i0 = (int)(Gpass + 2.0) - 1;
15116 n = omega2->size[1];
15117 }
15118
15119 emxInit_real_T(&b_omega2, 2);
15120 hb3 = b_omega2->size[0] * b_omega2->size[1];
15121 b_omega2->size[0] = 1;
15122 b_omega2->size[1] = n - i0;
15123 emxEnsureCapacity_real_T(b_omega2, hb3);
15124 loop_ub = n - i0;
15125 for (n = 0; n < loop_ub; n++) {
15126 b_omega2->data[b_omega2->size[0] * n] = omega2->data[i0 + n];
15127 }
15128
15129 b_generateCascadedResponseRx(enables, b_omega2, converter_rate, hb1_coeff,
15130 hb2_coeff, hb3_coeff_data, hb3_coeff_size, dec_int3_coeff_data,
15131 dec_int3_coeff_size, c_combinedResponse);
15132 if (Gpass + 2.0 > omega2->size[1]) {
15133 i0 = 0;
15134 n = 0;
15135 } else {
15136 i0 = (int)(Gpass + 2.0) - 1;
15137 n = omega2->size[1];
15138 }
15139
15140 hb3 = b_omega2->size[0] * b_omega2->size[1];
15141 b_omega2->size[0] = 1;
15142 b_omega2->size[1] = n - i0;
15143 emxEnsureCapacity_real_T(b_omega2, hb3);
15144 loop_ub = n - i0;
15145 for (n = 0; n < loop_ub; n++) {
15146 b_omega2->data[b_omega2->size[0] * n] = omega2->data[i0 + n];
15147 }
15148
15149 emxInit_creal_T(&d_combinedResponse, 2);
15150 b_analogresp(RxTx, b_omega2, converter_rate, b1_data, b1_size, a1, b2_data,
15151 b2_size, a2, r0);
15152 i0 = d_combinedResponse->size[0] * d_combinedResponse->size[1];
15153 d_combinedResponse->size[0] = 1;
15154 d_combinedResponse->size[1] = c_combinedResponse->size[1];
15155 emxEnsureCapacity_creal_T(d_combinedResponse, i0);
15156 loop_ub = c_combinedResponse->size[0] * c_combinedResponse->size[1];
15157 for (i0 = 0; i0 < loop_ub; i0++) {
15158 sigmax = c_combinedResponse->data[i0].re;
15159 apnd = c_combinedResponse->data[i0].im;
15160 re = r0->data[i0].re;
15161 im = r0->data[i0].im;
15162 d_combinedResponse->data[i0].re = sigmax * re - apnd * im;
15163 d_combinedResponse->data[i0].im = sigmax * im + apnd * re;
15164 }
15165
15166 b_abs(d_combinedResponse, fg);
15167 emxInit_real_T(&a, 2);
15168 if (b_strcmp(RxTx)) {
15169 rdivide(fg, dBinv(-Astop), omega);
15170 } else {
15171 sigma = FIR;
15172 b_sqrt(&sigma);
15173 i0 = a->size[0] * a->size[1];
15174 a->size[0] = 1;
15175 a->size[1] = fg->size[1];
15176 emxEnsureCapacity_real_T(a, i0);
15177 loop_ub = fg->size[0] * fg->size[1];
15178 for (i0 = 0; i0 < loop_ub; i0++) {
15179 a->data[i0] = sigma * fg->data[i0];
15180 }
15181
15182 rdivide(a, dBinv(-Astop), omega);
15183 }
15184
15185 emxInit_real_T(&wg, 2);
15186 sigma = dBinv(FIRdBmin);
15187 hb3 = omega->size[1];
15188 i0 = wg->size[0] * wg->size[1];
15189 wg->size[0] = 1;
15190 wg->size[1] = omega->size[1];
15191 emxEnsureCapacity_real_T(wg, i0);
15192 for (dec_int3 = 0; dec_int3 + 1 <= hb3; dec_int3++) {
15193 if ((omega->data[dec_int3] > sigma) || rtIsNaN(sigma)) {
15194 sigmax = omega->data[dec_int3];
15195 } else {
15196 sigmax = sigma;
15197 }
15198
15199 wg->data[dec_int3] = sigmax;
15200 }
15201
15202 if (phEQ == -1.0) {
15203 b_abs(rgN, sw);
15204 i0 = rgN->size[0] * rgN->size[1];
15205 rgN->size[0] = 1;
15206 rgN->size[1] = sw->size[1];
15207 emxEnsureCapacity_creal_T(rgN, i0);
15208 loop_ub = sw->size[0] * sw->size[1];
15209 for (i0 = 0; i0 < loop_ub; i0++) {
15210 rgN->data[i0].re = sw->data[i0];
15211 rgN->data[i0].im = 0.0;
15212 }
15213 }
15214
15215 emxInit_real_T(&weight, 2);
15216 b_abs(rg1, sw);
15217 rdivide(sw, dBinv(Apass / 2.0) - 1.0, fg);
15218 i0 = weight->size[0] * weight->size[1];
15219 weight->size[0] = 1;
15220 weight->size[1] = fg->size[1] + wg->size[1];
15221 emxEnsureCapacity_real_T(weight, i0);
15222 loop_ub = fg->size[1];
15223 for (i0 = 0; i0 < loop_ub; i0++) {
15224 weight->data[weight->size[0] * i0] = fg->data[fg->size[0] * i0];
15225 }
15226
15227 loop_ub = wg->size[1];
15228 for (i0 = 0; i0 < loop_ub; i0++) {
15229 weight->data[weight->size[0] * (i0 + fg->size[1])] = wg->data[wg->size[0] *
15230 i0];
15231 }
15232
15233 hb3 = 1;
15234 n = weight->size[1];
15235 sigma = weight->data[0];
15236 if (weight->size[1] > 1) {
15237 if (rtIsNaN(weight->data[0])) {
15238 dec_int3 = 2;
15239 exitg1 = false;
15240 while ((!exitg1) && (dec_int3 <= n)) {
15241 hb3 = dec_int3;
15242 if (!rtIsNaN(weight->data[dec_int3 - 1])) {
15243 sigma = weight->data[dec_int3 - 1];
15244 exitg1 = true;
15245 } else {
15246 dec_int3++;
15247 }
15248 }
15249 }
15250
15251 if (hb3 < weight->size[1]) {
15252 while (hb3 + 1 <= n) {
15253 if (weight->data[hb3] > sigma) {
15254 sigma = weight->data[hb3];
15255 }
15256
15257 hb3++;
15258 }
15259 }
15260 }
15261
15262 i0 = a->size[0] * a->size[1];
15263 a->size[0] = 1;
15264 a->size[1] = weight->size[1];
15265 emxEnsureCapacity_real_T(a, i0);
15266 loop_ub = weight->size[0] * weight->size[1];
15267 for (i0 = 0; i0 < loop_ub; i0++) {
15268 a->data[i0] = weight->data[i0];
15269 }
15270
15271 rdivide(a, sigma, weight);
15272
15273 /* Set up design for FIR filter */
15274 i0 = fg->size[0] * fg->size[1];
15275 fg->size[0] = 1;
15276 fg->size[1] = rgN->size[1];
15277 emxEnsureCapacity_real_T(fg, i0);
15278 loop_ub = rgN->size[0] * rgN->size[1];
15279 for (i0 = 0; i0 < loop_ub; i0++) {
15280 fg->data[i0] = rgN->data[i0].re;
15281 }
15282
15283 if (1.0 > Gpass + 1.0) {
15284 loop_ub = 0;
15285 } else {
15286 loop_ub = (int)(Gpass + 1.0);
15287 }
15288
15289 emxInit_real_T(&F1, 2);
15290 i0 = F1->size[0] * F1->size[1];
15291 F1->size[0] = 1;
15292 F1->size[1] = loop_ub;
15293 emxEnsureCapacity_real_T(F1, i0);
15294 for (i0 = 0; i0 < loop_ub; i0++) {
15295 F1->data[F1->size[0] * i0] = fg2->data[i0] * 2.0;
15296 }
15297
15298 if (Gpass + 2.0 > fg2->size[1]) {
15299 i0 = 0;
15300 n = 0;
15301 } else {
15302 i0 = (int)(Gpass + 2.0) - 1;
15303 n = fg2->size[1];
15304 }
15305
15306 emxInit_real_T(&F2, 2);
15307 hb3 = F2->size[0] * F2->size[1];
15308 F2->size[0] = 1;
15309 F2->size[1] = n - i0;
15310 emxEnsureCapacity_real_T(F2, hb3);
15311 loop_ub = n - i0;
15312 for (n = 0; n < loop_ub; n++) {
15313 F2->data[F2->size[0] * n] = fg2->data[i0 + n] * 2.0;
15314 }
15315
15316 if (1.0 > Gpass + 1.0) {
15317 loop_ub = 0;
15318 } else {
15319 loop_ub = (int)(Gpass + 1.0);
15320 }
15321
15322 emxInit_real_T(&A1, 2);
15323 i0 = A1->size[0] * A1->size[1];
15324 A1->size[0] = 1;
15325 A1->size[1] = loop_ub;
15326 emxEnsureCapacity_real_T(A1, i0);
15327 for (i0 = 0; i0 < loop_ub; i0++) {
15328 A1->data[A1->size[0] * i0] = fg->data[i0];
15329 }
15330
15331 if (Gpass + 2.0 > fg->size[1]) {
15332 i0 = 0;
15333 n = 0;
15334 } else {
15335 i0 = (int)(Gpass + 2.0) - 1;
15336 n = fg->size[1];
15337 }
15338
15339 emxInit_real_T(&A2, 2);
15340 hb3 = A2->size[0] * A2->size[1];
15341 A2->size[0] = 1;
15342 A2->size[1] = n - i0;
15343 emxEnsureCapacity_real_T(A2, hb3);
15344 loop_ub = n - i0;
15345 for (n = 0; n < loop_ub; n++) {
15346 A2->data[A2->size[0] * n] = fg->data[i0 + n];
15347 }
15348
15349 if (1.0 > Gpass + 1.0) {
15350 loop_ub = 0;
15351 } else {
15352 loop_ub = (int)(Gpass + 1.0);
15353 }
15354
15355 emxInit_real_T(&W1, 2);
15356 i0 = W1->size[0] * W1->size[1];
15357 W1->size[0] = 1;
15358 W1->size[1] = loop_ub;
15359 emxEnsureCapacity_real_T(W1, i0);
15360 for (i0 = 0; i0 < loop_ub; i0++) {
15361 W1->data[W1->size[0] * i0] = weight->data[i0];
15362 }
15363
15364 if (Gpass + 2.0 > weight->size[1]) {
15365 i0 = 0;
15366 n = 0;
15367 } else {
15368 i0 = (int)(Gpass + 2.0) - 1;
15369 n = weight->size[1];
15370 }
15371
15372 emxInit_real_T(&W2, 2);
15373 hb3 = W2->size[0] * W2->size[1];
15374 W2->size[0] = 1;
15375 W2->size[1] = n - i0;
15376 emxEnsureCapacity_real_T(W2, hb3);
15377 loop_ub = n - i0;
15378 for (n = 0; n < loop_ub; n++) {
15379 W2->data[W2->size[0] * n] = weight->data[i0 + n];
15380 }
15381
15382 emxInit_real_T(&tap_store, 2);
15383
15384 /* % Determine the number of taps for FIR */
15385 /* if strcmp(input.RxTx, 'Rx') */
15386 /* if hb3 == 1 */
15387 /* N = min(16*floor(input.converter_rate/(input.Rdata)),128); */
15388 /* else */
15389 /* N = min(16*floor(input.converter_rate/(2*input.Rdata)),128); */
15390 /* end */
15391 /* else */
15392 /* switch input.FIR */
15393 /* case 1 */
15394 /* Nmax = 64; */
15395 /* case 2 */
15396 /* Nmax = 128; */
15397 /* case 4 */
15398 /* Nmax = 128; */
15399 /* otherwise */
15400 /* error('Wrong FIR Type'); */
15401 /* end */
15402 /* N = min(16*floor(input.converter_rate*input.DAC_div/(2*input.Rdata)),Nmax); */
15403 /* end */
15404 clkFIR = maxTaps;
15405 sigma = maxTaps / 16.0;
15406 i0 = tap_store->size[0] * tap_store->size[1];
15407 tap_store->size[0] = (int)sigma;
15408 tap_store->size[1] = (int)maxTaps;
15409 emxEnsureCapacity_real_T(tap_store, i0);
15410 loop_ub = (int)sigma * (int)maxTaps;
15411 for (i0 = 0; i0 < loop_ub; i0++) {
15412 tap_store->data[i0] = 0.0;
15413 }
15414
15415 emxInit_real_T1(&Apass_actual_vector, 1);
15416 sigma = maxTaps / 16.0;
15417 i0 = Apass_actual_vector->size[0];
15418 Apass_actual_vector->size[0] = (int)sigma;
15419 emxEnsureCapacity_real_T1(Apass_actual_vector, i0);
15420 loop_ub = (int)sigma;
15421 for (i0 = 0; i0 < loop_ub; i0++) {
15422 Apass_actual_vector->data[i0] = 0.0;
15423 }
15424
15425 emxInit_real_T1(&Astop_actual_vector, 1);
15426 sigma = maxTaps / 16.0;
15427 i0 = Astop_actual_vector->size[0];
15428 Astop_actual_vector->size[0] = (int)sigma;
15429 emxEnsureCapacity_real_T1(Astop_actual_vector, i0);
15430 loop_ub = (int)sigma;
15431 for (i0 = 0; i0 < loop_ub; i0++) {
15432 Astop_actual_vector->data[i0] = 0.0;
15433 }
15434
15435 i = 1U;
15436
15437 /* Design filter */
15438 emxInit_real_T(&ccoef, 2);
15439 emxInit_real_T(&F4, 2);
15440 emxInit_real_T(&b_W1, 2);
15441 do {
15442 exitg2 = 0;
15443 if (int_FIR != 0.0) {
15444 b1[0] = F1->data[0];
15445 b1[1] = F1->data[F1->size[1] - 1];
15446 b1[2] = F2->data[0];
15447 b1[3] = F2->data[F2->size[1] - 1];
15448 i0 = a->size[0] * a->size[1];
15449 a->size[0] = 1;
15450 a->size[1] = A1->size[1] + A2->size[1];
15451 emxEnsureCapacity_real_T(a, i0);
15452 loop_ub = A1->size[1];
15453 for (i0 = 0; i0 < loop_ub; i0++) {
15454 a->data[a->size[0] * i0] = A1->data[A1->size[0] * i0];
15455 }
15456
15457 loop_ub = A2->size[1];
15458 for (i0 = 0; i0 < loop_ub; i0++) {
15459 a->data[a->size[0] * (i0 + A1->size[1])] = A2->data[A2->size[0] * i0];
15460 }
15461
15462 i0 = b_omega2->size[0] * b_omega2->size[1];
15463 b_omega2->size[0] = 1;
15464 b_omega2->size[1] = F1->size[1] + F2->size[1];
15465 emxEnsureCapacity_real_T(b_omega2, i0);
15466 loop_ub = F1->size[1];
15467 for (i0 = 0; i0 < loop_ub; i0++) {
15468 b_omega2->data[b_omega2->size[0] * i0] = F1->data[F1->size[0] * i0];
15469 }
15470
15471 loop_ub = F2->size[1];
15472 for (i0 = 0; i0 < loop_ub; i0++) {
15473 b_omega2->data[b_omega2->size[0] * (i0 + F1->size[1])] = F2->data
15474 [F2->size[0] * i0];
15475 }
15476
15477 i0 = b_W1->size[0] * b_W1->size[1];
15478 b_W1->size[0] = 1;
15479 b_W1->size[1] = W1->size[1] + W2->size[1];
15480 emxEnsureCapacity_real_T(b_W1, i0);
15481 loop_ub = W1->size[1];
15482 for (i0 = 0; i0 < loop_ub; i0++) {
15483 b_W1->data[b_W1->size[0] * i0] = W1->data[W1->size[0] * i0];
15484 }
15485
15486 loop_ub = W2->size[1];
15487 for (i0 = 0; i0 < loop_ub; i0++) {
15488 b_W1->data[b_W1->size[0] * (i0 + W1->size[1])] = W2->data[W2->size[0] *
15489 i0];
15490 }
15491
15492 firpm_cg(clkFIR - 1.0, b1, a, b_omega2, b_W1, ccoef);
15493 } else {
15494 /* Check different designs until we reach required ripple condition */
15495 sigma = db2mag(-Astop);
15496
15497 /* Peak Ripple */
15498 i0 = ccoef->size[0] * ccoef->size[1];
15499 ccoef->size[0] = 1;
15500 ccoef->size[1] = 1;
15501 emxEnsureCapacity_real_T(ccoef, i0);
15502 ccoef->data[0] = 0.0;
15503
15504 /* Predef type */
15505 dec_int3 = 0;
15506 exitg1 = false;
15507 while ((!exitg1) && (dec_int3 < 126)) {
15508 b1[0] = F1->data[0];
15509 b1[1] = F1->data[F1->size[1] - 1];
15510 b1[2] = F2->data[0];
15511 b1[3] = F2->data[F2->size[1] - 1];
15512 i0 = a->size[0] * a->size[1];
15513 a->size[0] = 1;
15514 a->size[1] = A1->size[1] + A2->size[1];
15515 emxEnsureCapacity_real_T(a, i0);
15516 loop_ub = A1->size[1];
15517 for (i0 = 0; i0 < loop_ub; i0++) {
15518 a->data[a->size[0] * i0] = A1->data[A1->size[0] * i0];
15519 }
15520
15521 loop_ub = A2->size[1];
15522 for (i0 = 0; i0 < loop_ub; i0++) {
15523 a->data[a->size[0] * (i0 + A1->size[1])] = A2->data[A2->size[0] * i0];
15524 }
15525
15526 i0 = b_omega2->size[0] * b_omega2->size[1];
15527 b_omega2->size[0] = 1;
15528 b_omega2->size[1] = F1->size[1] + F2->size[1];
15529 emxEnsureCapacity_real_T(b_omega2, i0);
15530 loop_ub = F1->size[1];
15531 for (i0 = 0; i0 < loop_ub; i0++) {
15532 b_omega2->data[b_omega2->size[0] * i0] = F1->data[F1->size[0] * i0];
15533 }
15534
15535 loop_ub = F2->size[1];
15536 for (i0 = 0; i0 < loop_ub; i0++) {
15537 b_omega2->data[b_omega2->size[0] * (i0 + F1->size[1])] = F2->data
15538 [F2->size[0] * i0];
15539 }
15540
15541 i0 = b_W1->size[0] * b_W1->size[1];
15542 b_W1->size[0] = 1;
15543 b_W1->size[1] = W1->size[1] + W2->size[1];
15544 emxEnsureCapacity_real_T(b_W1, i0);
15545 loop_ub = W1->size[1];
15546 for (i0 = 0; i0 < loop_ub; i0++) {
15547 b_W1->data[b_W1->size[0] * i0] = W1->data[W1->size[0] * i0];
15548 }
15549
15550 loop_ub = W2->size[1];
15551 for (i0 = 0; i0 < loop_ub; i0++) {
15552 b_W1->data[b_W1->size[0] * (i0 + W1->size[1])] = W2->data[W2->size[0] *
15553 i0];
15554 }
15555
15556 b_firpm_cg(3.0 + (double)dec_int3, b1, a, b_omega2, b_W1, ccoef, &valid,
15557 &sigmax);
15558
15559 /* Check if design meets specs */
15560 if ((sigmax < sigma) && valid) {
15561 exitg1 = true;
15562 } else {
15563 dec_int3++;
15564 }
15565 }
15566 }
15567
15568 /* Enable phase equalization and apply update to taps */
15569 if (phEQ != -1.0) {
15570 if (1 > fg2->size[1]) {
15571 i0 = 1;
15572 n = 1;
15573 hb3 = 0;
15574 } else {
15575 i0 = fg2->size[1];
15576 n = -1;
15577 hb3 = 1;
15578 }
15579
15580 i2 = fg->size[0] * fg->size[1];
15581 fg->size[0] = 1;
15582 fg->size[1] = div_s32_floor(hb3 - i0, n) + 1;
15583 emxEnsureCapacity_real_T(fg, i2);
15584 loop_ub = div_s32_floor(hb3 - i0, n);
15585 for (hb3 = 0; hb3 <= loop_ub; hb3++) {
15586 fg->data[fg->size[0] * hb3] = 0.5 - fg2->data[(i0 + n * hb3) - 1];
15587 }
15588
15589 if (1 > rgN->size[1]) {
15590 i0 = 1;
15591 n = 1;
15592 hb3 = 0;
15593 } else {
15594 i0 = rgN->size[1];
15595 n = -1;
15596 hb3 = 1;
15597 }
15598
15599 i2 = omega->size[0] * omega->size[1];
15600 omega->size[0] = 1;
15601 omega->size[1] = div_s32_floor(hb3 - i0, n) + 1;
15602 emxEnsureCapacity_real_T(omega, i2);
15603 loop_ub = div_s32_floor(hb3 - i0, n);
15604 for (hb3 = 0; hb3 <= loop_ub; hb3++) {
15605 omega->data[omega->size[0] * hb3] = rgN->data[(i0 + n * hb3) - 1].im;
15606 }
15607
15608 if (1 > weight->size[1]) {
15609 i0 = 1;
15610 n = 1;
15611 hb3 = 0;
15612 } else {
15613 i0 = weight->size[1];
15614 n = -1;
15615 hb3 = 1;
15616 }
15617
15618 i2 = sw->size[0] * sw->size[1];
15619 sw->size[0] = 1;
15620 sw->size[1] = div_s32_floor(hb3 - i0, n) + 1;
15621 emxEnsureCapacity_real_T(sw, i2);
15622 loop_ub = div_s32_floor(hb3 - i0, n);
15623 for (i2 = 0; i2 <= loop_ub; i2++) {
15624 sw->data[sw->size[0] * i2] = weight->data[(i0 + n * i2) - 1];
15625 }
15626
15627 if (1.0 > (8192.0 - Gstop) + 1.0) {
15628 loop_ub = 0;
15629 } else {
15630 loop_ub = (int)((8192.0 - Gstop) + 1.0);
15631 }
15632
15633 i2 = wg->size[0] * wg->size[1];
15634 wg->size[0] = 1;
15635 wg->size[1] = loop_ub;
15636 emxEnsureCapacity_real_T(wg, i2);
15637 for (i2 = 0; i2 < loop_ub; i2++) {
15638 wg->data[wg->size[0] * i2] = fg->data[i2] * 2.0;
15639 }
15640
15641 if ((8192.0 - Gstop) + 2.0 > fg->size[1]) {
15642 i2 = 0;
15643 dec_int3 = 0;
15644 } else {
15645 i2 = (int)((8192.0 - Gstop) + 2.0) - 1;
15646 dec_int3 = fg->size[1];
15647 }
15648
15649 i3 = F4->size[0] * F4->size[1];
15650 F4->size[0] = 1;
15651 F4->size[1] = dec_int3 - i2;
15652 emxEnsureCapacity_real_T(F4, i3);
15653 loop_ub = dec_int3 - i2;
15654 for (dec_int3 = 0; dec_int3 < loop_ub; dec_int3++) {
15655 F4->data[F4->size[0] * dec_int3] = fg->data[i2 + dec_int3] * 2.0;
15656 }
15657
15658 if (1.0 > (8192.0 - Gstop) + 1.0) {
15659 loop_ub = 0;
15660 } else {
15661 loop_ub = (int)((8192.0 - Gstop) + 1.0);
15662 }
15663
15664 if ((8192.0 - Gstop) + 2.0 > omega->size[1]) {
15665 i2 = 0;
15666 dec_int3 = 0;
15667 } else {
15668 i2 = (int)((8192.0 - Gstop) + 2.0) - 1;
15669 dec_int3 = omega->size[1];
15670 }
15671
15672 if (1.0 > (8192.0 - Gstop) + 1.0) {
15673 b_loop_ub = 0;
15674 } else {
15675 b_loop_ub = (int)((8192.0 - Gstop) + 1.0);
15676 }
15677
15678 if ((8192.0 - Gstop) + 2.0 > div_s32_floor(hb3 - i0, n) + 1) {
15679 i3 = 0;
15680 i0 = -1;
15681 } else {
15682 i3 = (int)((8192.0 - Gstop) + 2.0) - 1;
15683 i0 = div_s32_floor(hb3 - i0, n);
15684 }
15685
15686 if (int_FIR != 0.0) {
15687 sigma = clkFIR - 1.0;
15688 } else {
15689 sigma = (double)ccoef->size[1] - 1.0;
15690 }
15691
15692 b1[0] = wg->data[0];
15693 b1[1] = wg->data[wg->size[1] - 1];
15694 b1[2] = F4->data[0];
15695 b1[3] = F4->data[F4->size[1] - 1];
15696 n = b_W1->size[0] * b_W1->size[1];
15697 b_W1->size[0] = 1;
15698 b_W1->size[1] = (loop_ub + dec_int3) - i2;
15699 emxEnsureCapacity_real_T(b_W1, n);
15700 for (n = 0; n < loop_ub; n++) {
15701 b_W1->data[b_W1->size[0] * n] = omega->data[n];
15702 }
15703
15704 hb3 = dec_int3 - i2;
15705 for (n = 0; n < hb3; n++) {
15706 b_W1->data[b_W1->size[0] * (n + loop_ub)] = omega->data[i2 + n];
15707 }
15708
15709 n = a->size[0] * a->size[1];
15710 a->size[0] = 1;
15711 a->size[1] = wg->size[1] + F4->size[1];
15712 emxEnsureCapacity_real_T(a, n);
15713 loop_ub = wg->size[1];
15714 for (n = 0; n < loop_ub; n++) {
15715 a->data[a->size[0] * n] = wg->data[wg->size[0] * n];
15716 }
15717
15718 loop_ub = F4->size[1];
15719 for (n = 0; n < loop_ub; n++) {
15720 a->data[a->size[0] * (n + wg->size[1])] = F4->data[F4->size[0] * n];
15721 }
15722
15723 n = b_omega2->size[0] * b_omega2->size[1];
15724 b_omega2->size[0] = 1;
15725 b_omega2->size[1] = ((b_loop_ub + i0) - i3) + 1;
15726 emxEnsureCapacity_real_T(b_omega2, n);
15727 for (n = 0; n < b_loop_ub; n++) {
15728 b_omega2->data[b_omega2->size[0] * n] = sw->data[n];
15729 }
15730
15731 loop_ub = i0 - i3;
15732 for (i0 = 0; i0 <= loop_ub; i0++) {
15733 b_omega2->data[b_omega2->size[0] * (i0 + b_loop_ub)] = sw->data[i3 + i0];
15734 }
15735
15736 firpm_cg(sigma, b1, b_W1, a, b_omega2, fg);
15737 i0 = fg->size[1];
15738 for (dec_int3 = 0; dec_int3 < i0; dec_int3++) {
15739 fg->data[dec_int3] = -fg->data[dec_int3] * mpower(-1.0, (1.0 + (double)
15740 dec_int3) - 1.0);
15741 }
15742 } else {
15743 for (i0 = 0; i0 < 2; i0++) {
15744 b2[i0] = ccoef->size[i0];
15745 }
15746
15747 i0 = fg->size[0] * fg->size[1];
15748 fg->size[0] = 1;
15749 fg->size[1] = (int)b2[1];
15750 emxEnsureCapacity_real_T(fg, i0);
15751 loop_ub = (int)b2[1];
15752 for (i0 = 0; i0 < loop_ub; i0++) {
15753 fg->data[i0] = 0.0;
15754 }
15755 }
15756
15757 loop_ub = ccoef->size[1];
15758 for (i0 = 0; i0 < loop_ub; i0++) {
15759 tap_store->data[((int)i + tap_store->size[0] * i0) - 1] = ccoef->
15760 data[ccoef->size[0] * i0] + fg->data[fg->size[0] * i0];
15761 }
15762
15763 /* scoef ==0 when no EQ */
15764 determineBestFractionLength(tap_store, i, ccoef->size[1], a);
15765 loop_ub = a->size[1];
15766 for (i0 = 0; i0 < loop_ub; i0++) {
15767 tap_store->data[((int)i + tap_store->size[0] * i0) - 1] = a->data[a->size
15768 [0] * i0];
15769 }
15770
15771 if (b_strcmp(RxTx)) {
15772 if (1.0 > Gpass + 1.0) {
15773 loop_ub = 0;
15774 } else {
15775 loop_ub = (int)(Gpass + 1.0);
15776 }
15777
15778 if (1 > ccoef->size[1]) {
15779 b_loop_ub = 0;
15780 } else {
15781 b_loop_ub = ccoef->size[1];
15782 }
15783
15784 i0 = b_omega2->size[0] * b_omega2->size[1];
15785 b_omega2->size[0] = 1;
15786 b_omega2->size[1] = loop_ub;
15787 emxEnsureCapacity_real_T(b_omega2, i0);
15788 for (i0 = 0; i0 < loop_ub; i0++) {
15789 b_omega2->data[b_omega2->size[0] * i0] = omega2->data[i0];
15790 }
15791
15792 i0 = a->size[0] * a->size[1];
15793 a->size[0] = 1;
15794 a->size[1] = b_loop_ub;
15795 emxEnsureCapacity_real_T(a, i0);
15796 for (i0 = 0; i0 < b_loop_ub; i0++) {
15797 a->data[a->size[0] * i0] = tap_store->data[((int)i + tap_store->size[0] *
15798 i0) - 1];
15799 }
15800
15801 c_generateCascadedResponseRx(enables, b_omega2, converter_rate, hb1_coeff,
15802 hb2_coeff, hb3_coeff_data, hb3_coeff_size, dec_int3_coeff_data,
15803 dec_int3_coeff_size, a, c_combinedResponse);
15804 if (Gpass + 2.0 > omega2->size[1]) {
15805 i0 = 0;
15806 n = 0;
15807 } else {
15808 i0 = (int)(Gpass + 2.0) - 1;
15809 n = omega2->size[1];
15810 }
15811
15812 if (1 > ccoef->size[1]) {
15813 loop_ub = 0;
15814 } else {
15815 loop_ub = ccoef->size[1];
15816 }
15817
15818 hb3 = b_omega2->size[0] * b_omega2->size[1];
15819 b_omega2->size[0] = 1;
15820 b_omega2->size[1] = n - i0;
15821 emxEnsureCapacity_real_T(b_omega2, hb3);
15822 b_loop_ub = n - i0;
15823 for (n = 0; n < b_loop_ub; n++) {
15824 b_omega2->data[b_omega2->size[0] * n] = omega2->data[i0 + n];
15825 }
15826
15827 i0 = a->size[0] * a->size[1];
15828 a->size[0] = 1;
15829 a->size[1] = loop_ub;
15830 emxEnsureCapacity_real_T(a, i0);
15831 for (i0 = 0; i0 < loop_ub; i0++) {
15832 a->data[a->size[0] * i0] = tap_store->data[((int)i + tap_store->size[0] *
15833 i0) - 1];
15834 }
15835
15836 c_generateCascadedResponseRx(enables, b_omega2, converter_rate, hb1_coeff,
15837 hb2_coeff, hb3_coeff_data, hb3_coeff_size, dec_int3_coeff_data,
15838 dec_int3_coeff_size, a, rg1);
15839 if (1.0 > Gpass + 1.0) {
15840 loop_ub = 0;
15841 } else {
15842 loop_ub = (int)(Gpass + 1.0);
15843 }
15844
15845 i0 = b_omega2->size[0] * b_omega2->size[1];
15846 b_omega2->size[0] = 1;
15847 b_omega2->size[1] = loop_ub;
15848 emxEnsureCapacity_real_T(b_omega2, i0);
15849 for (i0 = 0; i0 < loop_ub; i0++) {
15850 b_omega2->data[b_omega2->size[0] * i0] = omega2->data[i0];
15851 }
15852
15853 c_analogresp(b_omega2, converter_rate, b1_data, b1_size, a1, b2_data,
15854 b2_size, a2, r0);
15855 i0 = d_combinedResponse->size[0] * d_combinedResponse->size[1];
15856 d_combinedResponse->size[0] = 1;
15857 d_combinedResponse->size[1] = r0->size[1];
15858 emxEnsureCapacity_creal_T(d_combinedResponse, i0);
15859 loop_ub = r0->size[0] * r0->size[1];
15860 for (i0 = 0; i0 < loop_ub; i0++) {
15861 re = r0->data[i0].re;
15862 im = r0->data[i0].im;
15863 sigmax = c_combinedResponse->data[i0].re;
15864 apnd = c_combinedResponse->data[i0].im;
15865 d_combinedResponse->data[i0].re = re * sigmax - im * apnd;
15866 d_combinedResponse->data[i0].im = re * apnd + im * sigmax;
15867 }
15868
15869 b_abs(d_combinedResponse, fg);
15870 if (Gpass + 2.0 > omega2->size[1]) {
15871 i0 = 0;
15872 n = 0;
15873 } else {
15874 i0 = (int)(Gpass + 2.0) - 1;
15875 n = omega2->size[1];
15876 }
15877
15878 hb3 = b_omega2->size[0] * b_omega2->size[1];
15879 b_omega2->size[0] = 1;
15880 b_omega2->size[1] = n - i0;
15881 emxEnsureCapacity_real_T(b_omega2, hb3);
15882 loop_ub = n - i0;
15883 for (n = 0; n < loop_ub; n++) {
15884 b_omega2->data[b_omega2->size[0] * n] = omega2->data[i0 + n];
15885 }
15886
15887 c_analogresp(b_omega2, converter_rate, b1_data, b1_size, a1, b2_data,
15888 b2_size, a2, r0);
15889 i0 = d_combinedResponse->size[0] * d_combinedResponse->size[1];
15890 d_combinedResponse->size[0] = 1;
15891 d_combinedResponse->size[1] = r0->size[1];
15892 emxEnsureCapacity_creal_T(d_combinedResponse, i0);
15893 loop_ub = r0->size[0] * r0->size[1];
15894 for (i0 = 0; i0 < loop_ub; i0++) {
15895 re = r0->data[i0].re;
15896 im = r0->data[i0].im;
15897 absa = rg1->data[i0].re;
15898 rg1_im = rg1->data[i0].im;
15899 d_combinedResponse->data[i0].re = re * absa - im * rg1_im;
15900 d_combinedResponse->data[i0].im = re * rg1_im + im * absa;
15901 }
15902
15903 b_abs(d_combinedResponse, omega);
15904 } else {
15905 /* TX */
15906 if (1.0 > Gpass + 1.0) {
15907 loop_ub = 0;
15908 } else {
15909 loop_ub = (int)(Gpass + 1.0);
15910 }
15911
15912 if (1 > ccoef->size[1]) {
15913 b_loop_ub = 0;
15914 } else {
15915 b_loop_ub = ccoef->size[1];
15916 }
15917
15918 i0 = b_omega2->size[0] * b_omega2->size[1];
15919 b_omega2->size[0] = 1;
15920 b_omega2->size[1] = loop_ub;
15921 emxEnsureCapacity_real_T(b_omega2, i0);
15922 for (i0 = 0; i0 < loop_ub; i0++) {
15923 b_omega2->data[b_omega2->size[0] * i0] = omega2->data[i0];
15924 }
15925
15926 i0 = a->size[0] * a->size[1];
15927 a->size[0] = 1;
15928 a->size[1] = b_loop_ub;
15929 emxEnsureCapacity_real_T(a, i0);
15930 for (i0 = 0; i0 < b_loop_ub; i0++) {
15931 a->data[a->size[0] * i0] = tap_store->data[((int)i + tap_store->size[0] *
15932 i0) - 1];
15933 }
15934
15935 c_generateCascadedResponseRx(enables, b_omega2, converter_rate, hb1_coeff,
15936 hb2_coeff, hb3_coeff_data, hb3_coeff_size, dec_int3_coeff_data,
15937 dec_int3_coeff_size, a, c_combinedResponse);
15938 if (Gpass + 2.0 > omega2->size[1]) {
15939 i0 = 0;
15940 n = 0;
15941 } else {
15942 i0 = (int)(Gpass + 2.0) - 1;
15943 n = omega2->size[1];
15944 }
15945
15946 if (1 > ccoef->size[1]) {
15947 loop_ub = 0;
15948 } else {
15949 loop_ub = ccoef->size[1];
15950 }
15951
15952 hb3 = b_omega2->size[0] * b_omega2->size[1];
15953 b_omega2->size[0] = 1;
15954 b_omega2->size[1] = n - i0;
15955 emxEnsureCapacity_real_T(b_omega2, hb3);
15956 b_loop_ub = n - i0;
15957 for (n = 0; n < b_loop_ub; n++) {
15958 b_omega2->data[b_omega2->size[0] * n] = omega2->data[i0 + n];
15959 }
15960
15961 i0 = a->size[0] * a->size[1];
15962 a->size[0] = 1;
15963 a->size[1] = loop_ub;
15964 emxEnsureCapacity_real_T(a, i0);
15965 for (i0 = 0; i0 < loop_ub; i0++) {
15966 a->data[a->size[0] * i0] = tap_store->data[((int)i + tap_store->size[0] *
15967 i0) - 1];
15968 }
15969
15970 c_generateCascadedResponseRx(enables, b_omega2, converter_rate, hb1_coeff,
15971 hb2_coeff, hb3_coeff_data, hb3_coeff_size, dec_int3_coeff_data,
15972 dec_int3_coeff_size, a, rg1);
15973 if (1.0 > Gpass + 1.0) {
15974 loop_ub = 0;
15975 } else {
15976 loop_ub = (int)(Gpass + 1.0);
15977 }
15978
15979 i0 = b_omega2->size[0] * b_omega2->size[1];
15980 b_omega2->size[0] = 1;
15981 b_omega2->size[1] = loop_ub;
15982 emxEnsureCapacity_real_T(b_omega2, i0);
15983 for (i0 = 0; i0 < loop_ub; i0++) {
15984 b_omega2->data[b_omega2->size[0] * i0] = omega2->data[i0];
15985 }
15986
15987 d_analogresp(b_omega2, converter_rate, b1_data, b1_size, a1, b2_data,
15988 b2_size, a2, r0);
15989 i0 = d_combinedResponse->size[0] * d_combinedResponse->size[1];
15990 d_combinedResponse->size[0] = 1;
15991 d_combinedResponse->size[1] = c_combinedResponse->size[1];
15992 emxEnsureCapacity_creal_T(d_combinedResponse, i0);
15993 loop_ub = c_combinedResponse->size[0] * c_combinedResponse->size[1];
15994 for (i0 = 0; i0 < loop_ub; i0++) {
15995 sigmax = c_combinedResponse->data[i0].re;
15996 apnd = c_combinedResponse->data[i0].im;
15997 re = r0->data[i0].re;
15998 im = r0->data[i0].im;
15999 d_combinedResponse->data[i0].re = sigmax * re - apnd * im;
16000 d_combinedResponse->data[i0].im = sigmax * im + apnd * re;
16001 }
16002
16003 b_abs(d_combinedResponse, fg);
16004 if (Gpass + 2.0 > omega2->size[1]) {
16005 i0 = 0;
16006 n = 0;
16007 } else {
16008 i0 = (int)(Gpass + 2.0) - 1;
16009 n = omega2->size[1];
16010 }
16011
16012 hb3 = b_omega2->size[0] * b_omega2->size[1];
16013 b_omega2->size[0] = 1;
16014 b_omega2->size[1] = n - i0;
16015 emxEnsureCapacity_real_T(b_omega2, hb3);
16016 loop_ub = n - i0;
16017 for (n = 0; n < loop_ub; n++) {
16018 b_omega2->data[b_omega2->size[0] * n] = omega2->data[i0 + n];
16019 }
16020
16021 d_analogresp(b_omega2, converter_rate, b1_data, b1_size, a1, b2_data,
16022 b2_size, a2, r0);
16023 i0 = d_combinedResponse->size[0] * d_combinedResponse->size[1];
16024 d_combinedResponse->size[0] = 1;
16025 d_combinedResponse->size[1] = rg1->size[1];
16026 emxEnsureCapacity_creal_T(d_combinedResponse, i0);
16027 loop_ub = rg1->size[0] * rg1->size[1];
16028 for (i0 = 0; i0 < loop_ub; i0++) {
16029 absa = rg1->data[i0].re;
16030 rg1_im = rg1->data[i0].im;
16031 re = r0->data[i0].re;
16032 im = r0->data[i0].im;
16033 d_combinedResponse->data[i0].re = absa * re - rg1_im * im;
16034 d_combinedResponse->data[i0].im = absa * im + rg1_im * re;
16035 }
16036
16037 b_abs(d_combinedResponse, omega);
16038 }
16039
16040 /* quantitative values about actual passband and stopband */
16041 hb3 = 1;
16042 n = fg->size[1];
16043 sigma = fg->data[0];
16044 if (fg->size[1] > 1) {
16045 if (rtIsNaN(fg->data[0])) {
16046 dec_int3 = 2;
16047 exitg1 = false;
16048 while ((!exitg1) && (dec_int3 <= n)) {
16049 hb3 = dec_int3;
16050 if (!rtIsNaN(fg->data[dec_int3 - 1])) {
16051 sigma = fg->data[dec_int3 - 1];
16052 exitg1 = true;
16053 } else {
16054 dec_int3++;
16055 }
16056 }
16057 }
16058
16059 if (hb3 < fg->size[1]) {
16060 while (hb3 + 1 <= n) {
16061 if (fg->data[hb3] > sigma) {
16062 sigma = fg->data[hb3];
16063 }
16064
16065 hb3++;
16066 }
16067 }
16068 }
16069
16070 hb3 = 1;
16071 n = fg->size[1];
16072 sigmax = fg->data[0];
16073 if (fg->size[1] > 1) {
16074 if (rtIsNaN(fg->data[0])) {
16075 dec_int3 = 2;
16076 exitg1 = false;
16077 while ((!exitg1) && (dec_int3 <= n)) {
16078 hb3 = dec_int3;
16079 if (!rtIsNaN(fg->data[dec_int3 - 1])) {
16080 sigmax = fg->data[dec_int3 - 1];
16081 exitg1 = true;
16082 } else {
16083 dec_int3++;
16084 }
16085 }
16086 }
16087
16088 if (hb3 < fg->size[1]) {
16089 while (hb3 + 1 <= n) {
16090 if (fg->data[hb3] < sigmax) {
16091 sigmax = fg->data[hb3];
16092 }
16093
16094 hb3++;
16095 }
16096 }
16097 }
16098
16099 Apass_actual_vector->data[(int)i - 1] = mag2db(sigma) - mag2db(sigmax);
16100 hb3 = 1;
16101 n = omega->size[1];
16102 sigma = omega->data[0];
16103 if (omega->size[1] > 1) {
16104 if (rtIsNaN(omega->data[0])) {
16105 dec_int3 = 2;
16106 exitg1 = false;
16107 while ((!exitg1) && (dec_int3 <= n)) {
16108 hb3 = dec_int3;
16109 if (!rtIsNaN(omega->data[dec_int3 - 1])) {
16110 sigma = omega->data[dec_int3 - 1];
16111 exitg1 = true;
16112 } else {
16113 dec_int3++;
16114 }
16115 }
16116 }
16117
16118 if (hb3 < omega->size[1]) {
16119 while (hb3 + 1 <= n) {
16120 if (omega->data[hb3] > sigma) {
16121 sigma = omega->data[hb3];
16122 }
16123
16124 hb3++;
16125 }
16126 }
16127 }
16128
16129 Astop_actual_vector->data[(int)i - 1] = -mag2db(sigma);
16130 if (int_FIR == 0.0) {
16131 if (1 > ccoef->size[1]) {
16132 loop_ub = 0;
16133 } else {
16134 loop_ub = ccoef->size[1];
16135 }
16136
16137 i0 = fg->size[0] * fg->size[1];
16138 fg->size[0] = 1;
16139 fg->size[1] = loop_ub;
16140 emxEnsureCapacity_real_T(fg, i0);
16141 for (i0 = 0; i0 < loop_ub; i0++) {
16142 fg->data[fg->size[0] * i0] = tap_store->data[tap_store->size[0] * i0];
16143 }
16144
16145 /* Apass_actual = Apass_actual_vector(1); */
16146 /* Astop_actual = Astop_actual_vector(1); */
16147 exitg2 = 1;
16148 } else if ((Apass_actual_vector->data[0] > Apass) ||
16149 (Astop_actual_vector->data[0] < Astop)) {
16150 if (1.0 > clkFIR) {
16151 loop_ub = 0;
16152 } else {
16153 loop_ub = (int)clkFIR;
16154 }
16155
16156 i0 = fg->size[0] * fg->size[1];
16157 fg->size[0] = 1;
16158 fg->size[1] = loop_ub;
16159 emxEnsureCapacity_real_T(fg, i0);
16160 for (i0 = 0; i0 < loop_ub; i0++) {
16161 fg->data[fg->size[0] * i0] = tap_store->data[tap_store->size[0] * i0];
16162 }
16163
16164 /* Apass_actual = Apass_actual_vector(1); */
16165 /* Astop_actual = Astop_actual_vector(1); */
16166 exitg2 = 1;
16167 } else if ((Apass_actual_vector->data[(int)i - 1] > Apass) ||
16168 (Astop_actual_vector->data[(int)i - 1] < Astop)) {
16169 if (1.0 > clkFIR + 16.0) {
16170 loop_ub = 0;
16171 } else {
16172 loop_ub = (int)(clkFIR + 16.0);
16173 }
16174
16175 i0 = fg->size[0] * fg->size[1];
16176 fg->size[0] = 1;
16177 fg->size[1] = loop_ub;
16178 emxEnsureCapacity_real_T(fg, i0);
16179 for (i0 = 0; i0 < loop_ub; i0++) {
16180 fg->data[fg->size[0] * i0] = tap_store->data[((int)i + tap_store->size[0]
16181 * i0) - 2];
16182 }
16183
16184 /* Apass_actual = Apass_actual_vector(i-1); */
16185 /* Astop_actual = Astop_actual_vector(i-1); */
16186 exitg2 = 1;
16187 } else {
16188 clkFIR -= 16.0;
16189 i++;
16190 }
16191 } while (exitg2 == 0);
16192
16193 emxFree_real_T(&b_omega2);
16194 emxFree_creal_T(&d_combinedResponse);
16195 emxFree_real_T(&b_W1);
16196 emxFree_creal_T(&r0);
16197 emxFree_creal_T(&c_combinedResponse);
16198 emxFree_real_T(&F4);
16199 emxFree_real_T(&sw);
16200 emxFree_real_T(&ccoef);
16201 emxFree_real_T(&Astop_actual_vector);
16202 emxFree_real_T(&Apass_actual_vector);
16203 emxFree_real_T(&tap_store);
16204 emxFree_real_T(&W2);
16205 emxFree_real_T(&W1);
16206 emxFree_real_T(&A2);
16207 emxFree_real_T(&A1);
16208 emxFree_real_T(&F2);
16209 emxFree_real_T(&F1);
16210 emxFree_real_T(&weight);
16211 emxFree_real_T(&wg);
16212 emxFree_creal_T(&rgN);
16213 emxFree_real_T(&omega2);
16214 emxFree_real_T(&fg2);
16215 emxFree_creal_T(&rg1);
16216 emxFree_real_T(&omega);
16217 emxFree_creal_T(&a2);
16218 emxFree_creal_T(&a1);
16219 if (c_strcmp(RxTx)) {
16220 if ((int_FIR == 1.0) && (FIR == 2.0)) {
16221 if (rt_remd_snf(fg->size[1], 32.0) != 0.0) {
16222 i0 = a->size[0] * a->size[1];
16223 a->size[0] = 1;
16224 a->size[1] = 16 + fg->size[1];
16225 emxEnsureCapacity_real_T(a, i0);
16226 for (i0 = 0; i0 < 8; i0++) {
16227 a->data[a->size[0] * i0] = 0.0;
16228 }
16229
16230 loop_ub = fg->size[1];
16231 for (i0 = 0; i0 < loop_ub; i0++) {
16232 a->data[a->size[0] * (i0 + 8)] = fg->data[fg->size[0] * i0];
16233 }
16234
16235 for (i0 = 0; i0 < 8; i0++) {
16236 a->data[a->size[0] * ((i0 + fg->size[1]) + 8)] = 0.0;
16237 }
16238
16239 i0 = fg->size[0] * fg->size[1];
16240 fg->size[0] = 1;
16241 fg->size[1] = a->size[1];
16242 emxEnsureCapacity_real_T(fg, i0);
16243 loop_ub = a->size[1];
16244 for (i0 = 0; i0 < loop_ub; i0++) {
16245 fg->data[fg->size[0] * i0] = a->data[a->size[0] * i0];
16246 }
16247 }
16248 } else {
16249 if ((int_FIR == 1.0) && (FIR == 4.0) && (rt_remd_snf(fg->size[1], 64.0) !=
16250 0.0)) {
16251 sigma = (ceil((double)fg->size[1] / 64.0) * 64.0 - (double)fg->size[1]) /
16252 2.0;
16253 i0 = a->size[0] * a->size[1];
16254 a->size[0] = 1;
16255 a->size[1] = ((int)sigma + fg->size[1]) + (int)sigma;
16256 emxEnsureCapacity_real_T(a, i0);
16257 loop_ub = (int)sigma;
16258 for (i0 = 0; i0 < loop_ub; i0++) {
16259 a->data[a->size[0] * i0] = 0.0;
16260 }
16261
16262 loop_ub = fg->size[1];
16263 for (i0 = 0; i0 < loop_ub; i0++) {
16264 a->data[a->size[0] * (i0 + (int)sigma)] = fg->data[fg->size[0] * i0];
16265 }
16266
16267 loop_ub = (int)sigma;
16268 for (i0 = 0; i0 < loop_ub; i0++) {
16269 a->data[a->size[0] * ((i0 + (int)sigma) + fg->size[1])] = 0.0;
16270 }
16271
16272 i0 = fg->size[0] * fg->size[1];
16273 fg->size[0] = 1;
16274 fg->size[1] = a->size[1];
16275 emxEnsureCapacity_real_T(fg, i0);
16276 loop_ub = a->size[1];
16277 for (i0 = 0; i0 < loop_ub; i0++) {
16278 fg->data[fg->size[0] * i0] = a->data[a->size[0] * i0];
16279 }
16280 }
16281 }
16282 }
16283
16284 emxFree_real_T(&a);
16285
16286 /* There will always be 128 taps output */
16287 memset(&firTapsPreScale[0], 0, sizeof(double) << 7);
16288 loop_ub = fg->size[1];
16289 for (i0 = 0; i0 < loop_ub; i0++) {
16290 firTapsPreScale[i0] = fg->data[fg->size[0] * i0];
16291 }
16292
16293 /* Calculate group delay */
16294 /* Hmd = dec_int_func(input.FIR,h(1:128)); */
16295 /* */
16296 /* if ~isempty(ver('fixedpoint')) && license('test','fixed_point_toolbox') && license('checkout','fixed_point_toolbox') */
16297 /* Hmd.Numerator = double(fi(Hmd.Numerator,true,16)); */
16298 /* end */
16299 /* if strcmp(input.RxTx, 'Rx') */
16300 /* addStage(dfilter, Hmd); */
16301 /* else */
16302 /* addStage(dfilter, Hmd, 1); */
16303 /* end */
16304 /* gd2c = grpdelay(Hmd,omega1,clkFIR).*(1/clkFIR); */
16305 /* gd2 = grpdelay_cg(firTapsPreScale,1,omega1,clkFIR).'.*(1/clkFIR); */
16306 /* */
16307 /* if input.phEQ == -1 */
16308 /* groupdelay = gd1 + gd2; */
16309 /* else */
16310 /* groupdelay = gd1 + gd2; */
16311 /* end */
16312 /* grpdelayvar = max(groupdelay)-min(groupdelay); */
16313 /* Determine Gains */
16314 hb3 = 1;
16315 sigma = firTapsPreScale[0];
16316 if (rtIsNaN(firTapsPreScale[0])) {
16317 dec_int3 = 2;
16318 exitg1 = false;
16319 while ((!exitg1) && (dec_int3 < 129)) {
16320 hb3 = dec_int3;
16321 if (!rtIsNaN(firTapsPreScale[dec_int3 - 1])) {
16322 sigma = firTapsPreScale[dec_int3 - 1];
16323 exitg1 = true;
16324 } else {
16325 dec_int3++;
16326 }
16327 }
16328 }
16329
16330 if (hb3 < 128) {
16331 while (hb3 + 1 < 129) {
16332 if (firTapsPreScale[hb3] > sigma) {
16333 sigma = firTapsPreScale[hb3];
16334 }
16335
16336 hb3++;
16337 }
16338 }
16339
16340 sigma = b_log2(sigma);
16341 sigma = ceil(sigma);
16342 switch ((int)(1.0 + sigma)) {
16343 case 2:
16344 hb3 = 6;
16345 break;
16346
16347 case 1:
16348 hb3 = 0;
16349 break;
16350
16351 case 0:
16352 hb3 = -6;
16353 break;
16354
16355 default:
16356 hb3 = -12;
16357 break;
16358 }
16359
16360 if (b_strcmp(RxTx)) {
16361 if (1.0 + sigma > 2.0) {
16362 hb3 = 6;
16363 }
16364 } else {
16365 if (FIR == 2.0) {
16366 hb3 += 6;
16367 } else {
16368 if (FIR == 4.0) {
16369 hb3 += 12;
16370 }
16371 }
16372
16373 if (hb3 > 0) {
16374 hb3 = 0;
16375 } else {
16376 if (hb3 < -6) {
16377 hb3 = -6;
16378 }
16379 }
16380 }
16381
16382 /* Scale taps */
16383 memcpy(&b_firTapsPreScale[0], &firTapsPreScale[0], sizeof(double) << 7);
16384 b_determineBestFractionLength(b_firTapsPreScale, firTapsPreScale);
16385 sigma = mpower(2.0, 16.0 - (1.0 + sigma));
16386 for (i0 = 0; i0 < 128; i0++) {
16387 sigmax = rt_roundd_snf(firTapsPreScale[i0] * sigma);
16388 if (sigmax < 32768.0) {
16389 if (sigmax >= -32768.0) {
16390 i4 = (short)sigmax;
16391 } else {
16392 i4 = MIN_int16_T;
16393 }
16394 } else if (sigmax >= 32768.0) {
16395 i4 = MAX_int16_T;
16396 } else {
16397 i4 = 0;
16398 }
16399
16400 outputTaps[i0] = i4;
16401 }
16402
16403 /* output = input; */
16404 /* %% Non-codegen outputs */
16405 /* % externally accessible fields */
16406 /* output.firtaps = firtaps; */
16407 /* output.nfirtaps = length(h); */
16408 /* %output.filter = dfilter; */
16409 /* output.gain = gain; */
16410 /* %output.Hm1 = Hm1; */
16411 /* %output.Hm2 = Hm2; */
16412 /* %output.Hm3 = Hm3; */
16413 /* %output.Hm4 = Hm4; */
16414 /* %output.Hmd = Hmd; */
16415 /* output.enables = enables; */
16416 /* */
16417 /* % internal fields used by the GUI */
16418 /* %output.Hanalog = Hanalog; */
16419 /* output.Apass_actual = Apass_actual; */
16420 /* output.Astop_actual = Astop_actual; */
16421 /* %output.delay = delay; */
16422 /* %output.grpdelayvar = grpdelayvar; */
16423 /* %output.Hd1 = Hd1; */
16424 /* %output.Hd2 = Hd2; */
16425 /* %output.Hmiddle = Hmiddle; */
16426 /* output.a1 = a1; */
16427 /* output.b1 = b1; */
16428 /* output.a2 = a2; */
16429 /* output.b2 = b2; */
16430 /* For codegen only output taps */
16431 *numOutputTaps = fg->size[1];
16432 *filterGain = hb3;
16433 emxFree_real_T(&fg);
16434 }
16435
16436 /*
16437 * Arguments : void
16438 * Return Type : void
16439 */
internal_design_filter_cg_initialize(void)16440 void internal_design_filter_cg_initialize(void)
16441 {
16442 rt_InitInfAndNaN(8U);
16443 }
16444
16445 /*
16446 * Arguments : void
16447 * Return Type : void
16448 */
internal_design_filter_cg_terminate(void)16449 void internal_design_filter_cg_terminate(void)
16450 {
16451 /* (no terminate code required) */
16452 }
16453
16454 /*
16455 * File trailer for internal_design_filter_cg.c
16456 *
16457 * [EOF]
16458 */
16459