// 2+1 emulation encoder for DigiSpark Tiny85 and with presetable deadband specifically for the cheap ebay 2-axis stick unit.
// Two propo channels and a momentary button for sequential throttle:
// With the rudder stick at neutral, a short button push for throttle in sequence(off, half, full, half), long push cuts motor from any position (Franks idea)
// Throttle pips: 1 for low, 2 for medium, 3 for high. Change to 4 for mid on the way down if preferred (if throtptr == 3 pip=32)
// Rudder & elevator trim by holding stick towards required direction & pressing button, 15 x 10uS increments either way, saved to flash.
// Please do not use bootloader, startup delay messes things up and can strip servo gears with some modules (eg Corona)
// Connections:
// Rudder & Elevator pots wired between ground and regulated 5v from the Digispark
// A2(pin3)=rudder. A3(pin2)=elev. D2(pin7)=throttle button to ground, D1(pin6) buzzer, pin4=Gnd, D0(pin5)=PPM out, pin8=VCC, pin1=nc.
// Stick calibration - hold button in, switch on, still holding button move stick and trim to extreme corners. Centralise stick & trim, release button.
// This will set medium throttle to half.  Calibration also zeros the stored rudder & elevator trim.
// To set medium throttle elsewhere, at the end of the calibration process, hold the elevator stick slightly off neutral and release the button.
// This offset from neutral will be the new mid throttle setting, and is saved to flash.
// Servo reversing by holding stick over on power up (saved to flash).
// Inactivity warning sound after 10 minutes with no stick movement.
// Phil_G on most forums, philg@talk21.com  http://www.singlechannel.co.uk

static int Futaba = 1;      // Set to 1 for Futaba channel order AETR. Set to 0 for JR/Spektrum order TAER
static int ppm = 0;
static int buzzer = 1;      // D1 is buzzer...
static int led = 1;         // ... and also built-in LED
static int thr_button = 2;  // seq throttle + calibration
int ppmPulse = 300, neutralPulse = 1200, pip = 0, raw2, raw3, calibrated = 1, rtrim = 0, etrim = 0, startup = 1, stickcalHi[] = {0, 0}, stickcalLo[] = {1023, 1023};
int rstickval, estickval, chtemp, channel[] = {0, 0, 0, 0, 0, 0, 8000}; // last is sync
byte reverse[] = {0, 0, 0, 0, 0, 0}, throtptr = 0, buttonState = 0, lastButtonState = 1;
int throt[] = {0, 500, 1000, 500}; // mid points overridden by config during calibration
unsigned int framecounter = 0, frames = 0, inact = 0;

#define deadband 30 // microseconds.  This is for the cheap ebay stick units which track nicely but centre poorly.

#include <EEPROM.h>

void setup()  {
  // make smaller if channel timings are low. Larger if timings too high.
  // OSCCAL = 76; // Digisparks seem to vary in speed, higher = faster. 76 suits my test board
  noInterrupts();
  pinMode(buzzer, OUTPUT);
  pinMode(ppm, OUTPUT);
  pinMode(thr_button, INPUT_PULLUP);
}

