- double trat = _tau ? dt/_tau : 100;
- double target = _pressure_in_node->getDoubleValue();
- double current = _pressure_out_node->getDoubleValue();
- // double delta = target - current;
- _pressure_out_node->setDoubleValue(fgGetLowPass(current, target, trat));
+ 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 : SGLimitsd::max();
+
+ 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) || (_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(
+ _tau > .0 ? fgGetLowPass(p, p_new, trat) : p_new
+ ); //setting new pressure in static system
+