C:/Documents and Settings/gw/My Documents/Other projects/flightos/Modules/pwmreceiver.h

Go to the documentation of this file.

#include "receiver.h"
#include "pins_arduino.h"

/*
  Copyright (c) 2010 Les Newell. All rights reserved/

  Some of this code are based on:
  AeroQuad v2.0 - January 2010
  www.AeroQuad.com

  Copyright (c) 2010 Ted Carancho.  All rights reserved.
  An Open Source Arduino based quadrocopter.

  Interrupt based method inspired by Dror Caspi
  http://www.rcgroups.com/forums/showpost.php?p=12356667&postcount=1639

  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/>.
*/


//a bunch of constants

#define MINONWIDTH 950
#define MAXONWIDTH 2075
#define MINOFFWIDTH 12000
#define MAXOFFWIDTH 24000
#define CENTER 1500
#define THROTTLEZERO 1000
#define MAX_ERRORS 50


#define MAX_PCINT_CHANNELS 2
#define EIGHTBITS 8
#define NOT_A_CHANNEL 127

#if RECEIVER_CHANNELS > 6
#error You cannot have more than 6 channels
#endif

/* If you want to port this code to another AVR you will probably need to change these:*/
static volatile uint8_t* channel2PCMSK[MAX_PCINT_CHANNELS] = {&PCMSK0,&PCMSK2}; //port change mask registers
static volatile uint8_t* channel2Input[MAX_PCINT_CHANNELS] = {&PINB,&PINK}; //ports to monitor
static const byte channel2PCIR[MAX_PCINT_CHANNELS]={1,4}; //PCIR bit masks
static const byte port2Channel[]=
{
    NOT_A_CHANNEL,
    NOT_A_CHANNEL, //port a
    0,             //port b
    NOT_A_CHANNEL, //port c
    NOT_A_CHANNEL, //port d
    NOT_A_CHANNEL, //port e
    NOT_A_CHANNEL, //port f
    NOT_A_CHANNEL, //port g
    NOT_A_CHANNEL, //port h
    NOT_A_CHANNEL, //port i
    NOT_A_CHANNEL, //port j
    1,             //port k
    NOT_A_CHANNEL, //port l
};


// Channel data
typedef struct
{
    char ok;
    unsigned long riseTime;
    unsigned long fallTime;
    unsigned long lastGoodWidth;
} pinTimingData;

//globals used by both PWMReceiver and the ISR
byte pwmReceiverErr;
byte pwmChannel2Data[RECEIVER_CHANNELS];
volatile pinTimingData pwmPinData[MAX_PCINT_CHANNELS][EIGHTBITS];
volatile pinTimingData* pwmReceiverChannelData[RECEIVER_CHANNELS];


//define the module.
class PWMReceiver : public Receiver
{
    DECLARE_MODULE(PWMReceiver, Receiver)

    //Only use this to expose sockets and parameters. Do most of your
    //initialisastion in Init()
    PWMReceiver()
    {
        AddSocket("PWMReceiver.ThrottleOffset", &m_offsets[THROTTLE]);
        AddSocket("PWMReceiver.YawOffset", &m_offsets[YAW]);
        AddSocket("PWMReceiver.PitchOffset", &m_offsets[PITCH]);
        AddSocket("PWMReceiver.RollOffset", &m_offsets[ROLL]);
        AddSocket("PWMReceiver.Aux1Offset", &m_offsets[AUX1]);
        AddSocket("PWMReceiver.Aux2Offset", &m_offsets[AUX2]);
        AddSocket("PWMReceiver.ThrottleGain", &m_gains[THROTTLE]);
        AddSocket("PWMReceiver.YawGain", &m_gains[YAW]);
        AddSocket("PWMReceiver.PitchGain", &m_gains[PITCH]);
        AddSocket("PWMReceiver.RollGain", &m_gains[ROLL]);
        AddSocket("PWMReceiver.Aux1Gain", &m_gains[AUX1]);
        AddSocket("PWMReceiver.Aux2Gain", &m_gains[AUX2]);
        for (int ct=0; ct< RECEIVER_CHANNELS; ct++)
        {
            m_offsets[ct] = CENTER;
            m_gains[ct] = 1;
        }
        m_offsets[THROTTLE] = THROTTLEZERO;
        m_gains[THROTTLE] = 0.5;
    }

