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