fir-filter.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_FIRFILTER_HH__
11 #define __SOT_FIRFILTER_HH__
12 
13 #include <cassert>
14 
15 #include <algorithm>
16 #include <iterator>
17 #include <vector>
18 
19 #include <dynamic-graph/all-signals.h>
20 #include <dynamic-graph/command-getter.h>
21 #include <dynamic-graph/command-setter.h>
22 #include <dynamic-graph/entity.h>
23 
24 namespace dg = dynamicgraph;
25 
26 namespace dynamicgraph {
27 namespace sot {
28 
29 namespace detail {
30 // GRMBL boost-sandox::circular_buffer smells... Why keep using 1.33?!
31 // As a workaround, only the part of circular_buffer's interface used
32 // here is implemented.
33 // Ugly, fatty piece of code.
34 template <class T> class circular_buffer {
35 public:
36  circular_buffer() : buf(1), start(0), numel(0) {}
37  void push_front(const T &data) {
38  if (start) {
39  --start;
40  } else {
41  start = buf.size() - 1;
42  }
43  buf[start] = data;
44  if (numel < buf.size()) {
45  ++numel;
46  }
47  }
48  void reset_capacity(size_t n) {
49  buf.resize(n);
50  start = 0;
51  numel = 0;
52  }
53  void reset_capacity(size_t n, const T &el) {
54  buf.clear();
55  buf.resize(n, el);
56  start = 0;
57  numel = 0;
58  }
59  T &operator[](size_t i) {
60  assert((i < numel) && "Youre accessing an empty buffer");
61  size_t index = (start + i) % buf.size();
62  return buf[index];
63  }
64  size_t size() const { return numel; }
65 
66 private:
67  std::vector<T> buf;
68  size_t start;
69  size_t numel;
70 }; // class circular_buffer
71 } // namespace detail
72 
73 template <class sigT, class coefT> class FIRFilter;
74 
75 namespace command {
76 using ::dynamicgraph::command::Command;
77 using ::dynamicgraph::command::Value;
78 
79 template <class sigT, class coefT> class SetElement : public Command {
80 public:
81  SetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
82  Value doExecute();
83 }; // class SetElement
84 
85 template <class sigT, class coefT> class GetElement : public Command {
86 public:
87  GetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
88  Value doExecute();
89 }; // class SetElement
90 } // namespace command
91 
92 using ::dynamicgraph::command::Getter;
93 using ::dynamicgraph::command::Setter;
94 
95 template <class sigT, class coefT> class FIRFilter : public Entity {
96 public:
97  virtual const std::string &getClassName() const {
98  return Entity::getClassName();
99  }
100  static std::string getTypeName(void) { return "Unknown"; }
101  static const std::string CLASS_NAME;
102 
103  std::string getDocString() const {
104  return "Finite impulse response filter\n"
105  "\n"
106  " Provide the following sum in output signal:\n"
107  " N \n"
108  " __ \n"
109  " y (n) = \\ c s (n-i) \n"
110  " /_ i \n"
111  " i=0 \n"
112  " \n"
113  " where\n"
114  " - c_i are coefficients stored in an array\n"
115  " - N is the size of the array\n"
116  " - s is the input signal.\n";
117  }
118 
119 public:
120  FIRFilter(const std::string &name)
121  : Entity(name), SIN(NULL, "sotFIRFilter(" + name + ")::input(T)::sin"),
122  SOUT(boost::bind(&FIRFilter::compute, this, _1, _2), SIN,
123  "sotFIRFilter(" + name + ")::output(T)::sout") {
124  signalRegistration(SIN << SOUT);
125  std::string docstring = " Set element at rank in array of coefficients\n"
126  "\n"
127  " Input:\n"
128  " - positive int: rank\n"
129  " - element\n";
130  addCommand("setElement",
131  new command::SetElement<sigT, coefT>(*this, docstring));
132  docstring = " Get element at rank in array of coefficients\n"
133  "\n"
134  " Input:\n"
135  " - positive int: rank\n"
136  " Return:\n"
137  " - element\n";
138  addCommand("getElement",
139  new command::GetElement<sigT, coefT>(*this, docstring));
140  docstring = " Set number of coefficients\n"
141  "\n"
142  " Input:\n"
143  " - positive int: size\n";
144  addCommand("setSize", new Setter<FIRFilter, unsigned>(
145  *this, &FIRFilter::resizeBuffer, docstring));
146 
147  docstring = " Get Number of coefficients\n"
148  "\n"
149  " Return:\n"
150  " - positive int: size\n";
151  addCommand("getSize", new Getter<FIRFilter, unsigned>(
152  *this, &FIRFilter::getBufferSize, docstring));
153  }
154 
155  virtual ~FIRFilter() {}
156 
157  virtual sigT &compute(sigT &res, int time) {
158  const sigT &in = SIN.access(time);
159  reset_signal(res, in);
160  data.push_front(in);
161 
162  size_t SIZE = std::min(data.size(), coefs.size());
163  for (size_t i = 0; i < SIZE; ++i) {
164  res += coefs[i] * data[i];
165  }
166 
167  return res;
168  }
169 
170  void resizeBuffer(const unsigned int &size) {
171  size_t s = static_cast<size_t>(size);
172  data.reset_capacity(s);
173  coefs.resize(s);
174  }
175 
176  unsigned int getBufferSize() const {
177  return static_cast<unsigned int>(coefs.size());
178  }
179 
180  void setElement(const unsigned int &rank, const coefT &coef) {
181  coefs[rank] = coef;
182  }
183 
184  coefT getElement(const unsigned int &rank) const { return coefs[rank]; }
185 
186  static void reset_signal(sigT & /*res*/, const sigT & /*sample*/) {}
187 
188 public:
189  SignalPtr<sigT, int> SIN;
190  SignalTimeDependent<sigT, int> SOUT;
191 
192 private:
193  std::vector<coefT> coefs;
195 }; // class FIRFilter
196 
197 namespace command {
198 using ::dynamicgraph::command::Command;
199 using ::dynamicgraph::command::Value;
200 using ::dynamicgraph::command::ValueHelper;
201 
202 template <class sigT, class coefT>
204  const std::string &docstring)
205  : Command(
206  entity,
207  boost::assign::list_of(Value::UNSIGNED)(ValueHelper<coefT>::TypeID),
208  docstring) {}
209 
210 template <class sigT, class coefT> Value SetElement<sigT, coefT>::doExecute() {
211  FIRFilter<sigT, coefT> &entity =
212  static_cast<FIRFilter<sigT, coefT> &>(owner());
213  std::vector<Value> values = getParameterValues();
214  unsigned int rank = values[0].value();
215  coefT coef = values[1].value();
216  entity.setElement(rank, coef);
217  return Value();
218 }
219 
220 template <class sigT, class coefT>
222  const std::string &docstring)
223  : Command(entity, boost::assign::list_of(Value::UNSIGNED), docstring) {}
224 
225 template <class sigT, class coefT> Value GetElement<sigT, coefT>::doExecute() {
226  FIRFilter<sigT, coefT> &entity =
227  static_cast<FIRFilter<sigT, coefT> &>(owner());
228  std::vector<Value> values = getParameterValues();
229  unsigned int rank = values[0].value();
230  return Value(entity.getElement(rank));
231 }
232 } // namespace command
233 
234 } // namespace sot
235 } // namespace dynamicgraph
236 
237 #endif
dynamicgraph::sot::FIRFilter::setElement
void setElement(const unsigned int &rank, const coefT &coef)
Definition: fir-filter.hh:180
dynamicgraph::sot::FIRFilter::getElement
coefT getElement(const unsigned int &rank) const
Definition: fir-filter.hh:184
dynamicgraph::sot::command::GetElement::GetElement
GetElement(FIRFilter< sigT, coefT > &entity, const std::string &docstring)
Definition: fir-filter.hh:221
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::detail::circular_buffer::reset_capacity
void reset_capacity(size_t n)
Definition: fir-filter.hh:48
dynamicgraph::sot::FIRFilter::FIRFilter
FIRFilter(const std::string &name)
Definition: fir-filter.hh:120
dynamicgraph::sot::detail::circular_buffer
Definition: fir-filter.hh:34
dynamicgraph::sot::command::GetElement::doExecute
Value doExecute()
Definition: fir-filter.hh:225
dynamicgraph::sot::detail::circular_buffer::push_front
void push_front(const T &data)
Definition: fir-filter.hh:37
dynamicgraph::sot::FIRFilter::compute
virtual sigT & compute(sigT &res, int time)
Definition: fir-filter.hh:157
dynamicgraph::sot::FIRFilter::reset_signal
static void reset_signal(sigT &, const sigT &)
Definition: fir-filter.hh:186
dynamicgraph::sot::FIRFilter::getDocString
std::string getDocString() const
Definition: fir-filter.hh:103
dynamicgraph::sot::command::SetElement::doExecute
Value doExecute()
Definition: fir-filter.hh:210
dynamicgraph::sot::command::SetElement
Definition: fir-filter.hh:79
dynamicgraph::sot::FIRFilter::getTypeName
static std::string getTypeName(void)
Definition: fir-filter.hh:100
dynamicgraph::sot::command::SetElement::SetElement
SetElement(FIRFilter< sigT, coefT > &entity, const std::string &docstring)
Definition: fir-filter.hh:203
dynamicgraph::sot::FIRFilter::SIN
SignalPtr< sigT, int > SIN
Definition: fir-filter.hh:189
dynamicgraph::sot::FIRFilter::resizeBuffer
void resizeBuffer(const unsigned int &size)
Definition: fir-filter.hh:170
dynamicgraph::sot::command::GetElement
Definition: fir-filter.hh:85
dynamicgraph::sot::detail::circular_buffer::reset_capacity
void reset_capacity(size_t n, const T &el)
Definition: fir-filter.hh:53
dynamicgraph::sot::FIRFilter::getBufferSize
unsigned int getBufferSize() const
Definition: fir-filter.hh:176
dynamicgraph::sot::FIRFilter::getClassName
virtual const std::string & getClassName() const
Definition: fir-filter.hh:97
dynamicgraph::sot::FIRFilter::CLASS_NAME
static const std::string CLASS_NAME
Definition: fir-filter.hh:101
dynamicgraph::sot::detail::circular_buffer::operator[]
T & operator[](size_t i)
Definition: fir-filter.hh:59
dynamicgraph::sot::FIRFilter
Definition: fir-filter.hh:73
dynamicgraph::sot::FIRFilter::SOUT
SignalTimeDependent< sigT, int > SOUT
Definition: fir-filter.hh:190
dynamicgraph::sot::detail::circular_buffer::size
size_t size() const
Definition: fir-filter.hh:64
dynamicgraph::sot::FIRFilter::~FIRFilter
virtual ~FIRFilter()
Definition: fir-filter.hh:155
dynamicgraph::sot::detail::circular_buffer::circular_buffer
circular_buffer()
Definition: fir-filter.hh:36