[rctxduino] / source / RCTXDuino / RCTXDuino.pde

#include <LiquidCrystal.h>
#include <TimerOne.h>
#include <EEPROM.h>

// Undefine this whenever a "release" or "flight-test" build is made.
// Defining DEBUG sets some crazy values for things like battery warning,
// and includes a whole bunch of debugging-related code ...
#define DEBUG 1

#define MAX_INPUTS 8

// Update this _every_ time a change in datastructures that
// can/will ber written to EEPROM is done. EEPROM data is
// read/written torectly into/from the data structures using
// pointers, so every time a data-set change occurs, the EEPROM
// format changes as well..
#define EEPROM_VERSION 7

// Some data is stored in fixed locations, e.g.:
//  * The EEPROM version number for the stored data (loc 0)
//  * The selected model configuration number (loc 1)
//  * (add any other fixed-loc's here for doc-purpose)
// This means that any pointer-math-operations need a BASE
// adress to start calc'ing from. This is defined as:
#define EE_BASE_ADDR 10 

// Having to repeat tedious base-address-calculations for the
// start of model data should be unnessecary. Plus, updating
// what data is stored before the models will mean that each
// of those calculations must be updated. A better approach is
// to define the calculation in a define!
// NOTE: If new data is added in front of the model data,
// this define must be updated!
#define EE_MDL_BASE_ADDR (EE_BASE_ADDR+(sizeof(input_cal_t)+ 10))

// Just as a safety-precaution, update/change this if a chip with
// a different internal EEPROM size is used. Atmega328p has 1024 bytes.
#define INT_EEPROM_SIZE 1024

#define MAX_MODELS 4 // Nice and random number..


// --------------- ADC related stuffs.... --------------------

struct input_cal_t // Struct type for input calibration values
{
        int min[MAX_INPUTS];
        int max[MAX_INPUTS];
        int center[MAX_INPUTS];
} ;
input_cal_t input_cal;

struct model_t
{
        int channels; // How many channels should PPM generate for this model ...
        float stick[8]; // The (potentially recalc'ed) value of stick/input channel.
        int raw[8];
        boolean rev[8];
        int dr[8];        // The Dual-rate array uses magic numbers :P
        /*      dr[0] = Input channel #1 of 2 for D/R switch #1. 0 means off, 1-4 valid values.
                dr[1] = Input channel #2 of 2 for D/R switch #1. 0 means off, 1-4 valid values.
                dr[2] = Input channel #1 of 2 for D/R switch #2. 0 means off, 1-4 valid values.
                dr[3] = Input channel #2 of 2 for D/R switch #2. 0 means off, 1-4 valid values.
                dr[4] = D/R value for switch # 1 LOW(off). Value -100 to 100 in steps of 5.
                dr[5] = D/R value for switch # 1 HIGH(on). Value -100 to 100 in steps of 5.
                dr[6] = D/R value for switch # 1 LOW(off). Value -100 to 100 in steps of 5.
                dr[7] = D/R value for switch # 1 HIGH(on). Value -100 to 100 in steps of 5. 
        */
};
volatile model_t model;
unsigned char current_model; // Using uchar to spend a single byte of mem..

// ----------------- Display related stuffs --------------------
LiquidCrystal lcd( 12, 11, 10,     6,  7,  8,  9);
// Parameters are: rs, rw, enable, d4, d5, d6, d7 pin numbers.

// ----------------- PPM related stuffs ------------------------
// The PPM generation is handled by Timer0 interrupts, and needs
// all modifiable variables to be global and volatile...

volatile long sum = 0;                  // Frame-time spent so far
volatile int cchannel = 0;              // Current channnel
volatile bool do_channel = true;        // Is next operation a channel or a separator


// All time values in usecs
// TODO:
// The timing here (and/or in the ISR) needs to be tweaked to provide valid
// RC PPM signals accepted by standard RC RX'es and the Microcopter...

#define framelength  21000              // Max length of frame
#define seplength      300              // Lenght of a channel separator
#define chmax         1600              // Max lenght of channel pulse
#define chmin          495              // Min length of channel
#define chwidht  (chmax - chmin)// Useable time of channel pulse

// ----------------- Menu/IU related stuffs --------------------

// Keys/buttons/switches for UI use, including dual-rate/expo
// are digital inputs connected to a 4051 multiplexer, giving
// 8 inputs on a single input pin.
#define KEY_UP    0
#define KEY_DOWN  1
#define KEY_RIGHT 2
#define KEY_LEFT  3
#define KEY_INC   4
#define KEY_DEC   5
#define KEY_DR1   6
#define KEY_DR2   7

// Voltage sense pin is connected to a 1/3'd voltage divider.
#define BATTERY_CONV (10 * 3 * (5.0f/1024.0f))

#ifdef DEBUG
// The following values are for DEBUGGING ONLY!!
#define BATTERY_LOW 92
#define BATTERY_CRITICAL 0
#else
#define BATTERY_LOW 92
#define BATTERY_CRITICAL 92
#endif

enum {
  VALUES,
  BATTERY,
  TIMER,
  CURMODEL,
  MENU
} 
displaystate;

enum {
  TOP,
  INVERTS,
  DUALRATES,
  EXPOS, // Some radios have "drawn curves", i.e. loopup tables stored in external EEPROM ...
  DEBUG_DUMP,
  SAVE
} 
menu_mainstate;
int menu_substate;

boolean keys[8];
boolean prev_keys[8];

int battery_val;

// The display/UI is handled only when more
// than UI_INTERVAL milliecs has passed since last...
#define UI_INTERVAL 250
unsigned long last = 0;

struct clock_timer_t 
{
        unsigned long start;
        unsigned long init;
        unsigned long value;
        boolean running;
} clock_timer;

