/*****************************************************************************/
/*
                              RC SimStick 2013

                             by Jaakko Haavisto


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
any later version. see <http://www.gnu.org/licenses/>
*/
/*****************************************************************************/


/*****************************************************************************/
// Includes:
/*****************************************************************************/
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif

/*****************************************************************************/
// Definions:
/*****************************************************************************/
// led blink speed in ms
#define LED_BLINK_SPEED 2000

// Flashing led on the board:
    #define LEDPIN_PINMODE              pinMode (13, OUTPUT);
    #define LEDPIN_TOGGLE               PINB |= 1<<5; //switch LEDPIN state (digital PIN 13)
    #define LEDPIN_OFF                  PORTB &= ~(1<<5);
    #define LEDPIN_ON                   PORTB |= (1<<5);

// How many input channels? (RX)
    #define RC_CHANNELS 6  // How many channels are in RX (note only 5 and 6 are supported)

//RX PIN assignment inside the port //for PORTD
    #define THROTTLEPIN                 2
    #define PITCHPIN                    3
    #define ELEPIN                      4
    #define AILPIN                      5
    #define RUDPIN                      6
    #define GYROPIN                     7

#if (RC_CHANNELS == 5)
    #define PCINT_PIN_COUNT            5
    #define PCINT_RX_BITS              (1<<2),(1<<3),(1<<4),(1<<5),(1<<6)
#endif
#if (RC_CHANNELS == 6)
    #define PCINT_PIN_COUNT             6
    #define PCINT_RX_BITS              (1<<2),(1<<3),(1<<4),(1<<5),(1<<6),(1<<7)
#endif
    #define PCINT_RX_PORT              PORTD
    #define PCINT_RX_MASK              PCMSK2
    #define PCIR_PORT_BIT              (1<<2)
    #define RX_PC_INTERRUPT            PCINT2_vect
    #define RX_PCINT_PIN_PORT          PIND

#if (RC_CHANNELS == 5)
    volatile uint16_t rcValue[RC_CHANNELS] = {1502, 1502, 1502, 1502, 1502};
    static uint8_t rcChannel[RC_CHANNELS]  = {ELEPIN, AILPIN, RUDPIN, THROTTLEPIN, PITCHPIN, GYROPIN};
#endif
#if (RC_CHANNELS == 6)
    volatile uint16_t rcValue[RC_CHANNELS] = {1502, 1502, 1502, 1502, 1502, 1502};
static uint8_t rcChannel[RC_CHANNELS]  = {ELEPIN, AILPIN, RUDPIN, THROTTLEPIN, PITCHPIN};
#endif

// Min max and default values
    #define PPMPULSEMIN             700
    #define SERVOMIN                1020
    #define SERVOMID                1520
    #define SERVOMAX                2000

// Pin where PPM signal comes out
    #define PPMOUTPUT               10 //(do not change - pin 10 tied to ISR)

// PPM output
    int PPM_array[9];
    int PPMFreq_uS = 22500;          // PPM frame length total in uS
    int Fixed_uS = 300;              // PPM frame padding LOW phase in uS

//Interrupt Timers
// Pulse length current total, used to calculate sync pulse
    int ACC_PPM_length = 0;
    int *PPM_pointer = PPM_array;
    int PPM_len;

/*****************************************************************************/
// Variables:
/*****************************************************************************/
    static unsigned long currentTime = 0;
    static unsigned long nextBlinkTime = 0;

    static uint8_t PCInt_RX_Pins[PCINT_PIN_COUNT] = {PCINT_RX_BITS};

// Table where all channels from RX are stored and ready for use.
    unsigned int rx_data[RC_CHANNELS];

// flag that marks if input from tx has been changed
    bool input_changed = false;

/*****************************************************************************/
// Macros:
/*****************************************************************************/
  #define RX_PIN_CHECK(pin_pos, rc_value_pos)                        \
    if (mask & PCInt_RX_Pins[pin_pos]) {                             \
      if (!(pin & PCInt_RX_Pins[pin_pos])) {                         \
        dTime = cTime-edgeTime[pin_pos];                             \
        if (900<dTime && dTime<2200) {                               \
          rcValue[rc_value_pos] = dTime;                             \
        }                                                            \
      } else edgeTime[pin_pos] = cTime;                              \
    }

