// 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 3
+#define EEPROM_VERSION 7
// Some data is stored in fixed locations, e.g.:
// * The EEPROM version number for the stored data (loc 0)
#define MAX_MODELS 4 // Nice and random number..
+
// --------------- ADC related stuffs.... --------------------
struct input_cal_t // Struct type for input calibration values
// 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 1550 // Max lenght of channel pulse
-#define chmin 620 // Min length of channel
+#define framelength 21500 // Max length of frame
+#define seplength 250 // Lenght of a channel separator
+#define chmax 1600 // Max lenght of channel pulse
+#define chmin 480 // Min length of channel
#define chwidht (chmax - chmin)// Useable time of channel pulse
// ----------------- Menu/IU related stuffs --------------------
model_defaults();
read_settings();
- pinMode(A5, OUTPUT); // PPM output pin
- do_channel = false;
- set_timer( seplength );
- Timer1.initialize(framelength);
- Timer1.attachInterrupt(ISR_timer);
-
displaystate = VALUES;
// Arduino believes all pins on Port C are Analog.
// 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 )
// data format changes.
// NOTE: This means that stored model conficuration is reset
// to defaults when the EEPROM version/format changes.
- model.channels = 6;
+ 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;
}
// ---------- Arduino main loop -----------------------
-void loop () {
-
- // Determine if the UI needs to run...
- boolean disp;
- if ( millis() - last > UI_INTERVAL ) {
- last = millis();
- disp = true;
- }
- else disp = false;
+void loop ()
+{
process_inputs();
displaystate = BATTERY;
}
- if ( disp )
- {
+ if ( millis() - last > UI_INTERVAL )
+ {
+ last = millis();
ui_handler();
}
+
+
if ( displaystate != MENU )
{
// Debugging: how long does the main loop take on avg,
void calibrate()
{
- int i, r0, r1, r2, adc_in;
+ int i, adc_in;
int num_calibrations = 200;
lcd.clear();
lcd.setCursor(0 , 1);
lcd.print("Done calibrating");
- Serial.print("Done calibrating");
+ Serial.print("Done calibrating");
delay(2000);
}
calibrate();
model_defaults();
// The following does not yet work...
- for ( i = 0; i < MAX_MODELS; i++);
+ for ( i = 0; i < MAX_MODELS; i++)
write_model_settings(i);
// 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 );
// Calculate the EEPROM start adress for the given model (mod_no)
model_address = EE_MDL_BASE_ADDR + (mod_no * sizeof(model_t));
-
- Serial.print("Models base addr: ");
- Serial.println( EE_MDL_BASE_ADDR );
- Serial.print("Model no: ");
- Serial.println( mod_no, 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) );
-
+
// Do not try to write the model to EEPROM if it won't fit.
if ( INT_EEPROM_SIZE < (model_address + sizeof(model_t)) )
{
// 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(input_cal_t); i++)
+ for (i = 0; i < sizeof(model_t); i++)
*p++ = EEPROM.read( model_address++ );
-
+
+ serial_dump_model();
+
lcd.setCursor(0 , 1);
lcd.print("... Loaded.");
delay(1000);
lcd.clear();
lcd.print("Saving model ");
- lcd.print(mod_no);
+ 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(input_cal_t); i++)
- EEPROM.write( model_address++, *p++);
-
+ for (i = 0; i < sizeof(model_t); i++)
+ EEPROM.write( model_address++, *p++);
+
lcd.setCursor(0 , 1);
lcd.print(".. done saving.");
- delay(1000);
+ delay(200);
}
+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();
+ }
+}
void scan_keys ( void )
{
- int i, r0, r1, r2;
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 (i=0; i<=7; i++) {
+ 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];
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;
- // Old format on stick values...
- /*
- model.stick[current_input] = ((float)adc_in - (float)input_cal.min[current_input]) / (float)(input_cal.max[current_input]-input_cal.min[current_input]);
- if ( model.rev[current_input] ) model.stick[current_input] = 1.0f - model.stick[current_input];
- */
-
// Dual-rate calculation :D
// This is very repetitive code. It should be fast, but it may waste code-space.
float dr_val;
// 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 ...
+ // 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,
void serial_debug()
{
int current_input;
- for (current_input=0; current_input<=7; current_input++) {
+ for (current_input=0; current_input<MAX_INPUTS; current_input++) {
Serial.print("Input #");
Serial.print(current_input);
Serial.println();
}
+
void dr_inputselect( int no, int in )
{
if ( model.dr[menu_substate] < 0 ) model.dr[menu_substate] = 4;
{
case VALUES:
int current_input;
- for (current_input=0; current_input<=7; 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
// 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;