git.defcon.no Git - rctxduino/blob - source/RCTXDuino/RCTXDuino.pde

   1 #include <LiquidCrystal.h>
   2 #include <TimerOne.h>
   3
   4 #define MAX_INPUTS 8
   5
   6 // --------------- ADC related stuffs.... --------------------
   7
   8 struct input_cal_t // Struct type for input calibration values
   9 {
  10         int min[MAX_INPUTS];
  11         int max[MAX_INPUTS];
  12         int center[MAX_INPUTS];
  13 } ;
  14 input_cal_t input_cal;
  15
  16 struct model_t
  17 {
  18         int channels; // How many channels should PPM generate for this model ...
  19         float stick[8]; // The (potentially recalc'ed) value of stick/input channel.
  20         int raw[8];
  21         boolean rev[8];
  22         int dr[8];        // The Dual-rate array uses magic numbers :P
  23         /*      dr[0] = Input channel #1 of 2 for D/R switch #1. 0 means off, 1-4 valid values.
  24                 dr[1] = Input channel #2 of 2 for D/R switch #1. 0 means off, 1-4 valid values.
  25                 dr[2] = Input channel #1 of 2 for D/R switch #2. 0 means off, 1-4 valid values.
  26                 dr[3] = Input channel #2 of 2 for D/R switch #2. 0 means off, 1-4 valid values.
  27                 dr[4] = D/R value for switch # 1 LOW(off). Value -100 to 100 in steps of 5.
  28                 dr[5] = D/R value for switch # 1 HIGH(on). Value -100 to 100 in steps of 5.
  29                 dr[6] = D/R value for switch # 1 LOW(off). Value -100 to 100 in steps of 5.
  30                 dr[7] = D/R value for switch # 1 HIGH(on). Value -100 to 100 in steps of 5.
  31         */
  32 };
  33 volatile model_t model;
  34
  35 // ----------------- Display related stuffs --------------------
  36 LiquidCrystal lcd( 12, 11, 10,     6,  7,  8,  9);
  37 // Parameters are: rs, rw, enable, d4, d5, d6, d7 pin numbers.
  38
  39 // ----------------- PPM related stuffs ------------------------
  40 // The PPM generation is handled by Timer0 interrupts, and needs
  41 // all modifiable variables to be global and volatile...
  42
  43 volatile long sum = 0;                  // Frame-time spent so far
  44 volatile int cchannel = 0;              // Current channnel
  45 volatile bool do_channel = true;        // Is next operation a channel or a separator
  46
  47
  48 // All time values in usecs
  49 // TODO:
  50 // The timing here (and/or in the ISR) needs to be tweaked to provide valid
  51 // RC PPM signals accepted by standard RC RX'es and the Microcopter...
  52
  53 #define framelength  21000              // Max length of frame
  54 #define seplength      300              // Lenght of a channel separator
  55 #define chmax         1550              // Max lenght of channel pulse
  56 #define chmin          620              // Min length of channel
  57 #define chwidht  (chmax - chmin)// Useable time of channel pulse
  58
  59 // ----------------- Menu/IU related stuffs --------------------
  60
  61 // Keys/buttons/switches for UI use, including dual-rate/expo
  62 // are digital inputs connected to a 4051 multiplexer, giving
  63 // 8 inputs on a single input pin.
  64 #define KEY_UP    0
  65 #define KEY_DOWN  1
  66 #define KEY_RIGHT 2
  67 #define KEY_LEFT  3
  68 #define KEY_INC   4
  69 #define KEY_DEC   5
  70 #define KEY_DR1   6
  71 #define KEY_DR2   7
  72
  73 // Voltage sense pin is connected to a 1/3'd voltage divider.
  74 #define BATTERY_CONV (10 * 3 * (5.0f/1024.0f))
  75 #define BATTERY_LOW 92
  76
  77 enum {
  78   VALUES,
  79   BATTERY,
  80   TIMER,
  81   MENU
  82 }
  83 displaystate;
  84
  85 enum {
  86   TOP,
  87   INVERTS,
  88   DUALRATES,
  89   EXPOS, // Some radios have "drawn curves", i.e. loopup tables stored in external EEPROM ...
  90   DEBUG,
  91   SAVE
  92 }
  93 menu_mainstate;
  94 int menu_substate;
  95
  96 boolean keys[8];
  97 boolean prev_keys[8];
  98
  99 int battery_val;
 100
 101 // The display/UI is handled only when more
 102 // than UI_INTERVAL milliecs has passed since last...
 103 #define UI_INTERVAL 250
 104 unsigned long last = 0;
 105
 106 struct clock_timer_t
 107 {
 108         unsigned long start;
 109         unsigned long init;
 110         unsigned long value;
 111         boolean running;
 112 } clock_timer;
 113
 114 // -----------------  DEBUG-STUFF --------------------
 115 unsigned long prev_loop_time;
 116 unsigned long avg_loop_time;
 117 unsigned long t;
 118
 119
 120 // ---------- CODE! -----------------------------------
 121
 122 // ---------- Arduino SETUP code ----------------------
 123 void setup(){
 124   pinMode(13, OUTPUT);   // led
 125
 126   pinMode(2, OUTPUT);    // s0
 127   pinMode(3, OUTPUT);    // s1
 128   pinMode(4, OUTPUT);    // s2
 129   pinMode(5, OUTPUT);    // e
 130
 131   lcd.begin(16,2);
 132   lcd.print("Starting....");
 133
 134   Serial.begin(9600);
 135   Serial.println("Starting....");
 136   delay(500);
 137   read_settings();
 138
 139   pinMode(A5, OUTPUT);  // PPM output pin
 140   do_channel = false;
 141   set_timer( seplength );
 142   Timer1.initialize(framelength);
 143   Timer1.attachInterrupt(ISR_timer);
 144
 145   displaystate = VALUES;
 146
 147   // Arduino believes all pins on Port C are Analog.
 148   // In reality they are tri-purpose; ADC, Digital, Digital Interrupts
 149   // Unfortunately the interrupt mode is unusable in this scenario, but digital I/O works :P
 150   pinMode(A2, INPUT);
 151   digitalWrite(A2, HIGH);
 152   scan_keys();
 153   if ( !keys[KEY_UP])
 154     calibrate();
 155
 156   // Debugging: how long does the main loop take on avg...
 157   t = micros();
 158   avg_loop_time = t;
 159   prev_loop_time = t;
 160
 161   // Setting this here to be sure I do not forget to init' it....
 162   // These initializations should be done by read_settings from eeprom,
 163   // and this "default model values" should probably be moved
 164   // out to a section of read_settings when handling "new model", or
 165   // to a separate model_defaults function...
 166   model.channels = 6;
 167   model.rev[0] = model.rev[1] = model.rev[2] = model.rev[3] =
 168   model.rev[4] = model.rev[5] = model.rev[6] = model.rev[7] = false;
 169   model.dr[0] = model.dr[1] = model.dr[2] = model.dr[3] = 0;
 170   model.dr[4] = model.dr[5] = model.dr[6] = model.dr[7] = 100;
 171
 172   // Initializing the stopwatch timer/clock values...
 173   clock_timer = (clock_timer_t){0, 0, 0, false};
 174 }
 175
 176 // ---------- Arduino main loop -----------------------
 177 void loop () {
 178
 179   // Determine if the UI needs to run...
 180   boolean disp;
 181   if ( millis() - last > UI_INTERVAL ) {
 182     last = millis();
 183     disp = true;
 184   }
 185   else disp = false;
 186
 187   process_inputs();
 188
 189   // Wasting a full I/O pin on battery status monitoring!
 190   battery_val = analogRead(1) * BATTERY_CONV;
 191   if ( battery_val < BATTERY_LOW ) {
 192     digitalWrite(13, 1); // Simulate alarm :P
 193     displaystate = BATTERY;
 194   }
 195
 196   if ( disp )
 197   {
 198     ui_handler();
 199   }
 200   if ( displaystate != MENU )
 201   {
 202     // Debugging: how long does the main loop take on avg,
 203     // when not handling the UI...
 204     t = micros();
 205     avg_loop_time = ( t - prev_loop_time + avg_loop_time ) / 2;
 206     prev_loop_time = t;
 207   }
 208
 209   // Whoa! Slow down partner! Let everything settle down before proceeding.
 210   delay(5);
 211 }
 212
 213 // ----- Simple support functions used by more complex functions ----
 214
 215 void set_ppm_output( bool state )
 216 {
 217   digitalWrite(A5, state); // Hard coded PPM output
 218 }
 219
 220 void set_timer(long time)
 221 {
 222   Timer1.detachInterrupt();
 223   Timer1.attachInterrupt(ISR_timer, time);
 224 }
 225
 226 boolean check_key( int key)
 227 {
 228   return ( !keys[key] && prev_keys[key] );
 229 }
 230
 231 void mplx_select(int pin)
 232 {
 233   digitalWrite(5, 1);
 234   delayMicroseconds(24);
 235
 236   digitalWrite(2, bitRead(pin,0));  // Arduino alias for non-modifying bitshift operation
 237   digitalWrite(3, bitRead(pin,1));  // us used to extract individual bits from the int (0..7)
 238   digitalWrite(4, bitRead(pin,2));  // Select the appropriate input by setting s1,s2,s3 and e
 239   digitalWrite(5, 0);               // on the 4051 multiplexer.
 240
 241   // May need to slow the following read down to be able to
 242   // get fully reliable values from the 4051 multiplex.
 243   delayMicroseconds(24);
 244
 245 }
 246
 247 // ----- "Complex" functions follow ---------------------------------
 248
 249 void calibrate()
 250 {
 251   int i, adc_in;
 252   int num_calibrations = 200;
 253
 254   lcd.clear();
 255   lcd.print("Move controls to");
 256   lcd.setCursor(0,1);
 257   lcd.print("their extremes..");
 258   Serial.print("Calibration. Move all controls to their extremes.");
 259
 260   for (i=0; i<MAX_INPUTS; i++) {
 261     input_cal.min[i] = 1024;
 262     input_cal.max[i] = 0;
 263   }
 264   while ( num_calibrations-- )
 265   {
 266     for (i=0; i<MAX_INPUTS; i++) {
 267       mplx_select(i);
 268       adc_in = analogRead(0);
 269
 270       // Naive min/max calibration
 271       if ( adc_in < input_cal.min[i] ) {
 272         input_cal.min[i] = adc_in;
 273       }
 274       if ( adc_in > input_cal.max[i] ) {
 275         input_cal.max[i] = adc_in;
 276       }
 277       delay(10);
 278     }
 279   }
 280
 281   // TODO: WILL need to do center-point calibration after min-max...
 282
 283   lcd.clear();
 284   lcd.print("Done calibrating");
 285
 286   Serial.print("Done calibrating");
 287   delay(2000);
 288 }
 289
 290 void read_settings(void)
 291 {
 292   // Dummy. Will be modified to read model settings from EEPROM
 293   for (int i=0; i<=7; i++) {
 294     input_cal.min[i] = 0;
 295         input_cal.center[i] = 512;
 296     input_cal.max[i] = 1024;
 297   }
 298 }
 299
 300 void write_settings(void)
 301 {
 302   // Dummy. Not used anywhere. Will be fleshed out to save settings to EEPROM.
 303 }
 304
 305 void scan_keys ( void )
 306 {
 307   boolean key_in;
 308
 309   // To get more inputs, another 4051 analog multiplexer is used,
 310   // but this time it is used for digital inputs. 8 digital inputs
 311   // on one input line, as long as proper debouncing and filtering
 312   // is done in hardware :P
 313   for (int i=0; i<=7; i++) {
 314     // To be able to detect that a key has changed state, preserve the previous..
 315     prev_keys[i] = keys[i];
 316
 317     // Select and read input.
 318     mplx_select(i);
 319     keys[i] = digitalRead(A2);
 320     delay(2);
 321   }
 322 }
 323
 324
 325 void process_inputs(void )
 326 {
 327   int current_input, adc_in, fact;
 328   float min, max;
 329
 330   for (current_input=0; current_input<MAX_INPUTS; current_input++) {
 331
 332     mplx_select(current_input);
 333     adc_in = analogRead(0);
 334
 335         model.raw[current_input] = adc_in;
 336         // New format on stick values
 337         // The calculations happen around the center point, the values
 338         // need to arrive at 0...100 of the range "center-to-edge",
 339         // and must end up as negative on the ... negative side of center.
 340
 341     if ( adc_in < input_cal.center[current_input] )
 342     {
 343                         // The stick is on the negative side, so the range is
 344                         // from the lowest possible value to center, and we must
 345                         // make this a negative percentage value.
 346             max = input_cal.min[current_input];
 347             min = input_cal.center[current_input];
 348                         fact = -100;
 349     }
 350     else
 351     {
 352                         // The stick is at center, or on the positive side.
 353                         // Thus, the range is from center to max, and
 354                         // we need positive percentages.
 355             min = input_cal.center[current_input];
 356             max = input_cal.max[current_input];
 357                         fact = 100;
 358     }
 359         // Calculate the percentage that the current stick position is at
 360         // in the given range, referenced to or from center, depending :P
 361     model.stick[current_input] =  fact * ((float)adc_in - min ) / (max - min);
 362
 363         // If this input is configured to be reversed, simply do a sign-flip :D
 364     if ( model.rev[current_input] ) model.stick[current_input] *= -1;
 365
 366     // Dual-rate calculation :D
 367     // This is very repetitive code. It should be fast, but it may waste code-space.
 368     float dr_val;
 369     // Test to see if dualrate-switch #1 applies to channel...
 370     if ( ( current_input == ( model.dr[0]-1) ) || ( current_input == ( model.dr[1]-1) ) )
 371     {
 372             if ( !keys[KEY_DR1] )
 373                     dr_val = ((float)model.dr[4])/100.0;
 374             else
 375                     dr_val = ((float)model.dr[5])/100.0;
 376
 377             model.stick[current_input] *= dr_val;
 378     }
 379     else
 380     // Test to see if dualrate-switch #1 applies to channel...
 381     if ( ( current_input == ( model.dr[2]-1) ) || ( current_input == ( model.dr[3]-1) ) )
 382     {
 383             if ( !keys[KEY_DR1] )
 384                     dr_val = ((float)model.dr[6])/100.0;
 385             else
 386                     dr_val = ((float)model.dr[7])/100.0;
 387
 388             model.stick[current_input] *= dr_val;
 389     }
 390   }
 391 }
 392
 393
 394 void ISR_timer(void)
 395 {
 396   Timer1.stop(); // Make sure we do not run twice while working :P
 397
 398   if ( !do_channel )
 399   {
 400     set_ppm_output( LOW );
 401     sum += seplength;
 402     do_channel = true;
 403     set_timer(seplength);
 404     return;
 405   }
 406
 407   if ( cchannel >= model.channels )
 408   {
 409     set_ppm_output( HIGH );
 410     long framesep = framelength - sum;
 411
 412     sum = 0;
 413     do_channel = false;
 414     cchannel = 0;
 415     set_timer ( framesep );
 416     return;
 417   }
 418
 419   if ( do_channel )
 420   {
 421     set_ppm_output( HIGH );
 422
 423         // New format on stick values
 424     // model.stick contains percentages, -100% to 100% in float. To make the timer-handling
 425     // here as simple as possible. We want to calc the channel value as a  "ratio-value",
 426     // a float in the range 0..1.0. So, by moving the lower bound to 0, then cutting the
 427     // range in half, and finally dividing by 100, we should get the ratio value.
 428     // Some loss of presicion occurs, perhaps the algo' should be reconsidered :P
 429     long next_timer = (( chwidht * ((model.stick[cchannel]+100)/200) ) + chmin);
 430     // Do sanity-check of next_timer compared to chmax and chmin...
 431     while ( chmax < next_timer ) next_timer--;
 432     while ( next_timer < chmin ) next_timer++;
 433
 434         // Update the sum of elapsed time
 435     sum += next_timer;
 436
 437     // Done with channel separator and value,
 438     // prepare for next channel...
 439     cchannel++;
 440     do_channel = false;
 441     set_timer ( next_timer );
 442     return;
 443   }
 444 }
 445
 446 void serial_debug()
 447 {
 448   int current_input;
 449   for (current_input=0; current_input<MAX_INPUTS; current_input++) {
 450
 451     Serial.print("Input #");
 452     Serial.print(current_input);
 453     Serial.print(" pct: ");
 454     Serial.print(model.stick[current_input]);
 455     Serial.print(" raw value: ");
 456     Serial.print(model.raw[current_input]);
 457     Serial.print(" min: ");
 458     Serial.print(input_cal.min[current_input]);
 459     Serial.print(" max: ");
 460     Serial.print(input_cal.max[current_input]);
 461     Serial.println();
 462   }
 463   Serial.print("Battery level is: ");
 464   Serial.println(battery_val);
 465
 466   Serial.print("Average loop time:");
 467   Serial.println(avg_loop_time);
 468
 469   Serial.println();
 470 }
 471
 472 void dr_inputselect( int no, int in )
 473 {
 474         if ( model.dr[menu_substate] < 0 ) model.dr[menu_substate] = 4;
 475         if ( model.dr[menu_substate] > 4 ) model.dr[menu_substate] = 0;
 476
 477         lcd.setCursor(0 , 0);
 478         lcd.print("D/R switch ");
 479         lcd.print( no + 1 );
 480         lcd.print("    ");
 481         lcd.setCursor(0 , 1);
 482         lcd.print("Input ");
 483         lcd.print(in+1);
 484
 485         lcd.print(":  ");
 486         if ( ! model.dr[menu_substate] ) lcd.print("Off");
 487         else lcd.print(model.dr[menu_substate]);
 488
 489         if ( check_key(KEY_INC) ) {
 490                 model.dr[menu_substate]++;
 491                 return;
 492         }
 493         else if ( check_key(KEY_DEC) ) {
 494                 model.dr[menu_substate]--;
 495                 return;
 496         }
 497         // Wrap around.
 498         return;
 499
 500 }
 501
 502 void dr_value()
 503 {
 504         int pos;
 505         int state;
 506
 507         if ( menu_substate == 4) state = keys[KEY_DR1];
 508         else state = keys[KEY_DR2];
 509
 510         pos = 4 + (menu_substate - 4) * 2;
 511         if (state) pos++;
 512
 513         lcd.setCursor(0 , 0);
 514         lcd.print("D/R switch ");
 515         lcd.print( menu_substate - 3 );
 516         lcd.print("    ");
 517         lcd.setCursor(0 , 1);
 518         lcd.print( state ? "HI" : "LO" );
 519         lcd.print(" Value :");
 520
 521         lcd.print( model.dr[pos] );
 522
 523         if ( !keys[KEY_INC] ) {
 524                 if ( model.dr[pos] < 100) model.dr[pos] += 5;
 525                 return;
 526         }
 527         else if ( !keys[KEY_DEC] ) {
 528                 if ( model.dr[pos] > -100) model.dr[pos] -= 5;
 529                 return;
 530         }
 531
 532         return;
 533 }
 534 void ui_handler()
 535 {
 536   int row;
 537   int col;
 538   scan_keys();
 539
 540   if ( displaystate != MENU )
 541   {
 542         menu_substate = 0;
 543     if ( check_key(KEY_UP) && displaystate == VALUES  ) {
 544       displaystate = BATTERY;
 545       return;
 546     }
 547     else if ( check_key(KEY_UP) && displaystate == BATTERY ) {
 548       displaystate = TIMER;
 549       return;
 550     }
 551     else if ( check_key(KEY_UP) && displaystate == TIMER ) {
 552       displaystate = VALUES;
 553       return;
 554     }
 555
 556     else if ( check_key(KEY_DOWN) ) {
 557       displaystate = MENU;
 558       return;
 559     }
 560   }
 561
 562   digitalWrite(13, digitalRead(13) ^ 1 );
 563
 564   switch ( displaystate )
 565   {
 566     case VALUES:
 567       int current_input;
 568       for (current_input=0; current_input<MAX_INPUTS; current_input++) {
 569         // In channel value display, do a simple calc
 570         // of the LCD row & column location. With 8 channels
 571         // we can fit eight channels as percentage values on
 572         // a simple 16x2 display...
 573         if ( current_input < 4 )
 574         {
 575           col = current_input * 4;
 576           row = 0;
 577         }
 578         else
 579         {
 580           col = (current_input-4) * 4;
 581           row = 1;
 582         }
 583         // Overwriting the needed positions with
 584         // blanks cause less display-flicker than
 585         // actually clearing the display...
 586         lcd.setCursor(col, row);
 587         lcd.print("    ");
 588         lcd.setCursor(col, row);
 589         // Display uses percents, while PPM uses ratio....
 590                 // New format on stick values
 591                 lcd.print( (int)model.stick[current_input] );
 592       }
 593       break;
 594
 595
 596     case BATTERY:
 597       lcd.clear();
 598       lcd.print("Battery level: ");
 599       lcd.setCursor(0 , 1);
 600       lcd.print( (float)battery_val/10);
 601       lcd.print("V");
 602       if ( battery_val < BATTERY_LOW ) lcd.print(" - WARNING");
 603       else lcd.print(" - OK");
 604       break;
 605
 606
 607
 608         case TIMER:
 609                 unsigned long delta;
 610                 int hours;
 611                 int minutes;
 612                 int seconds;
 613
 614                 lcd.clear();
 615                 lcd.print("Timer: ");
 616                 lcd.print( clock_timer.running ? "Running" : "Stopped" );
 617                 lcd.setCursor(5 , 1);
 618                 if ( clock_timer.running )
 619                 {
 620                         clock_timer.value = millis() - (clock_timer.start + clock_timer.init);
 621                 }
 622                 hours = ( clock_timer.value / 1000 ) / 3600;
 623                 clock_timer.value = clock_timer.value % 3600000;
 624                 minutes = ( clock_timer.value / 1000 ) / 60;
 625                 seconds = ( clock_timer.value / 1000 ) % 60;
 626                 if ( hours ) {
 627                         lcd.print(hours);
 628                         lcd.print(":");
 629                 }
 630                 if ( minutes < 10 ) lcd.print("0");
 631                 lcd.print( minutes );
 632                 lcd.print(":");
 633                 if ( seconds < 10 ) lcd.print("0");
 634                 lcd.print( seconds );
 635
 636         if ( check_key(KEY_INC) ) {
 637                         if ( !clock_timer.running && !clock_timer.start )
 638                         {
 639                                 clock_timer.start = millis();
 640                                 clock_timer.value = 0;
 641                                 clock_timer.running = true;
 642                         } else if ( !clock_timer.running && clock_timer.start ) {
 643                                 clock_timer.start = millis() - clock_timer.value;
 644                                 clock_timer.running = true;
 645                         } else if ( clock_timer.running ) {
 646                                 clock_timer.running = false;
 647                         }
 648                         return;
 649         } else if ( check_key(KEY_DEC) ) {
 650                         if ( !clock_timer.running && clock_timer.start ) {
 651                                 clock_timer.value = 0;
 652                                 clock_timer.start = 0;
 653                                 clock_timer.init = 0;
 654                         }
 655                         return;
 656         }
 657                 break;
 658
 659     case MENU:
 660       lcd.clear();
 661       switch ( menu_mainstate )
 662       {
 663         case TOP:
 664           lcd.print("In MENU mode!");
 665           lcd.setCursor(0 , 1);
 666           lcd.print("Esc UP. Scrl DN.");
 667           menu_substate = 0;
 668           if ( check_key(KEY_UP) ) {
 669             displaystate = VALUES;
 670             return;
 671           }
 672           else if ( check_key(KEY_DOWN) ) {
 673             menu_mainstate = INVERTS;
 674             return;
 675           }
 676           break;
 677
 678
 679         case INVERTS:
 680           if ( menu_substate >= model.channels ) menu_substate = 0;
 681           if ( menu_substate < 0) menu_substate = (model.channels - 1);
 682           lcd.print("Channel invert");
 683           lcd.setCursor(0 , 1);
 684           lcd.print("Ch ");
 685           lcd.print(menu_substate+1);
 686           lcd.print( (model.rev[menu_substate] ?  ": Invert" : ": Normal"));
 687
 688           if ( check_key(KEY_UP) ) {
 689             menu_mainstate = TOP;
 690             return;
 691           }
 692           else if ( check_key(KEY_DOWN) ) {
 693             menu_mainstate = DUALRATES;
 694             return;
 695           }
 696
 697           if ( check_key(KEY_RIGHT) ) {
 698             menu_substate++;
 699             return;
 700           }
 701           else if ( check_key(KEY_LEFT) ) {
 702             menu_substate--;
 703             return;
 704           }
 705           else if ( check_key(KEY_INC) || check_key(KEY_DEC) ) {
 706             model.rev[menu_substate] ^= 1;
 707             return;
 708           }
 709           break;
 710
 711         case DUALRATES:
 712           if ( menu_substate > 5 ) menu_substate = 0;
 713           if ( menu_substate < 0) menu_substate = 5;
 714
 715           if ( check_key(KEY_UP) ) {
 716             menu_mainstate = INVERTS;
 717             return;
 718           }
 719           if ( check_key(KEY_DOWN) ) {
 720             menu_mainstate = EXPOS;
 721             return;
 722           }
 723           if ( check_key(KEY_RIGHT) ) {
 724             menu_substate++;
 725             return;
 726           }
 727           else if ( check_key(KEY_LEFT) ) {
 728             menu_substate--;
 729             return;
 730           }
 731           switch (menu_substate)
 732                   {
 733                         case 0:
 734                                 dr_inputselect(0, 0);
 735                                 return;
 736                         case 1:
 737                                 dr_inputselect(0, 1);
 738                                 return;
 739                         case 2:
 740                                 dr_inputselect(1, 0);
 741                                 return;
 742                         case 3:
 743                                 dr_inputselect(1, 1);
 744                                 return;
 745                         case 4:
 746                         case 5:
 747                                 dr_value();
 748                                 return;
 749                         default:
 750                                 menu_substate = 0;
 751                                 break;
 752                   }
 753           break;
 754
 755         case EXPOS:
 756                    //________________
 757           lcd.print("Input expo curve");
 758           lcd.setCursor(0 , 1);
 759           lcd.print("Not implemented");
 760           // Possible, if input values are mapped to +/- 100 rather than 0..1 ..
 761           // plot ( x*(1 - 1.0*cos (x/(20*PI)) )) 0 to 100
 762           // Run in wolfram to see result, adjust the 1.0 factor to inc/red effect.
 763           // Problem: -100 to 100 is terribly bad presicion, esp. considering that
 764           // the values started as 0...1024, and we have 1000usec to "spend" on channels.
 765
 766                   // NEW IDEA provided my ivarf @ hig: use bezier curves og hermite curves!
 767                   // Looks like a promising idea, but the implementation is still a bitt off
 768                   // on the time-horizon :P
 769           if ( check_key(KEY_UP ) ) {
 770             menu_mainstate = DUALRATES;
 771             return;
 772           }
 773           if ( check_key(KEY_DOWN ) ) {
 774             menu_mainstate = DEBUG;
 775             return;
 776           }
 777           break;
 778
 779         case DEBUG:
 780           lcd.setCursor(0 , 0);
 781           lcd.print("Dumping debug to");
 782           lcd.setCursor(0 , 1);
 783           lcd.print("serial port 0");
 784           serial_debug();
 785           if ( check_key(KEY_UP ) ) {
 786             // FIXME: Remember to update the "Scroll up" state!
 787             menu_mainstate = EXPOS;
 788             return;
 789           } else if ( check_key(KEY_DOWN ) ) {
 790             menu_mainstate = SAVE;
 791             return;
 792           }
 793           break;
 794
 795         default:
 796           lcd.print("Not implemented");
 797           lcd.setCursor(0 , 1);
 798           lcd.print("Press DOWN...");
 799           if ( check_key(KEY_DOWN ) ) menu_mainstate = TOP;
 800       }
 801       break;
 802
 803
 804     default:
 805       // Invalid
 806       return;
 807   }
 808
 809   return;
 810 }
 811
 812
 813
 814
 815