home *** CD-ROM | disk | FTP | other *** search
/ Hackers Magazine 57 / CdHackersMagazineNr57.iso / Software / Multimedia / k3d-setup-0.7.11.0.exe / lib / site-packages / cgkit / fob.py < prev    next >
Encoding:
Python Source  |  2007-01-11  |  21.7 KB  |  714 lines
  1. # ***** BEGIN LICENSE BLOCK *****
  2. # Version: MPL 1.1/GPL 2.0/LGPL 2.1
  3. #
  4. # The contents of this file are subject to the Mozilla Public License Version
  5. # 1.1 (the "License"); you may not use this file except in compliance with
  6. # the License. You may obtain a copy of the License at
  7. # http://www.mozilla.org/MPL/
  8. #
  9. # Software distributed under the License is distributed on an "AS IS" basis,
  10. # WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License
  11. # for the specific language governing rights and limitations under the
  12. # License.
  13. #
  14. # The Original Code is the Python Computer Graphics Kit.
  15. #
  16. # The Initial Developer of the Original Code is Matthias Baas.
  17. # Portions created by the Initial Developer are Copyright (C) 2004
  18. # the Initial Developer. All Rights Reserved.
  19. #
  20. # Contributor(s):
  21. #
  22. # Alternatively, the contents of this file may be used under the terms of
  23. # either the GNU General Public License Version 2 or later (the "GPL"), or
  24. # the GNU Lesser General Public License Version 2.1 or later (the "LGPL"),
  25. # in which case the provisions of the GPL or the LGPL are applicable instead
  26. # of those above. If you wish to allow use of your version of this file only
  27. # under the terms of either the GPL or the LGPL, and not to allow others to
  28. # use your version of this file under the terms of the MPL, indicate your
  29. # decision by deleting the provisions above and replace them with the notice
  30. # and other provisions required by the GPL or the LGPL. If you do not delete
  31. # the provisions above, a recipient may use your version of this file under
  32. # the terms of any one of the MPL, the GPL or the LGPL.
  33. #
  34. # ***** END LICENSE BLOCK *****
  35. # $Id: fob.py,v 1.2 2005/04/29 15:10:22 mbaas Exp $
  36.  
  37.  
  38. import sys, string, time, struct
  39. from cgtypes import *
  40. from math import pi
  41. try:
  42.     import serial
  43.     serial_installed = True
  44. except:
  45.     serial_installed = False
  46.  
  47. # Commands
  48. ANGLES = 'W'
  49. ANGLE_ALIGN1 = 'J'
  50. ANGLE_ALIGN2 = 'q'
  51. BORESIGHT = 'u'
  52. BORESIGHT_REMOVE = 'v'
  53. BUTTON_MODE = 'M'
  54. BUTTON_READ = 'N'
  55. CHANGE_VALUE = 'P'
  56. EXAMINE_VALUE = 'O'
  57. FBB_RESET = '/'
  58. HEMISPHERE = 'L'
  59. MATRIX = 'X'
  60. METAL = 's'
  61. METAL_ERROR = 't'
  62. NEXT_TRANSMITTER = '0'
  63. OFFSET = 'K'
  64. POINT = 'B'
  65. POSITION = 'V'
  66. POSITION_ANGLES = 'Y'
  67. POSITION_MATRIX = 'Z'
  68. POSITION_QUATERNION = ']'
  69. QUATERNION = '\\'
  70. REFERENCE_FRAME1 = 'H'
  71. REFERENCE_FRAME2 = 'r'
  72. REPORT_RATE1 = 'Q'
  73. REPORT_RATE2 = 'R'
  74. REPORT_RATE8 = 'S'
  75. REPORT_RATE32 = 'T'
  76. RUN = 'F'
  77. SLEEP = 'G'
  78. STREAM = '@'
  79. STREAM_STOP = '?'
  80. SYNC = 'A'
  81. XOFF = 0x13
  82. XON = 0x11
  83.  
  84. # Change value/Examine Value parameters
  85. BIRD_STATUS = chr(0x00)
  86. SOFTWARE_REVISION_NUMBER = chr(0x01)
  87. BIRD_COMPUTER_CRYSTAL_SPEED = chr(0x02)
  88. POSITION_SCALING = chr(0x03)
  89. BIRD_MEASUREMENT_RATE = chr(0x07)
  90. SYSTEM_MODEL_IDENTIFICATION = chr(0x0f)
  91. FLOCK_SYSTEM_STATUS = chr(0x24)
  92. FBB_AUTO_CONFIGURATION = chr(0x32)
  93.  
  94. # Hemispheres
  95. FORWARD = chr(0) + chr(0)
  96. AFT =  chr(0) + chr(1)
  97. UPPER = chr(0xc) + chr(1)
  98. LOWER = chr(0xc) + chr(0)
  99. LEFT = chr(6) + chr(1)
  100. RIGHT = chr(6) + chr(0)
  101.  
  102. class DataError(Exception):
  103.     """Exception.
  104.  
  105.     This exception is thrown when the number of bytes returned from the bird
  106.     doesn't match what is expected."""
  107.     pass
  108.  
  109. def hexdump(data):
  110.     """data als Hex-Dump ausgeben."""
  111.     if len(data)>16:
  112.         hexdump(data[:16])
  113.         hexdump(data[16:])
  114.     else:
  115.         t = ""
  116.         for c in data:
  117.             s = hex(ord(c))[2:]
  118.             if len(s)==1:
  119.                 s="0"+s
  120.             print s,
  121.             if c in string.printable and c not in string.whitespace:
  122.                 t+=c
  123.             else:
  124.                 t+="."
  125.         print (48-3*len(data))*" ",
  126.         print t
  127.  
  128. class FOBRecord:
  129.     def __init__(self):
  130.         self.pos = (0.0, 0.0, 0.0)
  131.         self.angles = (0,0,0)
  132.         self.matrix = (0,0,0,0,0,0,0,0,0)
  133.         self.quat = (0,0,0,0)
  134.         self.metal = None
  135.  
  136. # BirdContext
  137. class BirdContext:
  138.     """Context class for one individual bird (sensor).
  139.  
  140.     This class stores the output mode in which a particular bird is
  141.     currently in.  This information is used to decode the data record
  142.     sent by the bird.
  143.     """
  144.     def __init__(self):
  145.  
  146.         # 144 for extended range transmitter (see p. 89 of the FOB manual)
  147.         # 2.54 to convert inches into cm
  148.         self.pos_scale = 144*2.54
  149.  
  150.         # Mode (ANGLE, POSITION, ...)
  151.         self.mode = POSITION_ANGLES
  152.         # Size of data records (default is POSITION/ANGLES)
  153.         self.record_size = 12
  154.         # Number of words in one record (record_size = 2*num_words [+ metal])
  155.         self.num_words = 6
  156.         self.scales = [self.pos_scale, self.pos_scale, self.pos_scale, 180, 180, 180]
  157.         self.metal_flag = False
  158.  
  159.     def angles(self, addr=None):
  160.         self.mode = ANGLES
  161.         self.num_words = 3
  162.         self.scales = [180,180,180]
  163.         self._calc_record_size()
  164.  
  165.     def position(self):
  166.         self.mode = POSITION
  167.         self.num_words = 3
  168.         self.scales = [self.pos_scale, self.pos_scale, self.pos_scale]
  169.         self._calc_record_size()
  170.  
  171.     def position_angles(self, addr=None):
  172.         """POSITION/ANGLES command."""
  173.         self.mode = POSITION_ANGLES
  174.         self.num_words = 6
  175.         self.scales = [self.pos_scale, self.pos_scale, self.pos_scale,180,180,180]
  176.         self._calc_record_size()
  177.  
  178.     def matrix(self, addr=None):
  179.         self.mode = MATRIX
  180.         self.num_words = 9
  181.         self.scales = [1,1,1,1,1,1,1,1,1]
  182.         self._calc_record_size()
  183.  
  184.     def position_matrix(self, addr=None):
  185.         """POSITION/MATRIX command."""
  186.         self.mode = POSITION_MATRIX
  187.         self.num_words = 12
  188.         self.scales = [self.pos_scale, self.pos_scale, self.pos_scale,1,1,1,1,1,1,1,1,1]
  189.         self._calc_record_size()
  190.  
  191.     def quaternion(self, addr=None):
  192.         """QUATERNION command."""
  193.         self.mode = QUATERNION
  194.         self.num_words = 4
  195.         self.scales = [1,1,1,1]
  196.         self._calc_record_size()
  197.  
  198.     def position_quaternion(self, addr=None):
  199.         """POSITION/QUATERNION command."""
  200.         self.mode = POSITION_QUATERNION
  201.         self.num_words = 7
  202.         self.scales = [self.pos_scale, self.pos_scale, self.pos_scale,1,1,1,1]
  203.         self._calc_record_size()
  204.  
  205.     def metal(self, flag):
  206.         self.metal_flag = flag
  207.         self._calc_record_size()
  208.  
  209.     def decode(self, s):
  210.         values = []
  211.         for i in range(self.num_words):
  212.             v = self.decodeWord(s[2*i:2*i+2])
  213.             v*=self.scales[i]
  214.             v=v/32768.0
  215.             values.append(v)
  216.  
  217. #        if self.metal_flag:
  218. #            print "Metal:",ord(s[-1])
  219.  
  220.         return values
  221.         
  222.  
  223.     def decodeWord(self, s):
  224.         """Decode the word in string s.
  225.  
  226.         s must contain exactly 2 bytes.
  227.         """
  228.  
  229.         ls = ord(s[0]) & 0x7f
  230.         ms = ord(s[1])
  231.         if ms&0x80==0x80:
  232.             print "MSB bit7 nicht 0!"
  233.         v = (ms<<9) | (ls<<2)
  234.         if v<0x8000:
  235.             return v
  236.         else:
  237.             return v-0x10000
  238.  
  239.     def _calc_record_size(self):
  240.         self.record_size = 2*self.num_words
  241. #        if self.metal_flag:
  242. #            self.record_size+=1
  243.         
  244.         
  245.  
  246. # FOB
  247. class FOB:
  248.     """Interface to the Ascension Flock Of Birds.
  249.  
  250.     This class can be used to communicate with the Ascension Flock
  251.     of Birds motion tracker (http://www.ascension-tech.com/).
  252.     The class directly accesses the serial port and has methods
  253.     that correspond to the commands described in the \em Flock \em of
  254.     \em Birds \em Installation \em and \em Operation \em Guide.
  255.  
  256.     Example usage:
  257.  
  258.     \code
  259.     fob = FOB()
  260.     # Open a connection on the serial port
  261.     fob.open()
  262.     # Initialize
  263.     fob.fbbAutoConfig(numbirds=4)
  264.     fob.fbbReset()
  265.     ...
  266.  
  267.     # Set output mode for bird 1 to "position"
  268.     fob.position(1)
  269.     
  270.     # Obtain a sensor value from bird 1
  271.     pos = fob.point(1)
  272.     ...
  273.     
  274.     fob.close()
  275.     \endcode
  276.     """
  277.     def __init__(self):
  278.         """Constructor."""
  279.         # Serial() instance
  280.         self.ser = None
  281.  
  282.         # Default-Context (Master)
  283.         self.defaultctx = BirdContext()
  284.         # A list of bird context classes (one per sensor)        
  285.         self.birds = []
  286.  
  287.     def __str__(self):
  288.         if self.ser==None:
  289.             serstr = "not connected"
  290.         else:
  291.             serstr = "port:%s, baudrate:%d"%(self.ser.portstr, self.ser.baudrate)
  292.         s = "Flock of Birds (%s):\n"%serstr
  293.         s += '  Device Description: "%s"\n'%self.examineValue(SYSTEM_MODEL_IDENTIFICATION)
  294.         status = self.examineValue(FLOCK_SYSTEM_STATUS)
  295.         for i in range(14):
  296.             s += "  Bird %2d: "%i
  297.             b = status[i]
  298.             if b&0x01:
  299.                 s += "ERT#0, "
  300.             if b&0x02:
  301.                 s += "ERT#1, "
  302.             if b&0x04:
  303.                 s += "ERT#2, "
  304.             if b&0x08:
  305.                 s += "ERT#3, "
  306.             if b&0x10:
  307.                 s += "Extended Range Controller, "
  308.             if b&0x80==0:
  309.                 s += "not "
  310.             s += "accessible, "
  311.             if b&0x40==0:
  312.                 s += "not "
  313.             s += "running, "
  314.             if b&0x20:
  315.                 s += "has a sensor"
  316.             else:
  317.                 s += "has no sensor"
  318.             if i<14:
  319.                 s += "\n"
  320.             
  321. #        c,dev,deps = self.examineValue(FBB_AUTO_CONFIGURATION)
  322. #        for i in range(14):
  323. #            s += "  Bird %2d: "%i
  324. #            if dev&(1<<i):
  325. #                s += "running, "
  326. #            else:
  327. #                s += "not running, "
  328. #            if deps&(1<<i):
  329. #                s += "dependent\n"
  330. #            else:
  331. #                s += "not dependent\n"
  332.         return s
  333.     
  334.     # open
  335.     def open(self, port=0, baudrate=115200, timeout=3):
  336.         """Open a connection to the flock.
  337.  
  338.         \param port (\c int) Port number (0,1,...)
  339.         \param baudrate (\c int) Baud rate
  340.         \param timeout (\c float) Time out value for RS232 operations
  341.         """
  342.         if not serial_installed:
  343.             raise RuntimeError, 'Cannot import the serial module (http://pyserial.sourceforge.net).'
  344.         
  345.         self.ser = serial.Serial(port=port,
  346.                                  baudrate=baudrate,
  347.                                  bytesize=serial.EIGHTBITS,
  348.                                  parity=serial.PARITY_NONE,
  349.                                  stopbits=serial.STOPBITS_ONE,
  350.                                  xonxoff=0,
  351.                                  rtscts=0,
  352.                                  timeout=timeout)
  353.         self.ser.setRTS(0)
  354.         ser = self.ser
  355. #        print "Port:",ser.portstr
  356. #        print "Baudrates:",ser.BAUDRATES
  357. #        print "Bytesizes:",ser.BYTESIZES
  358. #        print "Parities:",ser.PARITIES
  359. #        print "Stopbits:",ser.STOPBITS
  360. #        print "CTS:",ser.getCTS()
  361. #        print "DSR:",ser.getDSR()
  362. #        print "RI:",ser.getRI()
  363. #        print "CD:",ser.getCD()
  364. #        self.ser.setRTS(1)
  365. #        time.sleep(1.0)
  366. #        self.ser.setRTS(0)
  367.  
  368.     # close
  369.     def close(self):
  370.         if self.ser!=None:
  371.             self.ser.close()
  372.             self.ser = None
  373.  
  374.     def examineValue(self, param, addr=None):
  375.         """EXAMINE VALUE command.
  376.  
  377.         Here are the possible parameters and their return values
  378.  
  379.         - BIRD_STATUS: Integer (actually a word)
  380.         - SOFTWARE_REVISION_NUMBER: Tuple (int,fra). The version number is int.fra
  381.         - BIRD_COMPUTER_CRYSTAL_SPEED: Frequeny in MHz as an integer
  382.         - BIRD_MEASUREMENT_RATE: Measurement rate in cycles/sec (as float)
  383.         - SYSTEM_MODEL_IDENTIFICATION: Device description string
  384.         - FLOCK_SYSTEM_STATUS: 14-tuple with the status byte for each bird
  385.         - FBB_AUTO_CONFIGURATION: Tuple (configmode, devices, dependents)
  386.  
  387.         \param param Parameter number (there are symbolic constants)
  388.         \return Value
  389.         """
  390.         if addr!=None:
  391.             self.rs232ToFbb(addr)            
  392.         self._write(EXAMINE_VALUE+param)
  393.         # BIRD_STATUS
  394.         if param==BIRD_STATUS:
  395.             v = self._read(2, exc=True)
  396.             res = struct.unpack("<H", v)[0]
  397.         # SOFTWARE_REVISION_NUMBER
  398.         elif param==SOFTWARE_REVISION_NUMBER:
  399.             v = self._read(2, exc=True)
  400.             int = struct.unpack("B", v[1])[0]
  401.             fra = struct.unpack("B", v[0])[0]
  402.             res = (int, fra)
  403.         # BIRD_COMPUTER_CRYSTAL_SPEED
  404.         elif param==BIRD_COMPUTER_CRYSTAL_SPEED:
  405.             v = self._read(2, exc=True)
  406.             res = struct.unpack("B", v[0])[0]
  407.         # BIRD_MEASUREMENT_RATE
  408.         elif param==BIRD_MEASUREMENT_RATE:
  409.             v = self._read(2, exc=True)
  410.             rate = struct.unpack("<H", v)[0]
  411.             res = float(rate)/256.0
  412.         # SYSTEM_MODEL_IDENTIFICATION
  413.         elif param==SYSTEM_MODEL_IDENTIFICATION:
  414.             v = self._read(10, exc=True)
  415.             res = v.strip()
  416.         # FLOCK_SYSTEM_STATUS
  417.         elif param==FLOCK_SYSTEM_STATUS:
  418.             v = self._read(14, exc=True)
  419.             res = struct.unpack("BBBBBBBBBBBBBB", v)
  420.         # FBB AUTO-CONFIGURATION
  421.         elif param==FBB_AUTO_CONFIGURATION:
  422.             v = self._read(5, exc=True)
  423.             configmode = struct.unpack("B", v[0])[0]
  424.             dev = struct.unpack("<H", v[1:3])[0]
  425.             deps = struct.unpack("<H", v[3:])[0]
  426.             res = (configmode, dev, deps)
  427.         else:
  428.             raise ValueError, 'Unknown parameter: %s'%param
  429.  
  430.         return res
  431.  
  432.  
  433.     def fbbReset(self):
  434.         """FBB RESET command.
  435.  
  436.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  437.         """
  438.         self._write(FBB_RESET)
  439.  
  440.     def angles(self, addr=None):
  441.         """ANGLES command.
  442.  
  443.         \param addr Bird address (0,1,2...).
  444.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  445.         """
  446.         ctx = self._switch_mode(ANGLES, addr)
  447.         ctx.angles()
  448.  
  449.     def position(self, addr=None):
  450.         """POSITION command.
  451.  
  452.         \param addr Bird address (0,1,2...).
  453.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  454.         """
  455.         ctx = self._switch_mode(POSITION, addr)
  456.         ctx.position()
  457.  
  458.     def position_angles(self, addr=None):
  459.         """POSITION/ANGLES command.
  460.  
  461.         \param addr Bird address (0,1,2...).
  462.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  463.         """
  464.         ctx = self._switch_mode(POSITION_ANGLES, addr)
  465.         ctx.position_angles()
  466.  
  467.     def matrix(self, addr=None):
  468.         """MATRIX command.
  469.  
  470.         \param addr Bird address (0,1,2...).
  471.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  472.         """
  473.         ctx = self._switch_mode(MATRIX, addr)
  474.         ctx.matrix()
  475.  
  476.     def position_matrix(self, addr=None):
  477.         """POSITION/MATRIX command.
  478.  
  479.         \param addr Bird address (0,1,2...).
  480.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  481.         """
  482.         ctx = self._switch_mode(POSITION_MATRIX, addr)
  483.         ctx.position_matrix()
  484.  
  485.     def quaternion(self, addr=None):
  486.         """QUATERNION command.
  487.  
  488.         \param addr Bird address (0,1,2...).
  489.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  490.         """
  491.         ctx = self._switch_mode(QUATERNION, addr)
  492.         ctx.quaternion()
  493.  
  494.     def position_quaternion(self, addr=None):
  495.         """POSITION/QUATERNION command.
  496.  
  497.         \param addr Bird address (0,1,2...).
  498.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  499.         """
  500.         ctx = self._switch_mode(POSITION_QUATERNION, addr)
  501.         ctx.position_quaternion()        
  502.  
  503.     def point(self, addr=None):
  504.         """POINT command.
  505.  
  506.         \param addr Bird address (0,1,2...).
  507.         \return A list of sensor values (the number of values depends on the output mode).
  508.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  509.         """
  510.         if addr==None:
  511.             ctx = self.defaultctx
  512.         else:
  513.             self.rs232ToFbb(addr)
  514.             ctx = self.birds[addr]
  515.  
  516.         self._write(POINT)
  517.         while 1:
  518.             record = self._read()
  519.             if record=="":
  520.                 print "FOB: No feedback from the ERC"
  521.                 return
  522.             if ord(record[0])&0x80:
  523.                 break
  524.             print "FOB: Bit 7 not set"
  525.             
  526.         record += self._read(ctx.record_size-1)
  527.         if len(record)!=ctx.record_size:
  528.             print "Wrong number of bytes read (%d instead of %d)"%(len(record), ctx.record_size)
  529.             return None
  530. #        hexdump(res)
  531.  
  532.         res = ctx.decode(record)
  533. #        print res
  534.         return res
  535.  
  536.     def hemisphere(self, hemisphere, addr=None):
  537.         """HEMISPHERE command.
  538.  
  539.         \param hemisphere The hemisphere to use (one of FORWARD, AFT, UPPER, LOWER, LEFT, RIGHT)
  540.         \param addr Bird address (0,1,2...).
  541.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  542.         """
  543.         if addr!=None:
  544.             self.rs232ToFbb(addr)
  545.  
  546.         self._write(HEMISPHERE+hemisphere)            
  547.  
  548.     def run(self):
  549.         """RUN command.
  550.  
  551.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  552.         """
  553.         self._write(RUN)
  554.  
  555.     def sleep(self):
  556.         """SLEEP command.
  557.  
  558.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  559.         """
  560.         self._write(SLEEP)
  561.  
  562.     def metal(self, flag, data, addr=None):
  563.         """METAL command.
  564.  
  565.         \param flag (\c int) Flag (see Flock of Birds manual)
  566.         \param data (\c int) Data (see Flock of Birds manual)
  567.         \param addr Bird address (0,1,2...).
  568.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  569.         """
  570.         if addr==None:
  571.             ctx = self.defaultctx
  572.         else:
  573.             self.rs232ToFbb(addr)
  574.             ctx = self.birds[addr]
  575.         self._write(METAL+chr(flag)+chr(data))
  576.         if flag==0 and data==0:
  577.             ctx.metal(False)
  578.         else:
  579.             ctx.metal(True)
  580.  
  581.     def metal_error(self, addr=None):
  582.         """METAL ERROR command.
  583.  
  584.         \param addr Bird address (0,1,2...).
  585.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  586.         """
  587.         if addr==None:
  588.             ctx = self.defaultctx
  589.         else:
  590.             self.rs232ToFbb(addr)
  591.             ctx = self.birds[addr]
  592.  
  593.         self._write(METAL_ERROR)               
  594.         met = self._read(1)
  595.         if len(met)!=1:
  596.             print "Metal error byte nicht erhalten"
  597.             return
  598.         print "Metal error:",ord(met[0])
  599.         
  600.     def fbbAutoConfig(self, numbirds):
  601.         """FBB AUTO-CONFIGURATION command.
  602.  
  603.         Send the FBB AUTO-CONFIGURATION command (via CHANGE VALUE).
  604.  
  605.         \param numbirds (\c int) The number of birds to use (this is the parameter byte that the FBB AUTO-CONFIGURATION command expects)
  606.         \see Flock of Birds Installation and Operation Guide, chapter 7: \em RS232 \em Commands
  607.         \see Flock of Birds Installation and Operation Guide, chapter 8: \em Change \em Value / \em Examine \em Value
  608.         """
  609.         time.sleep(1.0)
  610.         self._write(CHANGE_VALUE+FBB_AUTO_CONFIGURATION+chr(numbirds))
  611.         time.sleep(1.0)
  612.         # First element is not used (addr 0 does not exist)
  613.         self.birds = [None]
  614.         for i in range(numbirds):
  615.             self.birds.append(BirdContext())
  616. #        res = self._read(2)
  617. #        print len(res)
  618.  
  619.     def rs232ToFbb(self, addr):
  620.         """Address a particular bird.
  621.  
  622.         Only for normal addressing mode!
  623.         This method has to be called before the actual command is issued.
  624.  
  625.         \param addr (\c int) Bird address (1-14)
  626.         """
  627.  
  628.         self._write(chr(0xf0+addr))
  629.  
  630.     def _switch_mode(self, mode, addr=None):
  631.         """Generic mode switch command.
  632.  
  633.         mode is one of ANGLES, POSITION, MATRIX, QUATERNION, POSITION_ANGLES,
  634.         POSITION_MATRIX, POSITION_QUATERNION.
  635.         addr is the bird addr or None.
  636.         The return value is the corresponding bird context. The calling
  637.         function must still call the appropriate mode switch method on the
  638.         context.
  639.  
  640.         \param mode Output mode
  641.         \param addr Bird address (0,1,2...)
  642.         \return A bird context class (BirdContext).
  643.         """
  644.         if addr==None:
  645.             ctx = self.defaultctx
  646.         else:
  647.             self.rs232ToFbb(addr)
  648.             ctx = self.birds[addr]
  649.         self._write(mode)
  650.         return ctx
  651.         
  652.     def _write(self, s):
  653.         """Send string s to the Bird."""
  654. #        print 70*"-"
  655. #        print "Host->Bird:"
  656. #        hexdump(s)
  657.         self.ser.write(s)
  658.  
  659.     def _read(self, size=1, exc=False):
  660.         """Receive size bytes of data from the bird.
  661.  
  662.         If the timeout value is exceeded an empty string is returned.
  663.  
  664.         \param size (\c int) Number of bytes to read.
  665.         \param exc (\c bool) If True an exception is thrown when the number of read bytes does not match
  666.         \return String containing the received bytes (or empty string).
  667.         """
  668.         v = self.ser.read(size)
  669.         if exc and len(v)!=size:
  670.             raise DataError, 'Expected %d bytes from the bird, but got %d'%(size, len(v))
  671.         return v
  672.  
  673. #    def _decode(self, s):
  674. #        values = []
  675. #        for i in range(self.num_words):
  676. #            v = self._decodeWord(s[2*i:2*i+2])
  677. #            v*=self.scales[i]
  678. #            v=v/32768.0
  679. #            values.append(v)
  680.  
  681. #        return values
  682.         
  683.  
  684. #    def _decodeWord(self, s):
  685. #        """Decode the word in string s.
  686.  
  687. #        s must contain exactly 2 bytes.
  688. #        """
  689.  
  690. #        ls = ord(s[0]) & 0x7f
  691. #        ms = ord(s[1])
  692. #        if ms&0x80==0x80:
  693. #            print "MSB bit7 nicht 0!"
  694. #        v = (ms<<9) | (ls<<2)
  695. #        if v<0x8000:
  696. #            return v
  697. #        else:
  698. #            return v-0x10000 
  699.         
  700.  
  701. #    def _readAll(self):
  702. #        buf = ""
  703. #        while 1:
  704. #            s = self._read()
  705. #            if s=="":
  706. #                break
  707. #            buf+=s
  708. #        print 70*"-"
  709. #        print "Bird->Host:"
  710. #        hexdump(buf)
  711.  
  712.     
  713.  
  714.