fgFDM.py
Go to the documentation of this file.
1 
2 #!/usr/bin/env python
3 # parse and construct FlightGear NET FDM packets
4 # Andrew Tridgell, November 2011
5 # released under GNU GPL version 2 or later
6 
7 from builtins import range
8 from builtins import object
9 import struct, math
10 
11 class fgFDMError(Exception):
12  '''fgFDM error class'''
13  def __init__(self, msg):
14  Exception.__init__(self, msg)
15  self.message = 'fgFDMError: ' + msg
16 
17 class fgFDMVariable(object):
18  '''represent a single fgFDM variable'''
19  def __init__(self, index, arraylength, units):
20  self.index = index
21  self.arraylength = arraylength
22  self.units = units
23 
24 class fgFDMVariableList(object):
25  '''represent a list of fgFDM variable'''
26  def __init__(self):
27  self.vars = {}
28  self._nextidx = 0
29 
30  def add(self, varname, arraylength=1, units=None):
31  self.vars[varname] = fgFDMVariable(self._nextidx, arraylength, units=units)
32  self._nextidx += arraylength
33 
34 class fgFDM(object):
35  '''a flightgear native FDM parser/generator'''
36  def __init__(self):
37  '''init a fgFDM object'''
38  self.FG_NET_FDM_VERSION = 24
39  self.pack_string = '>I 4x 3d 6f 11f 3f 2f I 4I 4f 4f 4f 4f 4f 4f 4f 4f 4f I 4f I 3I 3f 3f 3f I i f 10f'
40  self.values = [0]*98
41 
42  self.FG_MAX_ENGINES = 4
43  self.FG_MAX_WHEELS = 3
44  self.FG_MAX_TANKS = 4
45 
46  # supported unit mappings
47  self.unitmap = {
48  ('radians', 'degrees') : math.degrees(1),
49  ('rps', 'dps') : math.degrees(1),
50  ('feet', 'meters') : 0.3048,
51  ('fps', 'mps') : 0.3048,
52  ('knots', 'mps') : 0.514444444,
53  ('knots', 'fps') : 0.514444444/0.3048,
54  ('fpss', 'mpss') : 0.3048,
55  ('seconds', 'minutes') : 60,
56  ('seconds', 'hours') : 3600,
57  }
58 
59  # build a mapping between variable name and index in the values array
60  # note that the order of this initialisation is critical - it must
61  # match the wire structure
62  self.mapping = fgFDMVariableList()
63  self.mapping.add('version')
64 
65  # position
66  self.mapping.add('longitude', units='radians') # geodetic (radians)
67  self.mapping.add('latitude', units='radians') # geodetic (radians)
68  self.mapping.add('altitude', units='meters') # above sea level (meters)
69  self.mapping.add('agl', units='meters') # above ground level (meters)
70 
71  # attitude
72  self.mapping.add('phi', units='radians') # roll (radians)
73  self.mapping.add('theta', units='radians') # pitch (radians)
74  self.mapping.add('psi', units='radians') # yaw or true heading (radians)
75  self.mapping.add('alpha', units='radians') # angle of attack (radians)
76  self.mapping.add('beta', units='radians') # side slip angle (radians)
77 
78  # Velocities
79  self.mapping.add('phidot', units='rps') # roll rate (radians/sec)
80  self.mapping.add('thetadot', units='rps') # pitch rate (radians/sec)
81  self.mapping.add('psidot', units='rps') # yaw rate (radians/sec)
82  self.mapping.add('vcas', units='fps') # calibrated airspeed
83  self.mapping.add('climb_rate', units='fps') # feet per second
84  self.mapping.add('v_north', units='fps') # north velocity in local/body frame, fps
85  self.mapping.add('v_east', units='fps') # east velocity in local/body frame, fps
86  self.mapping.add('v_down', units='fps') # down/vertical velocity in local/body frame, fps
87  self.mapping.add('v_wind_body_north', units='fps') # north velocity in local/body frame
88  self.mapping.add('v_wind_body_east', units='fps') # east velocity in local/body frame
89  self.mapping.add('v_wind_body_down', units='fps') # down/vertical velocity in local/body
90 
91  # Accelerations
92  self.mapping.add('A_X_pilot', units='fpss') # X accel in body frame ft/sec^2
93  self.mapping.add('A_Y_pilot', units='fpss') # Y accel in body frame ft/sec^2
94  self.mapping.add('A_Z_pilot', units='fpss') # Z accel in body frame ft/sec^2
95 
96  # Stall
97  self.mapping.add('stall_warning') # 0.0 - 1.0 indicating the amount of stall
98  self.mapping.add('slip_deg', units='degrees') # slip ball deflection
99 
100  # Engine status
101  self.mapping.add('num_engines') # Number of valid engines
102  self.mapping.add('eng_state', self.FG_MAX_ENGINES) # Engine state (off, cranking, running)
103  self.mapping.add('rpm', self.FG_MAX_ENGINES) # Engine RPM rev/min
104  self.mapping.add('fuel_flow', self.FG_MAX_ENGINES) # Fuel flow gallons/hr
105  self.mapping.add('fuel_px', self.FG_MAX_ENGINES) # Fuel pressure psi
106  self.mapping.add('egt', self.FG_MAX_ENGINES) # Exhuast gas temp deg F
107  self.mapping.add('cht', self.FG_MAX_ENGINES) # Cylinder head temp deg F
108  self.mapping.add('mp_osi', self.FG_MAX_ENGINES) # Manifold pressure
109  self.mapping.add('tit', self.FG_MAX_ENGINES) # Turbine Inlet Temperature
110  self.mapping.add('oil_temp', self.FG_MAX_ENGINES) # Oil temp deg F
111  self.mapping.add('oil_px', self.FG_MAX_ENGINES) # Oil pressure psi
112 
113  # Consumables
114  self.mapping.add('num_tanks') # Max number of fuel tanks
115  self.mapping.add('fuel_quantity', self.FG_MAX_TANKS)
116 
117  # Gear status
118  self.mapping.add('num_wheels')
119  self.mapping.add('wow', self.FG_MAX_WHEELS)
120  self.mapping.add('gear_pos', self.FG_MAX_WHEELS)
121  self.mapping.add('gear_steer', self.FG_MAX_WHEELS)
122  self.mapping.add('gear_compression', self.FG_MAX_WHEELS)
123 
124  # Environment
125  self.mapping.add('cur_time', units='seconds') # current unix time
126  self.mapping.add('warp', units='seconds') # offset in seconds to unix time
127  self.mapping.add('visibility', units='meters') # visibility in meters (for env. effects)
128 
129  # Control surface positions (normalized values)
130  self.mapping.add('elevator')
131  self.mapping.add('elevator_trim_tab')
132  self.mapping.add('left_flap')
133  self.mapping.add('right_flap')
134  self.mapping.add('left_aileron')
135  self.mapping.add('right_aileron')
136  self.mapping.add('rudder')
137  self.mapping.add('nose_wheel')
138  self.mapping.add('speedbrake')
139  self.mapping.add('spoilers')
140 
141  self._packet_size = struct.calcsize(self.pack_string)
142 
143  self.set('version', self.FG_NET_FDM_VERSION)
144 
145  if len(self.values) != self.mapping._nextidx:
146  raise fgFDMError('Invalid variable list in initialisation')
147 
148  def packet_size(self):
149  '''return expected size of FG FDM packets'''
150  return self._packet_size
151 
152  def convert(self, value, fromunits, tounits):
153  '''convert a value from one set of units to another'''
154  if fromunits == tounits:
155  return value
156  if (fromunits,tounits) in self.unitmap:
157  return value * self.unitmap[(fromunits,tounits)]
158  if (tounits,fromunits) in self.unitmap:
159  return value / self.unitmap[(tounits,fromunits)]
160  raise fgFDMError("unknown unit mapping (%s,%s)" % (fromunits, tounits))
161 
162 
163  def units(self, varname):
164  '''return the default units of a variable'''
165  if not varname in self.mapping.vars:
166  raise fgFDMError('Unknown variable %s' % varname)
167  return self.mapping.vars[varname].units
168 
169 
170  def variables(self):
171  '''return a list of available variables'''
172  return sorted(list(self.mapping.vars.keys()),
173  key = lambda v : self.mapping.vars[v].index)
174 
175 
176  def get(self, varname, idx=0, units=None):
177  '''get a variable value'''
178  if not varname in self.mapping.vars:
179  raise fgFDMError('Unknown variable %s' % varname)
180  if idx >= self.mapping.vars[varname].arraylength:
181  raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % (
182  varname, idx, self.mapping.vars[varname].arraylength))
183  value = self.values[self.mapping.vars[varname].index + idx]
184  if units:
185  value = self.convert(value, self.mapping.vars[varname].units, units)
186  return value
187 
188  def set(self, varname, value, idx=0, units=None):
189  '''set a variable value'''
190  if not varname in self.mapping.vars:
191  raise fgFDMError('Unknown variable %s' % varname)
192  if idx >= self.mapping.vars[varname].arraylength:
193  raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % (
194  varname, idx, self.mapping.vars[varname].arraylength))
195  if units:
196  value = self.convert(value, units, self.mapping.vars[varname].units)
197  # avoid range errors when packing into 4 byte floats
198  if math.isinf(value) or math.isnan(value) or math.fabs(value) > 3.4e38:
199  value = 0
200  self.values[self.mapping.vars[varname].index + idx] = value
201 
202  def parse(self, buf):
203  '''parse a FD FDM buffer'''
204  try:
205  t = struct.unpack(self.pack_string, buf)
206  except struct.error as msg:
207  raise fgFDMError('unable to parse - %s' % msg)
208  self.values = list(t)
209 
210  def pack(self):
211  '''pack a FD FDM buffer from current values'''
212  for i in range(len(self.values)):
213  if math.isnan(self.values[i]):
214  self.values[i] = 0
215  return struct.pack(self.pack_string, *self.values)

mavlink
Author(s): Lorenz Meier
autogenerated on Sun Apr 7 2019 02:06:02