C:/Documents and Settings/gw/My Documents/Other projects/flightos/Modules/positionVTOL.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 "pid.h"



//define the module.
class PositionVTOL : public Module
{
    DECLARE_MODULE(PositionVTOL,Module)


    //Only use this to expose sockets and parameters. Do most of your
    //initialisastion in Init()
    PositionVTOL()
    {
        /*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("Command.Pitch",&m_commands[PITCH]);
        AddSocket("Command.Roll",&m_commands[ROLL]);
        AddSocket("Command.Yaw",&m_commands[YAW]);
        AddSocket("Command.Throttle",&m_commands[THROTTLE]);

        m_receiver[PITCH].Connect("Receiver.Pitch");
        m_receiver[ROLL].Connect("Receiver.Roll");
        m_receiver[YAW].Connect("Receiver.Yaw");
        m_receiver[THROTTLE].Connect("Receiver.Throttle");

        AddSocket("Position.Yaw.P",&m_yawGain);
        AddSocket("Position.Yaw.MaxRate",&m_yawRate);

        AddSocket("Position.Altitude.P",&m_altPid.p);
        AddSocket("Position.Altitude.I",&m_altPid.i);
        AddSocket("Position.Altitude.D",&m_altPid.d);
        AddSocket("Position.Altitude.ILimit",&m_altPid.iLim);
        AddSocket("Position.Altitude.HoverThrottle",&m_hoverThrottle); //Hover throttle level.
        m_altPid.p = 0.0025;
        m_altPid.i = 0;
        m_altPid.d = 0.0006;
        m_altPid.iLim = 0.5;

        AddSocket("Tst",&tst);
    }

    //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()
    {
        //Set how often Loop() should be executed (in milliseconds)
        SetInterval(20);
        //Set this module's priority. -1 is never execute, 100 is maximum.
        SetPriority (30);
        m_altitude.Connect("INS.Altitude");
        if(!m_altitude.IsConnected())
        {
            m_altitude.Connect("Altimeter.Altitude"); //fall back to raw altimter if no INS altitude is available
        }
        m_receiver[PITCH].Connect("Receiver.Pitch");
        m_receiver[ROLL].Connect("Receiver.Roll");
        m_receiver[YAW].Connect("Receiver.Yaw");
        m_receiver[THROTTLE].Connect("Receiver.Throttle");

        m_accels[AXISX].Connect("Accel.X");
        m_accels[AXISY].Connect("Accel.Y");
        m_accels[AXISZ].Connect("Accel.Z");

        m_flightMode.Connect("FlightMode.Status");
        m_heading.Connect("AHRS.Yaw");
    }

    //Here is where you do your work. It is called at the rate defined by SetInterval
    virtual void Loop(const unsigned long& interval)
    {


        if (m_flightMode < FLIGHTMODE_HHOLD)
        {
            for(int ct=0; ct< MAX_COMMANDS; ct++)
            {
                m_commands[ct] = m_receiver[ct];
            }
        }
        else
        {

            for(int ct=0; ct< MAX_COMMANDS; ct++)
            {
                m_commands[ct] = m_receiver[ct];
            }

            if (m_lastMode != m_flightMode) //just changed mode
            {
                m_altPid.ResetIntegral();
                m_calcHeading = m_heading;
                m_startHeading = m_heading;
                m_startPitch = m_receiver[PITCH];
                m_startRoll = m_receiver[ROLL];
            }

            m_calcHeading += (m_receiver[YAW] * 0.1);
            ClampAngle(m_calcHeading);
            tst = m_calcHeading;

            float s,c;
            c = m_heading - m_startHeading;
            s = sin(c);
            c = cos(c);

            float p = m_receiver[PITCH] - m_startPitch;
            float r = m_receiver[ROLL] - m_startRoll;

            m_commands[PITCH] = ((c * p) - (s * r)) + m_startPitch;
            m_commands[ROLL] =  ((s * p) + (c * r)) + m_startRoll;

            float res = AngleDiff(m_calcHeading, m_heading) * m_yawGain;
            if(fabs(res) > m_yawRate)
            {
                if(res > 0)
                {
                    res = m_yawRate;
                }else
                {
                    res = -m_yawRate;
                }
            }
            m_commands[YAW] = res;

/*            float cmdAlt = m_receiver[THROTTLE] * 20;

            m_commands[THROTTLE] = m_hoverThrottle +
                                  m_altPid.Apply(cmdAlt,m_altitude);
            if(m_commands[THROTTLE] > 0.85)
            {
                m_commands[THROTTLE] = 0.85;
            }*/
        }


        m_lastMode = m_flightMode;
    }

private:

    inline float AngleDiff(const float ang1, const float ang2) //ang1 - ang2
    {
        float d = ang1 - ang2;
        if(d > M_PI)
        {
            d -= M_PI * 2;
        }else if(d < -M_PI)
        {
            d += M_PI * 2;
        }
        return(d);
    }

    inline void ClampAngle(float& a)
    {
        while(fabs(a) > M_PI)
        {
            if(a > 0)
            {
                a -= M_PI * 2;
            }else
            {
                a += M_PI * 2;
            }
        }
    }

    PlugF m_altitude;
    ParameterF m_hoverThrottle;
    ParameterF m_yawGain;
    ParameterF m_yawRate;
    PID m_altPid;
    Plug8 m_flightMode;
    PlugF m_heading;
    float m_calcHeading;
    float m_startHeading;
    float m_startPitch;
    float m_startRoll;

    SocketF tst;

    SocketF m_commands[MAX_COMMANDS];
    PlugF m_receiver[MAX_COMMANDS];

    PlugF m_accels[MAX_AXES];
    byte m_lastMode;
    float m_hhThrottle;

};

PositionVTOL g_PositionVTOL;