#ifdef DEBUG
// -----------------  DEBUG-STUFF --------------------
unsigned long prev_loop_time;
unsigned long avg_loop_time;
unsigned long t;
#endif
// -----------------------LCD--------------------------


#define PIN_SCE   7 //Pin 3 on LCD
#define PIN_RESET 6 //Pin 4 on LCD
#define PIN_DC    5 //Pin 5 on LCD
#define PIN_SDIN  4 //Pin 6 on LCD
#define PIN_SCLK  3 //Pin 7 on LCD

//The DC pin tells the LCD if we are sending a command or data
#define LCD_COMMAND 0 
#define LCD_DATA  1

//You may find a different size screen, but this one is 84 by 48 pixels
#define LCD_X     84
#define LCD_Y     48

//This table contains the hex values that represent pixels
//for a font that is 5 pixels wide and 8 pixels high
static const byte ASCII[][5] = {
  {0x00, 0x00, 0x00, 0x00, 0x00} // 20  
  ,{0x00, 0x00, 0x5f, 0x00, 0x00} // 21 !
  ,{0x00, 0x07, 0x00, 0x07, 0x00} // 22 "
  ,{0x14, 0x7f, 0x14, 0x7f, 0x14} // 23 #
  ,{0x24, 0x2a, 0x7f, 0x2a, 0x12} // 24 $
  ,{0x23, 0x13, 0x08, 0x64, 0x62} // 25 %
  ,{0x36, 0x49, 0x55, 0x22, 0x50} // 26 &
  ,{0x00, 0x05, 0x03, 0x00, 0x00} // 27 '
  ,{0x00, 0x1c, 0x22, 0x41, 0x00} // 28 (
  ,{0x00, 0x41, 0x22, 0x1c, 0x00} // 29 )
  ,{0x14, 0x08, 0x3e, 0x08, 0x14} // 2a *
  ,{0x08, 0x08, 0x3e, 0x08, 0x08} // 2b +
  ,{0x00, 0x50, 0x30, 0x00, 0x00} // 2c ,
  ,{0x08, 0x08, 0x08, 0x08, 0x08} // 2d -
  ,{0x00, 0x60, 0x60, 0x00, 0x00} // 2e .
  ,{0x20, 0x10, 0x08, 0x04, 0x02} // 2f /
  ,{0x3e, 0x51, 0x49, 0x45, 0x3e} // 30 0
  ,{0x00, 0x42, 0x7f, 0x40, 0x00} // 31 1
  ,{0x42, 0x61, 0x51, 0x49, 0x46} // 32 2
  ,{0x21, 0x41, 0x45, 0x4b, 0x31} // 33 3
  ,{0x18, 0x14, 0x12, 0x7f, 0x10} // 34 4
  ,{0x27, 0x45, 0x45, 0x45, 0x39} // 35 5
  ,{0x3c, 0x4a, 0x49, 0x49, 0x30} // 36 6
  ,{0x01, 0x71, 0x09, 0x05, 0x03} // 37 7
  ,{0x36, 0x49, 0x49, 0x49, 0x36} // 38 8
  ,{0x06, 0x49, 0x49, 0x29, 0x1e} // 39 9
  ,{0x00, 0x36, 0x36, 0x00, 0x00} // 3a :
  ,{0x00, 0x56, 0x36, 0x00, 0x00} // 3b ;
  ,{0x08, 0x14, 0x22, 0x41, 0x00} // 3c <
  ,{0x14, 0x14, 0x14, 0x14, 0x14} // 3d =
  ,{0x00, 0x41, 0x22, 0x14, 0x08} // 3e >
  ,{0x02, 0x01, 0x51, 0x09, 0x06} // 3f ?
  ,{0x32, 0x49, 0x79, 0x41, 0x3e} // 40 @
  ,{0x7e, 0x11, 0x11, 0x11, 0x7e} // 41 A
  ,{0x7f, 0x49, 0x49, 0x49, 0x36} // 42 B
  ,{0x3e, 0x41, 0x41, 0x41, 0x22} // 43 C
  ,{0x7f, 0x41, 0x41, 0x22, 0x1c} // 44 D
  ,{0x7f, 0x49, 0x49, 0x49, 0x41} // 45 E
  ,{0x7f, 0x09, 0x09, 0x09, 0x01} // 46 F
  ,{0x3e, 0x41, 0x49, 0x49, 0x7a} // 47 G
  ,{0x7f, 0x08, 0x08, 0x08, 0x7f} // 48 H
  ,{0x00, 0x41, 0x7f, 0x41, 0x00} // 49 I
  ,{0x20, 0x40, 0x41, 0x3f, 0x01} // 4a J
  ,{0x7f, 0x08, 0x14, 0x22, 0x41} // 4b K
  ,{0x7f, 0x40, 0x40, 0x40, 0x40} // 4c L
  ,{0x7f, 0x02, 0x0c, 0x02, 0x7f} // 4d M
  ,{0x7f, 0x04, 0x08, 0x10, 0x7f} // 4e N
  ,{0x3e, 0x41, 0x41, 0x41, 0x3e} // 4f O
  ,{0x7f, 0x09, 0x09, 0x09, 0x06} // 50 P
  ,{0x3e, 0x41, 0x51, 0x21, 0x5e} // 51 Q
  ,{0x7f, 0x09, 0x19, 0x29, 0x46} // 52 R
  ,{0x46, 0x49, 0x49, 0x49, 0x31} // 53 S
  ,{0x01, 0x01, 0x7f, 0x01, 0x01} // 54 T
  ,{0x3f, 0x40, 0x40, 0x40, 0x3f} // 55 U
  ,{0x1f, 0x20, 0x40, 0x20, 0x1f} // 56 V
  ,{0x3f, 0x40, 0x38, 0x40, 0x3f} // 57 W
  ,{0x63, 0x14, 0x08, 0x14, 0x63} // 58 X
  ,{0x07, 0x08, 0x70, 0x08, 0x07} // 59 Y
  ,{0x61, 0x51, 0x49, 0x45, 0x43} // 5a Z
  ,{0x00, 0x7f, 0x41, 0x41, 0x00} // 5b [
  ,{0x02, 0x04, 0x08, 0x10, 0x20} // 5c \
  ,{0x00, 0x41, 0x41, 0x7f, 0x00} // 5d ]
  ,{0x04, 0x02, 0x01, 0x02, 0x04} // 5e ^
  ,{0x40, 0x40, 0x40, 0x40, 0x40} // 5f _
  ,{0x00, 0x01, 0x02, 0x04, 0x00} // 60 `
  ,{0x20, 0x54, 0x54, 0x54, 0x78} // 61 a
  ,{0x7f, 0x48, 0x44, 0x44, 0x38} // 62 b
  ,{0x38, 0x44, 0x44, 0x44, 0x20} // 63 c
  ,{0x38, 0x44, 0x44, 0x48, 0x7f} // 64 d
  ,{0x38, 0x54, 0x54, 0x54, 0x18} // 65 e
  ,{0x08, 0x7e, 0x09, 0x01, 0x02} // 66 f
  ,{0x0c, 0x52, 0x52, 0x52, 0x3e} // 67 g
  ,{0x7f, 0x08, 0x04, 0x04, 0x78} // 68 h
  ,{0x00, 0x44, 0x7d, 0x40, 0x00} // 69 i
  ,{0x20, 0x40, 0x44, 0x3d, 0x00} // 6a j 
  ,{0x7f, 0x10, 0x28, 0x44, 0x00} // 6b k
  ,{0x00, 0x41, 0x7f, 0x40, 0x00} // 6c l
  ,{0x7c, 0x04, 0x18, 0x04, 0x78} // 6d m
  ,{0x7c, 0x08, 0x04, 0x04, 0x78} // 6e n
  ,{0x38, 0x44, 0x44, 0x44, 0x38} // 6f o
  ,{0x7c, 0x14, 0x14, 0x14, 0x08} // 70 p
  ,{0x08, 0x14, 0x14, 0x18, 0x7c} // 71 q
  ,{0x7c, 0x08, 0x04, 0x04, 0x08} // 72 r
  ,{0x48, 0x54, 0x54, 0x54, 0x20} // 73 s
  ,{0x04, 0x3f, 0x44, 0x40, 0x20} // 74 t
  ,{0x3c, 0x40, 0x40, 0x20, 0x7c} // 75 u
  ,{0x1c, 0x20, 0x40, 0x20, 0x1c} // 76 v
  ,{0x3c, 0x40, 0x30, 0x40, 0x3c} // 77 w
  ,{0x44, 0x28, 0x10, 0x28, 0x44} // 78 x
  ,{0x0c, 0x50, 0x50, 0x50, 0x3c} // 79 y
  ,{0x44, 0x64, 0x54, 0x4c, 0x44} // 7a z
  ,{0x00, 0x08, 0x36, 0x41, 0x00} // 7b {
  ,{0x00, 0x00, 0x7f, 0x00, 0x00} // 7c |
  ,{0x00, 0x41, 0x36, 0x08, 0x00} // 7d }
  ,{0x10, 0x08, 0x08, 0x10, 0x08} // 7e ~
  ,{0x78, 0x46, 0x41, 0x46, 0x78} // 7f DEL
};








