blob: 099bc20fb94b6711a500296f13a7a3c4ffb0ae9b (
plain)
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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
|
/* -*- c++ -*- */
/*
* Copyright 2011,2013,2018 Free Software Foundation, Inc.
*
* This file is part of GNU Radio
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <gnuradio/blocks/control_loop.h>
#include <gnuradio/math.h>
#include <stdexcept>
namespace gr {
namespace blocks {
#define M_TWOPI (2.0f * GR_M_PI)
control_loop::control_loop(float loop_bw, float max_freq, float min_freq)
: d_phase(0), d_freq(0), d_max_freq(max_freq), d_min_freq(min_freq)
{
// Set the damping factor for a critically damped system
d_damping = sqrtf(2.0f) / 2.0f;
// Set the bandwidth, which will then call update_gains()
set_loop_bandwidth(loop_bw);
}
control_loop::~control_loop() {}
void control_loop::update_gains()
{
float denom = (1.0 + 2.0 * d_damping * d_loop_bw + d_loop_bw * d_loop_bw);
d_alpha = (4 * d_damping * d_loop_bw) / denom;
d_beta = (4 * d_loop_bw * d_loop_bw) / denom;
}
void control_loop::advance_loop(float error)
{
d_freq = d_freq + d_beta * error;
d_phase = d_phase + d_freq + d_alpha * error;
}
void control_loop::phase_wrap()
{
while (d_phase > M_TWOPI)
d_phase -= M_TWOPI;
while (d_phase < -M_TWOPI)
d_phase += M_TWOPI;
}
void control_loop::frequency_limit()
{
if (d_freq > d_max_freq)
d_freq = d_max_freq;
else if (d_freq < d_min_freq)
d_freq = d_min_freq;
}
/*******************************************************************
* SET FUNCTIONS
*******************************************************************/
void control_loop::set_loop_bandwidth(float bw)
{
if (bw < 0) {
throw std::out_of_range("control_loop: invalid bandwidth. Must be >= 0.");
}
d_loop_bw = bw;
update_gains();
}
void control_loop::set_damping_factor(float df)
{
if (df <= 0) {
throw std::out_of_range("control_loop: invalid damping factor. Must be > 0.");
}
d_damping = df;
update_gains();
}
void control_loop::set_alpha(float alpha)
{
if (alpha < 0 || alpha > 1.0) {
throw std::out_of_range("control_loop: invalid alpha. Must be in [0,1].");
}
d_alpha = alpha;
}
void control_loop::set_beta(float beta)
{
if (beta < 0 || beta > 1.0) {
throw std::out_of_range("control_loop: invalid beta. Must be in [0,1].");
}
d_beta = beta;
}
void control_loop::set_frequency(float freq)
{
if (freq > d_max_freq)
d_freq = d_min_freq;
else if (freq < d_min_freq)
d_freq = d_max_freq;
else
d_freq = freq;
}
void control_loop::set_phase(float phase)
{
d_phase = phase;
while (d_phase > M_TWOPI)
d_phase -= M_TWOPI;
while (d_phase < -M_TWOPI)
d_phase += M_TWOPI;
}
void control_loop::set_max_freq(float freq) { d_max_freq = freq; }
void control_loop::set_min_freq(float freq) { d_min_freq = freq; }
/*******************************************************************
* GET FUNCTIONS
*******************************************************************/
float control_loop::get_loop_bandwidth() const { return d_loop_bw; }
float control_loop::get_damping_factor() const { return d_damping; }
float control_loop::get_alpha() const { return d_alpha; }
float control_loop::get_beta() const { return d_beta; }
float control_loop::get_frequency() const { return d_freq; }
float control_loop::get_phase() const { return d_phase; }
float control_loop::get_max_freq() const { return d_max_freq; }
float control_loop::get_min_freq() const { return d_min_freq; }
} /* namespace blocks */
} /* namespace gr */
|
