Set control surface positions when using an external network fdm.

Fetch all pending remote fdm network packets so there is not chance of
  getting behind.
Add support for driving control panel lights.
Working on better modeling KX 155 tuning behavior.

diff --git a/src/FDM/ExternalNet.cxx b/src/FDM/ExternalNet.cxx
index 160dc2f8370802d71b88aa2ec9b7085275b481f5..49e432096240de1f3504a8b5c7f2a9fdc1204a5c 100644 (file)
--- a/src/FDM/ExternalNet.cxx
+++ b/src/FDM/ExternalNet.cxx
@@ -78,7 +78,14 @@ static void global2raw( FGRawCtrls *raw ) {
        raw->mixture[i] = node->getDoubleValue( "mixture", 0.0 );
        raw->prop_advance[i] = node->getDoubleValue( "propeller-pitch", 0.0 );
        raw->magnetos[i] = node->getIntValue( "magnetos", 0 );
+       if ( i == 0 ) {
+         // cout << "Magnetos -> " << node->getIntValue( "magnetos", 0 );
+       }
        raw->starter[i] = node->getBoolValue( "starter", false );
+       if ( i == 0 ) {
+         // cout << " Starter -> " << node->getIntValue( "stater", false )
+         //      << endl;
+       }
     }
     for ( i = 0; i < FGRawCtrls::FG_MAX_WHEELS; ++i ) {
        raw->brake[i] = node->getDoubleValue( "brakes", 0.0 );
@@ -214,6 +221,15 @@ static void net2global( FGNetFDM *net ) {
        }
 
         fgSetDouble("/surface-positions/flap-pos-norm", net->flap_deflection);
+       SGPropertyNode * node = fgGetNode("/controls", true);
+        fgSetDouble("/surface-positions/elevator-pos-norm", 
+                   node->getDoubleValue( "elevator" ));
+        fgSetDouble("/surface-positions/rudder-pos-norm", 
+                   node->getDoubleValue( "rudder" ));
+        fgSetDouble("/surface-positions/left-aileron-pos-norm", 
+                   node->getDoubleValue( "aileron" ));
+        fgSetDouble("/surface-positions/right-aileron-pos-norm", 
+                   -node->getDoubleValue( "aileron" ));
 
        /* these are ignored for now  ... */
        /*
@@ -329,9 +345,7 @@ void FGExternalNet::init() {
 }
 
 
-// Run an iteration of the EOM.  This is a NOP here because the flight
-// model values are getting filled in elsewhere (most likely from some
-// external source.)
+// Run an iteration of the EOM.
 void FGExternalNet::update( double dt ) {
     int length;
     int result;
@@ -348,7 +362,7 @@ void FGExternalNet::update( double dt ) {
 
     // Read next set of FDM data (blocking enabled to maintain 'sync')
     length = sizeof(fdm);
-    if ( (result = data_server.recv( (char *)(& fdm), length, 0)) >= 0 ) {
+    while ( (result = data_server.recv( (char *)(& fdm), length, 0)) >= 0 ) {
        SG_LOG( SG_IO, SG_DEBUG, "Success reading data." );
        net2global( &fdm );
     }

diff --git a/src/Main/fg_props.cxx b/src/Main/fg_props.cxx
index c3b9ebdb3d7584b0723e97ef68ffe57c56b83bab..88d535aaecd5b83e28ce72c2e24261fb6509dbe3 100644 (file)
--- a/src/Main/fg_props.cxx
+++ b/src/Main/fg_props.cxx
@@ -574,6 +574,7 @@ setFDMDataLogging (bool state)
 void
 fgInitProps ()
 {
+  cout << "start of fgInitProps()" << endl;
                                // Simulation
   fgTie("/sim/logging/priority", getLoggingPriority, setLoggingPriority);
   fgTie("/sim/logging/classes", getLoggingClasses, setLoggingClasses);
@@ -611,7 +612,8 @@ fgInitProps ()
   fgTie("/sim/temp/winding-ccw", getWindingCCW, setWindingCCW, false);
   fgTie("/sim/temp/full-screen", getFullScreen, setFullScreen);
   fgTie("/sim/temp/fdm-data-logging", getFDMDataLogging, setFDMDataLogging);
-       
+
+  cout << "end of fgInitProps()" << endl;
 }
 
 

diff --git a/src/Network/atc610x.cxx b/src/Network/atc610x.cxx
index eb9a81194617be0d3a7fbb26260e6a2c231e3e85..422ee1375042b034b88f63aa6e2e4b2b3ee9915d 100644 (file)
--- a/src/Network/atc610x.cxx
+++ b/src/Network/atc610x.cxx
@@ -181,6 +181,29 @@ static void ATC610xReadSwitches( int fd, unsigned char *switch_bytes ) {
 }
 
 
+// Turn a lamp on or off
+void ATC610xSetLamp( int fd, int channel, bool value ) {
+    // lamp channels 0-63 are written to LampPort0, channels 64-127
+    // are written to LampPort1
+
+    // bits 0-6 are the lamp address
+    // bit 7 is the value (on/off)
+
+    int result;
+
+    // Write the value
+    unsigned char buf[3];
+    buf[0] = channel;
+    buf[1] = value;
+    buf[2] = 0;
+    result = write( fd, buf, 2 );
+    if ( result != 2 ) {
+        cout << "Write failed" << endl;
+        exit( -1 );
+    }
+}
+
+
 // Open and initialize ATC 610x hardware
 bool FGATC610x::open() {
     if ( is_enabled() ) {
@@ -199,6 +222,7 @@ bool FGATC610x::open() {
 
     snprintf( lock_file, 256, "/proc/atc610x/board%d/lock", board );
     snprintf( analog_in_file, 256, "/proc/atc610x/board%d/analog_in", board );
+    snprintf( lamps_file, 256, "/proc/atc610x/board%d/lamps", board );
     snprintf( radios_file, 256, "/proc/atc610x/board%d/radios", board );
     snprintf( stepper_file, 256, "/proc/atc610x/board%d/steppers", board );
     snprintf( switches_file, 256, "/proc/atc610x/board%d/switches", board );
@@ -225,6 +249,15 @@ bool FGATC610x::open() {
        exit( -1 );
     }
 
+    lamps_fd = ::open( lamps_file, O_WRONLY );
+    if ( lamps_fd == -1 ) {
+       SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
+       char msg[256];
+       snprintf( msg, 256, "Error opening %s", lamps_file );
+       perror( msg );
+       exit( -1 );
+    }
+
     radios_fd = ::open( radios_file, O_RDWR );
     if ( radios_fd == -1 ) {
        SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
@@ -273,7 +306,7 @@ bool FGATC610x::open() {
 
     bool home = false;
     int timeout = 900;          // about 30 seconds
-    while ( ! home ) {
+    while ( ! home && timeout > 0 ) {
         if ( timeout % 150 == 0 ) {
             SG_LOG( SG_IO, SG_INFO, "waiting for compass = " << timeout );
         } else {
@@ -323,6 +356,14 @@ bool FGATC610x::open() {
 
     ATC610xRelease( lock_fd );
 
+    /////////////////////////////////////////////////////////////////////
+    // Blank the lamps
+    /////////////////////////////////////////////////////////////////////
+
+    for ( int i = 0; i < 128; ++i ) {
+        ATC610xSetLamp( lamps_fd, i, false );
+    }
+
     /////////////////////////////////////////////////////////////////////
     // Finished initing hardware
     /////////////////////////////////////////////////////////////////////
@@ -358,6 +399,10 @@ bool FGATC610x::open() {
     adf_freq = fgGetNode( "/radios/adf/frequencies/selected-khz", true );
     adf_stby_freq = fgGetNode( "/radios/adf/frequencies/standby-khz", true );
 
+    inner = fgGetNode( "/radios/marker-beacon/inner", true );
+    middle = fgGetNode( "/radios/marker-beacon/middle", true );
+    outer = fgGetNode( "/radios/marker-beacon/outer", true );
+
     return true;
 }
 
@@ -434,12 +479,26 @@ bool FGATC610x::do_analog_in() {
 }
 
 
+/////////////////////////////////////////////////////////////////////
+// Write the lights
+/////////////////////////////////////////////////////////////////////
+
+bool FGATC610x::do_lights() {
+
+    ATC610xSetLamp( lamps_fd, 4, inner->getBoolValue() );
+    ATC610xSetLamp( lamps_fd, 5, middle->getBoolValue() );
+    ATC610xSetLamp( lamps_fd, 3, outer->getBoolValue() );
+
+    return true;
+}
+
+
 /////////////////////////////////////////////////////////////////////
 // Read radio switches 
 /////////////////////////////////////////////////////////////////////
 
 bool FGATC610x::do_radio_switches() {
-    float freq, inc;
+    float freq, coarse_freq, fine_freq, inc;
 
     ATC610xReadRadios( radios_fd, radio_switch_data );
 
@@ -511,7 +570,7 @@ bool FGATC610x::do_radio_switches() {
        if ( com1_tuner_fine == 0x0c && last_com1_tuner_fine == 0x01 ) {
            inc = -0.025;
        } else if ( com1_tuner_fine == 0x01 && last_com1_tuner_fine == 0x0c ) {
-           inc = -0.025;
+           inc = 0.025;
        } else if ( com1_tuner_fine > last_com1_tuner_fine ) {
            inc = 0.025;
        } else {
@@ -523,7 +582,7 @@ bool FGATC610x::do_radio_switches() {
            inc = -1.0;
        } else if ( com1_tuner_course == 0x01
                    && last_com1_tuner_course == 0x0c ) {
-           inc = -1.0;
+           inc = 1.0;
        } else if ( com1_tuner_course > last_com1_tuner_course ) {
            inc = 1.0;
        } else {
@@ -552,7 +611,7 @@ bool FGATC610x::do_radio_switches() {
        if ( com2_tuner_fine == 0x0c && last_com2_tuner_fine == 0x01 ) {
            inc = -0.025;
        } else if ( com2_tuner_fine == 0x01 && last_com2_tuner_fine == 0x0c ) {
-           inc = -0.025;
+           inc = 0.025;
        } else if ( com2_tuner_fine > last_com2_tuner_fine ) {
            inc = 0.025;
        } else {
@@ -564,7 +623,7 @@ bool FGATC610x::do_radio_switches() {
            inc = -1.0;
        } else if ( com2_tuner_course == 0x01
                    && last_com2_tuner_course == 0x0c ) {
-           inc = -1.0;
+           inc = 1.0;
        } else if ( com2_tuner_course > last_com2_tuner_course ) {
            inc = 1.0;
        } else {
@@ -588,41 +647,54 @@ bool FGATC610x::do_radio_switches() {
     int nav1_tuner_course = radio_switch_data[9] & 0x0f;
     static int last_nav1_tuner_fine = nav1_tuner_fine;
     static int last_nav1_tuner_course = nav1_tuner_course;
+
+    freq = nav1_stby_freq->getFloatValue();
+    coarse_freq = (int)freq;
+    fine_freq = freq - coarse_freq;
+
     inc = 0.0;
     if ( nav1_tuner_fine != last_nav1_tuner_fine ) {
        if ( nav1_tuner_fine == 0x0c && last_nav1_tuner_fine == 0x01 ) {
-           inc = -0.05;
+         fine_freq -= 0.05;
        } else if ( nav1_tuner_fine == 0x01 && last_nav1_tuner_fine == 0x0c ) {
-           inc = -0.05;
+         fine_freq += 0.05;
        } else if ( nav1_tuner_fine > last_nav1_tuner_fine ) {
-           inc = 0.05;
+         fine_freq += 0.05;
        } else {
-           inc = -0.05;
+         fine_freq -= 0.05;
        }
     }
+    if ( fine_freq < 0.0 ) {
+       fine_freq = 0.95;
+    }
+    if ( fine_freq > 0.95 ) {
+       fine_freq = 0.0;
+    }
+
     if ( nav1_tuner_course != last_nav1_tuner_course ) {
        if ( nav1_tuner_course == 0x0c && last_nav1_tuner_course == 0x01 ) {
-           inc = -1.0;
+         coarse_freq -= 1.0;
        } else if ( nav1_tuner_course == 0x01
                    && last_nav1_tuner_course == 0x0c ) {
-           inc = -1.0;
+         coarse_freq += 1.0;
        } else if ( nav1_tuner_course > last_nav1_tuner_course ) {
-           inc = 1.0;
+         coarse_freq += 1.0;
        } else {
-           inc = -1.0;
+         coarse_freq -= 1.0;
        }
     }
+    if ( coarse_freq < 108.0 ) {
+       coarse_freq = 117.0;
+    }
+    if ( coarse_freq > 117.0 ) {
+       coarse_freq = 108.0;
+    }
+
     last_nav1_tuner_fine = nav1_tuner_fine;
     last_nav1_tuner_course = nav1_tuner_course;
 
-    freq = nav1_stby_freq->getFloatValue() + inc;
-    if ( freq < 108.0 ) {
-       freq = 117.95;
-    }
-    if ( freq > 117.95 ) {
-       freq = 108.0;
-    }
-    fgSetFloat( "/radios/nav[0]/frequencies/standby-mhz", freq );
+    fgSetFloat( "/radios/nav[0]/frequencies/standby-mhz",
+               coarse_freq + fine_freq );
 
     // Nav2 Tuner
     int nav2_tuner_fine = (radio_switch_data[17] >> 4) & 0x0f;
@@ -634,7 +706,7 @@ bool FGATC610x::do_radio_switches() {
        if ( nav2_tuner_fine == 0x0c && last_nav2_tuner_fine == 0x01 ) {
            inc = -0.05;
        } else if ( nav2_tuner_fine == 0x01 && last_nav2_tuner_fine == 0x0c ) {
-           inc = -0.05;
+           inc = 0.05;
        } else if ( nav2_tuner_fine > last_nav2_tuner_fine ) {
            inc = 0.05;
        } else {
@@ -646,7 +718,7 @@ bool FGATC610x::do_radio_switches() {
            inc = -1.0;
        } else if ( nav2_tuner_course == 0x01
                    && last_nav2_tuner_course == 0x0c ) {
-           inc = -1.0;
+           inc = 1.0;
        } else if ( nav2_tuner_course > last_nav2_tuner_course ) {
            inc = 1.0;
        } else {
@@ -989,9 +1061,9 @@ bool FGATC610x::do_switches() {
     if ( switch_matrix[board][6][3] == 1 ) {
        fgSetFloat( "/controls/flaps", 1.0 );
     } else if ( switch_matrix[board][5][3] == 1 ) {
-       fgSetFloat( "/controls/flaps", 0.66 );
+       fgSetFloat( "/controls/flaps", 2.0 / 3.0 );
     } else if ( switch_matrix[board][4][3] == 1 ) {
-       fgSetFloat( "/controls/flaps", 0.33 );
+       fgSetFloat( "/controls/flaps", 1.0 / 3.0 );
     } else if ( switch_matrix[board][4][3] == 0 ) {
        fgSetFloat( "/controls/flaps", 0.0 );
     }
@@ -1006,6 +1078,7 @@ bool FGATC610x::process() {
     if ( ATC610xLock( lock_fd ) > 0 ) {
 
        do_analog_in();
+       do_lights();
        do_radio_switches();
        do_radio_display();
        do_steppers();

diff --git a/src/Network/atc610x.hxx b/src/Network/atc610x.hxx
index 0b68462a1c1e4a5d68de67a9ecd8aec139560571..3adbf9a619a28b7096f7264da298737905c3fd52 100644 (file)
--- a/src/Network/atc610x.hxx
+++ b/src/Network/atc610x.hxx
@@ -51,12 +51,14 @@ class FGATC610x : public FGProtocol {
 
     int lock_fd;
     int analog_in_fd;
+    int lamps_fd;
     int radios_fd;
     int stepper_fd;
     int switches_fd;
 
     char lock_file[256];
     char analog_in_file[256];
+    char lamps_file[256];
     char radios_file[256];
     char stepper_file[256];
     char switches_file[256];
@@ -75,10 +77,12 @@ class FGATC610x : public FGProtocol {
     SGPropertyNode *nav1_freq, *nav1_stby_freq;
     SGPropertyNode *nav2_freq, *nav2_stby_freq;
     SGPropertyNode *adf_freq, *adf_stby_freq;
+    SGPropertyNode *inner, *middle, *outer;
 
     int dme_switch;
 
     bool do_analog_in();
+    bool do_lights();
     bool do_radio_switches();
     bool do_radio_display();
     bool do_steppers();