1 /* -*- c++ -*- */
2 /*
3 * Copyright 2011,2013,2018 Free Software Foundation, Inc.
4 *
5 * This file is part of GNU Radio
6 *
7 * GNU Radio is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 3, or (at your option)
10 * any later version.
11 *
12 * GNU Radio is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with GNU Radio; see the file COPYING. If not, write to
19 * the Free Software Foundation, Inc., 51 Franklin Street,
20 * Boston, MA 02110-1301, USA.
21 */
22
23 #ifdef HAVE_CONFIG_H
24 #include "config.h"
25 #endif
26
27 #include <gnuradio/blocks/control_loop.h>
28 #include <gnuradio/math.h>
29 #include <stdexcept>
30
31 namespace gr {
32 namespace blocks {
33
34 #define M_TWOPI (2.0f * GR_M_PI)
35
control_loop(float loop_bw,float max_freq,float min_freq)36 control_loop::control_loop(float loop_bw, float max_freq, float min_freq)
37 : d_phase(0), d_freq(0), d_max_freq(max_freq), d_min_freq(min_freq)
38 {
39 // Set the damping factor for a critically damped system
40 d_damping = sqrtf(2.0f) / 2.0f;
41
42 // Set the bandwidth, which will then call update_gains()
43 set_loop_bandwidth(loop_bw);
44 }
45
~control_loop()46 control_loop::~control_loop() {}
47
update_gains()48 void control_loop::update_gains()
49 {
50 float denom = (1.0 + 2.0 * d_damping * d_loop_bw + d_loop_bw * d_loop_bw);
51 d_alpha = (4 * d_damping * d_loop_bw) / denom;
52 d_beta = (4 * d_loop_bw * d_loop_bw) / denom;
53 }
54
advance_loop(float error)55 void control_loop::advance_loop(float error)
56 {
57 d_freq = d_freq + d_beta * error;
58 d_phase = d_phase + d_freq + d_alpha * error;
59 }
60
phase_wrap()61 void control_loop::phase_wrap()
62 {
63 while (d_phase > M_TWOPI)
64 d_phase -= M_TWOPI;
65 while (d_phase < -M_TWOPI)
66 d_phase += M_TWOPI;
67 }
68
frequency_limit()69 void control_loop::frequency_limit()
70 {
71 if (d_freq > d_max_freq)
72 d_freq = d_max_freq;
73 else if (d_freq < d_min_freq)
74 d_freq = d_min_freq;
75 }
76
77 /*******************************************************************
78 * SET FUNCTIONS
79 *******************************************************************/
80
set_loop_bandwidth(float bw)81 void control_loop::set_loop_bandwidth(float bw)
82 {
83 if (bw < 0) {
84 throw std::out_of_range("control_loop: invalid bandwidth. Must be >= 0.");
85 }
86
87 d_loop_bw = bw;
88 update_gains();
89 }
90
set_damping_factor(float df)91 void control_loop::set_damping_factor(float df)
92 {
93 if (df <= 0) {
94 throw std::out_of_range("control_loop: invalid damping factor. Must be > 0.");
95 }
96
97 d_damping = df;
98 update_gains();
99 }
100
set_alpha(float alpha)101 void control_loop::set_alpha(float alpha)
102 {
103 if (alpha < 0 || alpha > 1.0) {
104 throw std::out_of_range("control_loop: invalid alpha. Must be in [0,1].");
105 }
106 d_alpha = alpha;
107 }
108
set_beta(float beta)109 void control_loop::set_beta(float beta)
110 {
111 if (beta < 0 || beta > 1.0) {
112 throw std::out_of_range("control_loop: invalid beta. Must be in [0,1].");
113 }
114 d_beta = beta;
115 }
116
set_frequency(float freq)117 void control_loop::set_frequency(float freq)
118 {
119 if (freq > d_max_freq)
120 d_freq = d_max_freq;
121 else if (freq < d_min_freq)
122 d_freq = d_min_freq;
123 else
124 d_freq = freq;
125 }
126
set_phase(float phase)127 void control_loop::set_phase(float phase)
128 {
129 d_phase = phase;
130 while (d_phase > M_TWOPI)
131 d_phase -= M_TWOPI;
132 while (d_phase < -M_TWOPI)
133 d_phase += M_TWOPI;
134 }
135
set_max_freq(float freq)136 void control_loop::set_max_freq(float freq) { d_max_freq = freq; }
137
set_min_freq(float freq)138 void control_loop::set_min_freq(float freq) { d_min_freq = freq; }
139
140 /*******************************************************************
141 * GET FUNCTIONS
142 *******************************************************************/
143
get_loop_bandwidth() const144 float control_loop::get_loop_bandwidth() const { return d_loop_bw; }
145
get_damping_factor() const146 float control_loop::get_damping_factor() const { return d_damping; }
147
get_alpha() const148 float control_loop::get_alpha() const { return d_alpha; }
149
get_beta() const150 float control_loop::get_beta() const { return d_beta; }
151
get_frequency() const152 float control_loop::get_frequency() const { return d_freq; }
153
get_phase() const154 float control_loop::get_phase() const { return d_phase; }
155
get_max_freq() const156 float control_loop::get_max_freq() const { return d_max_freq; }
157
get_min_freq() const158 float control_loop::get_min_freq() const { return d_min_freq; }
159
160 } /* namespace blocks */
161 } /* namespace gr */
162