fgFDM.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # parse and construct FlightGear NET FDM packets
00003 # Andrew Tridgell, November 2011
00004 # released under GNU GPL version 2 or later
00005 
00006 import struct, math
00007 
00008 class fgFDMError(Exception):
00009     '''fgFDM error class'''
00010     def __init__(self, msg):
00011         Exception.__init__(self, msg)
00012         self.message = 'fgFDMError: ' + msg
00013 
00014 class fgFDMVariable(object):
00015     '''represent a single fgFDM variable'''
00016     def __init__(self, index, arraylength, units):
00017         self.index   = index
00018         self.arraylength = arraylength
00019         self.units = units
00020 
00021 class fgFDMVariableList(object):
00022     '''represent a list of fgFDM variable'''
00023     def __init__(self):
00024         self.vars = {}
00025         self._nextidx = 0
00026         
00027     def add(self, varname, arraylength=1, units=None):
00028         self.vars[varname] = fgFDMVariable(self._nextidx, arraylength, units=units)
00029         self._nextidx += arraylength
00030 
00031 class fgFDM(object):
00032     '''a flightgear native FDM parser/generator'''
00033     def __init__(self):
00034         '''init a fgFDM object'''
00035         self.FG_NET_FDM_VERSION = 24
00036         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'
00037         self.values = [0]*98
00038 
00039         self.FG_MAX_ENGINES = 4
00040         self.FG_MAX_WHEELS  = 3
00041         self.FG_MAX_TANKS   = 4
00042 
00043         # supported unit mappings
00044         self.unitmap = {
00045             ('radians', 'degrees') : math.degrees(1),
00046             ('rps',     'dps')     : math.degrees(1),
00047             ('feet',    'meters')  : 0.3048,
00048             ('fps',     'mps')     : 0.3048,
00049             ('knots',   'mps')     : 0.514444444,
00050             ('knots',   'fps')     : 0.514444444/0.3048,
00051             ('fpss',    'mpss')    : 0.3048,
00052             ('seconds', 'minutes') : 60,
00053             ('seconds', 'hours')   : 3600,
00054             }
00055 
00056         # build a mapping between variable name and index in the values array
00057         # note that the order of this initialisation is critical - it must
00058         # match the wire structure
00059         self.mapping = fgFDMVariableList()
00060         self.mapping.add('version')
00061 
00062         # position
00063         self.mapping.add('longitude', units='radians')  # geodetic (radians)
00064         self.mapping.add('latitude', units='radians')   # geodetic (radians)
00065         self.mapping.add('altitude', units='meters')    # above sea level (meters)
00066         self.mapping.add('agl', units='meters')         # above ground level (meters)
00067 
00068         # attitude
00069         self.mapping.add('phi', units='radians')        # roll (radians)
00070         self.mapping.add('theta', units='radians')      # pitch (radians)
00071         self.mapping.add('psi', units='radians')        # yaw or true heading (radians)
00072         self.mapping.add('alpha', units='radians')      # angle of attack (radians)
00073         self.mapping.add('beta', units='radians')       # side slip angle (radians)
00074 
00075         # Velocities
00076         self.mapping.add('phidot', units='rps')         # roll rate (radians/sec)
00077         self.mapping.add('thetadot', units='rps')       # pitch rate (radians/sec)
00078         self.mapping.add('psidot', units='rps')         # yaw rate (radians/sec)
00079         self.mapping.add('vcas', units='fps')           # calibrated airspeed
00080         self.mapping.add('climb_rate', units='fps')     # feet per second
00081         self.mapping.add('v_north', units='fps')        # north velocity in local/body frame, fps
00082         self.mapping.add('v_east', units='fps')         # east velocity in local/body frame, fps
00083         self.mapping.add('v_down', units='fps')         # down/vertical velocity in local/body frame, fps
00084         self.mapping.add('v_wind_body_north', units='fps')   # north velocity in local/body frame
00085         self.mapping.add('v_wind_body_east', units='fps')    # east velocity in local/body frame
00086         self.mapping.add('v_wind_body_down', units='fps')    # down/vertical velocity in local/body
00087 
00088         # Accelerations
00089         self.mapping.add('A_X_pilot', units='fpss')     # X accel in body frame ft/sec^2
00090         self.mapping.add('A_Y_pilot', units='fpss')     # Y accel in body frame ft/sec^2
00091         self.mapping.add('A_Z_pilot', units='fpss')     # Z accel in body frame ft/sec^2
00092 
00093         # Stall
00094         self.mapping.add('stall_warning')               # 0.0 - 1.0 indicating the amount of stall
00095         self.mapping.add('slip_deg', units='degrees')   # slip ball deflection
00096 
00097         # Engine status
00098         self.mapping.add('num_engines')                     # Number of valid engines
00099         self.mapping.add('eng_state', self.FG_MAX_ENGINES)  # Engine state (off, cranking, running)
00100         self.mapping.add('rpm',       self.FG_MAX_ENGINES)  # Engine RPM rev/min
00101         self.mapping.add('fuel_flow', self.FG_MAX_ENGINES)  # Fuel flow gallons/hr
00102         self.mapping.add('fuel_px',   self.FG_MAX_ENGINES)  # Fuel pressure psi
00103         self.mapping.add('egt',       self.FG_MAX_ENGINES)  # Exhuast gas temp deg F
00104         self.mapping.add('cht',       self.FG_MAX_ENGINES)  # Cylinder head temp deg F
00105         self.mapping.add('mp_osi',    self.FG_MAX_ENGINES)  # Manifold pressure
00106         self.mapping.add('tit',       self.FG_MAX_ENGINES)  # Turbine Inlet Temperature
00107         self.mapping.add('oil_temp',  self.FG_MAX_ENGINES)  # Oil temp deg F
00108         self.mapping.add('oil_px',    self.FG_MAX_ENGINES)  # Oil pressure psi
00109             
00110         # Consumables
00111         self.mapping.add('num_tanks')                       # Max number of fuel tanks
00112         self.mapping.add('fuel_quantity', self.FG_MAX_TANKS)
00113 
00114         # Gear status
00115         self.mapping.add('num_wheels')
00116         self.mapping.add('wow',              self.FG_MAX_WHEELS)
00117         self.mapping.add('gear_pos',         self.FG_MAX_WHEELS)
00118         self.mapping.add('gear_steer',       self.FG_MAX_WHEELS)
00119         self.mapping.add('gear_compression', self.FG_MAX_WHEELS)
00120 
00121         # Environment
00122         self.mapping.add('cur_time', units='seconds')       # current unix time
00123         self.mapping.add('warp',     units='seconds')       # offset in seconds to unix time
00124         self.mapping.add('visibility', units='meters')      # visibility in meters (for env. effects)
00125 
00126         # Control surface positions (normalized values)
00127         self.mapping.add('elevator')
00128         self.mapping.add('elevator_trim_tab')
00129         self.mapping.add('left_flap')
00130         self.mapping.add('right_flap')
00131         self.mapping.add('left_aileron')
00132         self.mapping.add('right_aileron')
00133         self.mapping.add('rudder')
00134         self.mapping.add('nose_wheel')
00135         self.mapping.add('speedbrake')
00136         self.mapping.add('spoilers')
00137 
00138         self._packet_size = struct.calcsize(self.pack_string)
00139 
00140         self.set('version', self.FG_NET_FDM_VERSION)
00141 
00142         if len(self.values) != self.mapping._nextidx:
00143             raise fgFDMError('Invalid variable list in initialisation')
00144 
00145     def packet_size(self):
00146         '''return expected size of FG FDM packets'''
00147         return self._packet_size
00148 
00149     def convert(self, value, fromunits, tounits):
00150         '''convert a value from one set of units to another'''
00151         if fromunits == tounits:
00152             return value
00153         if (fromunits,tounits) in self.unitmap:
00154             return value * self.unitmap[(fromunits,tounits)]
00155         if (tounits,fromunits) in self.unitmap:
00156             return value / self.unitmap[(tounits,fromunits)]
00157         raise fgFDMError("unknown unit mapping (%s,%s)" % (fromunits, tounits))
00158 
00159 
00160     def units(self, varname):
00161         '''return the default units of a variable'''
00162         if not varname in self.mapping.vars:
00163             raise fgFDMError('Unknown variable %s' % varname)
00164         return self.mapping.vars[varname].units
00165 
00166 
00167     def variables(self):
00168         '''return a list of available variables'''
00169         return sorted(list(self.mapping.vars.keys()),
00170                       key = lambda v : self.mapping.vars[v].index)
00171 
00172 
00173     def get(self, varname, idx=0, units=None):
00174         '''get a variable value'''
00175         if not varname in self.mapping.vars:
00176             raise fgFDMError('Unknown variable %s' % varname)
00177         if idx >= self.mapping.vars[varname].arraylength:
00178             raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % (
00179                 varname, idx, self.mapping.vars[varname].arraylength))
00180         value = self.values[self.mapping.vars[varname].index + idx]
00181         if units:
00182             value = self.convert(value, self.mapping.vars[varname].units, units)
00183         return value
00184 
00185     def set(self, varname, value, idx=0, units=None):
00186         '''set a variable value'''
00187         if not varname in self.mapping.vars:
00188             raise fgFDMError('Unknown variable %s' % varname)
00189         if idx >= self.mapping.vars[varname].arraylength:
00190             raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % (
00191                 varname, idx, self.mapping.vars[varname].arraylength))
00192         if units:
00193             value = self.convert(value, units, self.mapping.vars[varname].units)
00194         # avoid range errors when packing into 4 byte floats
00195         if math.isinf(value) or math.isnan(value) or math.fabs(value) > 3.4e38:
00196             value = 0
00197         self.values[self.mapping.vars[varname].index + idx] = value
00198 
00199     def parse(self, buf):
00200         '''parse a FD FDM buffer'''
00201         try:
00202             t = struct.unpack(self.pack_string, buf)
00203         except struct.error as msg:
00204             raise fgFDMError('unable to parse - %s' % msg)
00205         self.values = list(t)
00206 
00207     def pack(self):
00208         '''pack a FD FDM buffer from current values'''
00209         for i in range(len(self.values)):
00210             if math.isnan(self.values[i]):
00211                 self.values[i] = 0
00212         return struct.pack(self.pack_string, *self.values)


mavlink
Author(s): Lorenz Meier
autogenerated on Wed Sep 9 2015 18:06:17