void loop() {

  while (startup == 1 && digitalRead(thr_button) == 0) {
    // calibrate sticks
    calibrated = 0;
    raw2 = analogRead(2);
    if (raw2 > stickcalHi[0]) stickcalHi[0] = raw2;
    if (raw2 < stickcalLo[0]) stickcalLo[0] = raw2;
    raw3 = analogRead(3);
    if (raw3 > stickcalHi[1]) stickcalHi[1] = raw3;
    if (raw3 < stickcalLo[1]) stickcalLo[1] = raw3;
  }

  if (startup == 1 && calibrated == 0) {  // if power-up was with button pressed, save calibration values
    // addition for mk3, elev stick position at end of calibration sets throttle mid position
    throt[1] = map(raw3, stickcalLo[1], stickcalHi[1], 0, 1000);
    EEPROMWriteInt(0, stickcalLo[0]); EEPROMWriteInt(2, stickcalHi[0]);
    EEPROMWriteInt(4, stickcalLo[1]); EEPROMWriteInt(6, stickcalHi[1]);
    EEPROMWriteInt(8, throt[1]); EEPROMWriteInt(10, 200); EEPROMWriteInt(12, 200);
  }

  if (startup == 1) {
    stickcalLo[0] = EEPROMReadInt(0); stickcalHi[0] = EEPROMReadInt(2);
    stickcalLo[1] = EEPROMReadInt(4); stickcalHi[1] = EEPROMReadInt(6);
    reverse[0] = EEPROM.read(14) & 1;
    reverse[1] = EEPROM.read(16) & 1;
    rtrim = EEPROMReadInt(10) - 200; etrim = EEPROMReadInt(12) - 200; throt[1] = EEPROMReadInt(8); throt[3] = throt[1];
  }

  // read sticks & set unused channels
  
  channel[0] = map(analogRead(2), stickcalLo[0], stickcalHi[0], -400, 400); channel[0] = constrain(channel[0], -600, 600); // rudder
  if (channel[0] <= -deadband) channel[0]+=deadband;   else if (channel[0] >= deadband) channel[0]-=deadband; else channel[0]=0;
 
  channel[1] = map(analogRead(3), stickcalLo[1], stickcalHi[1], -400, 400); channel[1] = constrain(channel[1], -600, 600); // elevator
  if (channel[1] <= -deadband) channel[1]+=deadband;   else if (channel[1] >= deadband) channel[1]-=deadband; else channel[1]=0;
  
  channel[2] = -500;    // cyclic throttle
  // channel 4 will be reverse of channel 0
  channel[4] = 0;       // channel 5 neutral
  channel[5] = 0;       // channel 6 neutral
  rstickval = channel[0];
  estickval = channel[1];

  // if power-up was with button released and stick held over
  if (startup == 1 && calibrated == 1) {
    if (rstickval > 300 || rstickval < -300) {
      reverse[0] ^= B00000001;
      EEPROM.write(14, reverse[0]);
    }
    if (estickval > 300 || estickval < -300) {
      reverse[1] ^= B00000001;
      EEPROM.write(16, reverse[1]);
    }

  }

  // throttle button
  buttonState = digitalRead(thr_button);
  if (buttonState != lastButtonState) {
    if (buttonState == 0) { // if button has been pressed
      frames = 0;
      framecounter = 0;

      if (rstickval > 250 || rstickval < -250 || estickval > 250 || estickval < -250 ) { // if stick is over
        if (rstickval >  250 && rtrim <  145) {
          rtrim += 10;  // do rudder trim
          pip = 8; if (rtrim==0) pip=16;
        }
        if (rstickval < -250 && rtrim > -145) {
          rtrim -= 10;
          pip = 8; if (rtrim==0) pip=16;
        }

        if (estickval >  250 && etrim <  145) {
          etrim += 10;  // do elev trim
          pip = 8; if (etrim==0) pip=16;
        }
        if (estickval < -250 && etrim > -145) {
          etrim -= 10;
          pip = 8; if (etrim==0) pip=16;
        }
        EEPROMWriteInt(12, etrim + 200); // keep eeprom-stored values positive
        EEPROMWriteInt(10, rtrim + 200); // keep eeprom-stored values positive
      }
      else { //stick neutral, do throttle
        ++throtptr;
        if (throtptr == 0  || throtptr == 4) pip = 8;
        if (throtptr == 1 || throtptr == 3 ) pip = 16;
        if (throtptr == 2) pip = 24;
      }



    }
  }

  if (digitalRead(thr_button) == 0 && framecounter >= 50) {  // long press for throttle cut (Thanks Frank)
    throtptr = 0;
    pip = 8;
    frames = 0;
  }

  lastButtonState = buttonState;
  if (throtptr == 4) throtptr = 0;
  channel[2] += throt[throtptr];

  // end of once-only startup routines
  startup = 0;
  calibrated = 1;

  // format the frame
  channel[0] += rtrim;
  channel[1] += etrim;
  channel[6] = 0; // 6th element is sync
  for (int ch = 0; ch < 6; ch++) {
    channel[ch] += neutralPulse;

    if (reverse[ch] == 1) channel[ch] = 2400 - channel[ch];
    channel[6] += channel[ch];
  }
  channel[3] = 2400 - channel[0]; // reverse rudder on channel 4
  channel[6] = 17000 - channel[6];

  // this puts the channels in Futaba or Spektrum/JR channel order as required
  if (Futaba != 1) {
    chtemp = channel[2]; channel[2] = channel[1]; channel[1] = channel[0]; channel[0] = chtemp;
  }

  //send ppm frame, last channel holds sync value
  for (int ch = 0; ch < 7; ch++) {
    digitalWrite(ppm, HIGH);
    delayMicroseconds(ppmPulse);
    digitalWrite(ppm, LOW);
    delayMicroseconds(channel[ch]);
  }
  ++framecounter; ++frames;

  // inactivity timer. 10 mins is approx 30000 frames.
  if (rstickval < -150 || rstickval > 150) inact = 0;
  if (++inact > 30000 && bitRead(inact, 3) == 1 && bitRead(inact, 5) == 1) digitalWrite(buzzer, HIGH); else digitalWrite(buzzer, LOW);

  if (pip > 0) {
    digitalWrite(buzzer, bitRead(frames, 2));
    pip--;
  }
}

// This function will write a 2 byte integer to the eeprom at the specified address and address + 1
void EEPROMWriteInt(int p_address, int p_value)
{
  byte lowByte = p_value % 256;
  byte highByte = p_value / 256;
  EEPROM.write(p_address, lowByte);
  EEPROM.write(p_address + 1, highByte);
}

//This function will read a 2 byte integer from the eeprom at the specified address and address + 1
unsigned int EEPROMReadInt(int p_address)
{
  byte lowByte = EEPROM.read(p_address);
  byte highByte = EEPROM.read(p_address + 1);
  return lowByte + highByte * 256;
}