MedianFilter.hpp

Go to the documentation of this file.

#ifndef ORG_EEROS_CONTROL_MEDIANFILTER_HPP_
#define ORG_EEROS_CONTROL_MEDIANFILTER_HPP_

#include <eeros/control/Block1i1o.hpp>
#include <algorithm>
#include <type_traits>
#include <cmath>


namespace eeros {
    namespace control {

        template <size_t N, typename Tval = double>
        class MedianFilter : public Block1i1o<Tval> {

        public:
             MedianFilter() {
                zeroInitCurrentValues<Tval>();
             }


            virtual void run() {
                for(size_t i = 0; i < N-1; i++) {
                    currentValues[i] = currentValues[i+1];
                }

                currentValues[N-1] = this->in.getSignal().getValue();

                if(enabled) {
                    Tval temp[N]{};

                    std::copy(std::begin(currentValues), std::end(currentValues), std::begin(temp));
                    std::sort(std::begin(temp), std::end(temp));

                    currentMedianValue = temp[medianIndex];
                    this->out.getSignal().setValue(currentMedianValue);

                } else {
                    this->out.getSignal().setValue(this->in.getSignal().getValue());
                }

                this->out.getSignal().setTimestamp(this->in.getSignal().getTimestamp());
            }


            virtual void enable() {
                enabled = true;
            }


            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>
            friend std::ostream& operator<<(std::ostream& os, MedianFilter<No,ValT>& filter);


        protected:
            Tval currentValues[N]{};
            Tval currentMedianValue;
            bool enabled{true};
            constexpr static int medianIndex{static_cast<int>(floor(N/2))};


        private:
            template <typename S>
            typename std::enable_if<std::is_arithmetic<S>::value>::type zeroInitCurrentValues() {
                // is zeroed when initialized by default.
            }


            template <typename S>
            typename std::enable_if<!std::is_arithmetic<S>::value>::type zeroInitCurrentValues() {
                  for(size_t i = 0; i < N; i++) {
                    currentValues[i].zero();
                  }
            }
        };


        template <size_t N, typename Tval>
        std::ostream& operator<<(std::ostream& os, MedianFilter<N,Tval>& filter) {
            os << "Block MedianFilter: '" << filter.getName() << "' is enabled=";
            os << filter.enabled << ", ";

            os << "current median=" << filter.currentMedianValue << ", ";

            os << "medianIndex=" << filter.medianIndex << ", ";

            os << "current values:[" << filter.currentValues[0];
            for(size_t i = 1; i < N; i++){
                os << "," << filter.currentValues[i];
            }
            os << "]";
        }
    };
};

#endif /* ORG_EEROS_CONTROL_MEDIANFILTER_HPP_ */