bool home = false;
int timeout = 900; // about 30 seconds
+ timeout = 0;
while ( ! home && timeout > 0 ) {
if ( timeout % 150 == 0 ) {
SG_LOG( SG_IO, SG_INFO, "waiting for compass = " << timeout );
dme_kt = fgGetNode( "/radios/dme/speed-kt", true );
dme_nm = fgGetNode( "/radios/dme/distance-nm", true );
- navcom1_bus_power = fgGetNode( "/systems/electrical/outputs/navcomm[0]",
+ adf_bus_power = fgGetNode( "/systems/electrical/outputs/adf", true );
+ dme_bus_power = fgGetNode( "/systems/electrical/outputs/dme", true );
+ navcom1_bus_power = fgGetNode( "/systems/electrical/outputs/navcom[0]",
true );
- navcom2_bus_power = fgGetNode( "/systems/electrical/outputs/navcomm[1]",
+ navcom2_bus_power = fgGetNode( "/systems/electrical/outputs/navcom[1]",
true );
+ xpdr_bus_power = fgGetNode( "/systems/electrical/outputs/transponder",
+ true );
navcom1_power_btn = fgGetNode( "/radios/comm[0]/inputs/power-btn", true );
navcom2_power_btn = fgGetNode( "/radios/comm[1]/inputs/power-btn", true );
nav1_freq = fgGetNode( "/radios/nav[0]/frequencies/selected-mhz", true );
nav1_stby_freq
= fgGetNode( "/radios/nav[0]/frequencies/standby-mhz", true );
+ nav1_obs = fgGetNode( "/radios/nav[0]/radials/selected-deg", true );
nav2_freq = fgGetNode( "/radios/nav[1]/frequencies/selected-mhz", true );
nav2_stby_freq
= fgGetNode( "/radios/nav[1]/frequencies/standby-mhz", true );
+ nav2_obs = fgGetNode( "/radios/nav[1]/radials/selected-deg", true );
- adf_power = fgGetNode( "/radios/kr-87/inputs/power-btn", true );
+ adf_power_btn = fgGetNode( "/radios/kr-87/inputs/power-btn", true );
adf_vol = fgGetNode( "/radios/kr-87/inputs/volume", true );
adf_adf_btn = fgGetNode( "/radios/kr-87/inputs/adf-btn", true );
adf_bfo_btn = fgGetNode( "/radios/kr-87/inputs/bfo-btn", true );
// Read analog inputs
/////////////////////////////////////////////////////////////////////
-#define ATC_AILERON_CENTER 535
-#define ATC_ELEVATOR_TRIM_CENTER 512
-#define ATC_ELEVATOR_CENTER 543
-#define ATC_RUDDER_CENTER 519
-
// scale a number between min and max (with center defined) to a scale
// from -1.0 to 1.0
static double scale( int center, int min, int max, int value ) {
// mixture
tmp = scale( mixture_min->getIntValue(), mixture_max->getIntValue(),
- analog_in_data[7] );
+ analog_in_data[6] );
fgSetFloat( "/controls/mixture[0]", tmp );
fgSetFloat( "/controls/mixture[1]", tmp );
// throttle
- tmp = scale( mixture_min->getIntValue(), mixture_max->getIntValue(),
+ tmp = scale( throttle_min->getIntValue(), throttle_max->getIntValue(),
analog_in_data[8] );
fgSetFloat( "/controls/throttle[0]", tmp );
fgSetFloat( "/controls/throttle[1]", tmp );
+ // cout << "throttle = " << tmp << endl;
-#if 0
// rudder
+ /*
tmp = scale( rudder_center->getIntValue(), rudder_min->getIntValue(),
rudder_max->getIntValue(), analog_in_data[10] );
- fgSetFloat( "/controls/rudder", tmp );
-#endif
+ fgSetFloat( "/controls/rudder", -tmp );
+ */
// nav1 volume
tmp = (float)analog_in_data[25] / 1024.0f;
fgSetFloat( "/radios/kr-87/inputs/volume", tmp );
// nav2 obs tuner
- tmp = (float)analog_in_data[29] * 360.0f / 1024.0f;
+ static int last_obs2 = analog_in_data[29];
+ static double diff_ave = 0.0;
+ int diff = 0;
+
+ // cout << "val = " << analog_in_data[29] << " last_obs = " << last_obs2;
+
+#define FG_SECOND_TRY
+
+#if defined( FG_FIRST_TRY )
+ if ( analog_in_data[29] < 150 || analog_in_data[29] > 990 ) {
+ if ( last_obs2 > 512 && last_obs2 <= 990 ) {
+ diff = 1;
+ } else if ( last_obs2 >= 150 && last_obs2 <= 990 ) {
+ diff = -1;
+ }
+ } else if ( last_obs2 < 150 || last_obs2 > 990 ) {
+ if ( analog_in_data[29] > 512 && analog_in_data[29] <= 990 ) {
+ diff = -1;
+ } else if ( analog_in_data[29] >= 150 && analog_in_data[29] <= 990 ) {
+ diff = 1;
+ }
+ } else {
+ diff = analog_in_data[29] - last_obs2;
+ }
+#elif defined( FG_SECOND_TRY )
+ if ( analog_in_data[29] < 20 ) {
+ if ( last_obs2 >= 110 && last_obs2 < 512 ) {
+ diff = -1;
+ } else if ( last_obs2 >= 512 ) {
+ diff = 1;
+ }
+ last_obs2 = analog_in_data[29];
+ } else if ( analog_in_data[29] < 110 ) {
+ // do nothing
+ } else if ( last_obs2 < 20 ) {
+ if ( analog_in_data[29] >= 110 && analog_in_data[29] < 512 ) {
+ diff = 1;
+ } else if ( analog_in_data[29] >= 512 ) {
+ diff = -1;
+ }
+ last_obs2 = analog_in_data[29];
+ } else {
+ diff = analog_in_data[29] - last_obs2;
+ if ( abs(diff) > 200 ) {
+ // ignore
+ diff = 0;
+ }
+ last_obs2 = analog_in_data[29];
+ }
+#elif defined( FG_THIRD_TRY )
+ static bool ignore_next = false;
+ diff = analog_in_data[29] - last_obs2;
+ if ( abs(diff) > 200 ) {
+ // ignore
+ diff = 0;
+ ignore_next = true;
+ } else if ( ignore_next ) {
+ diff = 0;
+ ignore_next = false;
+ }
+ last_obs2 = analog_in_data[29];
+#endif
+
+ // cout << " diff = " << diff << endl;
+ if ( diff < -500 ) { diff += 1024; }
+ if ( diff > 500 ) { diff -= 1024; }
+
+ if ( fabs(diff_ave - diff) < 200 || fabs(diff) < fabs(diff_ave) ) {
+ diff_ave = (2.0/3.0) * diff_ave + (1.0/3.0) * diff;
+ }
+
+ tmp = nav2_obs->getDoubleValue() + (diff_ave * (60.0/914.0) );
+ while ( tmp >= 360.0 ) { tmp -= 360.0; }
+ while ( tmp < 0.0 ) { tmp += 360.0; }
fgSetFloat( "/radios/nav[1]/radials/selected-deg", tmp );
// nav1 obs tuner
coarse_freq + fine_freq / 20.0);
}
- if ( adf_power->getBoolValue() ) {
- // ADF Tuner
- int adf_tuner_fine = ((radio_switch_data[21] >> 4) & 0x0f) - 1;
- int adf_tuner_coarse = (radio_switch_data[21] & 0x0f) - 1;
- static int last_adf_tuner_fine = adf_tuner_fine;
- static int last_adf_tuner_coarse = adf_tuner_coarse;
+ // ADF Tuner
+
+ int adf_tuner_fine = ((radio_switch_data[21] >> 4) & 0x0f) - 1;
+ int adf_tuner_coarse = (radio_switch_data[21] & 0x0f) - 1;
+ static int last_adf_tuner_fine = adf_tuner_fine;
+ static int last_adf_tuner_coarse = adf_tuner_coarse;
+ if ( adf_has_power() ) {
// cout << "adf_stby_mode = " << adf_stby_mode->getIntValue() << endl;
if ( adf_count_mode->getIntValue() == 2 ) {
// tune count down timer
if ( value > 1799 ) { value -= 1600; }
}
- last_adf_tuner_fine = adf_tuner_fine;
- last_adf_tuner_coarse = adf_tuner_coarse;
-
if ( adf_count_mode->getIntValue() == 2 ) {
fgSetFloat( "/radios/kr-87/outputs/elapsed-timer", value );
} else {
}
}
}
+ last_adf_tuner_fine = adf_tuner_fine;
+ last_adf_tuner_coarse = adf_tuner_coarse;
+
// ADF buttons
fgSetInt( "/radios/kr-87/inputs/adf-btn",
// Transponder Tuner
int i;
int digit_tuner[4];
-
digit_tuner[0] = radio_switch_data[25] & 0x0f;
digit_tuner[1] = ( radio_switch_data[25] >> 4 ) & 0x0f;
digit_tuner[2] = radio_switch_data[29] & 0x0f;
digit_tuner[3] = ( radio_switch_data[29] >> 4 ) & 0x0f;
- static bool first_time = true;
+
static int last_digit_tuner[4];
+ static bool first_time = true;
if ( first_time ) {
first_time = false;
for ( i = 0; i < 4; ++i ) {
}
}
- int id_code = xpdr_id_code->getIntValue();
- int digit[4];
- int place = 1000;
- for ( i = 0; i < 4; ++i ) {
- digit[i] = id_code / place;
- id_code -= digit[i] * place;
- place /= 10;
- }
+ if ( xpdr_has_power() ) {
+ int id_code = xpdr_id_code->getIntValue();
+ int digit[4];
+ int place = 1000;
+ for ( i = 0; i < 4; ++i ) {
+ digit[i] = id_code / place;
+ id_code -= digit[i] * place;
+ place /= 10;
+ }
- for ( i = 0; i < 4; ++i ) {
- if ( digit_tuner[i] != last_digit_tuner[i] ) {
- diff = digit_tuner[i] - last_digit_tuner[i];
- if ( abs(diff) > 4 ) {
- // roll over
- if ( digit_tuner[i] < last_digit_tuner[i] ) {
- // going up
- diff = 15 - last_digit_tuner[i] + digit_tuner[i];
- } else {
- // going down
- diff = digit_tuner[i] - 15 - last_digit_tuner[i];
+ for ( i = 0; i < 4; ++i ) {
+ if ( digit_tuner[i] != last_digit_tuner[i] ) {
+ diff = digit_tuner[i] - last_digit_tuner[i];
+ if ( abs(diff) > 4 ) {
+ // roll over
+ if ( digit_tuner[i] < last_digit_tuner[i] ) {
+ // going up
+ diff = 15 - last_digit_tuner[i] + digit_tuner[i];
+ } else {
+ // going down
+ diff = digit_tuner[i] - 15 - last_digit_tuner[i];
+ }
}
+ digit[i] += diff;
}
- digit[i] += diff;
+ while ( digit[i] >= 8 ) { digit[i] -= 8; }
+ while ( digit[i] < 0 ) { digit[i] += 8; }
}
- while ( digit[i] >= 8 ) { digit[i] -= 8; }
- while ( digit[i] < 0 ) { digit[i] += 8; }
+
+ fgSetInt( "/radios/kt-70/inputs/digit1", digit[0] );
+ fgSetInt( "/radios/kt-70/inputs/digit2", digit[1] );
+ fgSetInt( "/radios/kt-70/inputs/digit3", digit[2] );
+ fgSetInt( "/radios/kt-70/inputs/digit4", digit[3] );
+ }
+ for ( i = 0; i < 4; ++i ) {
last_digit_tuner[i] = digit_tuner[i];
}
- fgSetInt( "/radios/kt-70/inputs/digit1", digit[0] );
- fgSetInt( "/radios/kt-70/inputs/digit2", digit[1] );
- fgSetInt( "/radios/kt-70/inputs/digit3", digit[2] );
- fgSetInt( "/radios/kt-70/inputs/digit4", digit[3] );
-
int tmp = 0;
for ( i = 0; i < 5; ++i ) {
if ( radio_switch_data[27] >> i & 0x01 ) {
char digits[10];
int i;
- if ( dme_switch != 0 ) {
+ if ( dme_has_power() ) {
// DME minutes
float minutes = dme_min->getFloatValue();
if ( minutes > 999 ) {
}
// ADF standby frequency / timer
- if ( adf_power->getBoolValue() ) {
+ if ( adf_has_power() ) {
if ( adf_stby_mode->getIntValue() == 0 ) {
// frequency
float adf_stby = adf_stby_freq->getFloatValue();
} else {
radio_display_data[34] = digits[0] << 4 | digits[1];
}
+ if ( adf_stby_mode->getIntValue() == 0 ) {
+ radio_display_data[35] = 0xff;
+ } else {
+ radio_display_data[35] = 0x0f;
+ }
} else {
radio_display_data[30] = 0xff;
radio_display_data[31] = 0xff;
radio_display_data[32] = 0xff;
radio_display_data[33] = 0xff;
radio_display_data[34] = 0xff;
+ radio_display_data[35] = 0xff;
}
// Transponder code and flight level
- if ( xpdr_func_knob->getIntValue() == 2 ) {
- // test mode
- radio_display_data[36] = 8 << 4 | 8;
- radio_display_data[37] = 8 << 4 | 8;
- radio_display_data[38] = 0xff;
- radio_display_data[39] = 8 << 4 | 0x0f;
- radio_display_data[40] = 8 << 4 | 8;
- } else if ( xpdr_func_knob->getIntValue() > 0 ) {
- // other on modes
- int id_code = xpdr_id_code->getIntValue();
- int place = 1000;
- for ( i = 0; i < 4; ++i ) {
- digits[i] = id_code / place;
- id_code -= digits[i] * place;
- place /= 10;
- }
- radio_display_data[36] = digits[2] << 4 | digits[3];
- radio_display_data[37] = digits[0] << 4 | digits[1];
- radio_display_data[38] = 0xff;
-
- if ( xpdr_func_knob->getIntValue() == 3 ||
- xpdr_func_knob->getIntValue() == 5 )
- {
- // do flight level display
- snprintf(digits, 7, "%03d", xpdr_flight_level->getIntValue() );
- for ( i = 0; i < 6; ++i ) {
- digits[i] -= '0';
- }
- radio_display_data[39] = digits[2] << 4 | 0x0f;
- radio_display_data[40] = digits[0] << 4 | digits[1];
+ if ( xpdr_has_power() ) {
+ if ( xpdr_func_knob->getIntValue() == 2 ) {
+ // test mode
+ radio_display_data[36] = 8 << 4 | 8;
+ radio_display_data[37] = 8 << 4 | 8;
+ radio_display_data[38] = 0xff;
+ radio_display_data[39] = 8 << 4 | 0x0f;
+ radio_display_data[40] = 8 << 4 | 8;
} else {
- // blank flight level display
- radio_display_data[39] = 0xff;
- radio_display_data[40] = 0xff;
+ // other on modes
+ int id_code = xpdr_id_code->getIntValue();
+ int place = 1000;
+ for ( i = 0; i < 4; ++i ) {
+ digits[i] = id_code / place;
+ id_code -= digits[i] * place;
+ place /= 10;
+ }
+ radio_display_data[36] = digits[2] << 4 | digits[3];
+ radio_display_data[37] = digits[0] << 4 | digits[1];
+ radio_display_data[38] = 0xff;
+
+ if ( xpdr_func_knob->getIntValue() == 3 ||
+ xpdr_func_knob->getIntValue() == 5 )
+ {
+ // do flight level display
+ snprintf(digits, 7, "%03d", xpdr_flight_level->getIntValue() );
+ for ( i = 0; i < 6; ++i ) {
+ digits[i] -= '0';
+ }
+ radio_display_data[39] = digits[2] << 4 | 0x0f;
+ radio_display_data[40] = digits[0] << 4 | digits[1];
+ } else {
+ // blank flight level display
+ radio_display_data[39] = 0xff;
+ radio_display_data[40] = 0xff;
+ }
}
} else {
// off
projects / flightgear.git / blobdiff