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


class RotaryWing : public FlightControl
{
    DECLARE_MODULE(RotaryWing,FlightControl);
public:

    RotaryWing()
    {
        //Expose sockets to the outside world
        AddSocket("RotaryWing.Pitch.P",&m_pids[PITCH].p); //Basic gyro based stabilization. Just keeps the craft steady
        AddSocket("RotaryWing.Pitch.I",&m_pids[PITCH].i);
        AddSocket("RotaryWing.Pitch.ILimit",&m_pids[PITCH].iLim);
        AddSocket("RotaryWing.Pitch.D",&m_pids[PITCH].d);
        AddSocket("RotaryWing.Roll.P",&m_pids[ROLL].p);
        AddSocket("RotaryWing.Roll.I",&m_pids[ROLL].i);
        AddSocket("RotaryWing.Roll.ILimit",&m_pids[ROLL].iLim);
        AddSocket("RotaryWing.Roll.D",&m_pids[ROLL].d);
        AddSocket("RotaryWing.Yaw.P",&m_pids[YAW].p);
        AddSocket("RotaryWing.Yaw.I",&m_pids[YAW].i);
        AddSocket("RotaryWing.Yaw.ILimit",&m_pids[YAW].iLim);
        AddSocket("RotaryWing.Yaw.D",&m_pids[YAW].d);

        AddSocket("RotaryWing.Stable.P.P", &m_stablePids[PITCH].p); //self leveling mode PIDs
        AddSocket("RotaryWing.Stable.P.I", &m_stablePids[PITCH].i);
        AddSocket("RotaryWing.Stable.P.D", &m_stablePids[PITCH].d);
        AddSocket("RotaryWing.Stable.R.P", &m_stablePids[ROLL].p);
        AddSocket("RotaryWing.Stable.R.I", &m_stablePids[ROLL].i);
        AddSocket("RotaryWing.Stable.R.D", &m_stablePids[ROLL].d);


        AddSocket("RotaryWing.BankComp",&m_zBoost); //Increase throttle to compensate for the current bank angle
        AddSocket("RotaryWing.CommandComp",&m_cmdBoost); //Increase throttle to compensate for commaded pitch/yaw

        for (int ct=0; ct< MAX_AXES; ct++)
        {
            m_pids[ct].p = 0.8;
            m_pids[ct].i = 0;
            m_pids[ct].d = 0.03;
        }
    }

    virtual void ParamChanged(Socket * param)
    {
    }

    virtual void Init()
    {
        FlightControl::Init();
        //Connect all of the inputs we need
        m_gyros[PITCH].Connect("Gyro.Pitch");
        m_gyros[ROLL].Connect("Gyro.Roll");
        m_gyros[YAW].Connect("Gyro.Yaw");
        m_commands[THROTTLE].Connect("Command.Throttle");
        m_commands[ROLL].Connect("Command.Roll");
        m_commands[PITCH].Connect("Command.Pitch");
        m_commands[YAW].Connect("Command.Yaw");
        m_Ahrs[PITCH].Connect("AHRS.Pitch");
        m_Ahrs[ROLL].Connect("AHRS.Roll");
        m_Ahrs[YAW].Connect("AHRS.Yaw");
        m_zAccel.Connect("Accel.X");
        m_acceleration.Connect("AHRS.Acceleration");

        SetInterval(5);
        SetPriority(40);
    }

    virtual void Reference()
    {
        for (int ct=0; ct< MAX_AXES; ct++)
        {
            m_pids[ct].ResetIntegral();
        }
    }

    virtual void Loop(const unsigned long& interval)
    {
        float bankCmd;
        //Increase throttle if we are banking
        if(m_zAccel < -0.5) //we are at a sane angle and acceleration
        {
            bankCmd = (-1 - m_acceleration / m_zAccel) * m_zBoost; //Compensate for steady state bank
            bankCmd += (fabs(m_axes.filtered[PITCH]) + fabs(m_axes.filtered[ROLL])) * m_cmdBoost; //Compensate for command changes
        }else
        {
            bankCmd = 0;
        }

        float values[MAX_COMMANDS];
        if (m_flightMode < FLIGHTMODE_ACRO)
        {
            for (int ct=0; ct< MAX_AXES; ct++)
            {
                m_pids[ct].ResetIntegral();
            }
        }

        if(false)
        {
            for (int ct=0; ct< YAW; ct++)
            {
                values[ct] = m_commands[ct];
            }
        }else
        {
            for (int ct=0; ct< YAW; ct++)
            {
                values[ct] = m_commands[ct] + m_stablePids[ct].Apply(m_commands[ct],m_Ahrs[ct] * 2);
            }
        }


        for (int ct=0; ct< YAW; ct++)
        {
            values[ct] = values[ct] + m_pids[ct].Apply(values[ct],m_gyros[ct]);
        }

        values[YAW] = m_commands[YAW] + m_pids[YAW].Apply(m_commands[YAW], m_gyros[YAW] * 0.2);
        values[THROTTLE] = m_commands[THROTTLE];

        m_axes.Update(values);
    }

private:

    PID m_pids[MAX_AXES];
    PID m_stablePids[2];
    ParameterF m_zBoost;
    ParameterF m_cmdBoost;
    PlugF m_acceleration;
    PlugF m_zAccel;
    PlugF m_Ahrs[MAX_AXES];
    PlugF m_gyros[MAX_AXES];
    PlugF m_commands[MAX_COMMANDS];
};

RotaryWing g_RotaryWing;