1 #include <LiquidCrystal.h>
7 // Update this _every_ time a change in datastructures that
8 // can/will ber written to EEPROM is done. EEPROM data is
9 // read/written torectly into/from the data structures using
10 // pointers, so every time a data-set change occurs, the EEPROM
11 // format changes as well..
12 #define EEPROM_VERSION 3
14 // Some data is stored in fixed locations, e.g.:
15 // * The EEPROM version number for the stored data (loc 0)
16 // * The selected model configuration number (loc 1)
17 // * (add any other fixed-loc's here for doc-purpose)
18 // This means that any pointer-math-operations need a BASE
19 // adress to start calc'ing from. This is defined as:
20 #define EE_BASE_ADDR 10
22 // Having to repeat tedious base-address-calculations for the
23 // start of model data should be unnessecary. Plus, updating
24 // what data is stored before the models will mean that each
25 // of those calculations must be updated. A better approach is
26 // to define the calculation in a define!
27 // NOTE: If new data is added in front of the model data,
28 // this define must be updated!
29 #define EE_MDL_BASE_ADDR (EE_BASE_ADDR+(sizeof(input_cal_t)+ 10))
31 // Just as a safety-precaution, update/change this if a chip with
32 // a different internal EEPROM size is used. Atmega328p has 1024 bytes.
33 #define INT_EEPROM_SIZE 1024
35 #define MAX_MODELS 4 // Nice and random number..
37 // --------------- ADC related stuffs.... --------------------
39 struct input_cal_t // Struct type for input calibration values
43 int center[MAX_INPUTS];
45 input_cal_t input_cal;
49 int channels; // How many channels should PPM generate for this model ...
50 float stick[8]; // The (potentially recalc'ed) value of stick/input channel.
53 int dr[8]; // The Dual-rate array uses magic numbers :P
54 /* dr[0] = Input channel #1 of 2 for D/R switch #1. 0 means off, 1-4 valid values.
55 dr[1] = Input channel #2 of 2 for D/R switch #1. 0 means off, 1-4 valid values.
56 dr[2] = Input channel #1 of 2 for D/R switch #2. 0 means off, 1-4 valid values.
57 dr[3] = Input channel #2 of 2 for D/R switch #2. 0 means off, 1-4 valid values.
58 dr[4] = D/R value for switch # 1 LOW(off). Value -100 to 100 in steps of 5.
59 dr[5] = D/R value for switch # 1 HIGH(on). Value -100 to 100 in steps of 5.
60 dr[6] = D/R value for switch # 1 LOW(off). Value -100 to 100 in steps of 5.
61 dr[7] = D/R value for switch # 1 HIGH(on). Value -100 to 100 in steps of 5.
64 volatile model_t model;
65 unsigned char current_model; // Using uchar to spend a single byte of mem..
67 // ----------------- Display related stuffs --------------------
68 LiquidCrystal lcd( 12, 11, 10, 6, 7, 8, 9);
69 // Parameters are: rs, rw, enable, d4, d5, d6, d7 pin numbers.
71 // ----------------- PPM related stuffs ------------------------
72 // The PPM generation is handled by Timer0 interrupts, and needs
73 // all modifiable variables to be global and volatile...
75 volatile long sum = 0; // Frame-time spent so far
76 volatile int cchannel = 0; // Current channnel
77 volatile bool do_channel = true; // Is next operation a channel or a separator
80 // All time values in usecs
82 // The timing here (and/or in the ISR) needs to be tweaked to provide valid
83 // RC PPM signals accepted by standard RC RX'es and the Microcopter...
85 #define framelength 21000 // Max length of frame
86 #define seplength 300 // Lenght of a channel separator
87 #define chmax 1550 // Max lenght of channel pulse
88 #define chmin 620 // Min length of channel
89 #define chwidht (chmax - chmin)// Useable time of channel pulse
91 // ----------------- Menu/IU related stuffs --------------------
93 // Keys/buttons/switches for UI use, including dual-rate/expo
94 // are digital inputs connected to a 4051 multiplexer, giving
95 // 8 inputs on a single input pin.
105 // Voltage sense pin is connected to a 1/3'd voltage divider.
106 #define BATTERY_CONV (10 * 3 * (5.0f/1024.0f))
107 #define BATTERY_LOW 92
122 EXPOS, // Some radios have "drawn curves", i.e. loopup tables stored in external EEPROM ...
130 boolean prev_keys[8];
134 // The display/UI is handled only when more
135 // than UI_INTERVAL milliecs has passed since last...
136 #define UI_INTERVAL 250
137 unsigned long last = 0;
147 // ----------------- DEBUG-STUFF --------------------
148 unsigned long prev_loop_time;
149 unsigned long avg_loop_time;
153 // ---------- CODE! -----------------------------------
155 // ---------- Arduino SETUP code ----------------------
157 pinMode(13, OUTPUT); // led
159 pinMode(2, OUTPUT); // s0
160 pinMode(3, OUTPUT); // s1
161 pinMode(4, OUTPUT); // s2
162 pinMode(5, OUTPUT); // e
165 lcd.print("Starting....");
168 Serial.println("Starting....");
174 pinMode(A5, OUTPUT); // PPM output pin
176 set_timer( seplength );
177 Timer1.initialize(framelength);
178 Timer1.attachInterrupt(ISR_timer);
180 displaystate = VALUES;
182 // Arduino believes all pins on Port C are Analog.
183 // In reality they are tri-purpose; ADC, Digital, Digital Interrupts
184 // Unfortunately the interrupt mode is unusable in this scenario, but digital I/O works :P
186 digitalWrite(A2, HIGH);
191 // Debugging: how long does the main loop take on avg...
197 // Initializing the stopwatch timer/clock values...
198 clock_timer = (clock_timer_t){0, 0, 0, false};
201 void model_defaults( void )
203 // This function provides default values for model data
204 // that is not a result of stick input, or in other words:
205 // provides defautls for all user-configurable model options.
207 // Remember to update this when a new option/element is added
208 // to the model_t struct (preferably before implementing the
209 // menu code that sets those options ...)
211 // This is used when a user wants a new, blank model, a reset
212 // of a configured model, and (most important) when EEPROM
213 // data format changes.
214 // NOTE: This means that stored model conficuration is reset
215 // to defaults when the EEPROM version/format changes.
217 model.rev[0] = model.rev[1] = model.rev[2] = model.rev[3] =
218 model.rev[4] = model.rev[5] = model.rev[6] = model.rev[7] = false;
219 model.dr[0] = model.dr[1] = model.dr[2] = model.dr[3] = 0;
220 model.dr[4] = model.dr[5] = model.dr[6] = model.dr[7] = 100;
224 // ---------- Arduino main loop -----------------------
227 // Determine if the UI needs to run...
229 if ( millis() - last > UI_INTERVAL ) {
237 // Wasting a full I/O pin on battery status monitoring!
238 battery_val = analogRead(1) * BATTERY_CONV;
239 if ( battery_val < BATTERY_LOW ) {
240 digitalWrite(13, 1); // Simulate alarm :P
241 displaystate = BATTERY;
248 if ( displaystate != MENU )
250 // Debugging: how long does the main loop take on avg,
251 // when not handling the UI...
253 avg_loop_time = ( t - prev_loop_time + avg_loop_time ) / 2;
257 // Whoa! Slow down partner! Let everything settle down before proceeding.
261 // ----- Simple support functions used by more complex functions ----
263 void set_ppm_output( bool state )
265 digitalWrite(A5, state); // Hard coded PPM output
268 void set_timer(long time)
270 Timer1.detachInterrupt();
271 Timer1.attachInterrupt(ISR_timer, time);
274 boolean check_key( int key)
276 return ( !keys[key] && prev_keys[key] );
279 void mplx_select(int pin)
282 delayMicroseconds(24);
284 digitalWrite(2, bitRead(pin,0)); // Arduino alias for non-modifying bitshift operation
285 digitalWrite(3, bitRead(pin,1)); // us used to extract individual bits from the int (0..7)
286 digitalWrite(4, bitRead(pin,2)); // Select the appropriate input by setting s1,s2,s3 and e
287 digitalWrite(5, 0); // on the 4051 multiplexer.
289 // May need to slow the following read down to be able to
290 // get fully reliable values from the 4051 multiplex.
291 delayMicroseconds(24);
295 // ----- "Complex" functions follow ---------------------------------
299 int i, r0, r1, r2, adc_in;
300 int num_calibrations = 200;
303 lcd.print("Move controls to");
305 lcd.print("their extremes..");
306 Serial.print("Calibration. Move all controls to their extremes.");
308 for (i=0; i<MAX_INPUTS; i++) {
309 input_cal.min[i] = 1024;
310 input_cal.center[i] = 512;
311 input_cal.max[i] = 0;
314 while ( num_calibrations-- )
316 for (i=0; i<MAX_INPUTS; i++) {
318 adc_in = analogRead(0);
320 // Naive min/max calibration
321 if ( adc_in < input_cal.min[i] ) {
322 input_cal.min[i] = adc_in;
324 if ( adc_in > input_cal.max[i] ) {
325 input_cal.max[i] = adc_in;
331 // TODO: WILL need to do center-point calibration after min-max...
334 lcd.print("Saving to EEPROM");
336 lcd.setCursor(0 , 1);
337 lcd.print("Done calibrating");
339 Serial.print("Done calibrating");
343 void write_calibration(void)
349 // Set p to be a pointer to the start of the input calibration struct.
350 p = (const byte*)(const void*)&input_cal;
352 // Iterate through the bytes of the struct...
353 for (i = 0; i < sizeof(input_cal_t); i++)
355 // Get a byte of data from the struct...
356 v = (unsigned char) *p;
357 // write it to EEPROM
358 EEPROM.write( EE_BASE_ADDR + i, v);
359 // and move the pointer to the next byte in the struct.
364 void read_settings(void)
371 if ( v != EEPROM_VERSION )
373 // All models have been reset. Set the current model to 0
375 EEPROM.write(1, current_model);
379 // The following does not yet work...
380 for ( i = 0; i < MAX_MODELS; i++);
381 write_model_settings(i);
384 // After saving calibration data and model defaults,
385 // update the saved version-identifier to the current ver.
386 EEPROM.write(0, EEPROM_VERSION);
389 // Read calibration values from EEPROM.
390 // This uses simple pointer-arithmetic and byte-by-byte
391 // to put bytes read from EEPROM to the data-struct.
392 p = (byte*)(void*)&input_cal;
393 for (i = 0; i < sizeof(input_cal_t); i++)
394 *p++ = EEPROM.read( EE_BASE_ADDR + i);
396 // Get the previously selected model from EEPROM.
397 current_model = EEPROM.read(1);
398 read_model_settings( current_model );
401 void read_model_settings(unsigned char mod_no)
408 // Calculate the EEPROM start adress for the given model (mod_no)
409 model_address = EE_MDL_BASE_ADDR + (mod_no * sizeof(model_t));
411 Serial.print("Models base addr: ");
412 Serial.println( EE_MDL_BASE_ADDR );
413 Serial.print("Model no: ");
414 Serial.println( mod_no, 10 );
415 Serial.print("Size of struct: ");
416 Serial.println( sizeof( model_t) );
417 Serial.print("Model address: ");
418 Serial.println( model_address );
419 Serial.print("End of model: ");
420 Serial.println( model_address + sizeof(model_t) );
422 // Do not try to write the model to EEPROM if it won't fit.
423 if ( INT_EEPROM_SIZE < (model_address + sizeof(model_t)) )
426 lcd.print("Aborting READ");
427 lcd.setCursor(0 , 1);
428 lcd.print("Invalid location");
434 lcd.print("Reading model ");
435 lcd.print( (int)mod_no );
437 // Pointer to the start of the model_t data struct,
438 // used for byte-by-byte reading of data...
439 p = (byte*)(void*)&model;
440 for (i = 0; i < sizeof(input_cal_t); i++)
441 *p++ = EEPROM.read( model_address++ );
443 lcd.setCursor(0 , 1);
444 lcd.print("... Loaded.");
448 void write_model_settings(unsigned char mod_no)
455 // Calculate the EEPROM start adress for the given model (mod_no)
456 model_address = EE_MDL_BASE_ADDR + (mod_no * sizeof(model_t));
458 // Do not try to write the model to EEPROM if it won't fit.
459 if ( INT_EEPROM_SIZE < (model_address + sizeof(model_t)) )
462 lcd.print("Aborting SAVE");
463 lcd.setCursor(0 , 1);
464 lcd.print("No room for data");
470 lcd.print("Saving model ");
473 // Pointer to the start of the model_t data struct,
474 // used for byte-by-byte reading of data...
475 p = (byte*)(void*)&model;
477 // Write/serialize the model data struct to EEPROM...
478 for (i = 0; i < sizeof(input_cal_t); i++)
479 EEPROM.write( model_address++, *p++);
481 lcd.setCursor(0 , 1);
482 lcd.print(".. done saving.");
488 void scan_keys ( void )
493 // To get more inputs, another 4051 analog multiplexer is used,
494 // but this time it is used for digital inputs. 8 digital inputs
495 // on one input line, as long as proper debouncing and filtering
496 // is done in hardware :P
497 for (i=0; i<=7; i++) {
498 // To be able to detect that a key has changed state, preserve the previous..
499 prev_keys[i] = keys[i];
501 // Select and read input.
503 keys[i] = digitalRead(A2);
509 void process_inputs(void )
511 int current_input, adc_in, fact;
514 for (current_input=0; current_input<MAX_INPUTS; current_input++) {
516 mplx_select(current_input);
517 adc_in = analogRead(0);
519 model.raw[current_input] = adc_in;
520 // New format on stick values
521 if ( adc_in < input_cal.center[current_input] )
523 max = input_cal.min[current_input];
524 min = input_cal.center[current_input];
529 min = input_cal.center[current_input];
530 max = input_cal.max[current_input];
533 model.stick[current_input] = fact * ((float)adc_in - min ) / (max - min);
534 if ( model.rev[current_input] ) model.stick[current_input] *= -1;
536 // Old format on stick values...
538 model.stick[current_input] = ((float)adc_in - (float)input_cal.min[current_input]) / (float)(input_cal.max[current_input]-input_cal.min[current_input]);
539 if ( model.rev[current_input] ) model.stick[current_input] = 1.0f - model.stick[current_input];
542 // Dual-rate calculation :D
543 // This is very repetitive code. It should be fast, but it may waste code-space.
545 // Test to see if dualrate-switch #1 applies to channel...
546 if ( ( current_input == ( model.dr[0]-1) ) || ( current_input == ( model.dr[1]-1) ) )
548 if ( !keys[KEY_DR1] )
549 dr_val = ((float)model.dr[4])/100.0;
551 dr_val = ((float)model.dr[5])/100.0;
553 model.stick[current_input] *= dr_val;
556 // Test to see if dualrate-switch #1 applies to channel...
557 if ( ( current_input == ( model.dr[2]-1) ) || ( current_input == ( model.dr[3]-1) ) )
559 if ( !keys[KEY_DR1] )
560 dr_val = ((float)model.dr[6])/100.0;
562 dr_val = ((float)model.dr[7])/100.0;
564 model.stick[current_input] *= dr_val;
572 Timer1.stop(); // Make sure we do not run twice while working :P
576 set_ppm_output( LOW );
579 set_timer(seplength);
583 if ( cchannel >= model.channels )
585 set_ppm_output( HIGH );
586 long framesep = framelength - sum;
591 set_timer ( framesep );
597 set_ppm_output( HIGH );
599 // New format on stick values
600 // model.stick contains percentages, -100% to 100% in float. To make the timer-handling
601 // here as simple as possible. We want to calc the channel value as a "ratio-value",
602 // a float in the range 0..1.0. So, by moving the lower bound to 0, then cutting the
603 // range in half, and finally dividing by 100, we should get the ratio value.
604 // Some loss of presicion occurs, perhaps the algo' should be reconsidered :P
605 long next_timer = (( chwidht * ((model.stick[cchannel]+100)/200) ) + chmin);
606 // Do sanity-check of next_timer compared to chmax ...
607 while ( chmax < next_timer ) next_timer--;
608 while ( next_timer < chmin ) next_timer++;
611 // Done with channel separator and value,
612 // prepare for next channel...
615 set_timer ( next_timer );
623 for (current_input=0; current_input<=7; current_input++) {
625 Serial.print("Input #");
626 Serial.print(current_input);
627 Serial.print(" pct: ");
628 Serial.print(model.stick[current_input]);
629 Serial.print(" raw value: ");
630 Serial.print(model.raw[current_input]);
631 Serial.print(" min: ");
632 Serial.print(input_cal.min[current_input]);
633 Serial.print(" max: ");
634 Serial.print(input_cal.max[current_input]);
637 Serial.print("Battery level is: ");
638 Serial.println(battery_val);
640 Serial.print("Average loop time:");
641 Serial.println(avg_loop_time);
645 void dr_inputselect( int no, int in )
647 if ( model.dr[menu_substate] < 0 ) model.dr[menu_substate] = 4;
648 if ( model.dr[menu_substate] > 4 ) model.dr[menu_substate] = 0;
650 lcd.setCursor(0 , 0);
651 lcd.print("D/R switch ");
654 lcd.setCursor(0 , 1);
659 if ( ! model.dr[menu_substate] ) lcd.print("Off");
660 else lcd.print(model.dr[menu_substate]);
662 if ( check_key(KEY_INC) ) {
663 model.dr[menu_substate]++;
666 else if ( check_key(KEY_DEC) ) {
667 model.dr[menu_substate]--;
680 if ( menu_substate == 4) state = keys[KEY_DR1];
681 else state = keys[KEY_DR2];
683 pos = 4 + (menu_substate - 4) * 2;
686 lcd.setCursor(0 , 0);
687 lcd.print("D/R switch ");
688 lcd.print( menu_substate - 3 );
690 lcd.setCursor(0 , 1);
691 lcd.print( state ? "HI" : "LO" );
692 lcd.print(" Value :");
694 lcd.print( model.dr[pos] );
696 if ( !keys[KEY_INC] ) {
697 if ( model.dr[pos] < 100) model.dr[pos] += 5;
700 else if ( !keys[KEY_DEC] ) {
701 if ( model.dr[pos] > -100) model.dr[pos] -= 5;
713 if ( displaystate != MENU )
716 if ( check_key(KEY_UP) && displaystate == VALUES ) {
717 displaystate = BATTERY;
720 else if ( check_key(KEY_UP) && displaystate == BATTERY ) {
721 displaystate = TIMER;
724 else if ( check_key(KEY_UP) && displaystate == TIMER ) {
725 displaystate = CURMODEL;
728 else if ( check_key(KEY_UP) && displaystate == CURMODEL ) {
729 displaystate = VALUES;
733 else if ( check_key(KEY_DOWN) ) {
739 digitalWrite(13, digitalRead(13) ^ 1 );
741 switch ( displaystate )
745 for (current_input=0; current_input<=7; current_input++) {
746 // In channel value display, do a simple calc
747 // of the LCD row & column location. With 8 channels
748 // we can fit eight channels as percentage values on
749 // a simple 16x2 display...
750 if ( current_input < 4 )
752 col = current_input * 4;
757 col = (current_input-4) * 4;
760 // Overwriting the needed positions with
761 // blanks cause less display-flicker than
762 // actually clearing the display...
763 lcd.setCursor(col, row);
765 lcd.setCursor(col, row);
766 // Display uses percents, while PPM uses ratio....
767 // New format on stick values
768 lcd.print( (int)model.stick[current_input] );
775 lcd.print("Battery level: ");
776 lcd.setCursor(0 , 1);
777 lcd.print( (float)battery_val/10);
779 if ( battery_val < BATTERY_LOW ) lcd.print(" - WARNING");
780 else lcd.print(" - OK");
792 lcd.print("Timer: ");
793 lcd.print( clock_timer.running ? "Running" : "Stopped" );
794 lcd.setCursor(5 , 1);
795 if ( clock_timer.running )
797 clock_timer.value = millis() - (clock_timer.start + clock_timer.init);
799 hours = ( clock_timer.value / 1000 ) / 3600;
800 clock_timer.value = clock_timer.value % 3600000;
801 minutes = ( clock_timer.value / 1000 ) / 60;
802 seconds = ( clock_timer.value / 1000 ) % 60;
807 if ( minutes < 10 ) lcd.print("0");
808 lcd.print( minutes );
810 if ( seconds < 10 ) lcd.print("0");
811 lcd.print( seconds );
813 if ( check_key(KEY_INC) ) {
814 if ( !clock_timer.running && !clock_timer.start )
816 clock_timer.start = millis();
817 clock_timer.value = 0;
818 clock_timer.running = true;
819 } else if ( !clock_timer.running && clock_timer.start ) {
820 clock_timer.start = millis() - clock_timer.value;
821 clock_timer.running = true;
822 } else if ( clock_timer.running ) {
823 clock_timer.running = false;
826 } else if ( check_key(KEY_DEC) ) {
827 if ( !clock_timer.running && clock_timer.start ) {
828 clock_timer.value = 0;
829 clock_timer.start = 0;
830 clock_timer.init = 0;
840 lcd.print("Model #: ");
841 lcd.print( (int)current_model );
842 lcd.setCursor(0 , 1);
843 lcd.print("NAME (not impl)");
850 switch ( menu_mainstate )
853 lcd.print("In MENU mode!");
854 lcd.setCursor(0 , 1);
855 lcd.print("Esc UP. Scrl DN.");
857 if ( check_key(KEY_UP) ) {
858 displaystate = VALUES;
861 else if ( check_key(KEY_DOWN) ) {
862 menu_mainstate = INVERTS;
869 if ( menu_substate >= model.channels ) menu_substate = 0;
870 if ( menu_substate < 0) menu_substate = (model.channels - 1);
871 lcd.print("Channel invert");
872 lcd.setCursor(0 , 1);
874 lcd.print(menu_substate+1);
875 lcd.print( (model.rev[menu_substate] ? ": Invert" : ": Normal"));
877 if ( check_key(KEY_UP) ) {
878 menu_mainstate = TOP;
881 else if ( check_key(KEY_DOWN) ) {
882 menu_mainstate = DUALRATES;
886 if ( check_key(KEY_RIGHT) ) {
890 else if ( check_key(KEY_LEFT) ) {
894 else if ( check_key(KEY_INC) || check_key(KEY_DEC) ) {
895 model.rev[menu_substate] ^= 1;
901 if ( menu_substate > 5 ) menu_substate = 0;
902 if ( menu_substate < 0) menu_substate = 5;
904 if ( check_key(KEY_UP) ) {
905 menu_mainstate = INVERTS;
908 if ( check_key(KEY_DOWN) ) {
909 menu_mainstate = EXPOS;
912 if ( check_key(KEY_RIGHT) ) {
916 else if ( check_key(KEY_LEFT) ) {
920 switch (menu_substate)
923 dr_inputselect(0, 0);
926 dr_inputselect(0, 1);
929 dr_inputselect(1, 0);
932 dr_inputselect(1, 1);
946 lcd.print("Input expo curve");
947 lcd.setCursor(0 , 1);
948 lcd.print("Not implemented");
949 // Possible, if input values are mapped to +/- 100 rather than 0..1 ..
950 // plot ( x*(1 - 1.0*cos (x/(20*PI)) )) 0 to 100
951 // Run in wolfram to see result, adjust the 1.0 factor to inc/red effect.
952 // Problem: -100 to 100 is terribly bad presicion, esp. considering that
953 // the values started as 0...1024, and we have 1000usec to "spend" on channels.
954 if ( check_key(KEY_UP ) ) {
955 menu_mainstate = DUALRATES;
958 if ( check_key(KEY_DOWN ) ) {
959 menu_mainstate = DEBUG;
965 lcd.setCursor(0 , 0);
966 lcd.print("Dumping debug to");
967 lcd.setCursor(0 , 1);
968 lcd.print("serial port 0");
970 if ( check_key(KEY_UP ) ) {
971 // FIXME: Remember to update the "Scroll up" state!
972 menu_mainstate = EXPOS;
974 } else if ( check_key(KEY_DOWN ) ) {
975 menu_mainstate = SAVE;
981 lcd.print("Not implemented");
982 lcd.setCursor(0 , 1);
983 lcd.print("Press DOWN...");
984 if ( check_key(KEY_DOWN ) ) menu_mainstate = TOP;