Statistics
| Branch: | Tag: | Revision:

gnuradio / gr-blocks / lib / control_loop.cc @ master

History | View | Annotate | Download (4.64 KB)

1
/* -*- c++ -*- */
2
/*
3
 * Copyright 2011,2013 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*M_PI)
35

36
    control_loop::control_loop(float loop_bw,
37
                               float max_freq, float min_freq)
38
      : d_phase(0), d_freq(0), d_max_freq(max_freq), d_min_freq(min_freq)
39
    {
40
      // Set the damping factor for a critically damped system
41
      d_damping = sqrtf(2.0f)/2.0f;
42

43
      // Set the bandwidth, which will then call update_gains()
44
      set_loop_bandwidth(loop_bw);
45
    }
46

47
    control_loop::~control_loop()
48
    {
49
    }
50

51
    void
52
    control_loop::update_gains()
53
    {
54
      float denom = (1.0 + 2.0*d_damping*d_loop_bw + d_loop_bw*d_loop_bw);
55
      d_alpha = (4*d_damping*d_loop_bw) / denom;
56
      d_beta = (4*d_loop_bw*d_loop_bw) / denom;
57
    }
58

59
    void
60
    control_loop::advance_loop(float error)
61
    {
62
      d_freq = d_freq + d_beta * error;
63
      d_phase = d_phase + d_freq + d_alpha * error;
64
    }
65

66
    void
67
    control_loop::phase_wrap()
68
    {
69
      while(d_phase>M_TWOPI)
70
        d_phase -= M_TWOPI;
71
      while(d_phase<-M_TWOPI)
72
        d_phase += M_TWOPI;
73
    }
74

75
    void
76
    control_loop::frequency_limit()
77
    {
78
      if(d_freq > d_max_freq)
79
        d_freq = d_max_freq;
80
      else if(d_freq < d_min_freq)
81
        d_freq = d_min_freq;
82
    }
83

84
    /*******************************************************************
85
     * SET FUNCTIONS
86
     *******************************************************************/
87

88
    void
89
    control_loop::set_loop_bandwidth(float bw)
90
    {
91
      if(bw < 0) {
92
        throw std::out_of_range ("control_loop: invalid bandwidth. Must be >= 0.");
93
      }
94

95
      d_loop_bw = bw;
96
      update_gains();
97
    }
98

99
    void
100
    control_loop::set_damping_factor(float df)
101
    {
102
      if(df <= 0) {
103
        throw std::out_of_range ("control_loop: invalid damping factor. Must be > 0.");
104
      }
105

106
      d_damping = df;
107
      update_gains();
108
    }
109

110
    void
111
    control_loop::set_alpha(float alpha)
112
    {
113
      if(alpha < 0 || alpha > 1.0) {
114
        throw std::out_of_range ("control_loop: invalid alpha. Must be in [0,1].");
115
      }
116
      d_alpha = alpha;
117
    }
118

119
    void
120
    control_loop::set_beta(float beta)
121
    {
122
      if(beta < 0 || beta > 1.0) {
123
        throw std::out_of_range ("control_loop: invalid beta. Must be in [0,1].");
124
      }
125
      d_beta = beta;
126
    }
127

128
    void
129
    control_loop::set_frequency(float freq)
130
    {
131
      if(freq > d_max_freq)
132
        d_freq = d_min_freq;
133
      else if(freq < d_min_freq)
134
        d_freq = d_max_freq;
135
      else
136
        d_freq = freq;
137
    }
138

139
    void
140
    control_loop::set_phase(float phase)
141
    {
142
      d_phase = phase;
143
      while(d_phase>M_TWOPI)
144
        d_phase -= M_TWOPI;
145
      while(d_phase<-M_TWOPI)
146
        d_phase += M_TWOPI;
147
    }
148

149
    void
150
    control_loop::set_max_freq(float freq)
151
    {
152
      d_max_freq = freq;
153
    }
154

155
    void
156
    control_loop::set_min_freq(float freq)
157
    {
158
      d_min_freq = freq;
159
    }
160

161
    /*******************************************************************
162
     * GET FUNCTIONS
163
     *******************************************************************/
164

165
    float
166
    control_loop::get_loop_bandwidth() const
167
    {
168
      return d_loop_bw;
169
    }
170

171
    float
172
    control_loop::get_damping_factor() const
173
    {
174
      return d_damping;
175
    }
176

177
    float
178
    control_loop::get_alpha() const
179
    {
180
      return d_alpha;
181
    }
182

183
    float
184
    control_loop::get_beta() const
185
    {
186
      return d_beta;
187
    }
188

189
    float
190
    control_loop::get_frequency() const
191
    {
192
      return d_freq;
193
    }
194

195
    float
196
    control_loop::get_phase() const
197
    {
198
      return d_phase;
199
    }
200

201
    float
202
    control_loop::get_max_freq() const
203
    {
204
      return d_max_freq;
205
    }
206

207
    float
208
    control_loop::get_min_freq() const
209
    {
210
      return d_min_freq;
211
    }
212

213
  } /* namespace blocks */
214
} /* namespace gr */