Merge pull request #10 from andreaskunz/pullReq_feat_MAFilter

feat(MAFilter): add new class

includes/eeros/control/MAFilter.hpp

@@ -0,0 +1,152 @@
+#ifndef ORG_EEROS_CONTROL_MAFILTER_HPP_
+#define ORG_EEROS_CONTROL_MAFILTER_HPP_
+
+#include <eeros/control/Block1i1o.hpp>
+#include <ostream>
+
+
+namespace eeros {
+	namespace control {
+
+		/**
+		 * A moving average filter (MAFilter) block is used to filter an input signal. 
+		 * The output signal value depends linearly on the current and various past
+		 * input signal values.
+		 * This is achieved by multiplying the current and the past values with
+		 * the specified coefficients. The results are accumulated and lead to the
+		 * output signal value. The following terms represents the operation performed in
+		 * this block.
+		 * 
+		 * y[t] = c[0]*x[t-N]  + c[1]*x[t-N+1]  + ... +  c[N]*x[t]
+		 *
+		 * Outp = c[0]*prev[0] + c[1]*prev[1]   + ... +  c[N]*Inp
+		 * 
+		 * MAFilter is a class template with two type and one non-type template arguments.
+		 * The two type template arguments specify the types which are used for the 
+		 * values and the coefficients when the class template is instanciated.
+		 * The non-type template argument specifies the number of coefficients and the
+		 * number of concidered past values respectively.
+		 * 
+		 * @tparam N - number of coefficients
+		 * @tparam Tval - value type (double - default type)
+		 * @tparam Tcoeff - coefficients type (Tval - default value)
+		 * 
+		 * @since v0.6
+		 */
+
+		template <size_t N, typename Tval = double, typename Tcoeff = Tval>
+		class MAFilter : public Block1i1o<Tval> {
+
+		public:
+			/**
+			 * Constructs a MAFilter instance with the coefficients coeff.\n
+			 * @param coeff - coefficients
+			 */
+			explicit MAFilter(Tcoeff (& coeff)[N]) : coefficients{coeff} {}
+
+
+			/**
+			 * Runs the filter algorithm.
+			 * 
+			 * Performs the calculation of the filtered output signal value.
+			 * Multiplies the current and past input signal values with the coefficients.
+			 * The coefficients weight the current and past input signal values. Finally, 
+			 * the resulting values are accumulated and yield to the output signal value
+			 * if the filter instance is enabled. Otherwise, the output signal value is 
+			 * set to the actual input signal value.
+			 * 
+			 * The timestamp value will not be altered.
+			 *
+			 * @see enable()
+			 * @see disable()
+			 */
+			virtual void run() {
+				Tval result{};
+				result = 0; // needed to zero initialize Matrix
+
+				for(size_t i = 0; i < N; i++) {
+					if(i < N-1) {
+						previousValues[i] = previousValues[i+1];
+					} else {
+						previousValues[i] = this->in.getSignal().getValue();
+					}
+
+					result += coefficients[i] * previousValues[i];
+				}
+
+				if(enabled) {
+					this->out.getSignal().setValue(result);
+				} else {
+					this->out.getSignal().setValue(this->in.getSignal().getValue());
+				}
+
+				this->out.getSignal().setTimestamp(this->in.getSignal().getTimestamp());
+			}
+
+
+			/**
+			 * Enables the filter.
+			 * 
+			 * If enabled, run() will set the output signal value to the accumulated values
+			 * which result from the current and the past values weighted by the coefficients. 
+			 * 
+			 * @see run()
+			 */
+			virtual void enable() {
+				enabled = true;
+			}
+
+
+			/**
+			 * Disables the filter.
+			 * 
+			 * If disabled, run() will set the output signal to the input signal.
+			 *
+			 * @see run()
+			 */
+			virtual void disable() {
+				enabled = false;
+			}
+
+
+			/*
+			 * Friend operator overload to give the operator overload outside 
+			 * the class access to the private fields.
+			 */
+			template <size_t No, typename ValT, typename CoeffT>
+			friend std::ostream& operator<<(std::ostream& os, MAFilter<No,ValT,CoeffT>& filter);
+
+
+		protected:
+			Tcoeff * coefficients;
+			Tval previousValues[N]{};
+			bool enabled{true};
+		};
+
+
+		/**
+		 * Operator overload (<<) to enable an easy way to print the state of a
+		 * MAFilter instance to an output stream.
+		 * Does not print a newline control character.
+		 */
+		template <size_t N, typename Tval, typename Tcoeff>
+		std::ostream& operator<<(std::ostream& os, MAFilter<N,Tval,Tcoeff>& filter) {
+			os << "Block MAFilter: '" << filter.getName() << "' is enabled=";
+			os << filter.enabled << ", ";
+
+			os << "coefficients:[" << filter.coefficients[0];
+			for(size_t i = 1; i < N; i++){
+				os << "," << filter.coefficients[i];
+			}
+			os << "], ";
+
+			os << "previousValues:[" << filter.previousValues[0];
+			for(size_t i = 1; i < N; i++){
+				os << "," << filter.previousValues[i];
+			}
+			os << "]";
+		}
+	};
+};
+
+#endif /* ORG_EEROS_CONTROL_MAFILTER_HPP_ */