// ---------- CODE! -----------------------------------

// ---------- Arduino SETUP code ----------------------
void setup(){
  pinMode(13, OUTPUT);   // led

  pinMode(2, OUTPUT);    // s0
  pinMode(3, OUTPUT);    // s1
  pinMode(4, OUTPUT);    // s2
  pinMode(5, OUTPUT);    // e

  lcd.begin(16,2);
  lcd.print("Starting....");

  Serial.begin(9600);
  Serial.println("Starting....");
  delay(500);
  
  model_defaults();
  read_settings();

  displaystate = VALUES;
  
  // Arduino believes all pins on Port C are Analog.
  // In reality they are tri-purpose; ADC, Digital, Digital Interrupts
  // Unfortunately the interrupt mode is unusable in this scenario, but digital I/O works :P
  pinMode(A2, INPUT); 
  digitalWrite(A2, HIGH);
  scan_keys();
  if ( !keys[KEY_UP])
    calibrate();
  
#ifdef DEBUG
  // Debugging: how long does the main loop take on avg...  
  t = micros();
  avg_loop_time = t;
  prev_loop_time = t;    
#endif
  
  // Initializing the stopwatch timer/clock values...
  clock_timer = (clock_timer_t){0, 0, 0, false};

  pinMode(A5, OUTPUT);  // PPM output pin  
  do_channel = false;
  set_timer( seplength );
  Timer1.initialize(framelength);
  Timer1.attachInterrupt(ISR_timer);  
  
}

void model_defaults( void )
{
  // This function provides default values for model data
  // that is not a result of stick input, or in other words:
  // provides defautls for all user-configurable model options.
  
  // Remember to update this when a new option/element is added
  // to the model_t struct (preferably before implementing the
  // menu code that sets those options ...)

  // This is used when a user wants a new, blank model, a reset
  // of a configured model, and (most important) when EEPROM
  // data format changes.
  // NOTE: This means that stored model conficuration is reset
  // to defaults when the EEPROM version/format changes.
  model.channels = 8;
  model.rev[0] = model.rev[1] = model.rev[2] = model.rev[3] = 
  model.rev[4] = model.rev[5] = model.rev[6] = model.rev[7] = false;
  model.dr[0] = model.dr[1] = model.dr[2] = model.dr[3] = 0;
  model.dr[4] = model.dr[5] = model.dr[6] = model.dr[7] = 100;

}

