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


PlugF Accel_Vector[MAX_AXES];
PlugF Gyro_Vector[MAX_AXES];
PlugF MAG_Heading;
SocketF roll;
SocketF pitch;
SocketF yaw;
ParameterF Kp_ROLLPITCH;
ParameterF Ki_ROLLPITCH;


float G_Dt=0.010;    // Integration time (DCM algorithm)  We will run the integration loop at 100Hz

#include "Vector.h"
#include "matrix.h"
#include "DCM.h"


class DcmAHRS : public Module
{
    DECLARE_MODULE(DcmAHRS,Module);
    public:

    DcmAHRS()
    {
        AddSocket("AHRS.Pitch",&pitch);
        AddSocket("AHRS.Roll",&roll);
        AddSocket("AHRS.Yaw",&yaw);
        AddSocket("DCMAHRS.RollPitch.P",&Kp_ROLLPITCH);
        AddSocket("DCMAHRS.RollPitch.I",&Ki_ROLLPITCH);
        Kp_ROLLPITCH = 0.4;
        Ki_ROLLPITCH = 0.0005;
    }

    virtual void ParamChanged(Socket * param)
    {
    }

    virtual void Init()
    {
        SetPriority (PRIORITY_NEVER); //don't run unless we initialize correctly
        SetInterval(G_Dt * 1000);
        Accel_Vector[AXISX].Connect("Accel.X");
        Accel_Vector[AXISY].Connect("Accel.Y");
        Accel_Vector[AXISZ].Connect("Accel.Z");
        Gyro_Vector[1].Connect("Gyro.Pitch");
        Gyro_Vector[0].Connect("Gyro.Roll");
        Gyro_Vector[2].Connect("Gyro.Yaw");
        MAG_Heading.Connect("Compass.Heading");
        for (int ct=0; ct< MAX_AXES; ct++)
        {
            if (!Accel_Vector[ct].IsConnected())
            {
                Errorln("DcmAHRS: I need accelerometers");
                return;
            }
            if (!Gyro_Vector[ct].IsConnected())
            {
                Errorln("DcmAHRS: I need gyros");
                return;
            }
        }
        SetPriority(20);
    }

    virtual void Reference()
    {
        DCM_reference();
    }

    virtual void Loop(const unsigned long& interval)
    {
        // Calculations...
        Matrix_update();
        Normalize();
        Drift_correction();
        Euler_angles();
        // ***
    }

private:


};

DcmAHRS m_DcmAHRS;