Work on nav2_obs tuner.

[flightgear.git] / src / Network / atc610x.cxx

diff --git a/src/Network/atc610x.cxx b/src/Network/atc610x.cxx
index 2a618c011580b350e6ffd6150d0dd30518869b1b..7a4d0714601d58459cdd7ce762b3b09b0f21dda8 100644 (file)
--- a/src/Network/atc610x.cxx
+++ b/src/Network/atc610x.cxx
@@ -323,6 +323,7 @@ bool FGATC610x::open() {
 
     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 );
@@ -421,10 +422,12 @@ bool FGATC610x::open() {
     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_btn = fgGetNode( "/radios/kr-87/inputs/power-btn", true );
     adf_vol = fgGetNode( "/radios/kr-87/inputs/volume", true );
@@ -500,11 +503,6 @@ bool FGATC610x::open() {
 // 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 ) {
@@ -588,17 +586,18 @@ bool FGATC610x::do_analog_in() {
     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;
@@ -613,7 +612,80 @@ bool FGATC610x::do_analog_in() {
     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
@@ -1387,12 +1459,18 @@ bool FGATC610x::do_radio_display() {
         } 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