// ---------- Arduino main loop -----------------------
void loop ()
{  

  process_inputs();

  // Wasting a full I/O pin on battery status monitoring!
  battery_val = analogRead(1) * BATTERY_CONV;
  if ( battery_val < BATTERY_LOW ) {
    digitalWrite(13, 1); // Simulate alarm :P
  }
  if ( battery_val < BATTERY_CRITICAL ) {
    displaystate = BATTERY;
  }

  if ( millis() - last > UI_INTERVAL ) 
  { 
    last = millis(); 
    ui_handler(); 
  }
  
#ifdef DEBUG
  if ( displaystate != MENU )
  {
    // Debugging: how long does the main loop take on avg,
    // when not handling the UI...
    t = micros();
    avg_loop_time = ( t - prev_loop_time + avg_loop_time ) / 2;
    prev_loop_time = t;    
  }
#endif

  // Whoa! Slow down partner! Let everything settle down before proceeding.
  delay(5);
}

// ----- Simple support functions used by more complex functions ----

void set_ppm_output( bool state )
{
  digitalWrite(A5, state); // Hard coded PPM output
}

void set_timer(long time)
{
  Timer1.detachInterrupt();
  Timer1.attachInterrupt(ISR_timer, time);  
}

boolean check_key( int key)
{
  return ( !keys[key] && prev_keys[key] );
}

void mplx_select(int pin)
{
  digitalWrite(5, 1);
  delayMicroseconds(24);

  digitalWrite(2, bitRead(pin,0));  // Arduino alias for non-modifying bitshift operation
  digitalWrite(3, bitRead(pin,1));  // us used to extract individual bits from the int (0..7)
  digitalWrite(4, bitRead(pin,2));  // Select the appropriate input by setting s1,s2,s3 and e
  digitalWrite(5, 0);               // on the 4051 multiplexer.

  // May need to slow the following read down to be able to
  // get fully reliable values from the 4051 multiplex.
  delayMicroseconds(24);

}

// ----- "Complex" functions follow ---------------------------------

void calibrate()
{
  int i, adc_in;
  int num_calibrations = 200;

  lcd.clear();
  lcd.print("Move controls to");
  lcd.setCursor(0,1);
  lcd.print("their extremes..");
  Serial.print("Calibration. Move all controls to their extremes.");

  for (i=0; i<MAX_INPUTS; i++) {
    input_cal.min[i] = 1024;
        input_cal.center[i] = 512;
    input_cal.max[i] = 0;
  }

  while ( num_calibrations-- )
  {
    for (i=0; i<MAX_INPUTS; i++) {
      mplx_select(i);
      adc_in = analogRead(0);

      // Naive min/max calibration
      if ( adc_in < input_cal.min[i] ) {
        input_cal.min[i] = adc_in;
      }
      if ( adc_in > input_cal.max[i] ) {
        input_cal.max[i] = adc_in;
      }
      delay(10);
    }
  }

  // TODO: WILL need to do center-point calibration after min-max...

  lcd.clear();
  lcd.print("Saving to EEPROM");
  write_calibration();
  lcd.setCursor(0 , 1);
  lcd.print("Done calibrating");
  
  Serial.print("Done calibrating");
  delay(2000);  
}

void write_calibration(void)
{
  int i;
  unsigned char v;
  const byte *p;
  
  // Set p to be a pointer to the start of the input calibration struct.
  p = (const byte*)(const void*)&input_cal;
  
  // Iterate through the bytes of the struct...
  for (i = 0; i < sizeof(input_cal_t); i++)
  {
    // Get a byte of data from the struct...
        v = (unsigned char) *p;
        // write it to EEPROM
        EEPROM.write( EE_BASE_ADDR + i, v);
        // and move the pointer to the next byte in the struct.
        *p++;
  }
}

void read_settings(void)
{
  int i;
  unsigned char v;
  byte *p;

  v = EEPROM.read(0);
  if ( v != EEPROM_VERSION )
  {
        // All models have been reset. Set the current model to 0
        current_model = 0;
        EEPROM.write(1, current_model);

        calibrate();
        model_defaults();
        // The following does not yet work...
        for ( i = 0; i < MAX_MODELS; i++)
                write_model_settings(i);

        
        // After saving calibration data and model defaults,
        // update the saved version-identifier to the current ver.
        EEPROM.write(0, EEPROM_VERSION);
  }

  // Read calibration values from EEPROM.
  // This uses simple pointer-arithmetic and byte-by-byte
  // to put bytes read from EEPROM to the data-struct.
  p = (byte*)(void*)&input_cal;
  for (i = 0; i < sizeof(input_cal_t); i++)
                *p++ = EEPROM.read( EE_BASE_ADDR + i);

  // Get the previously selected model from EEPROM.
  current_model = EEPROM.read(1);
  read_model_settings( current_model );
}

void read_model_settings(unsigned char mod_no)
{
  int model_address;
  int i;
  unsigned char v;
  byte *p;

  // Calculate the EEPROM start adress for the given model (mod_no)
  model_address = EE_MDL_BASE_ADDR + (mod_no * sizeof(model_t));

  // Do not try to write the model to EEPROM if it won't fit.
  if ( INT_EEPROM_SIZE < (model_address + sizeof(model_t)) )
  {
        lcd.clear();
        lcd.print("Aborting READ");
    lcd.setCursor(0 , 1);
    lcd.print("Invalid location");
    delay(2000);  
        return;
  }
  
  lcd.clear();
  lcd.print("Reading model ");
  lcd.print( (int)mod_no );
  
  // Pointer to the start of the model_t data struct,
  // used for byte-by-byte reading of data...
  p = (byte*)(void*)&model;
  for (i = 0; i < sizeof(model_t); i++)
                *p++ = EEPROM.read( model_address++ );

#ifdef DEBUG
  serial_dump_model();
#endif

  lcd.setCursor(0 , 1);
  lcd.print("... Loaded.");
  delay(1000);    
}

