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

//#include "1axisKalman.c"


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


    //Only use this to expose sockets and parameters. Do most of your
    //initialisastion in Init()
    AltEstimate()
    {
        /*Expose sockets to the outside world
        NOTE: the socket name must be a static string.
        Bad Things will happen if you use a string allocated on the stack
        You can use a pointer to a static string.*/
        AddSocket("INS.Altitude",&m_altitude);  //Estimated altitude
        AddSocket("AHRS.Acceleration",&m_acceleration);
        AddSocket("AltEstimate.AccelGain",&m_AccelGain); //Acceleration scaling factor for accel based estimation
        AddSocket("AltEstimate.AltimeterSmoothing",&m_altFilter); //Altimeter smoothing. 1= no smoothing
        AddSocket("zAcc", &m_zAcc); //For debugging
        AddSocket("offset", &m_offset); //for debugging
        AddSocket("velocity", &m_velocity); //Estimated velocity
        AddSocket("AccelAltitude",&m_accelAltitude); //Accelerometer altitude estimate

        m_AccelGain = 1/8;

    }

    //A parameter has changed. This happens at bootup if the EEPROM
    //contains valid data and at run-time if the user changes a
    //parameter over the serial link
    virtual void ParamChanged(Socket * param)
    {
    }

    //Initialise the module
    //set up hardware etc
    virtual void Init()
    {
        m_accels[AXISX].Connect("Accel.X");
        m_accels[AXISY].Connect("Accel.Y");
        m_accels[AXISZ].Connect("Accel.Z");
        m_rawAltitude.Connect("Altimeter.Altitude");
        //Set how often Loop() should be executed (in milliseconds)
        SetInterval(10);
        SetPriority(10);
    }


    virtual void Reference()
    {
        m_velocity = 0;
        m_altitude = 0;
        m_accelAltitude = 0;
    }

    //Here is where you do your work. It is called at the rate defined by SetInterval
    virtual void Loop(const unsigned long& interval)
    {
        m_acceleration = sqrt((m_accels[AXISX] * m_accels[AXISX]) + (m_accels[AXISY] * m_accels[AXISY]) + (m_accels[AXISZ] * m_accels[AXISZ]));
        if(m_acceleration > 3) //Saturated the sensor
        {
/*            m_rawAltitude = m_altitude;
            m_velocity = 0;*/
            return;
        }
        float aMag = m_acceleration -1;
        m_offset += (aMag - m_offset) / 1000;
        m_zAcc = aMag - m_offset;
        m_velocity += m_zAcc - (m_velocity / 1000);
        m_smoothedAlt += (m_rawAltitude - m_smoothedAlt) * m_altFilter;
        m_accelAltitude += (m_velocity * m_AccelGain) + ((m_smoothedAlt - m_accelAltitude) / 50);

//        m_altitude += ((m_accelAltitude - m_altitude) /60) + ((m_smoothedAlt - m_altitude) /5);
        m_altitude += ((m_accelAltitude - m_altitude) /10) + ((m_smoothedAlt - m_altitude) /10);
//        m_altitude = m_smoothedAlt + (m_velocity * m_AccelGain);

    }

private:
    PlugF m_accels[MAX_AXES];
    SocketF m_zAcc;
    SocketF m_offset;
    SocketF m_velocity;
    SocketF m_altitude;
    SocketF m_accelAltitude;
    SocketF m_acceleration;
    ParameterF m_altFilter;
    PlugF m_rawAltitude;
    float m_smoothedAlt;
    ParameterF m_AccelGain;

};

AltEstimate g_AltEstimate;