#include <makros.h>
#include <RCReceive.h>

/*
 FESC_Small.ino - Version 0.1.0
 Copyright (c) 2013 Wilfried Klaas.  All right reserved.
 
 This program is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 
 This library 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
 Lesser General Public License for more details.
 
 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
/*
  Steuerung eines Flug ESC mittels Car Fernsteuerng für den ATTiny x5. 
 Der Empfänger wird vom Pin 2 gelesen, 
 der Ausgang ist auf PB0 was in Arduino Pin 0 entspricht.
 Die Nullpunkte ist auf 1500us festgelegt.
 Es gibt 2 Modi, die über einen Jumper zwischen 3 und 4 (Pin 2/3 am Controller) angewählt werden können.
 Mode 1: FlugESC an CarRc
 Bei Vorwärtsfahrt (1,5ms-2ms) wird das Motorsignal gedehnt, also in den Bereich 1ms-2ms gebracht, 
 und an den Regler geschickt. 
 Bei Rückwärtsfahrt wird immer ein 1ms breites Signal an den Regler geschickt.
 Ebenso als Failsafe bei Empfängersignalverlust. Zus. blinkt dann auch noch die LED.
 
 Mode 2: Servowegerweiterung
 Das Empfängersignal (1ms-2ms) wird, ähnlich wie das Motorsignal, gedehnt, also in den Bereich 0,5ms-2,5ms gebracht, 
 und an den Servo geschickt. 
 Failsafe bei Empfängersignalverlust ist hier 1,5ms. Zus. blinkt dann auch hier die LED.
 */

// Definitionen
// Empfängeranschluss
const byte PIN_RC = 2;

// Servoausgänge
const byte SERVO0 = 0;
const byte SERVO1 = 1;

// freie Ein/Ausgänge
const byte AUX1 = 3;
const byte AUX2 = 4;

// Ein paar schöne Makros.
#ifndef between
#define between(a,x,y) ((a >=x) && (a <= y))
#endif

#define AUXOn(aux) \
PORTB |= _BV(aux)
// A: digitalWrite(aux, 1)

#define AUXOff(aux) \
PORTB &= ~_BV(aux)
// A: digitalWrite(aux, 0)

#define LEDOn() \
PORTB |= _BV(AUX1)
// A: digitalWrite(LED, 1)

#define LEDOff() \
PORTB &= ~_BV(AUX1)
// A: digitalWrite(LED, 0)

#define ServoOn() \
PORTB |= _BV(SERVO0)
// A: digitalWrite(SERVO0, 1)

#define ServoOff() \
PORTB &= ~_BV(SERVO0)
// A: digitalWrite(SERVO0, 0)

#define Blink() \
if ((millis() % blinkconst) > (blinkconst / 2)) { \
      LEDOn(); \
    } \
    else { \
      LEDOff(); \
    } \

// ----------------------------------------
// Hier geht's los
// ----------------------------------------

boolean servomode = false;
word blinkconst = 1000;
RCReceive rcReceiver;

void setup() {
  // Einstellung der Ports
  // 0 und 1 sind Ausgänge für Servos
  // 2,3 und 4 sind die Eingänge
  DDRB = _BV(AUX1) | _BV(SERVO0) | _BV(SERVO1);
  PORTB |= _BV(AUX2);  

  // Arduinoersatz
  /* A: 
   pinMode(SERVO0, OUTPUT);
   pinMode(SERVO1, OUTPUT);
   pinMode(AUX1, OUTPUT);
   pinMode(AUX2, INPUT_PULLUP);
  */
  delay(500);

  // Testen, ob Servo- oder FlugMode
  AUXOff(AUX1);
  // A: digitalWRite(AUX1, 0)
  servomode = ((PINB & _BV(AUX2)) == 0);
  // A: servomode = (digitalRead(AUX2) == 0);
  blinkconst = servomode ? 500 : 1000;  
  rcReceiver.attach(PIN_RC);
  delay(500);
}

// ----------------------------------------
// Hauptprogramm
// ----------------------------------------

word value = 0;
byte valueCount = 0;

void loop() {
  // Aktuellen RC-Wert lesen
  rcReceiver.poll();
  if (!rcReceiver.hasError() && rcReceiver.hasNP()) {
    LEDOff();
    doWork();
  } else if (rcReceiver.hasError()) {
    // Fehlerbehandlung failsafe oder sowas...
    Blink();
    servomode ? value = 1500 : value = 1000;
    cli();
    ServoOn();
    delayMicroseconds(value);
    ServoOff();
    sei();
  }
}

inline void doWork() {
  value = rcReceiver.getMsValue();
//  value = pulseIn(PIN_RC, HIGH, 25000); // Timeout 25ms
      if (servomode) {
        // Servo Modus
        // alle gültigen Signale werden verarbeitet
        if (between(value, 900, 2100)) {
          value = constrain(value, 1000, 2000);
//          value = ((value - 1000) * 2) + 500;
          // Im Arduino könnte man auch schreiben
          value = map(value, 1000, 2000, 700, 2300);
        } 
        else {
          // Failsafe
          value = 1500;
        }
      }
      else {
        // FlugEsc Modus
        // Nur die Vorwärtsfahrt (> 1500 us) wird berücksichtigt.
        if (between(value, 1500, 2200)) {
          value = constrain(value, 1500, 2000);
          value = ((value - 1500) * 2) + 1000;
          // Im Arduino könnte man auch schreiben
          // A: value = map(value, 1000, 2000, 500, 2500);
        } 
        else {
          // Failsafe
          value = 1000;
        }
    }
    cli();
    ServoOn();
    delayMicroseconds(value);
    ServoOff();
    sei();
}