#include <Arduino.h>

// FMS PIC protocol for connecting a Walkera DEVO-7 R/C
// transmitter to a PC.
// Implements the 19200 baud FMS PIC protocol
// Richard.Prinz@MIN.at 3.2013

// uncoment this line to see human readable
// values on the serial console
//#define DEBUG

// How many channels have your radio
#define NUM_CHANNELS      7

/*
#define MIN_MS            760
#define MID_MS            1528
#define MAX_MS            2284
#define RANGE_MS          1524
#define CO                5.97647059
*/

#define MIN_MS            700
#define MID_MS            1100
#define MAX_MS            1500
#define RANGE_MS          800
#define CO                3.13725490

// readed Channel values
int channel[NUM_CHANNELS];
unsigned int SYNCpulse = 0;
int PPMin = 4;
int deadCount = 100;

void setup()
{
  Serial.begin(19200);
  pinMode(PPMin, INPUT);
} 

void loop()
{
  // waits ultil synchronize arrives > 4 miliseconds
  // If pulse > 4 miliseconds, continue
  SYNCpulse = pulseIn(PPMin, HIGH);
  if(SYNCpulse > 3000)
  { 
    deadCount = 0;
    
    // Read the pulses of the remainig channels
    for(int i = 0; i < NUM_CHANNELS; i++)
      channel[i] = pulseIn(PPMin, HIGH);

#ifndef DEBUG
    // output FMS protocol
    Serial.write(255);
    for(int i = 0; i < NUM_CHANNELS; i++)
      OutputFMS(channel[i]);
#else
    // Prints all the values read for debugging
    Serial.print("Sync: ");
    Serial.print(FormatValue(SYNCpulse));
    
    Serial.print("    ");
    for(int i = 0; i < NUM_CHANNELS; i++)
    {
      Serial.print(FormatValue(channel[i]));
    }
    Serial.println();
    delay(200);
#endif
  }
  else
  {
    if(deadCount > 10)
    {
#ifndef DEBUG
      Serial.write(255);
#else
      Serial.println("DEAD");
#endif
    }
    else
      deadCount++;
  }
}

#ifndef DEBUG
void OutputFMS(int Value)
{
  int r = (Value - MIN_MS);
  if(r < 0)
    r = 0;
  else
    r = (int)(r / CO);
  if(r > 254)
    r = 254;
  Serial.write(r);
}
#else
char *FormatValue(int Value)
{
  static char strOut[13];
  int r = (Value - MIN_MS);
  if(r < 0)
    r = 0;
  else
    r = (int)(r / CO);
  if(r > 255)
    r = 255;
  snprintf(strOut, sizeof(strOut), "%06u %02x | ", Value, r);
  return strOut;
}
#endif