EEROS  1.0.0.0
API for the EEROS Real-Time Robotics Framework
MAFilter.hpp
Go to the documentation of this file.
1 #ifndef ORG_EEROS_CONTROL_MAFILTER_HPP_
2 #define ORG_EEROS_CONTROL_MAFILTER_HPP_
3 
5 #include <type_traits>
6 #include <ostream>
7 
8 
9 namespace eeros {
10  namespace control {
11 
38  template <size_t N, typename Tval = double, typename Tcoeff = Tval>
39  class MAFilter : public Block1i1o<Tval> {
40 
41  public:
46  explicit MAFilter(Tcoeff (& coeff)[N]) : coefficients{coeff} {
47  zeroInitPreviousValues<Tval>();
48  }
49 
50 
66  virtual void run() {
67  Tval actualValue = this->in.getSignal().getValue();
68  Tval result = coefficients[N-1] * actualValue;
69 
70  for(size_t i = 0; i < N-1; i++) {
72  result += coefficients[i] * previousValues[i];
73  }
74  previousValues[N-1] = actualValue;
75 
76  if(enabled) {
77  this->out.getSignal().setValue(result);
78  } else {
79  this->out.getSignal().setValue(this->in.getSignal().getValue());
80  }
81 
83  }
84 
85 
94  virtual void enable() {
95  enabled = true;
96  }
97 
98 
106  virtual void disable() {
107  enabled = false;
108  }
109 
110 
111  /*
112  * Friend operator overload to give the operator overload outside
113  * the class access to the private fields.
114  */
115  template <size_t No, typename ValT, typename CoeffT>
116  friend std::ostream& operator<<(std::ostream& os, MAFilter<No,ValT,CoeffT>& filter);
117 
118 
119  protected:
120  Tcoeff * coefficients;
121  Tval previousValues[N]{};
122  bool enabled{true};
123 
124 
125  private:
126  template <typename S>
127  typename std::enable_if<std::is_arithmetic<S>::value>::type zeroInitPreviousValues() {
128  // is zeroed when initialized by default.
129  }
130 
131 
132  template <typename S>
133  typename std::enable_if<!std::is_arithmetic<S>::value>::type zeroInitPreviousValues() {
134  for(size_t i = 0; i < N; i++) {
135  previousValues[i].zero();
136  }
137  }
138  };
139 
140 
146  template <size_t N, typename Tval, typename Tcoeff>
147  std::ostream& operator<<(std::ostream& os, MAFilter<N,Tval,Tcoeff>& filter) {
148  os << "Block MAFilter: '" << filter.getName() << "' is enabled=";
149  os << filter.enabled << ", ";
150 
151  os << "coefficients:[" << filter.coefficients[0];
152  for(size_t i = 1; i < N; i++){
153  os << "," << filter.coefficients[i];
154  }
155  os << "], ";
156 
157  os << "previousValues:[" << filter.previousValues[0];
158  for(size_t i = 1; i < N; i++){
159  os << "," << filter.previousValues[i];
160  }
161  os << "]";
162  }
163  };
164 };
165 
166 #endif /* ORG_EEROS_CONTROL_MAFILTER_HPP_ */
Tcoeff * coefficients
Definition: MAFilter.hpp:120
Definition: MAFilter.hpp:39
virtual Signal< T > & getSignal()
Definition: Output.hpp:16
virtual T getValue() const
Definition: Signal.hpp:49
Input< Tval > in
Definition: Block1i1o.hpp:27
Definition: Config.hpp:14
Output< Tval > out
Definition: Block1i1o.hpp:28
virtual void setValue(T newValue)
Definition: Signal.hpp:53
virtual void run()
Definition: MAFilter.hpp:66
virtual void enable()
Definition: MAFilter.hpp:94
virtual void setTimestamp(timestamp_t newTimestamp)
Definition: Signal.hpp:66
virtual void disable()
Definition: MAFilter.hpp:106
Definition: Block1i1o.hpp:12
virtual Signal< T > & getSignal()
Definition: Input.hpp:38
virtual timestamp_t getTimestamp() const
Definition: Signal.hpp:62
MAFilter(Tcoeff(&coeff)[N])
Definition: MAFilter.hpp:46
Tval previousValues[N]
Definition: MAFilter.hpp:121
bool enabled
Definition: MAFilter.hpp:122