Fix for bug 1304 - crash loading XML route

[flightgear.git] / src / Network / ATC-Inputs.cxx

diff --git a/src/Network/ATC-Inputs.cxx b/src/Network/ATC-Inputs.cxx
index 2f63b1be462098884fb150f2cc7bb33cfc6337fe..be59770f89fda441e7f2b9bdc8a26da1acfe0afd 100644 (file)
--- a/src/Network/ATC-Inputs.cxx
+++ b/src/Network/ATC-Inputs.cxx
@@ -16,7 +16,7 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //
 // $Id$
 
@@ -31,18 +31,27 @@
 #  include <sys/types.h>
 #  include <sys/stat.h>
 #  include <fcntl.h>
+#  include <stdlib.h>
+#  include <unistd.h>
+#  include <istream>
 #endif
 
-#include STL_STRING
+#include <errno.h>
+#include <math.h>
+#include <cstdio>
+
+#include <string>
 
 #include <simgear/debug/logstream.hxx>
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/props/props_io.hxx>
 
 #include <Main/fg_props.hxx>
 
 #include "ATC-Inputs.hxx"
 
-SG_USING_STD(string);
-
+using std::string;
+using std::vector;
 
 
 // Constructor: The _board parameter specifies which board to
@@ -218,18 +227,21 @@ bool FGATCInput::open() {
 /////////////////////////////////////////////////////////////////////
 
 // 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 ) {
+// from -1.0 to 1.0.  The deadband value is symmetric, so specifying
+// '1' will give you a deadband of +/-1
+static double scale( int center, int deadband, int min, int max, int value ) {
     // cout << center << " " << min << " " << max << " " << value << " ";
     double result;
     double range;
 
-    if ( value <= center ) {
-        range = center - min;
-        result = (value - center) / range;
+    if ( value <= (center - deadband) ) {
+        range = (center - deadband) - min;
+        result = (value - (center - deadband)) / range;
+    } else if ( value >= (center + deadband) ) {
+        range = max - (center + deadband);
+        result = (value - (center + deadband)) / range;            
     } else {
-        range = max - center;
-        result = (value - center) / range;            
+        result = 0.0;
     }
 
     if ( result < -1.0 ) result = -1.0;
@@ -259,6 +271,18 @@ static double scale( int min, int max, int value ) {
 }
 
 
+static double clamp( double min, double max, double value ) {
+    double result = value;
+
+    if ( result < min ) result = min;
+    if ( result > max ) result = max;
+
+    // cout << result << endl;
+
+    return result;   
+}
+
+
 static int tony_magic( int raw, int obs[3] ) {
     int result = 0;
 
@@ -393,10 +417,13 @@ bool FGATCInput::do_analog_in() {
             string name = "";
             string type = "";
             string subtype = "";
-            vector <SGPropertyNode *> output_nodes; output_nodes.clear();
+            vector <SGPropertyNode *> output_nodes;
             int center = -1;
             int min = 0;
             int max = 1023;
+           int deadband = 0;
+           int hysteresis = 0;
+            float offset = 0.0;
             float factor = 1.0;
             if ( cname == "channel" ) {
                 SGPropertyNode *prop;
@@ -431,6 +458,18 @@ bool FGATCInput::do_analog_in() {
                 if ( prop != NULL ) {
                     max = prop->getIntValue();
                 }
+                prop = child->getChild( "deadband" );
+                if ( prop != NULL ) {
+                    deadband = prop->getIntValue();
+                }
+                prop = child->getChild( "hysteresis" );
+                if ( prop != NULL ) {
+                    hysteresis = prop->getIntValue();
+                }
+                prop = child->getChild( "offset" );
+                if ( prop != NULL ) {
+                    offset = prop->getFloatValue();
+                }
                 prop = child->getChild( "factor" );
                 if ( prop != NULL ) {
                     factor = prop->getFloatValue();
@@ -451,12 +490,37 @@ bool FGATCInput::do_analog_in() {
                     {
                         // "Cook" the raw value
                         float scaled_value = 0.0f;
+
+                       if ( hysteresis > 0 ) {
+                           int last_raw_value = 0;
+                           prop = child->getChild( "last-raw-value", 0, true );
+                           last_raw_value = prop->getIntValue();
+
+                           if ( abs(raw_value - last_raw_value) < hysteresis )
+                           {
+                               // not enough movement stay put
+                               raw_value = last_raw_value;
+                           } else {
+                               // update last raw value
+                               prop->setIntValue( raw_value );
+                           }
+                       }
+
                         if ( center >= 0 ) {
-                            scaled_value = scale( center, min, max, raw_value );
+                            scaled_value = scale( center, deadband,
+                                                 min, max, raw_value );
                         } else {
                             scaled_value = scale( min, max, raw_value );
                         }
                         scaled_value *= factor;
+                        scaled_value += offset;
+
+                        // final sanity clamp
+                        if ( center >= 0 ) {
+                            scaled_value = clamp( -1.0, 1.0, scaled_value );
+                        } else {
+                            scaled_value = clamp( 0.0, 1.0, scaled_value );
+                        }
 
                         // update the property tree values
                         for ( j = 0; j < (int)output_nodes.size(); ++j ) {
@@ -467,11 +531,20 @@ bool FGATCInput::do_analog_in() {
                     // "Cook" the raw value
                     float scaled_value = 0.0f;
                     if ( center >= 0 ) {
-                        scaled_value = scale( center, min, max, raw_value );
+                        scaled_value = scale( center, deadband,
+                                             min, max, raw_value );
                     } else {
                         scaled_value = scale( min, max, raw_value );
                     }
                     scaled_value *= factor;
+                    scaled_value += offset;
+
+                    // final sanity clamp
+                    if ( center >= 0 ) {
+                        scaled_value = clamp( -1.0, 1.0, scaled_value );
+                    } else {
+                        scaled_value = clamp( 0.0, 1.0, scaled_value );
+                    }
 
                     // update the property tree values
                     for ( j = 0; j < (int)output_nodes.size(); ++j ) {
@@ -588,9 +661,13 @@ static void update_switch_matrix(
        unsigned char switches = switch_data[row];
 
        for( int column = 0; column < ATC_NUM_COLS; ++column ) {
-           switch_matrix[board][column][row] = switches & 1;
-           switches = switches >> 1;
-       }                       
+            if ( row < 8 ) {
+                switch_matrix[board][column][row] = switches & 1;
+            } else {
+                switch_matrix[board][row-8][8+column] = switches & 1;
+            }
+            switches = switches >> 1;
+        }
     }
 }                     
 
@@ -611,7 +688,7 @@ bool FGATCInput::do_switches() {
             string cname = child->getName();
             string name = "";
             string type = "";
-            vector <SGPropertyNode *> output_nodes; output_nodes.clear();
+            vector <SGPropertyNode *> output_nodes;
             int row = -1;
             int col = -1;
             float factor = 1.0;
@@ -814,7 +891,7 @@ bool FGATCInput::do_radio_switches() {
             if ( cname == "switch" ) {
                 string name = "";
                 string type = "";
-                vector <SGPropertyNode *> output_nodes; output_nodes.clear();
+                vector <SGPropertyNode *> output_nodes;
                 int byte_num = -1;
                 int right_shift = 0;
                 int mask = 0xff;