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

/*

Here is a sample configuration. It takes pitch and  roll and sends them out to
two servos. If you want to disable a channel, set the output channel to ""

#define CAMERA_PITCHSERVO "servo.2"
#define CAMERA_ROLLSERVO "servo.3"
#include "camera.h"



*/


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


    //Only use this to expose sockets and parameters. Do most of your
    //initialisastion in Init()
    Camera()
    {


#ifdef CAMERA_PITCHSERVO
        AddSocket("Camera.Pitch.Gain", &m_gains[PITCH]); //Scaling factor
        AddSocket("Camera.Pitch.Trim", &m_offsets[PITCH]); //Zero trim
        AddSocket("Camera.Pitch.Filter", &m_filters[PITCH]); //Filter. 1 = no filter
#endif

#ifdef CAMERA_ROLLSERVO
        AddSocket("Camera.Roll.Gain", &m_gains[ROLL]); //Scaling factor
        AddSocket("Camera.Roll.Trim", &m_offsets[ROLL]);//Zero trim
        AddSocket("Camera.Roll.Filter", &m_filters[ROLL]);//Filter. 1 = no filter
#endif
    }

    //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()
    {
#ifdef CAMERA_PITCHSERVO
        m_inputs[PITCH].Connect("AHRS.Pitch");
        m_outputs[PITCH].Connect(CAMERA_PITCHSERVO);
#endif
#ifdef CAMERA_ROLLSERVO
        m_inputs[ROLL].Connect("AHRS.Roll");
        m_outputs[ROLL].Connect(CAMERA_ROLLSERVO);
#endif


        //Set how often Loop() should be executed (in milliseconds)
        SetInterval(20);
        SetPriority (10);
    }

    //Here is where you do your work. It is called at the rate defined by SetInterval
    virtual void Loop(const unsigned long& interval)
    {
#ifdef CAMERA_PITCHSERVO
        HandleChannel(PITCH);
#endif
#ifdef CAMERA_ROLLSERVO
        HandleChannel(ROLL);
#endif
    }

private:
    inline void HandleChannel (uint8_t channel)
    {
        float tmp = (m_inputs[channel] * m_gains[channel]) + m_offsets[channel];
        m_outputs[channel] += (tmp - m_outputs[channel]) * m_filters[channel];
    }
    PlugF m_outputs[ROLL];
    PlugF m_inputs[ROLL];
    ParameterF m_offsets[ROLL];
    ParameterF m_gains[ROLL];
    ParameterF m_filters[ROLL];

};

Camera g_Camera;