From 8770e61d1b1be465ee81d35f00ada11acf6773b3 Mon Sep 17 00:00:00 2001
From: Eric van den Berg <eric@eric-home.eric.org>
Date: Sat, 16 Nov 2013 23:15:11 +0100
Subject: [PATCH] static port can be side-slip/alpha dependant, to add a part
 of total  pressure on the reported static pressure.

---
 src/Systems/static.cxx | 46 +++++++++++++++++++++++++++++++++++++-----
 src/Systems/static.hxx | 10 ++++++++-
 2 files changed, 50 insertions(+), 6 deletions(-)

diff --git a/src/Systems/static.cxx b/src/Systems/static.cxx
index f58d1ce3c..f8510fe61 100644
--- a/src/Systems/static.cxx
+++ b/src/Systems/static.cxx
@@ -1,6 +1,7 @@
 // static.cxx - the static air system.
 // Written by David Megginson, started 2002.
 //
+// Last modified by Eric van den Berg, 09 Nov 2013
 // This file is in the Public Domain and comes with no warranty.
 
 #ifdef HAVE_CONFIG_H
@@ -13,14 +14,17 @@
 
 #include <Main/fg_props.hxx>
 #include <Main/util.hxx>
+#include <simgear/constants.h>
+#include <simgear/sg_inlines.h>
 
 
 StaticSystem::StaticSystem ( SGPropertyNode *node )
     :
     _name(node->getStringValue("name", "static")),
     _num(node->getIntValue("number", 0)),
-    _tau(node->getDoubleValue("tau", 1))
-
+    _tau(node->getDoubleValue("tau", 1)),
+    _error_factor(node->getDoubleValue("error-factor", 0)),
+    _type(node->getIntValue("type", 0))
 {
 }
 
@@ -37,6 +41,10 @@ StaticSystem::init ()
     _serviceable_node = node->getChild("serviceable", 0, true);
     _pressure_in_node = fgGetNode("/environment/pressure-inhg", true);
     _pressure_out_node = node->getChild("pressure-inhg", 0, true);
+    _beta_node = fgGetNode("/orientation/side-slip-deg", true);
+    _alpha_node = fgGetNode("/orientation/alpha-deg", true);
+    _mach_node = fgGetNode("/velocities/mach", true);
+    SG_CLAMP_RANGE(_error_factor,0.0,1.0);     // making sure the error_factor is between 0 and 1
 
     reinit();
 }
@@ -62,10 +70,38 @@ void
 StaticSystem::update (double dt)
 {
     if (_serviceable_node->getBoolValue()) {
+        double p_new = _pressure_in_node->getDoubleValue();                         //current static pressure around aircraft
+        double p = _pressure_out_node->getDoubleValue();                            //last pressure in aircraft static system
+
+        double beta;
+        double alpha;
+        double mach;
         double trat = _tau ? dt/_tau : 100;
-        double target = _pressure_in_node->getDoubleValue();
-        double current = _pressure_out_node->getDoubleValue();
-        _pressure_out_node->setDoubleValue(fgGetLowPass(current, target, trat));
+
+        double proj_factor = 0;
+        double pt;
+        double qc_part;
+
+        if (_type == 1) {                                       // type 1 = static pressure dependent on side-slip only: static port on the fuselage
+            beta = _beta_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+            proj_factor = sin(beta);
+        }
+
+        if (_type == 2) {                                       // type 2 = static pressure dependent on aoa and side-slip: static port on the pitot tube
+            alpha = _alpha_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+            beta = _beta_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+            proj_factor = sqrt( 1.0 - cos(beta)*cos(beta) * cos(alpha)*cos(alpha) );
+        }
+
+        if ( (_type ==1) or  (_type == 2) ) {
+            mach = _mach_node->getDoubleValue();
+            pt = p_new * pow(1 + 0.2 * mach*mach*proj_factor*proj_factor, 3.5 );    //total pressure perpendicular to static port (=perpendicular to body x-axis)
+            qc_part = (pt - p_new) * _error_factor ;                            //part of impact pressure to be added to static pressure (due to sideslip)
+            p_new = p_new + qc_part;
+        }
+
+        _pressure_out_node->setDoubleValue(fgGetLowPass(p, p_new, trat));           //setting new pressure in static system
+
     }
 }
 
diff --git a/src/Systems/static.hxx b/src/Systems/static.hxx
index b50d32c5a..ccb30cafd 100644
--- a/src/Systems/static.hxx
+++ b/src/Systems/static.hxx
@@ -1,6 +1,7 @@
 // static.hxx - the static air system.
 // Written by David Megginson, started 2002.
 //
+// Last modified by Eric van den Berg, 09 November 2013
 // This file is in the Public Domain and comes with no warranty.
 
 
@@ -22,12 +23,14 @@
  *
  * /environment/pressure-inhg
  * /systems/"name"/serviceable
+ * /orientation/alpha-deg
+ * /orientation/side-slip-rad
+ * /velocities/mach
  *
  * Output properties:
  *
  * /systems/"name"/pressure-inhg
  *
- * TODO: support specific locations
  * TODO: support alternate air with errors
  */
 class StaticSystem : public SGSubsystem
@@ -50,9 +53,14 @@ private:
     std::string _name;
     int _num;
     double _tau;
+    double _error_factor;
+    int _type;
     SGPropertyNode_ptr _serviceable_node;
     SGPropertyNode_ptr _pressure_in_node;
     SGPropertyNode_ptr _pressure_out_node;
+    SGPropertyNode_ptr _beta_node;
+    SGPropertyNode_ptr _alpha_node;
+    SGPropertyNode_ptr _mach_node;
     
 };
 
-- 
2.39.5