/*****************************************************************************/
// Setup function
/*****************************************************************************/
void setup()
    {
    // Set PPM output pin
    pinMode(PPMOUTPUT, OUTPUT);

    // Initialize PPM channel array
    int i=0;
    for(i=0; i<8; i++)
        {
        PPM_array[i] = PPMPULSEMIN;
        }
    PPM_array[i] = -1;   // Mark end

    // Initialise ISR Timer 1 - PPM generation
    TCCR1A = B00110001;  // Compare register B used in mode 3
    TCCR1B = B00010010;  // WGM13 & CS11 set to 1
    TCCR1C = B00000000;  // All set to 0
    TIMSK1 = B00000010;  // Interrupt on compare B
    TIFR1  = B00000010;  // Interrupt on compare B
    OCR1A = PPMFreq_uS;  // PPM frequency
    OCR1B = Fixed_uS;    // PPM off time (lo padding)

    // PCINT activation
    for(uint8_t i = 0; i < PCINT_PIN_COUNT; i++)
        {
        PCINT_RX_PORT |= PCInt_RX_Pins[i];
        PCINT_RX_MASK |= PCInt_RX_Pins[i];
        }
    // PCINT activated only for the port dealing with [D0-D7] PINs on port B
    PCICR = PCIR_PORT_BIT;
    
// if more than 6 channels (not used yet, only for the future)
#if (RC_CHANNELS > 6)
    PCICR  |= (1 << 0) ; // PCINT activated also for PINS [D8-D13] on port B
    // enable interrupt on pin 8
    PCMSK0 = (1 << 0);
    // 8pin 0, 9pin 1, 10pin 2, 11pin 3, 12pin 4 and so on...
#endif
    }

/*****************************************************************************/
// Timer: (used for PPM creation)
/*****************************************************************************/
ISR(TIMER1_COMPA_vect)
    {
    PPM_len = *(PPM_pointer++);
    if(PPM_len > -1)
        {
        // Set pulse length
        OCR1A = PPM_len;
        // Add pulse length to accumulator
        ACC_PPM_length += PPM_len;
        }
    else
        {
        // Reset table position pointer
        PPM_pointer = PPM_array;
        // Calculate final sync pulse length
        OCR1A = PPMFreq_uS - ACC_PPM_length;
        // Reset accumulator
        ACC_PPM_length = 0;
        }
    }


/*****************************************************************************/
// Port change Interrupt
/*****************************************************************************/
// This ISR is common to every receiver channel, it is call everytime a change
// state occurs on a RX input pin.
ISR(RX_PC_INTERRUPT)
    {
    uint8_t mask;
    uint8_t pin;
    uint16_t cTime,dTime;
    static uint16_t edgeTime[8];
    static uint8_t PCintLast;
  
    // RX_PCINT_PIN_PORT indicates the state of each PIN for the arduino port
    // dealing with Ports digital pins
    pin = RX_PCINT_PIN_PORT;
   
    // doing a ^ between the current interruption and the last one indicates
    // wich pin changed
    mask = pin ^ PCintLast;
    // micros() return a uint32_t, but it is not usefull to keep the whole
    // bits => we keep only 16 bits
    cTime = micros();
    // System to know that there are changed data
    input_changed = true;
    // re enable other interrupts at this point, the rest of this interrupt is
    // not so time critical and can be interrupted safely
    sei();
    // we memorize the current state of all PINs [D0-D7]
    PCintLast = pin;
  
    // RX_PIN_CHECK(pin_pos, rc_value_pos), pin_pos = 0-6, rc_value_pos
    #if (PCINT_PIN_COUNT > 0)
      RX_PIN_CHECK(0,0);
    #endif
    #if (PCINT_PIN_COUNT > 1)
      RX_PIN_CHECK(1,1);
    #endif
    #if (PCINT_PIN_COUNT > 2)
      RX_PIN_CHECK(2,2);
    #endif
    #if (PCINT_PIN_COUNT > 3)
      RX_PIN_CHECK(3,3);
    #endif
    #if (PCINT_PIN_COUNT > 4)
      RX_PIN_CHECK(4,4);
    #endif
    #if (PCINT_PIN_COUNT > 5)
      RX_PIN_CHECK(5,5);
    #endif
    }