test/control/CMakeLists.txt

@@ -11,8 +11,10 @@ add_eeros_test_sources(SignalChecker.cpp)
 add_eeros_test_sources(SocketData.cpp)
 add_eeros_test_sources(Sum.cpp)
 add_eeros_test_sources(Transition.cpp)
+add_eeros_test_sources(MAFilter.cpp)
 add_eeros_test_sources(MedianFilter.cpp)
 
+
 # Blocks, old, delete when replaced by new tests
 add_subdirectory(d)
 add_subdirectory(mux)

test/control/MAFilter.cpp

@@ -0,0 +1,149 @@
+#include <eeros/control/MAFilter.hpp>
+#include <eeros/control/Constant.hpp>
+#include <eeros/math/Matrix.hpp>
+
+#include <gtest/gtest.h>
+
+#include <sstream>
+#include <string>
+
+using namespace eeros;
+using namespace eeros::control;
+
+
+TEST(MAFilterUnitTest, templateInstantiations) {
+  double dcoeffs[] = {0.5, 0.5};
+  MAFilter<2> f1{dcoeffs};
+
+  double d5coeffs[] = {0.2, 0.2, 0.2, 0.2, 0.2};
+  MAFilter<5> f2{d5coeffs};
+
+  float fcoeffs[] = {0.1, 0.1};
+  MAFilter<2,int,float> f3{fcoeffs};
+
+  int icoeffs[] = {1, 1}; 
+  MAFilter<2,int> f4{icoeffs};
+
+  using namespace math; 
+  MAFilter<2,Matrix<2,2>, double> f5{dcoeffs};
+  ASSERT_TRUE(true); // they would fail at compile time.
+}
+
+
+TEST(MAFilterUnitTest, doubleMAFilter) {
+  double coeffs[] = {0.5, 0.5};
+  MAFilter<2> ma{coeffs};
+
+  Constant<> c1{2};
+  c1.run();
+  ma.getIn().connect(c1.getOut());
+
+  // this test case also tests if enable is true by default.
+
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue(), 1);
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue(), 2);
+  ma.run();
+  ma.run();
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue(), 2);
+
+  ASSERT_EQ (c1.getOut().getSignal().getTimestamp(), ma.getOut().getSignal().getTimestamp());
+}
+
+
+TEST(MAFilterUnitTest, enableDisable) {
+  double coeffs[] = {0.5, 0.5};
+  MAFilter<2> ma{coeffs};
+
+  Constant<> c1{5};
+  c1.run();
+  ma.getIn().connect(c1.getOut());
+  ma.disable();
+
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue(), 5);
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue(), 5);
+
+  ma.enable();
+  ma.run();
+  // previous values will be stored regardless if filter is enabled or not.
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue(), 5);
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue(), 5);
+
+  ASSERT_EQ (c1.getOut().getSignal().getTimestamp(), ma.getOut().getSignal().getTimestamp());
+}
+
+
+TEST(MAFilterUnitTest, vectorMAFilter) {
+  using namespace math;
+
+  double coeffs[] = {0.1, 0.2, 0.3, 0.4};
+  MAFilter<4, Vector3, double> ma{coeffs};
+
+  Vector3 vec3{};
+  vec3 << 1, 2, 3;
+  Constant<Vector3> c1{vec3};
+  c1.run();
+  ma.getIn().connect(c1.getOut());
+
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[0], 0.4);
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[1], 0.8);
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[2], 1.2);
+
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[0], 0.7);
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[1], 1.4);
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[2], 2.1);
+
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[0], 0.9);
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[1], 1.8);
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[2], 2.7);
+
+  ma.run();
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[0], 1);
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[1], 2);
+  EXPECT_DOUBLE_EQ (ma.getOut().getSignal().getValue()[2], 3);
+
+  ASSERT_EQ (c1.getOut().getSignal().getTimestamp(), ma.getOut().getSignal().getTimestamp());
+}
+
+
+TEST(MAFilterUnitTest, printMAFilter) {
+  double coeffs[] = {0.2, 0.2, 0.2, 0.2, 0.2};
+  MAFilter<5> f1{coeffs};
+  f1.setName("my1stMAFilter");
+
+  Constant<> c1{1.0};
+  f1.getIn().connect(c1.getOut());
+
+  c1.run();
+  f1.run();
+
+  c1.setValue(2.0);
+  c1.run();
+  f1.run();
+
+  c1.setValue(3.0);
+  c1.run();
+  f1.run();
+
+  c1.setValue(4.0);
+  c1.run();
+  f1.run();
+
+  c1.setValue(3.14159);
+  c1.run();
+  f1.run();
+
+  std::stringstream sstream{};
+  sstream << f1;
+  std::string str1 = "Block MAFilter: 'my1stMAFilter' is enabled=1, coefficients:[0.2,0.2,0.2,0.2,0.2], previousValues:[1,2,3,4,3.14159]";
+  std::string str2 = sstream.str();
+  ASSERT_STREQ (str1.c_str(), str2.c_str());
+}