void write_model_settings(unsigned char mod_no)
{
  int model_address;
  int i;
  unsigned char v;
  byte *p;

  // Calculate the EEPROM start adress for the given model (mod_no)
  model_address = EE_MDL_BASE_ADDR + (mod_no * sizeof(model_t));
  
  // Do not try to write the model to EEPROM if it won't fit.
  if ( INT_EEPROM_SIZE < (model_address + sizeof(model_t)) )
  {
        lcd.clear();
        lcd.print("Aborting SAVE");
    lcd.setCursor(0 , 1);
    lcd.print("No room for data");
    delay(2000);  
        return;
  }
  
  lcd.clear();
  lcd.print("Saving model ");
  lcd.print( (int)mod_no);
  
  // Pointer to the start of the model_t data struct,
  // used for byte-by-byte reading of data...
  p = (byte*)(void*)&model;
  
  // Write/serialize the model data struct to EEPROM...
  for (i = 0; i < sizeof(model_t); i++)
                EEPROM.write( model_address++, *p++);
                
  lcd.setCursor(0 , 1);
  lcd.print(".. done saving.");
  delay(200);    
}

#ifdef DEBUG
void serial_dump_model ( void )
{
  int i;
  int model_address;
  // Calculate the EEPROM start adress for the given model (mod_no)
  model_address = EE_MDL_BASE_ADDR + (current_model * sizeof(model_t));
  Serial.print("Current model:    ");
  Serial.println( (int)current_model );
  Serial.print("Models base addr: ");
  Serial.println( EE_MDL_BASE_ADDR );
  Serial.print("Model no:         ");
  Serial.println( current_model, 10 );
  Serial.print("Size of struct:   ");
  Serial.println( sizeof( model_t) );
  Serial.print("Model address:    ");
  Serial.println( model_address );
  Serial.print("End of model:     ");
  Serial.println( model_address + sizeof(model_t) );

  Serial.println();
  
  Serial.print("Channel reversions: ");
  for ( i = 0; i<8; i++)
  {
        Serial.print(i);
        Serial.print("=");
        Serial.print(model.rev[i], 10);
        Serial.print(" ");
  }
  Serial.println();
  
  Serial.print("DR1 inp 0: ");
  Serial.println(model.dr[0]);
  Serial.print("DR1 inp 1: ");
  Serial.println(model.dr[1]);
  Serial.print("DR1 LO val: ");
  Serial.println(model.dr[4]);
  Serial.print("DR1 HI val: ");
  Serial.println(model.dr[5]);
  Serial.print("DR2 inp 0: ");
  Serial.println(model.dr[2]);
  Serial.print("DR2 inp 1: ");
  Serial.println(model.dr[3]);
  Serial.print("DR2 LO val: ");
  Serial.println(model.dr[6]);
  Serial.print("DR2 HI val: ");
  Serial.println(model.dr[7]);
  
  for (i=0; i<MAX_INPUTS; i++) {
    Serial.print("Input #");
    Serial.print(i);
    Serial.print(" pct: ");
    Serial.print(model.stick[i]);
    Serial.print(" min: ");
    Serial.print(input_cal.min[i]);
    Serial.print(" max: ");
    Serial.print(input_cal.max[i]);
    Serial.println();
  }  
}
#endif

void scan_keys ( void )
{
  boolean key_in;

  // To get more inputs, another 4051 analog multiplexer is used,
  // but this time it is used for digital inputs. 8 digital inputs
  // on one input line, as long as proper debouncing and filtering
  // is done in hardware :P
  for (int i=0; i<=7; i++) {
    // To be able to detect that a key has changed state, preserve the previous..
    prev_keys[i] = keys[i];

    // Select and read input.
    mplx_select(i);
    keys[i] = digitalRead(A2);
    delay(2);
  }
}


void process_inputs(void )
{
  int current_input, adc_in, fact;
  float min, max;

  for (current_input=0; current_input<MAX_INPUTS; current_input++) {

    mplx_select(current_input);
    adc_in = analogRead(0);

        model.raw[current_input] = adc_in;
        // New format on stick values
        // The calculations happen around the center point, the values
        // need to arrive at 0...100 of the range "center-to-edge",
        // and must end up as negative on the ... negative side of center.
        
    if ( adc_in < input_cal.center[current_input] )
    {
                        // The stick is on the negative side, so the range is
                        // from the lowest possible value to center, and we must
                        // make this a negative percentage value.
            max = input_cal.min[current_input];
            min = input_cal.center[current_input];
                        fact = -100;
    } 
    else 
    {
                        // The stick is at center, or on the positive side.
                        // Thus, the range is from center to max, and
                        // we need positive percentages.
            min = input_cal.center[current_input];
            max = input_cal.max[current_input];
                        fact = 100;
    }
        // Calculate the percentage that the current stick position is at
        // in the given range, referenced to or from center, depending :P
    model.stick[current_input] =  fact * ((float)adc_in - min ) / (max - min);
        
        // If this input is configured to be reversed, simply do a sign-flip :D
    if ( model.rev[current_input] ) model.stick[current_input] *= -1;

    // Dual-rate calculation :D
    // This is very repetitive code. It should be fast, but it may waste code-space.
    float dr_val;
    // Test to see if dualrate-switch #1 applies to channel...
    if ( ( current_input == ( model.dr[0]-1) ) || ( current_input == ( model.dr[1]-1) ) )
    {
            if ( !keys[KEY_DR1] )
                    dr_val = ((float)model.dr[4])/100.0; 
            else
                    dr_val = ((float)model.dr[5])/100.0;

            model.stick[current_input] *= dr_val; 
    }
    else
    // Test to see if dualrate-switch #1 applies to channel...
    if ( ( current_input == ( model.dr[2]-1) ) || ( current_input == ( model.dr[3]-1) ) )
    {
            if ( !keys[KEY_DR1] )
                    dr_val = ((float)model.dr[6])/100.0; 
            else
                    dr_val = ((float)model.dr[7])/100.0;

            model.stick[current_input] *= dr_val; 
    }
  }
}


