C:/Documents and Settings/gw/My Documents/Other projects/flightos/Modules/analoggyro.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 AnalogGyro : public Module
{
    DECLARE_MODULE(AnalogGyro,Module)

public:
    AnalogGyro()
    {
        AddSocket("Gyro.Pitch",&m_filter.filtered[PITCH]); //Scaled and smoothed values
        AddSocket("Gyro.Roll",&m_filter.filtered[ROLL]);
        AddSocket("Gyro.Yaw",&m_filter.filtered[YAW]);

        AddSocket("AnalogGyro.Filter.P",&m_filter.amounts[PITCH]); //Smoothing
        AddSocket("AnalogGyro.Filter.R",&m_filter.amounts[ROLL]);
        AddSocket("AnalogGyro.Filter.Y",&m_filter.amounts[YAW]);

        AddSocket("AnalogGyro.Gain.P",&m_gains[PITCH]); //Scale factors. Scale to radians/second
        AddSocket("AnalogGyro.Gain.R",&m_gains[ROLL]);
        AddSocket("AnalogGyro.Gain.Y",&m_gains[YAW]);
        for (int ct=0; ct<MAX_AXES; ct++)
        {
            m_gains[ct] = GYRO_SCALE;
        }
    }

    virtual void Init()
    {
        SetInterval(5);
        SetPriority(20);

        pinMode (AZPIN, OUTPUT);
        pinMode (AZYAWPIN, OUTPUT);
        digitalWrite(AZPIN, LOW);
        digitalWrite(AZYAWPIN, LOW);
        analogReference(EXTERNAL);
    }

    virtual void Reference()
    {
        digitalWrite(AZPIN, HIGH);
        digitalWrite(AZYAWPIN, HIGH);
        delayMicroseconds(750);
        digitalWrite(AZPIN, LOW);
        digitalWrite(AZYAWPIN, LOW);
        delay(8);
        m_filter.Reference(20);
    }

    virtual void ParamChanged(Socket * param)
    {
        m_filter.ParamChanged(param);
    }

    virtual void Loop(const unsigned long& interval)
    {
        float data[MAX_AXES];
        data[PITCH] = analogRead(GYROADC_PITCH) * m_gains[PITCH];
        data[ROLL] = analogRead(GYROADC_ROLL) * m_gains[ROLL];
        data[YAW] = analogRead(GYROADC_YAW) * m_gains[YAW];
        m_filter.Update(data);
    }

    MultiFilterRef<MAX_AXES> m_filter;
//    MultiFirFilterRef<MAX_AXES> m_filter;
    ParameterF m_gains[MAX_AXES];

};

AnalogGyro _AnalogGyro;