GCC Code Coverage Report

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