1 /* calc_fctns.c */
2
3 /*
4 08/28/2008 -- [IGD] Fixed a bug which used calucalated delay instead of estimated in
5 computation of phase
6 11/3/2005 -- [ET] Added 'wrap_phase()' function; moved 'use_delay()'
7 function from 'calc_fctns.c' to 'evalresp.c'.
8 */
9
10 #ifdef HAVE_CONFIG_H
11 #include <config.h>
12 #endif
13
14
15 /*------------------------------------------------------------------------------------*/
16 /* LOG: */
17 /* Version 3.2.20: iir_pz_trans() modified IGD 09/20/01 */
18 /* Starting with version 3.2.17, evalresp supports IIR type blockettes 54 */
19 /* Starting with version 3.2.17, evalresp supports blockette 55 of SEED */
20 /* which is Response List Blockette (LIST). List contains the frequency, */
21 /* amplitude and phase of the filter for a selected set of frequencies ONLY */
22 /* The current suggestion of DMC IRIS is not to do any interpolation of the */
23 /* and just leave as many frequencies in the output file as it used to be in */
24 /* the blockette 55. This suggestion leads to the obvious fact that the */
25 /* frequencies sampling requested by the user cannot be served if bl. 55 is used */
26 /* and the frequency vector should be overritten by what we actually do have in */
27 /* the blockette 55. Below we check if we use blockette 55 and if we do, we change */
28 /* the frequency vector */
29 /* Note that we assume that blockette 55 can occur only as a single filtering stage */
30 /* in the filter, cannot be mixed with the other types of the filters, therefore, */
31 /* blockette 55 should be the first stage only */
32 /* I. Dricker, ISTI, 06/22/00 for version 2.3.17 of evalresp */
33 /*------------------------------------------------------------------------------------*/
34
35 /* Notice: this version is modified by I.Dricker, ISTI i.dricker@isti.com 06/22/00 */
36 #include "./evresp.h"
37
38 #include <stdlib.h>
39 #include <string.h>
40
41 /*=================================================================
42 * Calculate response
43 *=================================================================*/
calc_resp(struct channel * chan,double * freq,int nfreqs,struct complex * output,char * out_units,int start_stage,int stop_stage,int useTotalSensitivityFlag)44 void calc_resp(struct channel *chan, double *freq, int nfreqs, struct complex *output,
45 char *out_units, int start_stage, int stop_stage, int useTotalSensitivityFlag) {
46 struct blkt *blkt_ptr;
47 struct stage *stage_ptr;
48 int i, j, units_code, eval_flag = 0, nc = 0, sym_fir = 0;
49 double w;
50 int matching_stages = 0, has_stage0 = 0;
51 struct complex of, val;
52 double corr_applied, estim_delay, delay;
53
54 /* if(start_stage && start_stage > chan->nstages) {
55 error_return(NO_STAGE_MATCHED, "calc_resp: %s start_stage=%d, highest stage found=%d)",
56 "No Matching Stages Found (requested",start_stage, chan->nstages);
57 } */
58
59 /* for each frequency */
60
61 for(i = 0; i < nfreqs; i++) {
62 w = twoPi * freq[i];
63 val.real = 1.0; val.imag = 0.0;
64
65 /* loop through the stages and filters for each stage, calculating
66 the response for each frequency for all stages */
67
68 stage_ptr = chan->first_stage;
69 units_code = stage_ptr->input_units;
70 for(j = 0; j < chan->nstages; j++) {
71 nc = 0;
72 sym_fir = 0;
73 if(!stage_ptr->sequence_no)
74 has_stage0 = 1;
75 if(start_stage >= 0 && stop_stage && (stage_ptr->sequence_no < start_stage ||
76 stage_ptr->sequence_no > stop_stage)) {
77 stage_ptr = stage_ptr->next_stage;
78 continue;
79 }
80 else if(start_stage >= 0 && !stop_stage && stage_ptr->sequence_no != start_stage) {
81 stage_ptr = stage_ptr->next_stage;
82 continue;
83 }
84 matching_stages++;
85 blkt_ptr = stage_ptr->first_blkt;
86 while(blkt_ptr) {
87 eval_flag = 0;
88 switch(blkt_ptr->type) {
89 case ANALOG_PZ:
90 case LAPLACE_PZ:
91 analog_trans(blkt_ptr, freq[i], &of);
92 eval_flag = 1;
93 break;
94 case IIR_PZ:
95 if(blkt_ptr->blkt_info.pole_zero.nzeros || blkt_ptr->blkt_info.pole_zero.npoles) {
96 iir_pz_trans(blkt_ptr, w, &of);
97 eval_flag = 1;
98 }
99 break;
100 case FIR_SYM_1:
101 case FIR_SYM_2:
102 if(blkt_ptr->type == FIR_SYM_1)
103 nc = (double) blkt_ptr->blkt_info.fir.ncoeffs*2 - 1;
104 else if(blkt_ptr->type == FIR_SYM_2)
105 nc = (double) blkt_ptr->blkt_info.fir.ncoeffs*2;
106 if(blkt_ptr->blkt_info.fir.ncoeffs) {
107 fir_sym_trans(blkt_ptr, w, &of);
108 sym_fir = 1;
109 eval_flag = 1;
110 }
111 break;
112 case FIR_ASYM:
113 nc = (double) blkt_ptr->blkt_info.fir.ncoeffs;
114 if(blkt_ptr->blkt_info.fir.ncoeffs) {
115 fir_asym_trans(blkt_ptr, w, &of);
116 sym_fir = -1;
117 eval_flag = 1;
118 }
119 break;
120 case DECIMATION:
121 if(nc != 0) {
122 /* IGD 08/27/08 Use estimated delay instead of calculated */
123 estim_delay = (double) blkt_ptr->blkt_info.decimation.estim_delay;
124 corr_applied = blkt_ptr->blkt_info.decimation.applied_corr;
125
126 /* Asymmetric FIR coefficients require a delay correction */
127 if ( sym_fir == -1 ) {
128 if (TRUE == use_delay(QUERY_DELAY))
129 delay = estim_delay;
130 else
131 delay = corr_applied;
132 }
133 /* Otherwise delay has already been handled in fir_sym_trans() */
134 else {
135 delay = 0;
136 }
137
138 calc_time_shift (delay, w, &of);
139
140 eval_flag = 1;
141 }
142 break;
143 case LIST: /* This option is added in version 2.3.17 I.Dricker*/
144 calc_list (blkt_ptr, i, &of); /*compute real and imag parts for the i-th ampl and phase */
145 eval_flag = 1;
146 break;
147 case IIR_COEFFS: /* This option is added in version 2.3.17 I.Dricker*/
148 iir_trans(blkt_ptr, w, &of);
149 eval_flag = 1;
150 break;
151 default:
152 break;
153 }
154 if(eval_flag)
155 zmul(&val, &of);
156 blkt_ptr = blkt_ptr->next_blkt;
157 }
158 stage_ptr = stage_ptr->next_stage;
159 }
160
161 /* if no matching stages were found, then report the error */
162
163 if(!matching_stages && !has_stage0) {
164 error_return(NO_STAGE_MATCHED, "calc_resp: %s start_stage=%d, highest stage found=%d)",
165 "No Matching Stages Found (requested",start_stage, chan->nstages);
166 }
167 else if(!matching_stages) {
168 error_return(NO_STAGE_MATCHED, "calc_resp: %s start_stage=%d, highest stage found=%d)",
169 "No Matching Stages Found (requested",start_stage, chan->nstages-1);
170 }
171
172 /* Write output for freq[i] in output[i] (note: unitScaleFact is a global variable
173 set by the 'check_units' function that is used to convert to 'MKS' units when the
174 the response was given as a displacement, velocity, or acceleration in units other
175 than meters) */
176 if (0 == useTotalSensitivityFlag) {
177 output[i].real = val.real * chan->calc_sensit * unitScaleFact;
178 output[i].imag = val.imag * chan->calc_sensit * unitScaleFact;
179 }
180 else {
181 output[i].real = val.real * chan->sensit * unitScaleFact;
182 output[i].imag = val.imag * chan->sensit * unitScaleFact;
183 }
184
185 convert_to_units(units_code, out_units, &output[i], w);
186 }
187
188 }
189
190 /*==================================================================
191 * Convert response to velocity first, then to specified units
192 *=================================================================*/
convert_to_units(int inp,char * out_units,struct complex * data,double w)193 void convert_to_units(int inp, char *out_units, struct complex *data, double w) {
194 int out = VEL, l;
195 struct complex scale_val;
196
197 /* if default units were specified by the user, no conversion is made,
198 otherwise convert to unit the user specified. */
199
200 if (out_units != NULL && (l=strlen(out_units)) > 0) {
201 curr_seq_no = -1;
202 if(!strncmp(out_units, "DEF", 3))
203 return;
204 else if(!strncmp(out_units, "DIS", 3)) out = DIS;
205 else if(!strncmp(out_units, "VEL", 3)) out = VEL;
206 else if(!strncmp(out_units, "ACC", 3)) out = ACC;
207 else {
208 error_return(BAD_OUT_UNITS, "convert_to_units: bad output units");
209 }
210 }
211 else out = VEL;
212
213 if (inp == DIS) {
214 if (out == DIS) return;
215 if (w != 0.0) {
216 scale_val.real = 0.0; scale_val.imag = -1.0/w;
217 zmul(data, &scale_val);
218 }
219 else data->real = data->imag = 0.0;
220 }
221 else if (inp == ACC) {
222 if (out == ACC) return;
223 scale_val.real = 0.0; scale_val.imag = w;
224 zmul(data, &scale_val);
225 }
226
227 if (out == DIS) {
228 scale_val.real = 0.0; scale_val.imag = w;
229 zmul(data, &scale_val);
230 }
231 else if (out == ACC) {
232 if (w != 0.0) {
233 scale_val.real = 0.0; scale_val.imag = -1.0/w;
234 zmul(data, &scale_val);
235 }
236 else data->real = data->imag = 0.0;
237 }
238
239 }
240
241
242 /*==================================================================
243 * Response of a digital IIR filter
244 * This code is modified fom the FORTRAN subroutine written and tested by
245 * Bob Hutt (ASL USGS). Evaluates phase directly from imaginary and real
246 * parts of IIR filter coefficients.
247 * C translation from FORTRAN function: Ilya Dricker (ISTI), i.dricker@isti.com
248 * Version 0.2 07/12/00
249 *================================================================*/
iir_trans(struct blkt * blkt_ptr,double wint,struct complex * out)250 void iir_trans(struct blkt *blkt_ptr, double wint, struct complex *out) {
251
252
253 double h0;
254 double xre, xim, phase;
255 double amp;
256 double t, w;
257 double *cn, *cd; /* numerators and denominators */
258 int nn, nd, in, id;
259
260 struct blkt *next_ptr;
261
262
263 h0 = blkt_ptr->blkt_info.coeff.h0; /* set a sensitivity */
264 next_ptr = blkt_ptr->next_blkt;
265
266 /* Sample interaval = 1/sample rate */
267 t = next_ptr->blkt_info.decimation.sample_int;
268
269 /* Numerator coeffs number */
270 nn = blkt_ptr->blkt_info.coeff.nnumer;
271 /* Denominator coeffs number */
272 nd = blkt_ptr->blkt_info.coeff.ndenom;
273
274 /* Numerators */
275 cn = blkt_ptr->blkt_info.coeff.numer;
276 /* Denominators */
277 cd = blkt_ptr->blkt_info.coeff.denom;
278
279 /* Calculate radial freq. time sample interval */
280 w = wint * t;
281
282 /* Handle numerator */
283 xre= cn[0];
284 xim = 0.0;
285
286 if (nn != 1) {
287 for (in=1; in<nn; in++) {
288 xre+=cn[in]*cos(-(in*w));
289 xim+=cn[in]*sin(-(in*w));
290 }
291 }
292 amp=sqrt(xre*xre + xim*xim);
293 phase=atan2(xim,xre);
294
295
296
297 /* handle denominator */
298
299 xre= cd[0];
300 xim = 0.0;
301
302 if(nd !=1 ) {
303 for (id = 1; id < nd; id ++) {
304 xre+=cd[id]*cos(-(id*w));
305 xim+=cd[id]*sin(-(id*w));
306 }
307 }
308
309 amp /= (sqrt(xre*xre+xim*xim));
310 phase = (phase - atan2(xim,xre)) ;
311
312
313
314
315 out->real = amp * cos(phase) * h0;
316 out->imag = amp * sin(phase) * h0;
317
318 }
319
320
321 /*================================================================
322 * Response of blockette 55 (Response List Blockette)
323 * Function introduced in version 3.2.17 of evalresp
324 * Ilya Dricker ISTI (.dricker@isti.com) 06/22/00
325 *===============================================================*/
calc_list(struct blkt * blkt_ptr,int i,struct complex * out)326 void calc_list (struct blkt *blkt_ptr, int i, struct complex *out) {
327 double amp, phase;
328 double halfcirc = 180;
329
330 amp = blkt_ptr->blkt_info.list.amp[i];
331 phase = blkt_ptr->blkt_info.list.phase[i];
332 phase = phase/halfcirc * (double) Pi; /*convert the phase to radians from the default degrees */
333 out->real = amp * cos(phase);
334 out->imag = amp * sin(phase);
335
336 }
337
338 /*==================================================================
339 * Response of analog filter
340 *=================================================================*/
analog_trans(struct blkt * blkt_ptr,double freq,struct complex * out)341 void analog_trans(struct blkt *blkt_ptr, double freq, struct complex *out) {
342 int nz, np, i;
343 struct complex *ze, *po, denom, num, omega, temp;
344 double h0, mod_squared;
345
346 if (blkt_ptr->type == LAPLACE_PZ) freq = twoPi * freq;
347 omega.imag = freq;
348 omega.real = 0.0;
349 denom.real = denom.imag = num.real = num.imag = 1.0;
350
351 ze = blkt_ptr->blkt_info.pole_zero.zeros;
352 nz = blkt_ptr->blkt_info.pole_zero.nzeros;
353 po = blkt_ptr->blkt_info.pole_zero.poles;
354 np = blkt_ptr->blkt_info.pole_zero.npoles;
355 h0 = blkt_ptr->blkt_info.pole_zero.a0;
356
357 for (i = 0; i < nz; i++) {
358 /* num=num*(omega-zero[i]) */
359 temp.real = omega.real - ze[i].real;
360 temp.imag = omega.imag - ze[i].imag;
361 zmul(&num, &temp);
362 }
363 for (i = 0; i < np; i++) {
364 /* denom=denom*(omega-pole[i]) */
365 temp.real = omega.real - po[i].real;
366 temp.imag = omega.imag - po[i].imag;
367 zmul(&denom, &temp);
368 }
369
370 /* gain*num/denum */
371
372 temp.real = denom.real;
373 temp.imag = -denom.imag;
374 zmul(&temp, &num);
375 mod_squared = denom.real*denom.real + denom.imag*denom.imag;
376
377 if (mod_squared != 0.) {
378 temp.real /= mod_squared;
379 temp.imag /= mod_squared;
380 }
381 else if (temp.real != 0.0 || temp.imag != 0.0) {
382 fprintf(stderr,"%s WARNING (analog_trans): Numerical problem detected. Result might be wrong.", myLabel);
383 fprintf(stderr,"%s\t Execution continuing.\n", myLabel);
384 }
385
386 out->real = h0 * temp.real;
387 out->imag = h0 * temp.imag;
388 }
389
390 /*==================================================================
391 * Response of symetrical FIR filters
392 *=================================================================*/
fir_sym_trans(struct blkt * blkt_ptr,double w,struct complex * out)393 void fir_sym_trans(struct blkt *blkt_ptr, double w, struct complex *out) {
394 double *a, h0, wsint;
395 struct blkt *next_ptr;
396 int na;
397 int k, fact;
398 double R = 0.0, sint;
399
400 a = blkt_ptr->blkt_info.fir.coeffs;
401 na = blkt_ptr->blkt_info.fir.ncoeffs;
402 next_ptr = blkt_ptr->next_blkt;
403 h0 = blkt_ptr->blkt_info.fir.h0;
404 sint = next_ptr->blkt_info.decimation.sample_int;
405 wsint = w * sint;
406
407 if (blkt_ptr->type == FIR_SYM_1) {
408 for (k=0; k<(na-1); k++) {
409 fact = na-(k+1);
410 R += a[k] * cos(wsint * fact);
411 }
412 out->real = (a[k] + 2.0*R) * h0;
413 out->imag = 0.;
414 }
415 else if (blkt_ptr->type == FIR_SYM_2) {
416 for (k=0; k<na; k++) {
417 fact = na-(k+1);
418 R += a[k] * cos(wsint * (fact+0.5));
419 }
420 out->real = 2.0*R * h0;
421 out->imag = 0.;
422 }
423 }
424
425 /*==================================================================
426 * Response of asymetrical FIR filters
427 *=================================================================*/
fir_asym_trans(struct blkt * blkt_ptr,double w,struct complex * out)428 void fir_asym_trans(struct blkt *blkt_ptr, double w, struct complex *out) {
429 double *a, h0, sint;
430 struct blkt *next_ptr;
431 int na;
432 int k;
433 double R = 0.0, I = 0.0;
434 double wsint, y;
435 double mod, pha;
436
437 a = blkt_ptr->blkt_info.fir.coeffs;
438 na = blkt_ptr->blkt_info.fir.ncoeffs;
439 next_ptr = blkt_ptr->next_blkt;
440 h0 = blkt_ptr->blkt_info.fir.h0;
441 sint = next_ptr->blkt_info.decimation.sample_int;
442 wsint = w * sint;
443
444 for (k = 1; k < na; k++) {
445 if (a[k] != a[0]) break;
446 }
447 if (k == na) {
448 if (wsint == 0.0) out->real = 1.;
449 else out->real = (sin(wsint/2.*na) / sin(wsint/2.)) * a[0];
450 out->imag = 0;
451 return;
452 }
453
454 for (k = 0; k < na; k++) {
455 y = wsint * k;
456 R += a[k] * cos(y);
457 I += a[k] * -sin(y);
458 }
459
460 mod = sqrt(R*R + I*I);
461 pha = atan2(I,R);
462 R = mod * cos(pha);
463 I = mod * sin(pha);
464 out->real = R * h0;
465 out->imag = I * h0;
466 }
467
468 /*==================================================================
469 * Response of IIR filters
470 *=================================================================*/
iir_pz_trans(struct blkt * blkt_ptr,double w,struct complex * out)471 void iir_pz_trans(struct blkt *blkt_ptr, double w, struct complex *out) {
472 struct complex *ze, *po;
473 double h0, sint, wsint;
474 struct blkt *next_ptr;
475 int nz, np;
476 int i;
477 double mod = 1.0, pha = 0.0;
478 double R, I;
479 double c, s;
480
481 ze = blkt_ptr->blkt_info.pole_zero.zeros;
482 nz = blkt_ptr->blkt_info.pole_zero.nzeros;
483 po = blkt_ptr->blkt_info.pole_zero.poles;
484 np = blkt_ptr->blkt_info.pole_zero.npoles;
485 h0 = blkt_ptr->blkt_info.pole_zero.a0;
486 next_ptr = blkt_ptr->next_blkt;
487 sint = next_ptr->blkt_info.decimation.sample_int;
488 wsint = w * sint;
489
490 c = cos(wsint);
491 s = sin(wsint); /* IGD 10/21/02 instead of -: pointed by Sleeman */
492 for (i = 0; i < nz; i++) {
493 R = c - ze[i].real; /* IGD 09/20/01 instead of + */
494 I = s - ze[i].imag; /* IGD 09/20/01 instead of + */
495 mod *= sqrt(R*R + I*I);
496 if (R == 0.0 && I == 0.0) pha += 0.0;
497 else pha += atan2(I, R);
498 }
499 for (i = 0; i < np; i++) {
500 R = c - po[i].real; /* IGD 09/20/01 instead of + */
501 I = s - po[i].imag; /* IGD 09/20/01 instead of + */
502 mod /= sqrt(R*R +I*I);
503 if (R == 0.0 && I == 0.0) pha += 0.0;
504 else pha -= atan2(I, R);
505 }
506 out->real = mod * cos(pha) * h0;
507 out->imag = mod * sin(pha) * h0;
508 }
509
510 /*==================================================================
511 * calculate the phase shift equivalent to the time shift
512 * delta at the frequence w (rads/sec)
513 *=================================================================*/
calc_time_shift(double delta,double w,struct complex * out)514 void calc_time_shift(double delta, double w, struct complex *out) {
515 out->real = cos(w*delta);
516 out->imag = sin(w*delta);
517 }
518
519
520
521 /*==================================================================
522 * Complex multiplication: complex version of val1 *= val2;
523 *=================================================================*/
zmul(struct complex * val1,struct complex * val2)524 void zmul(struct complex *val1, struct complex *val2) {
525 double r, i;
526 r = val1->real*val2->real - val1->imag*val2->imag;
527 i = val1->imag*val2->real + val1->real*val2->imag;
528 val1->real = r;
529 val1->imag = i;
530 }
531
532 /*=================================================================
533 * Normalize response
534 *=================================================================*/
norm_resp(struct channel * chan,int start_stage,int stop_stage,int hide_sensitivity_mismatch_warning)535 void norm_resp(struct channel *chan, int start_stage, int stop_stage,
536 int hide_sensitivity_mismatch_warning) {
537 // TODO - NULL assignments below made blindly to fix compiler warning. bug?
538 struct stage *stage_ptr;
539 struct blkt *fil, *last_fil = NULL, *main_filt = NULL;
540 int i, main_type, reset_gain, skipped_stages = 0;
541 double w, f;
542 double percent_diff;
543 struct complex of, df;
544
545 /* -------- TEST 1 -------- */
546 /*
547 A single stage response must specify a stage gain, a stage zero
548 sensitivity, or both. If the gain has been set, simply drop through
549 to the next test. If the gain is zero, set the gain equal to the
550 sensitivity and then go to the next test. */
551
552 if (chan->nstages == 1 ) { /* has no stage 0, does it have a gain??? */
553 stage_ptr = chan->first_stage;
554 fil = stage_ptr->first_blkt;
555 while(fil != (struct blkt *)NULL && fil->type != GAIN) {
556 last_fil = fil;
557 fil = last_fil->next_blkt;
558 }
559 if (fil == (struct blkt *)NULL) {
560 error_return(ILLEGAL_RESP_FORMAT, "norm_resp; no stage gain defined, zero sensitivity");
561 }
562 }
563 else if (chan->nstages == 2 ) { /* has a stage 0??? */
564 stage_ptr = chan->first_stage;
565 fil = stage_ptr->first_blkt;
566 while(fil != (struct blkt *)NULL && fil->type != GAIN) {
567 last_fil = fil;
568 fil = last_fil->next_blkt;
569 }
570 if (fil == (struct blkt *)NULL) {
571 if (chan->sensit == 0.0) {
572 error_return(ILLEGAL_RESP_FORMAT, "norm_resp; no stage gain defined, zero sensitivity");
573 }
574 else {
575 fil = alloc_gain();
576 fil->blkt_info.gain.gain = chan->sensit;
577 fil->blkt_info.gain.gain_freq = chan->sensfreq;
578 last_fil->next_blkt = fil;
579 }
580 }
581 }
582
583 /* -------- TEST 2 -------- */
584 /*
585
586 Compute the variable chan->calc_sensit. This value would normally be the
587 same as chan->sensit (the stage zero sensitivity) if the sensitivity
588 has been given and all the channel gains are at the sensitivity frequency.
589
590 Note that we must verify that each filter gain and normalization is
591 at the sensitivity frequency, before we can compute the overall
592 channel sensitivity. If the frequencies are different, calculate the
593 gain for the stage at the same frequency as the channel sensitivity
594 frequency.
595
596 Note on terminology:
597
598 chan->sensit = stage zero sensitivity read from input file
599 (i.e. the total sensitivity for a given channel).
600 chan->sensfreq = frequency at which chan-sensit is reported.
601 chan->calc_sensit = product of the individual stage gains. This is computed
602 below. The computation is done at the frequency
603 chan->sensfreq. */
604
605 /* Loop thru stages, checking for stage gains of zero */
606
607 stage_ptr = chan->first_stage;
608 for(i = 0; i < chan->nstages; i++) {
609 fil = stage_ptr->first_blkt;
610 curr_seq_no = stage_ptr->sequence_no;
611 while(fil) {
612 if (fil->type == GAIN && fil->blkt_info.gain.gain == 0.0 ) {
613 error_return(ILLEGAL_RESP_FORMAT, "norm_resp; zero stage gain");
614 }
615 fil = fil->next_blkt;
616 }
617 stage_ptr = stage_ptr->next_stage;
618 }
619
620 /*
621 If necessary, loop thru filters, looking for a non-zero frequency to
622 use for chan->sensfreq. If the channel sensitivity is defined, then
623 we will use chan->sensfreq, regardless of whether it is zero or not.
624 Otherwise, we use the last non-zero filter gain frequency. Since
625 filters are typically for low pass purposes, the last non-zero
626 frequency is likely the best choice, as its pass band is the
627 narrowest. If all the frequencies are zero, then use zero! */
628
629 if ( chan->sensit == 0.0 ) {
630 stage_ptr = chan->first_stage;
631 for(i = 0; i < chan->nstages; i++) {
632 fil = stage_ptr->first_blkt;
633 while(fil) {
634 if (fil->type == GAIN && fil->blkt_info.gain.gain_freq != 0.0 )
635 chan->sensfreq = fil->blkt_info.gain.gain_freq;
636 fil = fil->next_blkt;
637 }
638 stage_ptr = stage_ptr->next_stage;
639 }
640 }
641
642 /*
643 Loop thru filters, evaluate the filter gains and normalizations at
644 the single frequency, chan->sensfreq. Use the stage gains to
645 calculate a total channel sensitivity. */
646
647 chan->calc_sensit = 1.0;
648 f = chan->sensfreq;
649 w = twoPi * f;
650
651 stage_ptr = chan->first_stage;
652 for(i = 0; i < chan->nstages; i++) {
653 if(start_stage >= 0 && stop_stage && (stage_ptr->sequence_no < start_stage ||
654 stage_ptr->sequence_no > stop_stage)) {
655 if(stage_ptr->sequence_no)
656 skipped_stages = 1;
657 stage_ptr = stage_ptr->next_stage;
658 continue;
659 }
660 else if(start_stage >= 0 && !stop_stage && stage_ptr->sequence_no != start_stage) {
661 if(stage_ptr->sequence_no)
662 skipped_stages = 1;
663 stage_ptr = stage_ptr->next_stage;
664 continue;
665 }
666 fil = stage_ptr->first_blkt;
667 curr_seq_no = stage_ptr->sequence_no;
668 main_type = 0;
669 while(fil) {
670 switch(fil->type) {
671 case DECIMATION:
672 case REFERENCE:
673 break;
674 case ANALOG_PZ:
675 case LAPLACE_PZ:
676 case IIR_PZ:
677 case FIR_SYM_1:
678 case FIR_SYM_2:
679 case FIR_ASYM:
680 case IIR_COEFFS: /* IGD New type from v 3.2.17 */
681 main_filt = fil;
682 main_type = fil->type;
683 break;
684 case GAIN:
685 if(curr_seq_no) {
686 if((fil->blkt_info.gain.gain_freq != chan->sensfreq) ||
687 ((main_type == ANALOG_PZ ||
688 main_type == LAPLACE_PZ ||
689 main_type == IIR_PZ) &&
690 (main_filt->blkt_info.pole_zero.a0_freq != chan->sensfreq))) {
691
692 reset_gain = 1;
693 if(main_type == ANALOG_PZ || main_type == LAPLACE_PZ) {
694 main_filt->blkt_info.pole_zero.a0 = 1.0;
695 analog_trans(main_filt, fil->blkt_info.gain.gain_freq, &df);
696 if(df.real == 0.0 && df.imag == 0.0) {
697 error_return(ILLEGAL_FILT_SPEC,
698 "norm_resp: Gain frequency of zero found in bandpass analog filter");
699 }
700 analog_trans(main_filt, f, &of);
701 if(of.real == 0.0 && of.imag == 0.0) {
702 error_return(ILLEGAL_FILT_SPEC,
703 "norm_resp: Chan. Sens. frequency found with bandpass analog filter");
704 }
705 }
706 else if(main_type == IIR_PZ) {
707 main_filt->blkt_info.pole_zero.a0 = 1.0;
708 iir_pz_trans(main_filt, twoPi * fil->blkt_info.gain.gain_freq, &df);
709 iir_pz_trans(main_filt, w, &of);
710 }
711 else if((main_type == FIR_SYM_1 || main_type == FIR_SYM_2) &&
712 main_filt->blkt_info.fir.ncoeffs) {
713 main_filt->blkt_info.fir.h0 = 1.0;
714 fir_sym_trans(main_filt, twoPi * fil->blkt_info.gain.gain_freq, &df);
715 fir_sym_trans(main_filt, w, &of);
716 }
717 else if(main_type == FIR_ASYM && main_filt->blkt_info.fir.ncoeffs) {
718 main_filt->blkt_info.fir.h0 = 1.0;
719 fir_asym_trans(main_filt, twoPi * fil->blkt_info.gain.gain_freq, &df);
720 fir_asym_trans(main_filt, w, &of);
721 }
722 else if(main_type == IIR_COEFFS) { /*IGD - new case for 3.2.17 */
723 main_filt->blkt_info.coeff.h0 = 1.0;
724 iir_trans(main_filt, twoPi * fil->blkt_info.gain.gain_freq, &df);
725 iir_trans(main_filt, w, &of);
726 }
727
728 else
729 reset_gain = 0;
730
731 if(reset_gain) {
732 fil->blkt_info.gain.gain /= sqrt(df.real*df.real + df.imag*df.imag);
733 fil->blkt_info.gain.gain *= sqrt(of.real*of.real + of.imag*of.imag);
734 fil->blkt_info.gain.gain_freq = f;
735 if(main_type == ANALOG_PZ || main_type == LAPLACE_PZ ||
736 main_type == IIR_PZ) {
737 main_filt->blkt_info.pole_zero.a0 = 1.0 / sqrt(of.real*of.real + of.imag*of.imag);
738 main_filt->blkt_info.pole_zero.a0_freq = f;
739 }
740 else if(main_type == FIR_SYM_1 || main_type == FIR_SYM_2 || main_type == FIR_ASYM) {
741 main_filt->blkt_info.fir.h0 = 1.0 / sqrt(of.real*of.real + of.imag*of.imag);
742 }
743 else if(main_type == IIR_COEFFS) {
744 main_filt->blkt_info.coeff.h0 = 1.0 / sqrt(of.real*of.real + of.imag*of.imag);
745 }
746 }
747 }
748 /* compute new overall sensitivity. */
749 chan->calc_sensit *= fil->blkt_info.gain.gain;
750 if(chan->nstages == 1) {
751 chan->sensit = chan->calc_sensit;
752 }
753 }
754 break;
755 default:
756 break;
757 }
758 fil = fil->next_blkt;
759 }
760 stage_ptr = stage_ptr->next_stage;
761 }
762
763 /* -------- TEST 3 -------- */
764 /* Finally, print a warning, if necessary */
765
766 if(!hide_sensitivity_mismatch_warning && !skipped_stages && chan->sensit != 0.0) {
767 percent_diff = fabs((chan->sensit - chan->calc_sensit) / chan->sensit);
768
769 if (percent_diff >= 0.05) {
770 fprintf(stderr,"%s WARNING (norm_resp): computed and reported sensitivities", myLabel);
771 fprintf(stderr,"%s differ by more than 5 percent. \n", myLabel);
772 fprintf(stderr,"%s\t Execution continuing.\n", myLabel);
773 fflush(stderr);
774 }
775 }
776 }
777
778 /* IGD 04/05/04 Phase unwrapping function
779 * It works only inside a loop over phases.
780 *
781 */
782 double
unwrap_phase(double phase,double prev_phase,double range,double * added_value)783 unwrap_phase(double phase, double prev_phase, double range, double *added_value)
784 {
785 phase += *added_value;
786 if (fabs(phase - prev_phase) > range/2)
787 {
788 if (phase - prev_phase > 0)
789 {
790 *added_value -= range;
791 phase -= range;
792 }
793 else
794 {
795 *added_value += range;
796 phase += range;
797 }
798 }
799 return phase;
800 }
801
802 /*
803 * wrap_phase: Wraps a set of phase values so that all of the values are
804 * between "-range" and "+range" (inclusive). This function is called
805 * iteratively, once for each phase value.
806 * phase - phase value to process.
807 * range - range value to use.
808 * added_value - pointer to offset value used for each call.
809 * Returns: The "wrapped" version of the given phase value.
810 */
wrap_phase(double phase,double range,double * added_value)811 double wrap_phase(double phase, double range, double *added_value)
812 {
813 phase += *added_value;
814 if(phase > range/2)
815 {
816 *added_value -= range;
817 phase -= range;
818 }
819 else if(phase < -range/2)
820 {
821 *added_value += range;
822 phase += range;
823 }
824 return phase;
825 }
826
827