C:/Documents and Settings/gw/My Documents/Other projects/flightos/Modules/analogaccel.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/>.
*/


class AnalogAccel : public Module
{
    DECLARE_MODULE(AnalogAccel,Module)

    AnalogAccel()
    {
        AddSocket("Accel.X",&m_filter.filtered[AXISX]); //Filtered and scaled accelerometer values
        AddSocket("Accel.Y",&m_filter.filtered[AXISY]);
        AddSocket("Accel.Z",&m_filter.filtered[AXISZ]);

        AddSocket("AnalogAccel.Filter.X",&m_filter.amounts[AXISX]); //Smoothing.
        AddSocket("AnalogAccel.Filter.Y",&m_filter.amounts[AXISY]);
        AddSocket("AnalogAccel.Filter.Z",&m_filter.amounts[AXISZ]);

        AddSocket("AnalogAccel.Offset.X",&m_offsets[AXISX]); //Zero offsets
        AddSocket("AnalogAccel.Offset.Y",&m_offsets[AXISY]);
        AddSocket("AnalogAccel.Offset.Z",&m_offsets[AXISZ]);

        AddSocket("AnalogAccel.Gain.X",&m_gains[AXISX]); //Scale factors. Scale to units of 1G
        AddSocket("AnalogAccel.Gain.Y",&m_gains[AXISY]);
        AddSocket("AnalogAccel.Gain.Z",&m_gains[AXISZ]);
        for (int ct=0; ct<MAX_AXES; ct++)
        {
            m_gains[ct] = ACCEL_SCALE;
            m_offsets[ct] = ACCEL_OFFSET;
        }
    }

    virtual void ParamChanged(Socket * param)
    {
        //Update immediately if someone changes the offset
/*        if (param == &m_zOffset)
        {
            m_filter.zeros[AXISZ] = m_zOffset;
        }
        m_filter.ParamChanged(param);*/
    }

    virtual void Init()
    {
        SetInterval(5);
        SetPriority(20);
        analogReference(EXTERNAL);
    }

    virtual void Reference()
    {
//        m_filter.Reference(40);
    }

    virtual void Loop(const unsigned long& interval)
    {
        float data[MAX_AXES];

        data[AXISX] = (analogRead(ACCEL_ADC_X) + m_offsets[AXISX]) * m_gains[AXISX];
        data[AXISY] = (analogRead(ACCEL_ADC_Y) + m_offsets[AXISY]) * m_gains[AXISY];
        data[AXISZ] = (analogRead(ACCEL_ADC_Z) + m_offsets[AXISZ]) * m_gains[AXISZ];
/*        if (m_filter.Referencing())
        {
            m_filter.Update(data);
            if (!m_filter.Referencing()) //we have just referenced
            {
                //The Z axis is now zero and it should be 1G
                //so manually set it
                m_filter.zeros[AXISZ] = m_offsets;
            }
            return;
        }*/
        m_filter.Update(data);
    }

    MultiFilter<MAX_AXES> m_filter;
    ParameterF m_offsets[MAX_AXES];
    ParameterF m_gains[MAX_AXES];

};

AnalogAccel g_AnalogAccel;