1 /*
2 * Copyright (c) 2014 Sippy Software, Inc., http://www.sippysoft.com
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 */
27
28 #include <math.h>
29
30 #include "rtpp_math.h"
31
32 void
PFD_init(struct PFD * pfd_p,double phi_round)33 PFD_init(struct PFD *pfd_p, double phi_round)
34 {
35
36 pfd_p->target_clk = 0.0;
37 pfd_p->phi_round = phi_round;
38 }
39
40 double
PFD_get_error(struct PFD * pfd_p,double dtime)41 PFD_get_error(struct PFD *pfd_p, double dtime)
42 {
43 double next_clk, err0r;
44
45 if (pfd_p->phi_round > 0.0) {
46 dtime = trunc(dtime * pfd_p->phi_round) / pfd_p->phi_round;
47 }
48
49 next_clk = trunc(dtime) + 1.0;
50 if (pfd_p->target_clk == 0.0) {
51 pfd_p->target_clk = next_clk;
52 return (0.0);
53 }
54
55 err0r = pfd_p->target_clk - dtime;
56
57 if (err0r > 0) {
58 pfd_p->target_clk = next_clk + 1.0;
59 } else {
60 pfd_p->target_clk = next_clk;
61 }
62
63 return (err0r);
64 }
65
66 double
sigmoid(double x)67 sigmoid(double x)
68 {
69
70 return (x / (1 + fabs(x)));
71 }
72
73 double
recfilter_apply(struct recfilter * f,double x)74 recfilter_apply(struct recfilter *f, double x)
75 {
76
77 f->lastval = f->a * x + f->b * f->lastval;
78 if (f->peak_detect != 0) {
79 if (f->lastval > f->maxval) {
80 f->maxval = f->lastval;
81 } if (f->lastval < f->minval) {
82 f->minval = f->maxval;
83 }
84 }
85 return f->lastval;
86 }
87
88 double
recfilter_apply_int(struct recfilter * f,int x)89 recfilter_apply_int(struct recfilter *f, int x)
90 {
91
92 f->lastval = f->a * (double)(x) + f->b * f->lastval;
93 if (f->peak_detect != 0) {
94 if (f->lastval > f->maxval) {
95 f->maxval = f->lastval;
96 } if (f->lastval < f->minval) {
97 f->minval = f->lastval;
98 }
99 }
100 return f->lastval;
101 }
102
103 void
recfilter_init(struct recfilter * f,double fcoef,double initval,int peak_detect)104 recfilter_init(struct recfilter *f, double fcoef, double initval, int peak_detect)
105 {
106
107 f->lastval = initval;
108 f->a = 1.0 - fcoef;
109 f->b = fcoef;
110 if (peak_detect != 0) {
111 f->peak_detect = 1;
112 f->maxval = initval;
113 f->minval = initval;
114 } else {
115 f->peak_detect = 0;
116 f->maxval = 0;
117 f->minval = 0;
118 }
119 }
120
121 double
freqoff_to_period(double freq_0,double foff_c,double foff_x)122 freqoff_to_period(double freq_0, double foff_c, double foff_x)
123 {
124
125 return (1.0 / freq_0 * (1 + foff_c * foff_x));
126 }
127