From: curt <curt>
Date: Mon, 12 Apr 2004 19:17:47 +0000 (+0000)
Subject: Add a pressure rate helper function for Roy's KAP140 autopilot model.
X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=088a7a83b49112a571a04e34825feceea623d6cf;p=flightgear.git

Add a pressure rate helper function for Roy's KAP140 autopilot model.
---

diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx
index 9aa550c55..5d5556e7b 100644
--- a/src/Autopilot/xmlauto.cxx
+++ b/src/Autopilot/xmlauto.cxx
@@ -26,6 +26,7 @@
 
 #include <Main/fg_props.hxx>
 #include <Main/globals.hxx>
+#include <Main/util.hxx>
 
 #include "xmlauto.hxx"
 
@@ -773,7 +774,42 @@ static void update_helper( double dt ) {
     static SGPropertyNode *vs_fpm
         = fgGetNode( "/autopilot/internal/vert-speed-fpm", true );
 
-    vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );  
+    vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
+
+
+    // Calculate static port pressure rate in [inhg/s].
+    // Used to determine vertical speed.
+    static SGPropertyNode *static_pressure
+	= fgGetNode( "/systems/static[0]/pressure-inhg", true );
+    static SGPropertyNode *pressure_rate
+	= fgGetNode( "/autopilot/internal/pressure-rate", true );
+    static SGPropertyNode *filter_time 
+	= fgGetNode( "/autopilot/internal/ft", true );
+
+    static double last_static_pressure = 0.0;
+    static double last_pressure_rate = 0.0;
+
+    double Tf = filter_time->getDoubleValue();
+
+    if ( dt > 0.0 && Tf > 0.0) {
+	double current_static_pressure = static_pressure->getDoubleValue();
+
+	double current_pressure_rate = 
+	    ( current_static_pressure - last_static_pressure ) / dt;
+
+	double W = dt/Tf;
+
+        // Low Pass Filter
+	current_pressure_rate =
+	    (1.0/(W + 1.0))*last_pressure_rate +
+	    ((W)/(W + 1.0))*current_pressure_rate;
+
+	pressure_rate->setDoubleValue(current_pressure_rate);
+
+	last_static_pressure = current_static_pressure;
+	last_pressure_rate = current_pressure_rate;
+    }
+
 }