void ISR_timer(void)
{
  Timer1.stop(); // Make sure we do not run twice while working :P

  if ( !do_channel )
  {
    set_ppm_output( LOW );
    sum += seplength;
    do_channel = true;
    set_timer(seplength);
    return;
  }

  if ( cchannel >= model.channels )
  {
    set_ppm_output( HIGH );
    long framesep = framelength - sum;

    sum = 0;
    do_channel = false;
    cchannel = 0;
    set_timer ( framesep );
    return;
  }  

  if ( do_channel )
  {
    set_ppm_output( HIGH );

        // New format on stick values
    // model.stick contains percentages, -100% to 100% in float. To make the timer-handling
    // here as simple as possible. We want to calc the channel value as a  "ratio-value",
    // a float in the range 0..1.0. So, by moving the lower bound to 0, then cutting the
    // range in half, and finally dividing by 100, we should get the ratio value.
    // Some loss of presicion occurs, perhaps the algo' should be reconsidered :P
    long next_timer = (( chwidht * ((model.stick[cchannel]+100)/200) ) + chmin);
    // Do sanity-check of next_timer compared to chmax and chmin...
    while ( chmax < next_timer ) next_timer--;
    while ( next_timer < chmin ) next_timer++;
        
        // Update the sum of elapsed time
    sum += next_timer;

    // Done with channel separator and value,
    // prepare for next channel...
    cchannel++;
    do_channel = false;
    set_timer ( next_timer );
    return;
  }
}


#ifdef DEBUG
void serial_debug()
{
  int current_input;
  for (current_input=0; current_input<MAX_INPUTS; current_input++) {

    Serial.print("Input #");
    Serial.print(current_input);
    Serial.print(" pct: ");
    Serial.print(model.stick[current_input]);
    Serial.print(" raw value: ");
    Serial.print(model.raw[current_input]);
    Serial.print(" min: ");
    Serial.print(input_cal.min[current_input]);
    Serial.print(" max: ");
    Serial.print(input_cal.max[current_input]);
    Serial.println();
  }
  Serial.print("Battery level is: ");
  Serial.println(battery_val);

  Serial.print("Average loop time:");
  Serial.println(avg_loop_time);

  Serial.print("Free RAM:");
  Serial.print( FreeRam() );
  Serial.println();
}
#endif

void dr_inputselect( int no, int in )
{
        if ( model.dr[menu_substate] < 0 ) model.dr[menu_substate] = 4;
        if ( model.dr[menu_substate] > 4 ) model.dr[menu_substate] = 0;

        lcd.setCursor(0 , 0);
        lcd.print("D/R switch ");
        lcd.print( no + 1 );
        lcd.print("    ");
        lcd.setCursor(0 , 1);
        lcd.print("Input ");
        lcd.print(in+1);
        
        lcd.print(":  ");
        if ( ! model.dr[menu_substate] ) lcd.print("Off");
        else lcd.print(model.dr[menu_substate]);
        
        if ( check_key(KEY_INC) ) {
                model.dr[menu_substate]++;
                return;
        }
        else if ( check_key(KEY_DEC) ) {
                model.dr[menu_substate]--;
                return;
        }
        // Wrap around.
        return; 

}