/*****************************************************************************/
// For other interrupts    
/*****************************************************************************/
#if (RC_CHANNELS > 6)
    /* this ISR is a simplification of the previous one for PROMINI on port D
       it's simplier because we know the interruption deals only with one PIN:
       bit 0 of PORT B, ie Arduino PIN 8
       or bit 4 of PORTB, ie Arduino PIN 12
     => no need to check which PIN has changed */
ISR(PCINT0_vect)
    {
    uint8_t pin;
    uint16_t cTime,dTime;
    static uint16_t edgeTime;
    
    pin = PINB;
    cTime = micros();
    sei();
    //indicates if the bit 0 of the arduino port [B0-B7] is not at a high state
    // (so that we match here only descending PPM pulse)
    if (!(pin & 1<<0)) 
        {
        dTime = cTime-edgeTime;
        // just a verification: the value must be in the range [1000;2000]
        // + some margin
        if (900<dTime && dTime<2200) rcValue[0] = dTime; 
        }
    else
        {
        // if the bit 2 is at a high state (ascending PPM pulse), we memorize
        // the time
        edgeTime = cTime;
        }
    }
#endif

/*****************************************************************************/
// Function for reading channel values and storing those to rx_data table.
/*****************************************************************************/
uint16_t readRawRC()
    {
    uint16_t data;
    uint8_t oldSREG;
    oldSREG = SREG; cli(); // Let's disable interrupts
    //data = rcValue[rcChannel[chan]]; 
    // Let's copy the data Atomically
    rx_data[0] = rcValue[0];
    rx_data[1] = rcValue[1];
    rx_data[2] = rcValue[2];
    rx_data[3] = rcValue[3];
    rx_data[4] = rcValue[4];
#if (RC_CHANNELS >= 6)
    rx_data[5] = rcValue[5];
#endif
    input_changed = false;
    SREG = oldSREG;        // Let's restore interrupt state
    // We return the value correctly copied when the IRQ's where disabled
    return data;
    }

/*****************************************************************************/
// Function to fill PPM table which is used to PPM output
/*****************************************************************************/
void OutPutPPM()
    {
    for(int i = 0; i < RC_CHANNELS; i++)
        {
        if( rx_data[i] > SERVOMAX )
            {
            rx_data[i] = SERVOMAX;
            }
        if( rx_data[i] < SERVOMIN )
            {
            rx_data[i] = SERVOMIN;
            }
        }

    // Load PPM channel array with final channel values
    PPM_array[0] = rx_data[0];
    PPM_array[1] = rx_data[1];
    PPM_array[2] = rx_data[2];
    PPM_array[3] = rx_data[3];
    PPM_array[4] = rx_data[4];
#if (RC_CHANNELS >= 6)
    PPM_array[5] = rx_data[5];
#endif
#if (RC_CHANNELS >= 7)
    PPM_array[6] = rx_data[6];
#endif
#if (RC_CHANNELS >= 8)
    PPM_array[7] = rx_data[7];
#endif
    }

/*****************************************************************************/
// LED blinking function
/*****************************************************************************/
void BlinkLed()
    {
    if( currentTime > nextBlinkTime )
        {
        LEDPIN_TOGGLE;
        nextBlinkTime = currentTime + (unsigned long)LED_BLINK_SPEED * 1000;
        }
    }

/*****************************************************************************/
// Main Loop
/*****************************************************************************/
void loop ()
    {
    if( input_changed )
        {
        readRawRC();
        OutPutPPM();
        }
    BlinkLed();
    
    currentTime = micros();
    }