only consider positive airspeed in longitudinal axis
[fg:hoorays-flightgear.git] / src / Systems / pitot.cxx
1 // pitot.cxx - the pitot air system.
2 // Written by David Megginson, started 2002.
3 //
4 // This file is in the Public Domain and comes with no warranty.
5
6 #include <simgear/constants.h>
7
8 #include <Main/fg_props.hxx>
9 #include <Main/util.hxx>
10
11 #include "pitot.hxx"
12
13
14 PitotSystem::PitotSystem ( SGPropertyNode *node )
15     :
16     num(0),
17     name("pitot")
18 {
19     int i;
20     for ( i = 0; i < node->nChildren(); ++i ) {
21         SGPropertyNode *child = node->getChild(i);
22         string cname = child->getName();
23         string cval = child->getStringValue();
24         if ( cname == "name" ) {
25             name = cval;
26         } else if ( cname == "number" ) {
27             num = child->getIntValue();
28         } else {
29             SG_LOG( SG_SYSTEMS, SG_WARN, "Error in systems config logic" );
30             if ( name.length() ) {
31                 SG_LOG( SG_SYSTEMS, SG_WARN, "Section = " << name );
32             }
33         }
34     }
35 }
36
37 PitotSystem::PitotSystem ( int i )
38 {
39     num = i;
40     name = "pitot";
41 }
42
43 PitotSystem::~PitotSystem ()
44 {
45 }
46
47 void
48 PitotSystem::init ()
49 {
50     string branch;
51     branch = "/systems/" + name;
52
53     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
54     _serviceable_node = node->getChild("serviceable", 0, true);
55     _pressure_node = fgGetNode("/environment/pressure-inhg", true);
56     _density_node = fgGetNode("/environment/density-slugft3", true);
57     _velocity_node = fgGetNode("/velocities/airspeed-kt", true);
58     _slip_angle = fgGetNode("/orientation/side-slip-rad", true);
59     _total_pressure_node = node->getChild("total-pressure-inhg", 0, true);
60 }
61
62 void
63 PitotSystem::bind ()
64 {
65 }
66
67 void
68 PitotSystem::unbind ()
69 {
70 }
71
72 #ifndef INHGTOPSF
73 # define INHGTOPSF (2116.217/29.9212)
74 #endif
75
76 #ifndef PSFTOINHG
77 # define PSFTOINHG (1/INHGTOPSF)
78 #endif
79
80
81 void
82 PitotSystem::update (double dt)
83 {
84     if (_serviceable_node->getBoolValue()) {
85                                 // The pitot tube sees the forward
86                                 // velocity in the body axis.
87         double p = _pressure_node->getDoubleValue() * INHGTOPSF;
88         double r = _density_node->getDoubleValue();
89         double v = _velocity_node->getDoubleValue() * SG_KT_TO_FPS;
90         v *= cos(_slip_angle->getDoubleValue());
91         if (v < 0.0)
92             v = 0.0;
93         double q = 0.5 * r * v * v; // dynamic
94         _total_pressure_node->setDoubleValue((p + q) * PSFTOINHG);
95     }
96 }
97
98 // end of pitot.cxx