void dr_value()
{
        int pos;
        int state;

        if ( menu_substate == 4) state = keys[KEY_DR1];
        else state = keys[KEY_DR2];
        
        pos = 4 + (menu_substate - 4) * 2;
        if (state) pos++;
        
        lcd.setCursor(0 , 0);
        lcd.print("D/R switch ");
        lcd.print( menu_substate - 3 );
        lcd.print("    ");
        lcd.setCursor(0 , 1);
        lcd.print( state ? "HI" : "LO" );
        lcd.print(" Value :");
        
        lcd.print( model.dr[pos] );
        
        if ( !keys[KEY_INC] ) {
                if ( model.dr[pos] < 100) model.dr[pos] += 5;
                return;
        }
        else if ( !keys[KEY_DEC] ) {
                if ( model.dr[pos] > -100) model.dr[pos] -= 5;
                return;
        }
        
        return;
}
void ui_handler()
{
  int row; 
  int col;
  scan_keys();

  if ( displaystate != MENU )
  {
        menu_substate = 0;
    if ( check_key(KEY_UP) && displaystate == VALUES  ) { 
      displaystate = BATTERY; 
      return; 
    }
    else if ( check_key(KEY_UP) && displaystate == BATTERY ) { 
      displaystate = TIMER; 
      return; 
    }
    else if ( check_key(KEY_UP) && displaystate == TIMER ) { 
      displaystate = CURMODEL; 
      return; 
    }
    else if ( check_key(KEY_UP) && displaystate == CURMODEL ) { 
      displaystate = VALUES; 
      return; 
    }

    else if ( check_key(KEY_DOWN) ) { 
      displaystate = MENU; 
      return; 
    }
  }

  digitalWrite(13, digitalRead(13) ^ 1 );

  switch ( displaystate )
  {
    case VALUES:
      int current_input;
      for (current_input=0; current_input<MAX_INPUTS; current_input++) {
        // In channel value display, do a simple calc
        // of the LCD row & column location. With 8 channels
        // we can fit eight channels as percentage values on
        // a simple 16x2 display...
        if ( current_input < 4 )
        {
          col = current_input * 4; 
          row = 0;
        } 
        else 
        {
          col = (current_input-4) * 4;      
          row = 1;
        }
        // Overwriting the needed positions with
        // blanks cause less display-flicker than
        // actually clearing the display...
        lcd.setCursor(col, row);
        lcd.print("    ");
        lcd.setCursor(col, row);
        // Display uses percents, while PPM uses ratio....
                // New format on stick values
                lcd.print( (int)model.stick[current_input] );
      }
      break;


    case BATTERY:    
      lcd.clear();
      lcd.print("Battery level: ");
      lcd.setCursor(0 , 1);
      lcd.print( (float)battery_val/10);
      lcd.print("V");
      if ( battery_val < BATTERY_LOW ) lcd.print(" - WARNING");
      else lcd.print(" - OK");
      break;
      
    

        case TIMER:
                unsigned long delta;
                int hours;
                int minutes;
                int seconds;
                
                lcd.clear();
                lcd.print("Timer: ");
                lcd.print( clock_timer.running ? "Running" : "Stopped" );
                lcd.setCursor(5 , 1);
                if ( clock_timer.running )
                {
                        clock_timer.value = millis() - (clock_timer.start + clock_timer.init);
                }
                hours = ( clock_timer.value / 1000 ) / 3600;
                clock_timer.value = clock_timer.value % 3600000;
                minutes = ( clock_timer.value / 1000 ) / 60;
                seconds = ( clock_timer.value / 1000 ) % 60;
                if ( hours ) {
                        lcd.print(hours);
                        lcd.print(":");
                }
                if ( minutes < 10 ) lcd.print("0");
                lcd.print( minutes );
                lcd.print(":");
                if ( seconds < 10 ) lcd.print("0");
                lcd.print( seconds );
                
        if ( check_key(KEY_INC) ) { 
                        if ( !clock_timer.running && !clock_timer.start )
                        {
                                clock_timer.start = millis();
                                clock_timer.value = 0;
                                clock_timer.running = true;
                        } else if ( !clock_timer.running && clock_timer.start ) {
                                clock_timer.start = millis() - clock_timer.value;
                                clock_timer.running = true;
                        } else if ( clock_timer.running ) {
                                clock_timer.running = false;
                        }
                        return; 
        } else if ( check_key(KEY_DEC) ) { 
                        if ( !clock_timer.running && clock_timer.start ) {
                                clock_timer.value = 0;
                                clock_timer.start = 0;
                                clock_timer.init = 0;
                        }
                        return; 
        }
                break;


                
        case CURMODEL:    
      lcd.clear();
      lcd.print("Model #: ");
          lcd.print( (int)current_model );
      lcd.setCursor(0 , 1);
      lcd.print("NAME (not impl)");
      break;


          
    case MENU:
      lcd.clear();
      switch ( menu_mainstate )
      {
        case TOP:
          lcd.print("In MENU mode!");
          lcd.setCursor(0 , 1);
          lcd.print("Esc UP. Scrl DN.");
          menu_substate = 0;
          if ( check_key(KEY_UP) ) { 
            displaystate = VALUES; 
            return; 
          }
          else if ( check_key(KEY_DOWN) ) { 
            menu_mainstate = INVERTS; 
            return; 
          }
          break;
    
    
        case INVERTS:
          if ( menu_substate >= model.channels ) menu_substate = 0;
          if ( menu_substate < 0) menu_substate = (model.channels - 1);
          lcd.print("Channel invert");
          lcd.setCursor(0 , 1);
          lcd.print("Ch ");
          lcd.print(menu_substate+1);
          lcd.print( (model.rev[menu_substate] ?  ": Invert" : ": Normal"));

          if ( check_key(KEY_UP) ) { 
            menu_mainstate = TOP; 
            return; 
          }
          else if ( check_key(KEY_DOWN) ) { 
            menu_mainstate = DUALRATES; 
            return; 
          }

          if ( check_key(KEY_RIGHT) ) { 
            menu_substate++; 
            return; 
          }
          else if ( check_key(KEY_LEFT) ) { 
            menu_substate--; 
            return; 
          }
          else if ( check_key(KEY_INC) || check_key(KEY_DEC) ) { 
            model.rev[menu_substate] ^= 1; 
            return; 
          }
          break;
          
        case DUALRATES:
          if ( menu_substate > 5 ) menu_substate = 0;
          if ( menu_substate < 0) menu_substate = 5;
                
          if ( check_key(KEY_UP) ) { 
            menu_mainstate = INVERTS; 
            return; 
          }          
          if ( check_key(KEY_DOWN) ) {
            menu_mainstate = EXPOS;
            return;
          }
          if ( check_key(KEY_RIGHT) ) { 
            menu_substate++; 
            return; 
          }
          else if ( check_key(KEY_LEFT) ) { 
            menu_substate--; 
            return; 
          }
          switch (menu_substate)
                  {
                        case 0:
                                dr_inputselect(0, 0);
                                return;
                        case 1:
                                dr_inputselect(0, 1);
                                return;
                        case 2:
                                dr_inputselect(1, 0);
                                return;
                        case 3:
                                dr_inputselect(1, 1);
                                return;
                        case 4:
                        case 5:
                                dr_value();
                                return;
                        default:
                                menu_substate = 0;
                                break;
                  }               
          break;
        
        case EXPOS:
                   //________________
          lcd.print("Input expo curve");
          lcd.setCursor(0 , 1);
          lcd.print("Not implemented");
          // Possible, if input values are mapped to +/- 100 rather than 0..1 ..
          // plot ( x*(1 - 1.0*cos (x/(20*PI)) )) 0 to 100
          // Run in wolfram to see result, adjust the 1.0 factor to inc/red effect.
          // Problem: -100 to 100 is terribly bad presicion, esp. considering that
          // the values started as 0...1024, and we have 1000usec to "spend" on channels.
                  
                  // NEW IDEA provided my ivarf @ hig: use bezier curves og hermite curves!
                  // Looks like a promising idea, but the implementation is still a bitt off
                  // on the time-horizon :P
          if ( check_key(KEY_UP ) ) { 
            menu_mainstate = DUALRATES; 
            return; 
          }          
#ifdef DEBUG          
          if ( check_key(KEY_DOWN ) ) {
            menu_mainstate = DEBUG_DUMP;
            return;
          }
#else
          if ( check_key(KEY_DOWN ) ) {
            menu_mainstate = TOP;
            return;
          }

#endif
          break;

#ifdef DEBUG          
        case DEBUG_DUMP:
          lcd.setCursor(0 , 0);
          lcd.print("Dumping debug to");
          lcd.setCursor(0 , 1);
          lcd.print("serial port 0");
          serial_debug();          
          if ( check_key(KEY_UP ) ) { 
            // FIXME: Remember to update the "Scroll up" state!
            menu_mainstate = EXPOS; 
            return; 
          } else if ( check_key(KEY_DOWN ) ) {
            menu_mainstate = SAVE;
            return;
          }
          break;
#endif    
        default:
          lcd.print("Not implemented");
          lcd.setCursor(0 , 1);
          lcd.print("Press DOWN...");
          if ( check_key(KEY_DOWN ) ) menu_mainstate = TOP;
      }
      break;
      
      
    default:
      // Invalid
      return;
  }

  return;
}

