sigpro/cdb.c at master · sbashford/sigpro · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
// cdb.c

#include <math.h>
#include "sigpro.h"

#ifndef WIN32
#define _hypot  hypot
#endif

FUNC(void) sp_cdb(	// complex decibels (20*log10)
    float *x, 		// complex-input array
    float *y, 		// real-output array (dB)
    int n		// complex array size
)
{
    double  a;
    int     i, j, k;

    for (i = 0; i < n; i++) {
	j = i * 2;
	k = j + 1;
	a = _hypot(x[j], x[k]);
	if (a < 1e-40) {
	    y[i] = -800;
	} else {
	    y[i] = (float) (20 * log10(a));
	}
    }
}

FUNC(void)
sp_cgd(float *x, float *y, int n, double df)
{
    double  a, dp, ph, pr;
    int     i, j, k;
    static double tpi = 2 * M_PI;

    pr = 0;
    for (i = 0; i < n; i++) {
	j = i * 2;
	k = j + 1;
	a = _hypot(x[j], x[k]);
	if (a < 1e-40) {
	    ph = 0;
	} else {
	    ph = (float) (atan2(x[k], x[j]) / tpi);
	}
	dp = pr - ph;
	while (dp < -0.5) {
	    dp += 1;
	}
	while (dp > 0.5) {
	    dp -= 1;
	}
	y[i] = (float) (dp / df);
	pr = ph;
    }
}

FUNC(void)
sp_cph(float *x, float *y, int n)
{
    double  a;
    int     i, j, k;
    static double tpi = 2 * M_PI;

    for (i = 0; i < n; i++) {
	j = i * 2;
	k = j + 1;
	a = _hypot(x[j], x[k]);
	if (a < 1e-40) {
	    y[i] = 0;
	} else {
	    y[i] = (float) (atan2(x[k], x[j]) / tpi);
	}
    }
}

FUNC(void)
sp_unwrap(float *x, float *y, int n)
{
    int     i;

    y[0] = x[0];
    for (i = 1; i < n; i++) {
        y[i] = x[i] + (float) floor(y[i - 1] - x[i] + 0.5);
    }
}