C:/Documents and Settings/gw/My Documents/Other projects/flightos/Libraries/multifilter.h

Go to the documentation of this file.

/*
  Copyright (c) 2010 Les Newell. All rights reserved

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef MULTIFILTER_H_INCLUDED
#define MULTIFILTER_H_INCLUDED

//This class will filter any number of floating point axes. It also has built in zero referencing

template <int N>
class MultiFilterRef
{
public:
    MultiFilterRef()
    {
        for (int ct=0; ct < N; ct++)
        {
            filtered[ct] = 0;
            zeros[ct] = 0;
            amounts[ct] = 0;
        }
        m_referencing = 0;
    }

    void ParamChanged(Socket * param)
    {
        for(int ct=0; ct< N; ct++)
        {
            if(param == &amounts[ct])
            {
                if(amounts[ct] > 1) //greater than 1 is BAD
                {
                    amounts[ct] = 1;
                }
            }
        }
    }


    void Update(float values[N])
    {
        if (m_referencing)
        {
            m_referencing--;
            for (int ct=0; ct< N; ct++)
            {
                m_averages[ct] += values[ct];
            }
            if (m_referencing == 0)
            {
                for (int ct=0; ct< N; ct++)
                {
                    zeros[ct] = -m_averages[ct] / m_refCounts;
                    filtered[ct] = 0;
                }
            }
        }

        for (int ct=0; ct< N; ct++)
        {
            filtered[ct] += ((values[ct] + zeros[ct]) - filtered[ct]) * amounts[ct];
        }
    }

    void Reference(unsigned char count)
    {
        m_refCounts = count;
        m_referencing = count;
        for (int ct=0; ct < N; ct++)
        {
            m_averages[ct] = 0;
        }
    }

    inline bool Referencing()
    {
        return(m_referencing);
    }

    SocketF filtered[N];
    ParameterF amounts[N];
    float zeros[N];

protected:
    unsigned char m_refCounts;
    float m_averages[N];
    unsigned char m_referencing;
};


//This class will filter any number of floating point axes.

template <int N>
class MultiFilter
{
public:
    MultiFilter()
    {
        for (int ct=0; ct < N; ct++)
        {
            filtered[ct] = 0;
            amounts[ct] = 0;
        }
    }

    void Update(float values[N])
    {
        for (int ct=0; ct< N; ct++)
        {
            filtered[ct] += (values[ct] - filtered[ct]) * amounts[ct];
        }
    }

    SocketF filtered[N];
    ParameterF amounts[N];

};


class FIRFloat
{
public:

    FIRFloat()
    {
        m_data = NULL;
        m_sizeMultiplier = 0;
        m_acc = 0;
        Init(0);
    }

    ~FIRFloat()
    {
        free(m_data);
    }

    void Init(unsigned char size)
    {
        m_size = size;
        m_index = 0;
        m_data = (float *)realloc(m_data, sizeof(float) * m_size);
        m_acc = 0;
        memset(m_data,0,sizeof(float) * m_size);
        m_sizeMultiplier = 1 / (float)m_size;
    }

    float Update(const float& nextValue)
    {
        if (m_size <= 1)
        {
            return(nextValue);
        }
        m_index ++;
        if (m_index >= m_size)
        {
            m_index =0;
        }
        m_acc -= m_data[m_index];
        m_data[m_index] = nextValue;
        m_acc += nextValue;
        return(m_acc * m_sizeMultiplier);
    }


private:
    int m_index;
    int m_size;
    float * m_data;
    float m_acc;
    float m_sizeMultiplier;
};



template <int N>
class MultiFirFilterRef
{
public:
    MultiFirFilterRef()
    {
        for (int ct=0; ct < N; ct++)
        {
            filtered[ct] = 0;
            zeros[ct] = 0;
        }
        m_referencing = 0;
    }

    void ParamChanged(Socket * param)
    {
        for(int ct=0; ct< N; ct++)
        {
            if(param == &amounts[ct])
            {
                m_firs[ct].Init(amounts[ct]);
            }
        }
    }

    void Update(float values[N])
    {
        if (m_referencing)
        {
            m_referencing--;
            for (int ct=0; ct< N; ct++)
            {
                m_averages[ct] += values[ct];
            }
            if (m_referencing == 0)
            {
                for (int ct=0; ct< N; ct++)
                {
                    zeros[ct] = -m_averages[ct] / m_refCounts;
                    filtered[ct] = 0;
                }
            }
        }

        for (int ct=0; ct< N; ct++)
        {
            filtered[ct] = m_firs[ct].Update(values[ct]) + zeros[ct];
        }
    }

    void Reference(unsigned char count)
    {
        m_refCounts = count;
        m_referencing = count;
        for (int ct=0; ct < N; ct++)
        {
            m_averages[ct] = 0;
        }
    }

    inline bool Referencing()
    {
        return(m_referencing);
    }

    SocketF filtered[N];
    Parameter8 amounts[N];
    float zeros[N];

protected:
    unsigned char m_refCounts;
    unsigned char m_referencing;
    float m_averages[N];
    FIRFloat m_firs[N];
};


#endif// #ifndef MULTIFILTER_H_INCLUDED