// ----------- LCD related functions ---------------

void LCDgotoXY(int x, int y) {
  LCDWrite(0, 0x80 | x);  // Column.
  LCDWrite(0, 0x40 | y);  // Row.  ?
}

//This takes a large array of bits and sends them to the LCD
void LCDBitmap(char my_array[]){
  for (int index = 0 ; index < (LCD_X * LCD_Y / 8) ; index++)
    LCDWrite(LCD_DATA, my_array[index]);
}

//This function takes in a character, looks it up in the font table/array
//And writes it to the screen
//Each character is 8 bits tall and 5 bits wide. We pad one blank column of
//pixels on each side of the character for readability.
void LCDCharacter(char character) {
  LCDWrite(LCD_DATA, 0x00); //Blank vertical line padding

  for (int index = 0 ; index < 5 ; index++)
    LCDWrite(LCD_DATA, ASCII[character - 0x20][index]);
    //0x20 is the ASCII character for Space (' '). The font table starts with this character

  LCDWrite(LCD_DATA, 0x00); //Blank vertical line padding
}

//Given a string of characters, one by one is passed to the LCD
void LCDString(char *characters) {
  while (*characters)
    LCDCharacter(*characters++);
}

//Clears the LCD by writing zeros to the entire screen
void LCDClear(void) {
  for (int index = 0 ; index < (LCD_X * LCD_Y / 8) ; index++)
    LCDWrite(LCD_DATA, 0x00);
    
  LCDgotoXY(0, 0); //After we clear the display, return to the home position
}

//This sends the magical commands to the PCD8544
void LCDInit(void) {

  //Configure control pins
  pinMode(PIN_SCE, OUTPUT);
  pinMode(PIN_RESET, OUTPUT);
  pinMode(PIN_DC, OUTPUT);
  pinMode(PIN_SDIN, OUTPUT);
  pinMode(PIN_SCLK, OUTPUT);

  //Reset the LCD to a known state
  digitalWrite(PIN_RESET, LOW);
  digitalWrite(PIN_RESET, HIGH);

  LCDWrite(LCD_COMMAND, 0x21); //Tell LCD that extended commands follow
  LCDWrite(LCD_COMMAND, 0xB0); //Set LCD Vop (Contrast): Try 0xB1(good @ 3.3V) or 0xBF if your display is too dark
  LCDWrite(LCD_COMMAND, 0x04); //Set Temp coefficent
  LCDWrite(LCD_COMMAND, 0x14); //LCD bias mode 1:48: Try 0x13 or 0x14

  LCDWrite(LCD_COMMAND, 0x20); //We must send 0x20 before modifying the display control mode
  LCDWrite(LCD_COMMAND, 0x0C); //Set display control, normal mode. 0x0D for inverse
}

//There are two memory banks in the LCD, data/RAM and commands. This 
//function sets the DC pin high or low depending, and then sends
//the data byte
void LCDWrite(byte data_or_command, byte data) {
  digitalWrite(PIN_DC, data_or_command); //Tell the LCD that we are writing either to data or a command

  //Send the data
  digitalWrite(PIN_SCE, LOW);
  shiftOut(PIN_SDIN, PIN_SCLK, MSBFIRST, data);
  digitalWrite(PIN_SCE, HIGH);
}



#ifdef DEBUG
/* The following code is taken from the 
   Arduino FAT16 Library by William Greiman 
   The code may or may-not survive in the long run,
   depending on what licensing-terms we decide on.
   The license will be open source, but the FAT16lib
   is GPL v3, and I (fishy) am personally not so sure about that... 
   
   On the other hand... This code is a very "intuitive approach",
   so contacting the author may give us the option of relicencing just this bit...
*/
static int FreeRam(void) {
  extern int  __bss_end;
  extern int* __brkval;
  int free_memory;
  if (reinterpret_cast<int>(__brkval) == 0) {
    // if no heap use from end of bss section
    free_memory = reinterpret_cast<int>(&free_memory)
                  - reinterpret_cast<int>(&__bss_end);
  } else {
    // use from top of stack to heap
    free_memory = reinterpret_cast<int>(&free_memory)
                  - reinterpret_cast<int>(__brkval);
  }
  return free_memory;
}
#endif