    //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()
    {

        byte pcicrMask = 0;
        memset((void *)pwmPinData,0,sizeof(pwmPinData));

        for (int ct = 0; ct < RECEIVER_CHANNELS; ct++)
        {
            pinMode(receiverChannel[ct], INPUT);
            pwmReceiverChannelData[ct] = NULL;
            int pin = receiverChannel[ct];
            int port = digitalPinToPort(pin);
            byte mask = digitalPinToBitMask(pin);
            if (port == NOT_A_PORT)
            {
                Error("Invalid receiver pin number ");
                Errorln(pin);
                continue;
            }
            int channel = port2Channel[port];
            if (channel == NOT_A_CHANNEL)
            {
                Error("Reciever cannot use this pin: ");
                Errorln(pin);
                continue;
            }

            int maskBit = 0;
            for (;maskBit < EIGHTBITS; maskBit++)
            {
                if ((mask >> maskBit) == 1)
                {
                    break;
                }
            }
            if (maskBit >= EIGHTBITS)
            {
                Errorln("Invalid mask");
                continue;
            }
            *channel2PCMSK[channel] |= mask;
            pcicrMask |= channel2PCIR[channel];
            pwmReceiverChannelData[ct] = &pwmPinData[channel][maskBit];
            pwmReceiverChannelData[ct]->lastGoodWidth = CENTER;
        }
        //enable interrupts on the respecive pins
        PCICR |= pcicrMask;
        pwmReceiverErr = MAX_ERRORS;

        SetInterval(20);
        SetPriority(30);
    }

    virtual void Loop(const unsigned long& interval)
    {
        for (byte ct=0; ct < RECEIVER_CHANNELS; ct++)
        {
            volatile pinTimingData* data = pwmReceiverChannelData[ct];
            if (data) //pin has been defined
            {
                int pos = data->lastGoodWidth -  m_offsets[ct];
                m_channels[ct] = (pos * 2) * m_gains[ct] / 1000;
                if (ct >= AUX1)
                {
                    if (pos < -300)
                    {
                        m_switches[ct - AUX1] = 0;
                    }
                    else if (pos > 300)
                    {
                        m_switches[ct - AUX1] = 2;
                    }
                    else
                    {
                        m_switches[ct - AUX1] = 1;
                    }
                }
            }
        }
    }

private:

    Parameter16 m_offsets[RECEIVER_CHANNELS];
    ParameterF m_gains[RECEIVER_CHANNELS];

public: //these are accessed by the ISR so they must be static

};


void PWMReceiverISR(uint8_t pins, uint8_t channel)
{
    static byte intLast[MAX_PCINT_CHANNELS] = {0,0};
    uint8_t bit;
    uint8_t changedPins;
    uint32_t time;
    uint32_t currentTime = micros(); //get the time the interrupt was called
    changedPins = pins ^ intLast[channel];
    intLast[channel] = pins;
    //Serial.println(curr,DEC);
    changedPins &= *channel2PCMSK[channel]; //mask out pins that we aren't interested in
    for (uint8_t i=0; i < 8; i++)
    {
        bit = 0x01 << i;
        if (bit & changedPins)    //this pin has changed
        {
            volatile pinTimingData& data = pwmPinData[channel][i];
            // for each pin changed, record time of change
            if (bit & pins)
            {
                //pin gone high
                time = currentTime - data.fallTime;
                data.riseTime = currentTime;
                data.ok &= (time >= MINOFFWIDTH) && (time <= MAXOFFWIDTH);
            }
            else
            {
                time = currentTime - data.riseTime;
                data.fallTime = currentTime;
                if ((time >= MINONWIDTH) && (time <= MAXONWIDTH))
                {
                    if (data.ok)
                    {
                        data.lastGoodWidth = time;
                    }
                    else
                    {
                        data.ok = true;
                        if (pwmReceiverErr)
                        {
                            pwmReceiverErr --;
                        }
                    }
                }
            }
        }
    }
}

SIGNAL(PCINT0_vect)
{
    PWMReceiverISR(*channel2Input[0] ,0);
}

SIGNAL(PCINT2_vect)
{
    PWMReceiverISR(*channel2Input[1] ,1);
}


PWMReceiver g_PWMReceiver;