payloads.py
Go to the documentation of this file.
00001 #! /usr/bin/env python -m
00002 # -*- coding: utf-8 -*-
00003 #     _____
00004 #    /  _  \
00005 #   / _/ \  \
00006 #  / / \_/   \
00007 # /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00008 # \  / \_/ \  / /  _\| |  | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_   _|| | | |
00009 #  \ \_/ \_/ /  | |  | |  | └─┐| |_| || └┘ / | └┘_/| |_| |  | |  | └─┘ |
00010 #   \  \_/  /   | |_ | |_ | ┌─┘|  _  || |\ \ | |   |  _  |  | |  | ┌─┐ |
00011 #    \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00012 #            ROBOTICS™
00013 #
00014 #  File: payloads.py
00015 #  Desc: Horizon Protocol Message Definitions
00016 #  
00017 #  Copyright © 2010 Clearpath Robotics, Inc. 
00018 #  All Rights Reserved
00019 # 
00020 #  Redistribution and use in source and binary forms, with or without
00021 #  modification, are permitted provided that the following conditions are met:
00022 #      * Redistributions of source code must retain the above copyright
00023 #        notice, this list of conditions and the following disclaimer.
00024 #      * Redistributions in binary form must reproduce the above copyright
00025 #        notice, this list of conditions and the following disclaimer in the
00026 #        documentation and/or other materials provided with the distribution.
00027 #      * Neither the name of Clearpath Robotics, Inc. nor the
00028 #        names of its contributors may be used to endorse or promote products
00029 #        derived from this software without specific prior written permission.
00030 # 
00031 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00032 #  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00033 #  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
00034 #  ARE DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00035 #  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00036 #  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00037 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00038 #  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00039 #  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00040 #  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00041 #
00042 #  Please send comments, questions, or patches to code@clearpathrobotics.com
00043 #
00044 
00045 
00046 
00047 
00048 ################################################################################
00049 # Script
00050 
00051 
00052 
00053 # Check if run as a script
00054 if __name__ == "__main__":
00055     
00056     # Warn of Module ONLY status
00057     print ("ERROR: clearpath.horizon.payloads is a module and can NOT be run"\
00058            " as a script!\nFor a command-line interface demo of Horizon, run:"\
00059            "\n  python -m clearpath.horizon.demo\n"\
00060            "For Horizon message forwarding, run:\n"\
00061            "  python -m clearpath.horizon.forward")
00062 
00063     # Exit Error
00064     import sys
00065     sys.exit(1)
00066 
00067 
00068 
00069 
00070 ################################################################################
00071 # Module
00072 
00073 
00074 
00075 ## @package clearpath.horizon.payloads
00076 #  Horizon Protocol Message Payloads Python Module
00077 # 
00078 #  Horizon Protocol Message Payload Definitions                               \n
00079 #  Abstracted from knowing message codes and message header.                  \n
00080 #  Supported Horizon version(s): 0.1 - 1.0
00081 #
00082 #  @author     Ryan Gariepy
00083 #  @author     Malcolm Robert
00084 #  @author     Michael Purvis
00085 #  @date       25/01/10
00086 #  @req        clearpath.utils                                                \n
00087 #  @version    1.0
00088 #
00089 #  @section USE
00090 #
00091 #  The intended purpose of this module is to provide the various Payload
00092 #  definitions for the various messages within Horizon. The Payload class
00093 #  abstracts a payload and can represent a payload within a message without
00094 #  having any knowledge of the contained format whereas subclasses should know
00095 #  how payload data is formatted for one or more messages that they represent.
00096 #  These classes have no knowledge of message fields that do not fall within
00097 #  the payload field.
00098 #
00099 #  @section HISTORY
00100 #  Version 0.1 - 0.3 {Ryan Gariepy}
00101 #  - Initial Creation as protocol.py
00102 #
00103 #  Version 0.4 {Malcolm Robert}
00104 #  - Move to horizon_messages.py
00105 #  - Added manipulator payloads
00106 #  - Added safety system payloads
00107 #  - Added payload abstraction
00108 #  - Added logging
00109 #  - Added version support
00110 #  - Added Doxygen documentation
00111 #  - Changed version scheme to match Horizon doc
00112 #  - Horizon support for v0.4
00113 #
00114 #  Version 0.5
00115 #  - Added GPADC payload
00116 #  - Horizon support for v0.5
00117 #
00118 #  Version 0.6
00119 #  - Added content to platform info payload
00120 #  - Move to messages.py
00121 #
00122 #  Version 0.7
00123 #  - Added reset payload
00124 #  - Fixed number scales
00125 #  - Horizon support for v0.7
00126 #
00127 #  Version 0.8
00128 #  - Added power status payload
00129 #  - Added raw encoders payload
00130 #  - Horizon support for v0.8
00131 #
00132 #  Version 1.0
00133 #  - Move to payloads.py
00134 #  - Horizon support for v 0.1 - 1.0
00135 #  - Python 2.6+ & 3.x compatible
00136 #
00137 
00138 """Horizon Protocol Message Payload Definitions 
00139 
00140    Copyright © 2010 Clearpath Robotics, Inc.
00141    All rights reserved
00142    
00143    Created: 25/01/10
00144    Authors: Ryan Gariepy & Malcolm Robert
00145    Version: 1.0
00146    """
00147 
00148 
00149 # Required Clearpath Modules
00150 from .. import utils            # Clearpath Utilities
00151 
00152 # Required Python Modules
00153 import datetime                 # Date & Time Manipulation
00154 import logging                  # Logging Utilities
00155 import string
00156 import math                     # Math Constants and Functions
00157 
00158 
00159 # Module Support
00160 ## Module Version
00161 __version__  = "1.0"
00162 """Module Version"""
00163 ## SVN Code Revision
00164 __revision__ = "$Revision: 920 $"
00165 """ SVN Code Revision"""
00166 
00167 
00168 ## Message Log
00169 logger = logging.getLogger('clearpath.horizon.payloads')
00170 """Horizon Message Payloads Module Log"""
00171 logger.setLevel(logging.NOTSET)
00172 logger.addHandler(utils.NullLoggingHandler())
00173 logger.propagate = False
00174 logger.debug("Loading clearpath.horizon.payloads ...")  
00175 
00176 
00177 
00178 
00179 ################################################################################
00180 # Horizon Payload Superclass
00181     
00182     
00183     
00184 ## Horizon Payload
00185 #
00186 #  Represents the basic payload of a Horizon message.                         \n
00187 #  To be inherited for specific message payloads.
00188 #
00189 #  @warning Data should not be modified once created
00190 #
00191 #  @since 0.1
00192 #  
00193 #  @pydoc
00194 class Payload(object):
00195     """Horizon Protocol Message Payload"""
00196     
00197     ## Create A Horizon Message Payload
00198     #
00199     #  Constructor for the Horizon Payload class.                             \n
00200     #  Creates a basic message payload which is simply a byte list.           \n
00201     #                                                                         \n
00202     #  Subclass overrides should throw ValueError if invalid format and 
00203     #  LookupError if it has version detection problems (if supported).
00204     #
00205     #  @param  raw            Raw data buffer to parse
00206     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00207     #  @throws LookupError    If auto-detect version fails
00208     #  @throws ValueError     If values are out of range or if raw is invalid
00209     #
00210     #  @pydoc
00211     def __init__(self, raw = [], timestamp = 0):
00212         """Create A Horizon Message Payload"""
00213         
00214         # Class Variables
00215         self.data = []
00216         self.timestamp = timestamp
00217         
00218         # Doesn't actually do anything, just copies raw
00219         self.data = raw
00220         logger.debug("%s: Raw payload data: %s" % (self.__class__.__name__, 
00221                      str(self)))
00222         
00223         
00224     ## Hex String Representation
00225     #
00226     #  Return the entire payload in a string of hex characters.
00227     # 
00228     #  @return String of hex info
00229     #
00230     #  @pydoc
00231     def __str__(self):
00232         """Return the entire payload in a string of hex characters."""
00233         return ' '.join(map(utils.hex,self.data))
00234     
00235     
00236     ## Copy Instance
00237     #
00238     #  @return A deep copy of this object
00239     #
00240     #  @pydoc
00241     def copy(self):
00242         """Copy Instance"""
00243         
00244         # determine required data
00245         raw = None
00246         if self.data != None: raw = self.data[:]
00247         
00248         # create new copy
00249         logger.debug("%s: Creating copy." % self.__class__.__name__)
00250         return self.__class__(raw = raw, 
00251                               timestamp = self.timestamp)
00252     
00253     
00254     ## Human Readable Payload String
00255     #
00256     #  @return Human readable string representation of the payload
00257     #
00258     #  @pydoc
00259     def print_format(self):
00260         """Return the payload as a human readable string"""
00261         
00262         return "Payload: %s\n" % str(self)
00263     
00264     
00265     ## Raw Bytes Payload Representation
00266     #
00267     #  Convert the payload into raw bytes (character string for Python 2.x)
00268     #  useful for writing to devices.
00269     #
00270     #  @return raw bytes
00271     #
00272     #  @pydoc 
00273     def raw_string(self):
00274         """Returns the data converted into raw bytes."""
00275         
00276         return utils.to_bytes(self.data)
00277     
00278 
00279 
00280 
00281 ################################################################################
00282 # Horizon Payloads
00283     
00284     
00285    
00286 ## Horizon Message Payload - Null
00287 #
00288 #  Represents the null payload (payload of a message without a payload).
00289 #
00290 #  @warning Data should not be modified once created
00291 #
00292 #  @since 0.1
00293 #
00294 #  @pydoc
00295 class Null(Payload):
00296     """Horizon Message Payload - Null"""
00297     
00298     
00299     ## Create A Horizon Message Payload - Null 
00300     #
00301     #  Constructor for the Horizon Message Payload - Null  Class.             \n
00302     #  Version auto-detection is unsupported.
00303     #
00304     #  @param  raw            Raw Payload data byte list to parse
00305     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00306     #  @param  version        Horizon Protocol Version,
00307     #                         (-1,*) represents the newest version,
00308     #                         (0,0) auto-detect the version (if supported)
00309     #  @throws LookupError    If auto-detect version fails
00310     #  @throws ValueError     If values are out of range or if raw is invalid
00311     #
00312     #  @pydoc
00313     def __init__(self, raw = None, timestamp = 0):
00314         """Create A Horizon Message Payload - Null"""
00315         
00316         # Verify Length
00317         if raw == None: raw = []
00318         if len(raw) != 0:
00319             raise ValueError("Invalid length!")
00320                 
00321         # Pass on to super-class
00322         Payload.__init__(self, raw = raw, 
00323                          timestamp = timestamp)
00324     
00325     
00326     ## Human Readable Payload String
00327     def print_format(self):
00328         """Return the payload as a human readable string"""
00329         
00330         return "Payload: NULL"
00331 
00332 
00333 ## Horizon Message Payload - Acknowledgment 
00334 #
00335 #  Represents the payload of a standard acknowledgment message.
00336 #
00337 #  @warning Data should not be modified once created
00338 #
00339 #  @since 0.1
00340 #
00341 #  @pydoc
00342 class Ack(Payload):
00343     """Horizon Message Payload - Acknowledgment"""
00344     
00345     
00346     
00347     ## Create A Horizon Message Payload - Acknowledgment 
00348     #
00349     #  Constructor for the Horizon Message Payload - Acknowledgment  Class.   \n
00350     #  The constructor can be called two different ways:
00351     #  - Ack(bad_checksum,raw=None,...)                        \n
00352     #    Create an acknowledgment message payload to send.                    \n 
00353     #  - Ack(raw,timestamp)                                    \n
00354     #    Parse raw data (most likely received) into payload variables.        \n 
00355     #
00356     #  @param  bad_bandwidth  Not enough bandwidth
00357     #  @param  bad_checksum   Message had a bad checksum
00358     #  @param  bad_code       Message type is unsupported
00359     #  @param  bad_code_count Too many subscription types
00360     #  @param  bad_format     Message format is bad
00361     #  @param  bad_frequency  Subscription frequency is too high
00362     #  @param  bad_values     Message value(s) are out of range
00363     #  @param  raw            Raw Payload data byte list to parse
00364     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00365     #  @param  version        Horizon Protocol Version,
00366     #                         (-1,*) represents the newest version,
00367     #                         (0,0) auto-detect the version (if supported)
00368     #  @throws LookupError    If auto-detect version fails
00369     #  @throws ValueError     If values are out of range or if raw is invalid
00370     #
00371     #  @pydoc
00372     def __init__(self, bad_bandwidth = False, bad_checksum = False, 
00373                  bad_code = False, bad_code_count = False, bad_format = False, 
00374                  bad_frequency = False, bad_values = False, 
00375                  raw = None, timestamp = 0):
00376         """Create A Horizon Acknowledgment Message Payload"""
00377         
00378         # Class Variables
00379         self.bad_bandwidth = False
00380         self.bad_checksum = False
00381         self.bad_code = False
00382         self.bad_code_count = False
00383         self.bad_format = False
00384         self.bad_frequency = False
00385         self.bad_values = False
00386         
00387         # Create Constructor
00388         if raw == None:
00389             pass
00390         # Parse Constructor
00391         else:
00392             
00393             # Verify Length
00394             if len(raw) != 2:
00395                 raise ValueError("Invalid length!")
00396                 
00397             # Pass on to super-class
00398             Payload.__init__(self, raw = raw, timestamp = timestamp) 
00399             
00400             # Verify Unused Space
00401             if utils.to_unsigned_short(raw) & 0xFF80 != 0: 
00402                 raise ValueError("Invalid format!")
00403             
00404             # Extract checksum
00405             if utils.to_unsigned_short(raw) & 0x0001 > 0:
00406                 self.bad_checksum = True
00407             
00408             # Extract type
00409             if utils.to_unsigned_short(raw) & 0x0002 > 0:
00410                 self.bad_code = True
00411             
00412             # Extract format
00413             if utils.to_unsigned_short(raw) & 0x0004 > 0:
00414                 self.bad_format = True
00415             
00416             # Extract Range
00417             if utils.to_unsigned_short(raw) & 0x0008 > 0:
00418                 self.bad_values = True
00419             
00420             # Extract bandwidth
00421             if utils.to_unsigned_short(raw) & 0x0010 > 0:
00422                 self.bad_bandwidth = True
00423             
00424             # Extract frequency
00425             if utils.to_unsigned_short(raw) & 0x0020 > 0:
00426                 self.bad_frequency = True
00427             
00428             # Extract code count
00429             if utils.to_unsigned_short(raw) & 0x0040 > 0:
00430                 self.bad_code_count = True
00431     
00432    
00433     ## Human Readable Payload String
00434     def print_format(self):
00435         """Return the payload as a human readable string"""
00436         
00437         return "Bad Checksum: %d\nBad Code: %d\nBad Format: %d\n"\
00438                "Bad Values: %d\nBad Bandwidth: %d\nBad Frequency: %d\n"\
00439                "Bad Code Count: %d" % (self.bad_checksum, self.bad_code, 
00440                 self.bad_format, self.bad_values, self.bad_bandwidth, self.bad_frequency, 
00441                 self.bad_code_count)
00442                 
00443 
00444 ## Horizon Message Payload - Request Subscription
00445 #
00446 #  Represents the payload of a common request message
00447 #
00448 #  @warning Data should not be modified once created
00449 #
00450 #  @since 0.1
00451 #
00452 #  @section Subscriptions
00453 #  @copydoc subscriptions
00454 #
00455 #  @section data Request Data
00456 #  @copydoc request
00457 #
00458 #  @pydoc
00459 class Subscribe(Payload):
00460     """Horizon Message Payload - Subscribe"""
00461     
00462     
00463     ## Create A Horizon Message Payload - Request
00464     #
00465     #  Constructor for the Horizon Message Payload - Request Class.           \n
00466     #  The constructor can be called two different ways:
00467     #  - Request(subscription, raw=None, version, timestamp)   \n
00468     #    Create a request message payload to send.                            \n 
00469     #  - Request(raw, version, timestamp)                      \n
00470     #    Parse raw data (most likely received) into payload variables.        \n 
00471     #
00472     #  @param  raw            Raw Payload data byte list to parse
00473     #  @param  subscription   Subscription Frequency in Hz
00474     #                         0 - immediate, 0xFFFF - off
00475     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00476     #  @param  verify         Verify the length? (useful for subclasses)
00477     #  @param  version        Horizon Protocol Version,
00478     #                         (-1,*) represents the newest version,
00479     #                         (0,0) auto-detect the version (if supported)
00480     #  @throws LookupError    If auto-detect version fails
00481     #  @throws ValueError     If values are out of range or if raw is invalid
00482     #
00483     #  @pydoc
00484     def __init__(self, raw = None, subscription = 0, 
00485                  timestamp = 0, verify = True):
00486         """Create A Horizon Message Payload - Request"""
00487         
00488         # Class Variables
00489         ## Subscription Frequency
00490         self.subscription = 0
00491 
00492         # Create Constructor
00493         if raw == None:
00494             data = []
00495             
00496             # test subscription
00497             if subscription < 0 or subscription > 0x0FFFF:
00498                 raise ValueError("Invalid subscription!")
00499 
00500             self.subscription = subscription
00501             data = utils.from_unsigned_short(subscription)
00502             
00503             # Pass on to super-class
00504             Payload.__init__(self, raw = data, timestamp = timestamp)
00505         else:
00506             # Not a parseable message -- outbound-only
00507             Payload.__init__(self, raw = raw, timestamp = timestamp)
00508     ## Human Readable Payload String
00509     def print_format(self):
00510         """Return the payload as a human readable string"""
00511         
00512         return "Subscription: %s" % self.subscription
00513                 
00514 
00515 class Echo(Null):
00516     """Horizon Message Payload - Echo"""
00517     
00518     
00519     ## Create A Horizon Message Payload - Echo 
00520     #
00521     #  Constructor for the Horizon Message Payload - Echo  Class.             \n
00522     #  Version auto-detection is unsupported.
00523     #
00524     #  @see Null.__init__
00525     #
00526     #  @pydoc
00527     def __init__(self, raw = None, timestamp= 0):
00528         """Create A Horizon Message Payload - Null"""
00529         
00530         # Pass on to super-class
00531         Null.__init__(self, raw = raw,   timestamp = timestamp)
00532     
00533     
00534     ## Human Readable Payload String
00535     def print_format(self):
00536         """Return the payload as a human readable string"""
00537         
00538         return "Echo"
00539 
00540 
00541 class PlatformInfo(Payload):
00542     """Horizon Message Payload - Platform Information"""
00543     
00544     
00545     ## Create A Horizon Message Payload - Platform Information
00546     #
00547     #  Constructor for the Horizon Message Payload - Platform Information
00548     #  Class.                                                                 \n
00549     #  The constructor can be called two different ways:
00550     #  - PlatformInformation(model, raw=None, version, ...)    \n
00551     #    Create a command message payload to send.                            \n 
00552     #  - PlatformInformation(raw, version, timestamp)          \n
00553     #    Parse raw data (most likely received) into payload variables.        \n 
00554     #
00555     #  @param  model          The Platform Model
00556     #  @param  raw            Raw Payload data byte list to parse
00557     #  @param  revision       The Platform Model Revision Number
00558     #  @param  serial         The Platform Serial Number
00559     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00560     #  @param  version        Horizon Protocol Version,
00561     #                         (-1,*) represents the newest version,
00562     #                         (0,0) auto-detect the version (if supported)
00563     #  @throws LookupError    If auto-detect version fails
00564     #  @throws ValueError     If values are out of range or if raw is invalid
00565     #
00566     #  @pydoc
00567     def __init__(self, passcode = 0, model = '', revision = 0, serial = 0, 
00568                  raw = None, timestamp = 0):
00569         """Create A Horizon Message Payload - Platform Information"""
00570         
00571         # Class Variables
00572         self.passcode = passcode
00573         self.model = ''
00574         self.revision = 0
00575         self.serial = 0x00000000
00576         
00577         # Create Constructor - assume this is a set_platform_info payload
00578         if raw == None:
00579             data = utils.from_unsigned_short(self.passcode)
00580             
00581             # test model
00582             if not all(ord(c) < 256 for c in model):
00583                 raise ValueError("Invalid ASCII model!")
00584 
00585             if (len(model) > 64 or len(model) < 2):
00586                 raise ValueError("Model must be 1-63 characters!")
00587             self.model = model
00588             data += utils.from_byte(len(model))
00589             data += utils.from_ascii(model)
00590             
00591             # test revision
00592             if revision < 0 or revision > 255:
00593                 raise ValueError("Revision must be 0-255!")
00594             self.revision = revision
00595             data += utils.from_byte(revision)
00596             
00597             # test serial
00598             if serial < 0 or serial > 0xFFFFFFFF:
00599                 raise ValueError("Serial must be 0-4294967295!")
00600 
00601             self.serial = serial
00602             data += utils.from_unsigned_int(serial)
00603             
00604             # Pass on to super-class
00605             Payload.__init__(self, raw = data, 
00606                              timestamp = timestamp)
00607         
00608         # Parse Constructor
00609         else:
00610             # Pass on to super-class
00611             Payload.__init__(self, raw = raw,
00612                                                           timestamp = timestamp)            
00613             # Extract Model
00614             self.model = utils.to_ascii(raw[1:-5])
00615             logger.debug("%s model: %s" % (self.__class__.__name__, self.model))
00616                 
00617             # Extract Revision
00618             self.revision = utils.to_byte(raw[-5:-4])
00619             logger.debug("%s revision: %d" % (self.__class__.__name__, self.revision))
00620             
00621             # Extract Serial
00622             self.serial = utils.to_unsigned_int(raw[-4:])
00623             logger.debug("%s serial: %d" % (self.__class__.__name__, self.serial))
00624     
00625     ## Human Readable Payload String
00626     def print_format(self):
00627         """Return the payload as a human readable string"""
00628         
00629         return "Platform Model: %s\nPlatform Model Revision: %d\n"\
00630                "Platform Serial Number: %08X" % (self.model,
00631                                                  self.revision,
00632                                                  self.serial)
00633     
00634     
00635 class PlatformName(Payload):
00636     """Horizon Message Payload - Platform Name"""
00637     
00638     
00639     ## Create A Horizon Message Payload - Platform Name
00640     #
00641     #  Constructor for the Horizon Message Payload - Platform Name Class.     \n
00642     #  The constructor can be called two different ways:
00643     #  - PlatformName(name, raw=None, version, timestamp)      \n
00644     #    Create a command message payload to send.                            \n 
00645     #  - PlatformName(raw, version, timestamp)                 \n
00646     #    Parse raw data (most likely received) into payload variables.        \n 
00647     #
00648     #  @param  name           Platform Name
00649     #  @param  raw            Raw Payload data byte list to parse
00650     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00651     #  @param  version        Horizon Protocol Version,
00652     #                         (-1,*) represents the newest version,
00653     #                         (0,0) auto-detect the version (if supported)
00654     #  @throws LookupError    If auto-detect version fails
00655     #  @throws ValueError     If values are out of range or if raw is invalid
00656     #
00657     #  @pydoc
00658     def __init__(self, name = 'Clearpath1', raw = None, 
00659                  timestamp = 0):
00660         """Create A Horizon Message Payload - Platform Name"""
00661         
00662         # Class Variables
00663         ## Platform Name
00664         self.name = 'Clearpath1'
00665         
00666         # Create Constructor
00667         if raw == None:
00668             data = []
00669             
00670             # test name
00671             if not all(ord(c) < 256 for c in name):
00672                 raise ValueError("Invalid ASCII name!")
00673 
00674             if not (len(name) > 0 and ord(name[len(name)-1]) == 0):
00675                 name += '\0'
00676             if len(name) > 64 or len(name) < 2:
00677                 raise ValueError("Name must be 1-63 characters!")
00678 
00679             self.name = name
00680             data += utils.from_byte(len(name))
00681             data += utils.from_ascii(name)
00682             
00683             # Pass on to super-class
00684             Payload.__init__(self, raw = data,                                     timestamp = timestamp)
00685         
00686         # Parse Constructor
00687         else:
00688             
00689             # Verify Length
00690             if len(raw)-1 != raw[:1][0]:
00691                 raise ValueError("Name must be 1-63 characters!")
00692 
00693                 
00694             # Pass on to super-class
00695             Payload.__init__(self, raw = raw, timestamp = timestamp)
00696             
00697             # Extract Name
00698             self.name = utils.to_ascii(raw[1:])
00699             logger.debug("%s name: %s" % (self.__class__.__name__, 
00700                      self.name))
00701     
00702     
00703     ## Human Readable Payload String
00704     def print_format(self):
00705         """Return the payload as a human readable string"""
00706         
00707         return "Name: %s" % self.name
00708     
00709     
00710 ## Horizon Message Payload - Platform Time
00711 #
00712 #  Represents the payload of the command message 'platform time'
00713 #  @warning Data should not be modified once created
00714 #
00715 #  @since 0.3
00716 #
00717 #  @pydoc
00718 class PlatformTime(Payload):
00719     """Horizon Message Payload - Platform Time"""
00720 
00721     
00722     ## Create A Horizon Message Payload - Platform Time
00723     #
00724     #  Constructor for the Horizon Message Payload - Platform Time Class.     \n
00725     #  The constructor can be called two different ways:
00726     #  - PlatformTime(time, raw=None, version, timestamp)      \n
00727     #    Create a command message payload to send.                            \n 
00728     #  - PlatformTime(raw, version, timestamp)                 \n
00729     #    Parse raw data (most likely received) into payload variables.        \n 
00730     #
00731     #  @param  time           Platform Time (0-4294967295)
00732     #  @param  raw            Raw Payload data byte list to parse
00733     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00734     #  @param  version        Horizon Protocol Version,
00735     #                         (-1,*) represents the newest version,
00736     #                         (0,0) auto-detect the version (if supported)
00737     #  @throws LookupError    If auto-detect version fails
00738     #  @throws ValueError     If values are out of range or if raw is invalid
00739     #
00740     #  @pydoc
00741     def __init__(self, time = 0, raw = None, 
00742                  timestamp = 0):
00743         """Create A Horizon Message Payload - Platform Time"""
00744         
00745         # Class Variables
00746         ## Platform Time
00747         self.time = 0
00748         
00749         # Create Constructor
00750         if raw == None:
00751             data = []
00752             
00753             # test time
00754             if time < 0 or time > 4294967295:
00755                 raise ValueError("Time must be within 50 days!")
00756 
00757             self.time = time
00758             data += utils.from_unsigned_int(time)
00759             
00760             # Pass on to super-class
00761             Payload.__init__(self, raw = data, timestamp = timestamp)
00762         
00763         # Parse Constructor
00764         else:
00765             
00766             # Verify Length
00767             if len(raw) != 4:
00768                 raise ValueError("Bad length!")
00769 
00770                 
00771             # Pass on to super-class
00772             Payload.__init__(self, raw = raw, timestamp = timestamp)
00773             
00774             # Extract Time
00775             self.time = utils.to_unsigned_int(raw)
00776             logger.debug("%s time: %d" % (self.__class__.__name__, 
00777                      self.time))
00778     
00779     
00780     ## Human Readable Payload String
00781     def print_format(self):
00782         """Return the payload as a human readable string"""
00783         
00784         return "Time: %d" % self.time
00785  
00786 
00787 ## Horizon Message Payload - Firmware Information
00788 #
00789 #  Represents the payload of the data message 'firmware information'
00790 #  @warning Data should not be modified once created
00791 #
00792 #  @since 0.1
00793 #
00794 #  @pydoc
00795 class FirmwareInfo(Payload):
00796     """Horizon Message Payload - Firmare Information"""
00797     
00798     ## Create A Horizon Message Payload - Firmware Information
00799     #
00800     #  Constructor for the Horizon Message Payload - Firmware Information
00801     #  Class.                                                                 \n
00802     #  The constructor can be called two different ways:
00803     #  - FirmwareInformation(firmware, raw=None, version,...)  \n
00804     #    Create a command message payload to send.                            \n 
00805     #  - FirmwareInformation(raw, version, timestamp)          \n
00806     #    Parse raw data (most likely received) into payload variables.        \n 
00807     #    Version auto-detection is supported.
00808     #
00809     #  @param  firmware       Firmware version (major,minor)
00810     #  @param  raw            Raw Payload data byte list to parse
00811     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00812     #  @param  version        Horizon Protocol Version,
00813     #                         (-1,*) represents the newest version,
00814     #                         (0,0) auto-detect the version (if supported)
00815     #  @param  written        Time & Date written (year: 2000-2127)
00816     #  @throws LookupError    If auto-detect version fails
00817     #  @throws ValueError     If values are out of range or if raw is invalid
00818     #
00819     #  @pydoc
00820     def __init__(self, firmware = tuple([1,0]), raw = None, 
00821                  timestamp = 0, 
00822                  written = datetime.datetime(2000,1,1,0,0)):
00823         """Create A Horizon Message Payload - Firmware Information"""
00824 
00825         # Verify Length
00826         if len(raw) != 8:
00827             raise ValueError("Bad length!")
00828             
00829         # Extract / Verify Version
00830         v = tuple([utils.to_byte(raw[2:3]),
00831                         utils.to_byte(raw[3:4])])
00832         self.version = v
00833         logger.debug("%s version: %d.%d" % (self.__class__.__name__, 
00834                  self.version[0], self.version[1]))
00835             
00836         # Pass on to super-class
00837         Payload.__init__(self, raw = raw, 
00838                                                 timestamp = timestamp)
00839         
00840         # Extract Firmware Version
00841         self.firmware = tuple([utils.to_byte(raw[0:1]),
00842                                 utils.to_byte(raw[1:2])])
00843         logger.debug("%s firmware: %d.%d" % (self.__class__.__name__, 
00844                                              self.firmware[0], self.firmware[1]))
00845         
00846         # Extract Write Time
00847         time = utils.to_unsigned_int(raw[4:8])
00848         year = 2000 + ((time >> 21) & 0x7F)
00849         month = 1 + ((time >> 17) & 0x0F)
00850         day = 1 + ((time >> 11) & 0x3F)
00851         hour = ((time >> 6) & 0x1F)
00852         minute = ((time) & 0x3F)
00853 
00854         try:
00855             self.written = datetime.datetime(year,month,day,hour,minute)
00856         except ValueError as ex:
00857             raise ValueError(ex)
00858         logger.debug("%s write: %s" % (self.__class__.__name__, 
00859                                        self.written))
00860     
00861     
00862     ## Human Readable Payload String
00863     def print_format(self):
00864         """Return the payload as a human readable string"""
00865         
00866         return "Firmware: %d.%d\nProtocol: %d.%d\nWrite: %s" % (
00867                 self.firmware[0], self.firmware[1], self.version[0], 
00868                 self.version[1], self.written)
00869 
00870 
00871 ## Horizon Message Payload - System Status
00872 #
00873 #  Represents the payload of the data message 'system status'
00874 #  @warning Data should not be modified once created
00875 #
00876 #  @since 0.1
00877 #
00878 #  @pydoc
00879 class SystemStatus(Payload):
00880     """Horizon Message Payload - System Status"""
00881     
00882     
00883     ## Create A Horizon Message Payload - System Status
00884     #
00885     #  Constructor for the Horizon Message Payload - System Status Class.     \n
00886     #  The constructor can be called two different ways:
00887     #  - SystemStatus(uptime, voltage, raw=None, version,...)  \n
00888     #    Create a command message payload to send.                            \n 
00889     #  - SystemStatus(raw, version, timestamp)                 \n
00890     #    Parse raw data (most likely received) into payload variables.        \n 
00891     #
00892     #  @param  current        A list of currents [-320A,320A]
00893     #  @param  raw            Raw Payload data byte list to parse
00894     #  @param  temperature    A list of temperatures [-320 degC, 320 degC]
00895     #  @param  timestamp      Payload Send / Create Time (milliseconds)
00896     #  @param  uptime         System uptime ([0,4294967295] milliseconds)
00897     #  @param  version        Horizon Protocol Version,
00898     #                         (-1,*) represents the newest version,
00899     #                         (0,0) auto-detect the version (if supported)
00900     #  @param  voltage        A list of voltages [-320V,320V]
00901     #  @throws LookupError    If auto-detect version fails
00902     #  @throws ValueError     If raw is invalid
00903     #
00904     #  @pydoc
00905     def __init__(self, uptime = 0, voltage = [], current = [], temperature = [],
00906                  raw = None, timestamp = 0):
00907         """Create A Horizon Message Payload - System Status"""
00908         
00909         # Class Variables
00910         self.uptime = 0
00911         self.currents = []
00912         self.temperatures = []
00913         self.voltages = []
00914         
00915  
00916         # Parse Constructor
00917         if raw != None:
00918             # Verify Length
00919             length = 4;
00920             voltc = 0
00921             ampc = 0
00922             tempc = 0
00923             if len(raw) > length:
00924                 voltc = raw[4]
00925                 length += 1 + voltc*2;
00926                 if len(raw) > length:
00927                     ampc = raw[length]
00928                     length += 1 + ampc*2
00929                     if len(raw) > length:
00930                         tempc = raw[length]
00931                         length += 1 + tempc*2
00932                         if length != len(raw):
00933                             length = -1
00934                     else: length = -1
00935                 else: length = -1
00936             else: length = -1
00937             if length == -1:
00938                 raise ValueError("Measurement counts do not match raw data length!")
00939 
00940                     
00941             # Pass on to super-class
00942             Payload.__init__(self, raw = raw, timestamp = timestamp)
00943                 
00944             # Extract Uptime
00945             self.uptime = utils.to_unsigned_int(raw[:4])
00946             logger.debug("%s uptime: %d" % (self.__class__.__name__, 
00947                                             self.uptime))
00948                 
00949             # Extract Voltages
00950             voltc = raw[4]
00951             for i in range(0,voltc):
00952                 self.voltages.append(
00953                             utils.to_short(raw[5+i*2:i*2+7])/100.0)
00954             logger.debug("%s voltages: %s" % (self.__class__.__name__, 
00955                                               ' '.join(map(str,self.voltages))))
00956                 
00957             # Extract Currents
00958             ampc = raw[voltc*2+5]
00959             for i in range(0,ampc):
00960                 self.currents.append(utils.to_short(
00961                                         raw[voltc*2+i*2+6:i*2+8+voltc*2])/100.0)
00962             logger.debug("%s currents: %s" % (self.__class__.__name__, 
00963                                               ' '.join(map(str,self.currents))))
00964                 
00965             # Extract Temperatures
00966             tempc = raw[voltc*2+ampc*2+6]
00967             for i in range(0,tempc):
00968                 self.temperatures.append(utils.to_short(
00969                           raw[voltc*2+ampc*2+i*2+7:i*2+9+voltc*2+ampc*2])/100.0)
00970             logger.debug("%s temperatures: %s" % (self.__class__.__name__, 
00971                                         ' '.join(map(str,self.temperatures))))
00972     
00973     
00974     ## Human Readable Payload String
00975     def print_format(self):
00976         """Return the payload as a human readable string"""
00977         
00978         return "Uptime: %d\nVoltages: %s\nCurrents: %s\nTemperatures: %s" % (
00979                 self.uptime, 'V '.join(map(str,self.voltages)) + 'V', 
00980                 'A '.join(map(str,self.currents)) + 'A',
00981                 '℃ '.join(map(str,self.temperatures)) + ' degC')
00982 
00983 
00984 ## Horizon Message Payload - Power Status
00985 #
00986 #  Represents the payload of the data message 'power status'
00987 #  @warning Data should not be modified once created
00988 #
00989 #  @since 0.8
00990 #
00991 #  @pydoc
00992 class PowerStatus(Payload):
00993     """Horizon Message Payload - Power Status"""
00994  
00995     ## Create A Horizon Message Payload - Power Status
00996     #
00997     #  Constructor for the Horizon Message Payload - Power Status Class.      \n
00998     #  The constructor can be called two different ways:
00999     #  - SystemStatus(charges, raw=None, version,...)          \n
01000     #    Create a command message payload to send.                            \n 
01001     #  - SystemStatus(raw, version, timestamp)                 \n
01002     #    Parse raw data (most likely received) into payload variables.        \n 
01003     #
01004     #  @param  charges        List of battery percentages
01005     #  @param  capacities     List of battery capacities
01006     #  @param  descriptions   List of tuple([present,in_use,type])
01007     #  @param  raw            Raw Payload data byte list to parse
01008     #  @param  timestamp      Payload Send / Create Time (milliseconds)
01009     #  @param  version        Horizon Protocol Version,
01010     #                         (-1,*) represents the newest version,
01011     #                         (0,0) auto-detect the version (if supported)
01012     #  @throws LookupError    If auto-detect version fails
01013     #  @throws ValueError     If raw is invalid
01014     #
01015     #  @pydoc
01016     def __init__(self, charges = [], capacities = [], descriptions = [],
01017                  raw = None, timestamp = 0):
01018         """Create A Horizon Message Payload - Power Status"""
01019         
01020         # Class Variables
01021         ## Charge Measurements
01022         self.charges = []
01023         ## Capacity Measurements
01024         self.capacities = []
01025         ## Battery Descriptions
01026         self.descriptions = []
01027         
01028         # Create Constructor
01029         if raw == None:
01030             data = []
01031             
01032             # Verify Lengths
01033             if len(charges) != len(capacities) or \
01034                     len(capacities) != len(descriptions) or \
01035                     len(charges) < 1 or len(charges) > 255:
01036                 raise ValueError("Number of batteries must be [0,255]!")
01037 
01038             data = utils.from_byte([len(charges)])
01039             
01040             # Verify Charges
01041             for c in charges:
01042                 if c < 0 or c > 100:
01043                     raise ValueError("Charges must be [0,100]!")
01044                 data += utils.from_short(int(c*100))
01045             
01046             # Verify Capacities
01047             for c in capacities:
01048                 if c < 0 or c > 32000:
01049                     raise ValueError("Capacities must be [0,32000]!")
01050                 data += utils.from_short(int(c))
01051             
01052             # Verify Descriptions
01053             for d in descriptions:
01054                 if d[2] < 0 or (d[2] > 2 and d[2] != 8):
01055                     raise ValueError(
01056                                     "Description types must be [0|1|2|8]!")
01057                 desc = 0xCF
01058                 if d[0] == False:
01059                     desc = desc & 0x7F
01060                 if d[1] == False:
01061                     desc = desc & 0xBF
01062                 desc = desc & (0xFF | (0xFF & d[2]))
01063                 data += utils.from_byte([desc])
01064             
01065             # Pass on to super-class
01066             Payload.__init__(self, raw = data, timestamp = timestamp)
01067         
01068         # Parse Constructor
01069         else:
01070             
01071             # Verify Length
01072             if len(raw) != raw[0]*5 + 1:
01073                 raise ValueError("Measurement counts do not match raw data length!")
01074 
01075                     
01076             # Pass on to super-class
01077             Payload.__init__(self, raw = raw, timestamp = timestamp)
01078                 
01079             # Extract Charges
01080             for i in range(0,raw[0]):
01081                 self.charges.append(
01082                             utils.to_short(raw[1+i*2:i*2+3])/100.0)
01083             logger.debug("%s charges: %s" % (self.__class__.__name__, 
01084                                             '% '.join(map(str,self.charges))))
01085                 
01086             # Extract Capacities
01087             for i in range(0,raw[0]):
01088                 self.capacities.append(utils.to_short(
01089                                     raw[raw[0]*2+i*2+1:i*2+3+raw[0]*2]))
01090             logger.debug("%s capacities: %s" % (self.__class__.__name__, 
01091                                         ' '.join(map(str,self.capacities))))
01092                 
01093             # Extract Descriptions
01094             for i in range(0,raw[0]):
01095                 desc = utils.to_byte(raw[raw[0]*4+i+1:i+2+raw[0]*4])
01096                 self.descriptions.append(tuple([desc & 0x80 > 0, 
01097                                                  desc & 0x40 > 0,
01098                                                  desc & 0x0F]))
01099             logger.debug("%s descriptions: %s" % (self.__class__.__name__, 
01100                                         ' '.join(map(str,self.descriptions))))
01101     
01102     
01103     ## Human Readable Payload String
01104     def print_format(self):
01105         """Return the payload as a human readable string"""
01106         
01107         return "Charges: %s\nCapacities: %s\nDescriptions: %s" % (
01108                 '% '.join(map(str,self.charges)) + '%', 
01109                 'W-Hr '.join(map(str,self.capacities)) + 'W-Hr',
01110                 ' '.join(map(str,self.descriptions)))
01111               
01112 
01113 
01114 ## Horizon Message Payload - Processor Status
01115 #
01116 class ProcessorStatus(Payload):
01117     """Horizon Message Payload - Power Status"""
01118  
01119     def __init__(self, raw = None, timestamp = 0):
01120         """Create A Horizon Message Payload - Processor Status"""
01121         
01122         # Class Variables
01123         ## Charge Measurements
01124         self.errors = []
01125 
01126         # Create Constructor
01127         if raw != None:
01128 
01129             # Verify Length
01130             if len(raw) != raw[0] * 2 + 1:
01131                 raise ValueError("Bad length!")
01132                     
01133             # Pass on to super-class
01134             Payload.__init__(self, raw = raw, timestamp = timestamp)
01135                 
01136             # Extract Errors
01137             for i in range(0,raw[0]):
01138                 self.errors.append(
01139                     utils.to_short(raw[1+i*2:i*2+3]))
01140             logger.debug("%s errors: %s" % (self.__class__.__name__, 
01141                                             '  '.join(map(str, self.errors))))
01142     
01143     ## Human Readable Payload String
01144     def print_format(self):
01145         """Return the payload as a human readable string"""
01146         
01147         return "Errors: %s\n" % ' '.join(map(str, self.errors))
01148 
01149 
01150 ## Horizon Message Payload - Safety System
01151 #
01152 #  Represents the payload of the command and data messages 'safety system'
01153 #  @warning Data should not be modified once created
01154 #
01155 #  @since 0.4
01156 #
01157 #
01158 #  @pydoc
01159 class SafetyStatus(Payload):
01160     """Horizon Message Payload - Safety System"""
01161     
01162     # Class Constants
01163     ## Emergency Stop flag mask
01164     EMERGENCY_STOP = 0x0001
01165     
01166     
01167     ## Create A Horizon Message Payload - Safety System
01168     #
01169     #  Constructor for the Horizon Message Payload - Safety System Class.     \n
01170     #  The constructor can be called two different ways:
01171     #  - SafetySystem(flags, raw=None, version, timestamp)     \n
01172     #    Create a command message payload to send.                            \n 
01173     #  - SafetySystem(raw, version, timestamp)                 \n
01174     #    Parse raw data (most likely received) into payload variables.        \n 
01175     #
01176     #  @param  flags          Platform Safety System Flags
01177     #  @param  raw            Raw Payload data byte list to parse
01178     #  @param  timestamp      Payload Send / Create Time (milliseconds)
01179     #  @param  version        Horizon Protocol Version,
01180     #                         (-1,*) represents the newest version,
01181     #                         (0,0) auto-detect the version (if supported)
01182     #  @throws LookupError    If auto-detect version fails
01183     #  @throws ValueError     If values are out of range or if raw is invalid
01184     #
01185     #  @pydoc
01186     def __init__(self, flags = 0x0000, raw = None, 
01187                  timestamp = 0):
01188         """Create A Horizon Message Payload - Platform Name"""
01189         
01190         # Class Variables
01191         ## Platform Safety System Flags
01192         self._flags = 0x0000
01193         
01194         # Create Constructor
01195         if raw == None:
01196             data = []
01197             
01198             # test flags
01199             if flags < 0 or flags > 65535:
01200                 raise ValueError("Invalid flags!")
01201 
01202             self._flags = flags
01203             data = utils.from_unsigned_short(flags)
01204             
01205             # Pass on to super-class
01206             Payload.__init__(self, raw = data, timestamp = timestamp)
01207         
01208         # Parse Constructor
01209         else:
01210             
01211             # Verify Length
01212             if len(raw) != 2:
01213                 raise ValueError("Bad length!")
01214                 
01215             # Pass on to super-class
01216             Payload.__init__(self, raw = raw, timestamp = timestamp)
01217             
01218             # Extract Flags
01219             self._flags = utils.to_unsigned_short(raw)
01220             logger.debug("%s flags: 0x%04X" % (self.__class__.__name__, 
01221                      self._flags))
01222     
01223     
01224     ## Human Readable Payload String
01225     def print_format(self):
01226         """Return the payload as a human readable string"""
01227         
01228         return "Flags: 0x%04X" % self._flags
01229                 
01230                 
01231     ## Has Emergency Stop Set?
01232     #
01233     #  @return is the emergency stop platform safety system flag set
01234     #
01235     #  @pydoc
01236     def has_emergency_stop(self):
01237         """Has Emergency Stop Set?"""
01238         
01239         return (self._flags & self.EMERGENCY_STOP) == self.EMERGENCY_STOP
01240                 
01241                 
01242     ## Get Flags
01243     #
01244     #  @return the platform safety system flags
01245     #
01246     #  @pydoc
01247     def get_flags(self):
01248         """Get Flags"""
01249         
01250         return self._flags
01251     
01252     
01253     # Class Properties
01254     ## Platform Safety System Flags
01255     flags = property(fget=get_flags, doc="Platform Safety System Flags")
01256 
01257 
01258 
01259 ## Horizon Message Payload - Differential Speed
01260 #
01261 #  Represents the payload of the command and data messages 'differential speed'
01262 #  @warning Data should not be modified once created
01263 #
01264 #  @pydoc
01265 class DifferentialSpeed(Payload):
01266     """Horizon Message Payload - Differential Speed"""
01267     
01268     
01269     ## Create A Horizon Message Payload - Differential Speed
01270     #
01271     #  Constructor for the Horizon Message Payload - Differential Speed.      \n
01272     #  The constructor can be called two different ways:
01273     #  - DifferentialSpeed(l_speed, r_accel, raw=None,...)     \n
01274     #    Create a command message payload to send.                            \n 
01275     #  - DifferentialSpeed(raw, version, timestamp)            \n
01276     #    Parse raw data (most likely received) into payload variables.        \n 
01277     #
01278     #  @param  left_accel     Left Acceleration (m/s^2)
01279     #  @param  left_speed     Left Speed (m/s)
01280     #  @param  right_accel    Right Acceleration (m/s^2)
01281     #  @param  right_speed    Right Speed (m/s)
01282     #  @param  raw            Raw Payload data byte list to parse
01283     #  @param  timestamp      Payload Send / Create Time (milliseconds)
01284     #  @param  version        Horizon Protocol Version,
01285     #                         (-1,*) represents the newest version,
01286     #                         (0,0) auto-detect the version (if supported)
01287     #  @throws LookupError    If auto-detect version fails
01288     #  @throws ValueError     If values are out of range or if raw is invalid
01289     #
01290     #  @pydoc
01291     def __init__(self, left_speed = 0, right_speed = 0, left_accel = 0, right_accel = 0, 
01292                  raw = None, timestamp = 0):
01293         """Create A Horizon Message Payload - Differential Speed"""
01294         
01295         # Class Variables
01296         self.left_accel = 0
01297         self.left_speed = 0
01298         self.right_accel = 0
01299         self.right_speed = 0
01300         
01301         # Create Constructor
01302         if raw == None:
01303             data = []
01304             
01305             # test left speed
01306             if left_speed < -320 or left_speed > 320:
01307                 raise ValueError("Left Speed must be [-320,320]!")
01308 
01309             self.left_speed = left_speed
01310             data = utils.from_short(int(left_speed * 100))
01311             
01312             # test right speed
01313             if right_speed < -320 or right_speed > 320:
01314                 raise ValueError("Right Speed must be [-320,320]!")
01315 
01316             self.right_speed = right_speed
01317             data += utils.from_short(int(right_speed * 100))
01318             
01319             # test left acceleration
01320             if left_accel < 0 or left_accel > 320:
01321                 raise ValueError("Left Acceleration must be [0,320]!")
01322 
01323             self.left_accel = left_accel
01324             data += utils.from_short(int(left_accel * 100))
01325             
01326             # test right acceleration
01327             if right_accel < 0 or right_accel > 320:
01328                 raise ValueError("Right Acceleration must be [0,320]!")
01329 
01330             self.right_accel = right_accel
01331             data += utils.from_short(int(right_accel * 100))
01332             
01333             # Pass on to super-class
01334             Payload.__init__(self, raw = data, timestamp = timestamp)
01335         
01336         # Parse Constructor
01337         else:
01338             
01339             # Verify Length
01340             if len(raw) != 8:
01341                 raise ValueError( "Bad length!")
01342 
01343                 
01344             # Pass on to super-class
01345             Payload.__init__(self, raw = raw, timestamp = timestamp)
01346             
01347             # Extract Left Speed
01348             self.left_speed = utils.to_short(raw[0:2]) / 100.0
01349             logger.debug("%s left speed: %fm/s" % \
01350                          (self.__class__.__name__, self.left_speed))
01351             
01352             # Extract Right Speed
01353             self.right_speed = utils.to_short(raw[2:4]) / 100.0
01354             logger.debug("%s right speed: %fm/s" % \
01355                          (self.__class__.__name__, self.right_speed))
01356             
01357             # Extract Left Acceleration
01358             self.left_accel = utils.to_short(raw[4:6]) / 100.0
01359             logger.debug("%s left acceleration: %fm/s^2" % \
01360                          (self.__class__.__name__, self.left_accel))
01361             
01362             # Extract Right Acceleration
01363             self.right_accel = utils.to_short(raw[6:8]) / 100.0
01364             logger.debug("%s right acceleration: %fm/s^2" % \
01365                          (self.__class__.__name__, self.right_accel))
01366     
01367     
01368     ## Human Readable Payload String
01369     def print_format(self):
01370         """Return the payload as a human readable string"""
01371         
01372         return "Left Speed: %fm/s\nRight Speed: %fm/s\n"\
01373                "Left Acceleration: %fm/s^2\nRight Acceleration: %fm/s^2" % (
01374                     self.left_speed, self.right_speed, self.left_accel, self.right_accel)
01375 
01376 
01377 
01378     
01379     
01380 
01381 ## Horizon Message Payload - Differential Control
01382 #
01383 #  Represents the payload of the command and data messages 'differential 
01384 #  control'
01385 #  @warning Data should not be modified once created
01386 #
01387 #
01388 #  @pydoc
01389 class DifferentialControl(Payload):
01390     """Horizon Message Payload - Differential Control"""
01391     
01392     ## Create A Horizon Message Payload - Differential Control
01393     #
01394     #  Constructor for the Horizon Message Payload - Differential Control.    \n
01395     #  The constructor can be called two different ways:
01396     #  - DifferentialControl(l_p, l_d, r_i, raw=None,...)      \n
01397     #    Create a command message payload to send.                            \n 
01398     #  - DifferentialControl(raw, version, timestamp)          \n
01399     #    Parse raw data (most likely received) into payload variables.        \n 
01400     #
01401     #  @param  left_d            Left derivative constant
01402     #  @param  left_ffwd         Left ffwd-forward constant
01403     #  @param  left_i            Left integral constant
01404     #  @param  left_sat        Left integral sat
01405     #  @param  left_p            Left proportional constant
01406     #  @param  left_stic     Left stic compenstation
01407     #  @param  right_d            Right derivative constant
01408     #  @param  right_ffwd         Right ffwd-forward constant
01409     #  @param  right_i            Right integral constant
01410     #  @param  right_sat        Right integral sat
01411     #  @param  right_p            Right proportional constant
01412     #  @param  right_stic     Right stic compenstation
01413     #  @param  raw            Raw Payload data byte list to parse
01414     #  @param  timestamp      Payload Send / Create Time (milliseconds)
01415     #  @param  version        Horizon Protocol Version,
01416     #                         (-1,*) represents the newest version,
01417     #                         (0,0) auto-detect the version (if supported)
01418     #  @throws LookupError    If auto-detect version fails
01419     #  @throws ValueError     If values are out of range or if raw is invalid
01420     #
01421     #  @pydoc
01422     def __init__(self, left_p = 0.0, left_i = 0.0, left_d = 0.0, 
01423                  left_ffwd = 0.0, left_stic = 0.0, left_sat = 0.0, 
01424                  right_p = 0.0, right_i = 0.0, right_d = 0.0, right_ffwd = 0.0, 
01425                  right_stic = 0.0, right_sat = 0.0,
01426                  raw = None, timestamp = 0):
01427         """Create A Horizon Message Payload - Differential Control"""
01428         
01429         # Class Variables
01430         self.left_p = 0.0
01431         self.left_i = 0.0
01432         self.left_d = 0.0
01433         self.left_ffwd = 0.0
01434         self.left_stic = 0.0
01435         self.left_sat = 0.0
01436         self.right_p = 0.0
01437         self.right_i = 0.0
01438         self.right_d = 0.0
01439         self.right_ffwd = 0.0
01440         self.right_stic = 0.0
01441         self.right_sat = 0.0
01442 
01443         # Create Constructor
01444         if raw == None:
01445             data = []
01446             if left_p < -320 or left_p > 320:
01447                 raise ValueError("Left proportional constant must be [-320,320]!")
01448             self.left_p = left_p
01449             data = utils.from_short(int(left_p*100))
01450 
01451             if left_i < -320 or left_i > 320:
01452                 raise ValueError("Left integral constant must be [-320,320]!")
01453             self.left_i = left_i
01454             data += utils.from_short(int(left_i*100))
01455             
01456             if left_d < -320 or left_d > 320:
01457                 raise ValueError("Left derivative constant must be [-320,320]!")
01458             self.left_d = left_d
01459             data += utils.from_short(int(left_d*100))
01460             
01461             if left_ffwd < -320 or left_ffwd > 320:
01462                 raise ValueError("Left ffwd-forward constant must be [-320,320]!")
01463             self.left_f = left_ffwd
01464             data += utils.from_short(int(left_ffwd*100))
01465             
01466             if left_stic < 0 or left_stic > 100:
01467                 raise ValueError("Left stic compensation must be [0,100]!")
01468             self.left_s = left_stic
01469             data += utils.from_short(int(left_stic*100))
01470             
01471             if left_sat < 0 or left_sat > 100:
01472                 raise ValueError("Left integral sat must be [0,100]!")
01473             self.left_l = left_sat
01474             data += utils.from_short(int(left_sat*100))
01475             
01476             if right_p < -320 or right_p > 320:
01477                 raise ValueError("Right proportional constant must be [-320,320]!")
01478             self.right_p = right_p
01479             data += utils.from_short(int(right_p*100))
01480             
01481             if right_i < -320 or right_i > 320:
01482                 raise ValueError("Right integral constant must be [-320,320]!")
01483             self.right_i = right_i
01484             data += utils.from_short(int(right_i*100))
01485             
01486             if right_d < -320 or right_d > 320:
01487                 raise ValueError("Right derivative constant must be [-320,320]!")
01488             self.right_d = right_d
01489             data += utils.from_short(int(right_d*100))
01490             
01491             if right_ffwd < -320 or right_ffwd > 320:
01492                 raise ValueError("Right ffwd-forward constant must be [-320,320]!")
01493             self.right_ffwd = right_ffwd
01494             data += utils.from_short(int(right_ffwd*100))
01495             
01496             if right_stic < 0 or right_stic > 100:
01497                 raise ValueError("Right stic compensation must be [0,100]!")
01498             self.right_stic = right_stic
01499             data += utils.from_short(int(right_stic*100))
01500             
01501             if right_sat < 0 or right_sat > 100:
01502                 raise ValueError("Right integral sat must be [0,100]!")
01503             self.right_sat = right_sat
01504             data += utils.from_short(int(right_sat*100))
01505             
01506             # Pass on to super-class
01507             Payload.__init__(self, raw = data, timestamp = timestamp)
01508         
01509         # Parse Constructor
01510         else:
01511             
01512             # Verify Length
01513             if len(raw) != 24:
01514                 raise ValueError( "Bad length!")
01515 
01516             # Pass on to super-class
01517             Payload.__init__(self, raw = raw, timestamp = timestamp)
01518             self.left_p = utils.to_short(raw[0:2]) / 100.0
01519             self.left_i = utils.to_short(raw[2:4]) / 100.0
01520             self.left_d = utils.to_short(raw[4:6]) / 100.0
01521             self.left_ffwd = utils.to_short(raw[6:8]) / 100.0
01522             self.left_stic = utils.to_short(raw[8:10]) / 100.0
01523             self.left_sat = utils.to_short(raw[10:12]) / 100.0
01524             self.right_p = utils.to_short(raw[12:14]) / 100.0
01525             self.right_i = utils.to_short(raw[14:16]) / 100.0
01526             self.right_d = utils.to_short(raw[16:18]) / 100.0
01527             self.right_ffwd = utils.to_short(raw[18:20]) / 100.0
01528             self.right_stic = utils.to_short(raw[20:22]) / 100.0
01529             self.right_sat = utils.to_short(raw[22:24]) / 100.0
01530 
01531     
01532     ## Human Readable Payload String
01533     def print_format(self):
01534         """Return the payload as a human readable string"""
01535         
01536         return "Left P: %f\nLeft I: %f\nLeft D: %f\nLeft feed-forward: %f\n"\
01537                "Left stic compensation: %f\nLeft integral sat: %f\n"\
01538                "Right P: %f\nRight I: %f\nRight D: %f\nRight feed-forward: %f"\
01539                "\nRight stic compensation: %f\nRight integral sat: %f\n"\
01540                 % (
01541             self.left_p, self.left_i, self.left_d, self.left_ffwd, self.left_stic, self.left_sat,
01542             self.right_p, self.right_i, self.right_d, self.right_ffwd, self.right_stic, self.right_sat)
01543    
01544 
01545 class DifferentialCurrentControl(DifferentialControl):
01546     pass
01547 
01548 
01549 
01550 ## Horizon Message Payload - Differential Output
01551 #
01552 #  Represents the payload of the command and data messages 'differential motors'
01553 #  @warning Data should not be modified once created
01554 #
01555 #  @section data Differential Motors Data
01556 #  @copydoc differential_motors
01557 #
01558 #  @pydoc
01559 class DifferentialOutput(Payload):
01560     """Horizon Message Payload - Differential Motors"""
01561     
01562     
01563     ## Create A Horizon Message Payload - Differential Motors
01564     #
01565     #  Constructor for the Horizon Message Payload - Differential Speed.      \n
01566     #  The constructor can be called two different ways:
01567     #  - DifferentialMotors(left, right, raw=None,...)         \n
01568     #    Create a command message payload to send.                            \n 
01569     #  - DifferentialMotors(raw, version, timestamp)           \n
01570     #    Parse raw data (most likely received) into payload variables.        \n 
01571     #
01572     #  @param  left           Left Motor Output
01573     #  @param  raw            Raw Payload data byte list to parse
01574     #  @param  right          Right Motor Output
01575     #  @param  timestamp      Payload Send / Create Time (milliseconds)
01576     #  @param  version        Horizon Protocol Version,
01577     #                         (-1,*) represents the newest version,
01578     #                         (0,0) auto-detect the version (if supported)
01579     #  @throws LookupError    If auto-detect version fails
01580     #  @throws ValueError     If values are out of range or if raw is invalid
01581     #
01582     #  @pydoc
01583     def __init__(self, left = 0, right = 0, raw = None, 
01584                  timestamp = 0):
01585         """Create A Horizon Message Payload - Differential Motors"""
01586         
01587         # Class Variables
01588         self.left = 0
01589         self.right = 0
01590         
01591         # Create Constructor
01592         if raw == None:
01593             data = []
01594             
01595             # test left
01596             if left < -100 or left > 100:
01597                 raise ValueError("Left Motor must be [-100,100]!")
01598 
01599             self.left = left
01600             data = utils.from_short(int(left*100))
01601             
01602             # test right
01603             if right < -100 or right > 100:
01604                 raise ValueError("Right Motor must be [-100,100]!")
01605 
01606             self.right = right
01607             data += utils.from_short(int(right*100))
01608             
01609             # Pass on to super-class
01610             Payload.__init__(self, raw = data, timestamp = timestamp)
01611         
01612         # Parse Constructor
01613         else:
01614             
01615             # Verify Length
01616             if len(raw) != 4:
01617                 raise ValueError( "Bad length!")
01618 
01619                 
01620             # Pass on to super-class
01621             Payload.__init__(self, raw = raw, timestamp = timestamp)
01622             
01623             # Extract Left
01624             self.left = utils.to_short(raw[0:2])/100.0
01625             logger.debug("%s left motor: %f%%" % \
01626                          (self.__class__.__name__, self.left))
01627             
01628             # Extract Right
01629             self.right = utils.to_short(raw[2:4])/100.0
01630             logger.debug("%s right motor: %f%%" % \
01631                          (self.__class__.__name__, self.right))
01632     
01633     
01634     ## Human Readable Payload String
01635     def print_format(self):
01636         """Return the payload as a human readable string"""
01637         
01638         return "Left Motor: %f%%\nRight Motor: %f%%" % (self.left, self.right)
01639 
01640 
01641 class DifferentialCurrent(DifferentialOutput):
01642     pass
01643 
01644 ## Horizon Message Payload - Ackermann Output
01645 #
01646 #  Represents the payload of the command and data messages 'ackermann servos'
01647 #  @warning Data should not be modified once created
01648 #
01649 #  @since 0.1
01650 #
01651 #
01652 #  @pydoc
01653 class AckermannOutput(Payload):
01654     """Horizon Message Payload - Ackermann Servos"""
01655     
01656     
01657     ## Create A Horizon Message Payload - Ackermann Servos
01658     #
01659     #  Constructor for the Horizon Message Payload - Ackermann Servos.        \n
01660     #  The constructor can be called two different ways:
01661     #  - AckermannServos(steering, throttle, raw=None,...)     \n
01662     #    Create a command message payload to send.                            \n 
01663     #  - AckermannServos(raw, version, timestamp)              \n
01664     #    Parse raw data (most likely received) into payload variables.        \n 
01665     #
01666     #  @param  brake          The brake position
01667     #  @param  raw            Raw Payload data byte list to parse
01668     #  @param  steering       The steering position
01669     #  @param  throttle       The throttle position
01670     #  @param  timestamp      Payload Send / Create Time (milliseconds)
01671     #  @param  version        Horizon Protocol Version,
01672     #                         (-1,*) represents the newest version,
01673     #                         (0,0) auto-detect the version (if supported)
01674     #  @throws LookupError    If auto-detect version fails
01675     #  @throws ValueError     If values are out of range or if raw is invalid
01676     #
01677     #  @pydoc
01678     def __init__(self, steering = 0, throttle = 0, brake = 0, 
01679                  raw = None, timestamp = 0):
01680         """Create A Horizon Message Payload - Ackermann Servos"""
01681         
01682         # Class Variables
01683         ## Brake
01684         self.brake = 0
01685         ## Steering
01686         self.steering = 0
01687         ## Throttle
01688         self.throttle = 0
01689         
01690         # Create Constructor
01691         if raw == None:
01692             data = []
01693             
01694             # test steering
01695             if steering < -100 or steering > 100:
01696                 raise ValueError("Steering must be [-100,100]!")
01697 
01698             self.steering = steering
01699             data += utils.from_short(int(steering*100))
01700             
01701             # test throttle
01702             if throttle < -100 or throttle > 100:
01703                 raise ValueError("Throttle must be [-100,100]!")
01704 
01705             self.throttle = throttle
01706             data += utils.from_short(int(throttle*100))
01707             
01708             # test brake
01709             if brake < 0 or brake > 100:
01710                 raise ValueError("Brake must be [0,100]!")
01711 
01712             self.brake = brake
01713             data += utils.from_short(int(brake*100))
01714             
01715             # Pass on to super-class
01716             Payload.__init__(self, raw = data, timestamp = timestamp)
01717         
01718         # Parse Constructor
01719         else:
01720 
01721             # Verify Length
01722             if len(raw) != 6:
01723                 raise ValueError( "Bad length!")
01724 
01725             # Pass on to super-class
01726             Payload.__init__(self, raw = raw, timestamp = timestamp)
01727             
01728             # Extract Steering
01729             self.steering = utils.to_short(raw[0:2]) / 100.0
01730             logger.debug("%s steering: %f%%" % \
01731                          (self.__class__.__name__, self.steering))
01732             
01733             # Extract Throttle
01734             self.throttle = utils.to_short(raw[2:4]) / 100.0
01735             logger.debug("%s throttle: %f%%" % \
01736                          (self.__class__.__name__, self.throttle))
01737             
01738             # Extract Brake
01739             self.brake = utils.to_short(raw[4:6]) / 100.0
01740             logger.debug("%s brake: %f%%" % \
01741                          (self.__class__.__name__, self.brake))
01742     
01743     
01744     ## Human Readable Payload String
01745     def print_format(self):
01746         """Return the payload as a human readable string"""
01747         
01748         return "Steering: %f%%\nThrottle: %f%%\nBrake: %f%%" % (
01749                                     self.steering, self.throttle, self.brake)
01750 
01751 
01752 ## Horizon Message Payload - Velocity
01753 #
01754 #  Represents the payload of the command and data messages 'velocity'
01755 #  @warning Data should not be modified once created
01756 #
01757 #  @since 0.1
01758 #
01759 #
01760 #  @pydoc
01761 class Velocity(Payload):
01762     """Horizon Message Payload - Velocity"""
01763     
01764     
01765     ## Create A Horizon Message Payload - Velocity
01766     #
01767     #  Constructor for the Horizon Message Payload - Velocity.                \n
01768     #  The constructor can be called two different ways:
01769     #  - Velocity(trans, rot, accel, raw=None, ...)            \n
01770     #    Create a command message payload to send.                            \n 
01771     #  - Velocity(raw, version, timestamp)                     \n
01772     #    Parse raw data (most likely received) into payload variables.        \n 
01773     #
01774     #  @param  accel          The desired translational acceleration (m/s^2)
01775     #  @param  flags          0x02 - Automatic Transmission, 0x01 - Dynamic
01776     #                         Compensation, 0x03 - Both, 0x00 - None.
01777     #                         DEPRECATED: not in Horizon as of v0.4.
01778     #  @param  raw            Raw Payload data byte list to parse
01779     #  @param  rot            The desired rotational speed (rad/s)
01780     #  @param  timestamp      Payload Send / Create Time (milliseconds)
01781     #  @param  trans          The desired translational speed (m/s)
01782     #  @param  version        Horizon Protocol Version,
01783     #                         (-1,*) represents the newest version,
01784     #                         (0,0) auto-detect the version (if supported)
01785     #  @throws LookupError    If auto-detect version fails
01786     #  @throws ValueError     If values are out of range or if raw is invalid
01787     #
01788     #  @pydoc
01789     def __init__(self, trans = 0, rot = 0, accel = 0, 
01790                  raw = None, timestamp = 0):
01791         """Create A Horizon Message Payload - Velocity"""
01792         
01793         # Class Variables
01794         ## Translational Acceleration
01795         self.accel = 0
01796         ## Rotational speed
01797         self.rot = 0
01798         ## Translational speed
01799         self.trans = 0
01800         
01801         # Create Constructor
01802         if raw == None:
01803             data = []
01804             
01805             # test translation
01806             if trans < -320 or trans > 320:
01807                 raise ValueError("Translational Speed must be [-320,320]!")
01808 
01809             self.trans = trans
01810             data += utils.from_short(int(trans*100))
01811             
01812             # test rotation
01813             if rot < -320 or rot > 320:
01814                 raise ValueError("Rotational Speed must be [-320,320]!")
01815 
01816             self.rot = rot
01817             data += utils.from_short(int(rot*100))
01818             
01819             # test acceleration
01820             if accel < 0 or accel > 320:
01821                 raise ValueError("Translational Acceleration must be [0,320]!")
01822 
01823             self.accel = accel
01824             data += utils.from_short(int(accel*100))
01825             
01826             # Pass on to super-class
01827             Payload.__init__(self, raw = data, timestamp = timestamp)
01828         
01829         # Parse Constructor
01830         else:
01831             
01832             # Verify Length
01833             if (len(raw) != 6):
01834                 raise ValueError( "Bad length!")
01835 
01836                 
01837             # Pass on to super-class
01838             Payload.__init__(self, raw = raw, timestamp = timestamp)
01839             
01840             # Extract Translational
01841             self.trans = utils.to_short(raw[0:2]) / 100.0
01842             logger.debug("%s: translational speed: %fm/s" % \
01843                          (self.__class__.__name__, self.trans))
01844             
01845             # Extract Rotational
01846             self.rot = utils.to_short(raw[2:4]) / 100.0
01847             logger.debug("%s: rotational speed: %frad/s" % \
01848                          (self.__class__.__name__, self.rot))
01849             
01850             # Extract Acceleration
01851             self.accel = utils.to_short(raw[4:6]) / 100.0
01852             logger.debug("%s: translational acceleration: %fm/s^2" % \
01853                          (self.__class__.__name__, self.accel))
01854     
01855     
01856     ## Human Readable Payload String
01857     def print_format(self):
01858         """Return the payload as a human readable string"""
01859         
01860         return "Translational Speed: %fm/s\nRotational Speed: %frad/s\n"\
01861                "Translational Acceleration: %fm/s^2" % (self.trans, 
01862                                                         self.rot, self.accel)
01863 
01864 
01865 ## Horizon Message Payload - Turn
01866 #
01867 #  Represents the payload of the command and data messages 'turn'
01868 #  @warning Data should not be modified once created
01869 #
01870 #  @since 0.3
01871 #
01872 #
01873 #  @pydoc
01874 class Turn(Payload):
01875     """Horizon Message Payload - Turn"""
01876     
01877     
01878     ## Create A Horizon Message Payload - Turn
01879     #
01880     #  Constructor for the Horizon Message Payload - Turn.                    \n
01881     #  The constructor can be called two different ways:
01882     #  - Turn(trans, turn, accel, raw=None, ...)               \n
01883     #    Create a command message payload to send.                            \n 
01884     #  - Turn(raw, version, timestamp)                         \n
01885     #    Parse raw data (most likely received) into payload variables.        \n 
01886     #
01887     #  @param  accel          The desired translational acceleration (m/s^2)
01888     #  @param  flags          0x02 - Automatic Transmission, 0x01 - Dynamic
01889     #                         Compensation, 0x03 - Both, 0x00 - None.
01890     #                         DEPRECATED: not in Horizon as of v0.4.
01891     #  @param  raw            Raw Payload data byte list to parse
01892     #  @param  timestamp      Payload Send / Create Time (milliseconds)
01893     #  @param  trans          The desired translational speed (m/s)
01894     #  @param  turn           The desired turn radius
01895     #  @param  version        Horizon Protocol Version,
01896     #                         (-1,*) represents the newest version,
01897     #                         (0,0) auto-detect the version (if supported)
01898     #  @throws LookupError    If auto-detect version fails
01899     #  @throws ValueError     If values are out of range or if raw is invalid
01900     #
01901     #  @pydoc
01902     def __init__(self, trans = 0, rad = 0, accel = 0, 
01903                  raw = None, timestamp = 0):
01904         """Create A Horizon Message Payload - Turn"""
01905         
01906         # Class Variables
01907         self.accel = 0
01908         self.rad = 0
01909         self.trans = 0
01910         
01911         # Create Constructor
01912         if raw == None:
01913             data = []
01914             
01915             # test translation
01916             if trans < -320 or trans > 320:
01917                 raise ValueError("Translational Speed must be [-320,320]!")
01918 
01919             self.trans = trans
01920             data += utils.from_short(int(trans*100))
01921             
01922             # test turn
01923             if rad < -320 or rad > 320:
01924                 raise ValueError("Turn Radius must be [-320,320]!")
01925 
01926             self.rad = rad
01927             data += utils.from_short(int(rad*100))
01928             
01929             # test acceleration
01930             if accel < 0 or accel > 320:
01931                 raise ValueError("Translational Acceleration must be [0,320]!")
01932 
01933             self.accel = accel
01934             data += utils.from_short(int(accel*100))
01935             
01936             # Pass on to super-class
01937             Payload.__init__(self, raw=data, timestamp=timestamp)
01938         
01939         # Parse Constructor
01940         else:
01941             
01942             # Verify Length
01943             if (len(raw) != 6):
01944                 raise ValueError( "Bad length!")
01945 
01946                 
01947             # Pass on to super-class
01948             Payload.__init__(self, raw = raw, timestamp = timestamp)
01949             
01950             # Extract Translational
01951             self.trans = utils.to_short(raw[0:2]) / 100.0
01952             logger.debug("%s translational speed: %fm/s" % \
01953                          (self.__class__.__name__, self.trans))
01954             
01955             # Extract Turn Radius
01956             self.rad = utils.to_short(raw[2:4]) / 100.0
01957             logger.debug("%s turn radius: %fm" % \
01958                          (self.__class__.__name__, self.rad))
01959             
01960             # Extract Acceleration
01961             self.accel = utils.to_short(raw[4:6]) / 100.0
01962             logger.debug("%s translational acceleration: %fm/s^2" % \
01963                          (self.__class__.__name__, self.accel))
01964     
01965     
01966     
01967     ## Human Readable Payload String
01968     def print_format(self):
01969         """Return the payload as a human readable string"""
01970         
01971         return "Translational Speed: %fm/s\nTurn Radius: %fm\n"\
01972                "Translational Acceleration: %fm/s^2" % (self.trans, 
01973                                                         self.rad, self.accel)
01974 
01975 
01976 ## Horizon Message Payload - Max Speed
01977 #
01978 #  Represents the payload of the command and data messages 'max speed'
01979 #  @warning Data should not be modified once created
01980 #
01981 #  @since 0.1
01982 #
01983 #
01984 #  @pydoc
01985 class MaxSpeed(Payload):
01986     """Horizon Message Payload - Max Speed"""
01987     
01988     ## Create A Horizon Message Payload - Max Speed
01989     #
01990     #  Constructor for the Horizon Message Payload - Max Speed.               \n
01991     #  The constructor can be called two different ways:
01992     #  - MaxSpeed(forward, reverse, raw=None, ...)             \n
01993     #    Create a command message payload to send.                            \n 
01994     #  - MaxSpeed(raw, version, timestamp)                     \n
01995     #    Parse raw data (most likely received) into payload variables.        \n 
01996     #
01997     #  @param  forward        The maximum forward speed
01998     #  @param  raw            Raw Payload data byte list to parse
01999     #  @param  reverse        The maximum reverse speed
02000     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02001     #  @param  version        Horizon Protocol Version,
02002     #                         (-1,*) represents the newest version,
02003     #                         (0,0) auto-detect the version (if supported)
02004     #  @throws LookupError    If auto-detect version fails
02005     #  @throws ValueError     If values are out of range or if raw is invalid
02006     #
02007     #  @pydoc
02008     def __init__(self, forward = 0, reverse = 0, raw= None, 
02009                  timestamp = 0):
02010         """Create A Horizon Message Payload - Max Speed"""
02011         
02012         # Class Variables
02013         ## Max Forward Speed
02014         self.forward = 0
02015         ## Max Reverse Speed
02016         self.reverse = 0
02017         
02018         # Create Constructor
02019         if raw == None:
02020             data = []
02021             
02022             # test forward
02023             if forward < 0 or forward > 320:
02024                 raise ValueError("Forward Speed must be [0,320]!")
02025 
02026             self.forward = forward
02027             data = utils.from_short(int(forward * 100))
02028             
02029             # test reverse
02030             if reverse < 0 or reverse > 320:
02031                 raise ValueError("Reverse Speed must be [0,320]!")
02032 
02033             self.reverse = reverse
02034             data += utils.from_short(int(reverse * 100))
02035             
02036             # Pass on to super-class
02037             Payload.__init__(self, raw = data, timestamp = timestamp)
02038         
02039         # Parse Constructor
02040         else:
02041             
02042             # Verify Length
02043             if len(raw) != 4:
02044                 raise ValueError("Bad length!")
02045 
02046                 
02047             # Pass on to super-class
02048             Payload.__init__(self, raw = raw, timestamp = timestamp)
02049             
02050             # Extract Forward
02051             self.forward = utils.to_short(raw[0:2]) / 100.0
02052             logger.debug("%s forward speed: %fm/s" % \
02053                          (self.__class__.__name__, self.forward))
02054             
02055             # Extract Reverse
02056             self.reverse = utils.to_short(raw[2:4]) / 100.0
02057             logger.debug("%s reverse speed: %fm/s" % \
02058                          (self.__class__.__name__, self.reverse))
02059     
02060     
02061     
02062     ## Human Readable Payload String
02063     def print_format(self):
02064         """Return the payload as a human readable string"""
02065         
02066         return "Max Forward Speed: %fm/s\nMax Reverse Speed: %fm/s"\
02067                % (self.forward, self.reverse)
02068                 
02069 
02070 ## Horizon Message Payload - Max Acceleration
02071 #
02072 #  Represents the payload of the command and data messages 'max acceleration'
02073 #  @warning Data should not be modified once created
02074 #
02075 #  @since 0.1
02076 #
02077 #  @pydoc
02078 class MaxAccel(Payload):
02079     """Horizon Message Payload - Max Acceleration"""
02080     
02081     
02082     ## Create A Horizon Message Payload - Max Acceleration
02083     #
02084     #  Constructor for the Horizon Message Payload - Max Acceleration.        \n
02085     #  The constructor can be called two different ways:
02086     #  - MaxAcceleration(forward, reverse, raw=None, ...)      \n
02087     #    Create a command message payload to send.                            \n 
02088     #  - MaxAcceleration(raw, version, timestamp)              \n
02089     #    Parse raw data (most likely received) into payload variables.        \n 
02090     #
02091     #  @param  forward        The maximum forward acceleration
02092     #  @param  raw            Raw Payload data byte list to parse
02093     #  @param  reverse        The maximum reverse acceleration
02094     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02095     #  @param  version        Horizon Protocol Version,
02096     #                         (-1,*) represents the newest version,
02097     #                         (0,0) auto-detect the version (if supported)
02098     #  @throws LookupError    If auto-detect version fails
02099     #  @throws ValueError     If values are out of range or if raw is invalid
02100     #
02101     #  @pydoc
02102     def __init__(self, forward = 0, reverse = 0, raw= None, 
02103                  timestamp = 0):
02104         """Create A Horizon Message Payload - Max Acceleration"""
02105         
02106         # Class Variables
02107         self.forward = 0
02108         self.reverse = 0
02109         
02110         # Create Constructor
02111         if raw == None:
02112             data = []
02113             
02114             # test forward
02115             if forward < 0 or forward > 320:
02116                 raise ValueError("Forward Acceleration must be [0, 320]!")
02117 
02118             self.forward = forward
02119             data = utils.from_short(int(forward * 100))
02120             
02121             # test reverse
02122             if reverse < 0 or reverse > 320:
02123                 raise ValueError("Reverse Acceleration must be [0, 320]!")
02124 
02125             self.reverse = reverse
02126             data += utils.from_short(int(reverse * 100))
02127             
02128             # Pass on to super-class
02129             Payload.__init__(self, raw = data, 
02130                                                           timestamp = timestamp)
02131         
02132         # Parse Constructor
02133         else:
02134             
02135             # Verify Length
02136             if len(raw) != 4:
02137                 raise ValueError( "Bad length!")
02138 
02139                 
02140             # Pass on to super-class
02141             Payload.__init__(self, raw = raw, timestamp = timestamp)
02142             
02143             # Extract Forward
02144             self.forward = utils.to_short(raw[0:2]) / 100.0
02145             logger.debug("%s forward acceleration: %fm/s^2" % \
02146                          (self.__class__.__name__, self.forward))
02147             
02148             # Extract Reverse
02149             self.reverse = utils.to_short(raw[2:4]) / 100.0
02150             logger.debug("%s reverse acceleration: %fm/s^2" % \
02151                          (self.__class__.__name__, self.reverse))
02152     
02153     
02154     
02155     ## Human Readable Payload String
02156     def print_format(self):
02157         """Return the payload as a human readable string"""
02158         
02159         return "Max Forward Acceleration: %fm/s^2\n"\
02160                 "Max Reverse Acceleration: %fm/s^2"\
02161                % (self.forward, self.reverse)
02162 
02163 
02164 ## Horizon Message Payload - Gear
02165 #
02166 #  Represents the payload of the command message 'gear'
02167 #  @warning Data should not be modified once created
02168 #
02169 #  @since 0.1
02170 #
02171 #  @pydoc
02172 class Gear(Payload):
02173     """Horizon Message Payload - Gear"""
02174     
02175     
02176     
02177     ## Create A Horizon Message Payload - Gear
02178     #
02179     #  Constructor for the Horizon Message Payload - Gear.                    \n
02180     #  The constructor can be called two different ways:
02181     #  - Gear(gear, raw=None,...)                              \n
02182     #    Create a command message payload to send.                            \n 
02183     #  - Gear(raw, version, timestamp)                         \n
02184     #    Parse raw data (most likely received) into payload variables.        \n 
02185     #
02186     #  @param  gear           The desired gear [-128,127]
02187     #  @param  raw            Raw Payload data byte list to parse
02188     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02189     #  @param  version        Horizon Protocol Version,
02190     #                         (-1,*) represents the newest version,
02191     #                         (0,0) auto-detect the version (if supported)
02192     #  @throws LookupError    If auto-detect version fails
02193     #  @throws ValueError     If values are out of range or if raw is invalid
02194     #
02195     #  @pydoc
02196     def __init__(self, gear = -1,
02197                  raw = None, 
02198                  timestamp = 0):
02199         """Create A Horizon Message Payload - Gear"""
02200         
02201         # Class Variables
02202         ## Gear
02203         self._gear = -1
02204         
02205         # Create Constructor
02206         if raw == None:
02207             data = []
02208             
02209             # test Gear
02210             if gear < -128 or gear > 127:
02211                 raise ValueError("Gear must be [-128,127]!")
02212 
02213             self._gear = gear
02214             data += utils.from_char(gear)
02215             
02216             # Pass on to super-class
02217             Payload.__init__(self, raw = data, timestamp = timestamp)
02218         
02219         # Parse Constructor
02220         else:
02221             
02222             # Verify Length
02223             if len(raw) != 1:
02224                 raise ValueError( "Bad length!")
02225 
02226                 
02227             # Pass on to super-class
02228             Payload.__init__(self, raw = raw, timestamp = timestamp)
02229             
02230             # Extract Gear
02231             self._gear = utils.to_char(raw[0:1])
02232             logger.debug("%s gear: %d" % \
02233                          (self.__class__.__name__, self._gear))
02234     
02235     
02236     ## Human Readable Payload String
02237     def print_format(self):
02238         """Return the payload as a human readable string"""
02239         
02240         return "Gear: %d" % (self._gear)
02241                 
02242                 
02243     ## Get Gear
02244     #
02245     #  @return the gear
02246     #
02247     #  @pydoc
02248     def get_gear(self):
02249         """Get Gear"""
02250         
02251         return self._gear
02252     
02253     
02254     # Class Properties
02255     ## Gear
02256     gear = property(fget=get_gear, doc="Gear")
02257 
02258 
02259 ## Horizon Message Payload - Gear Status
02260 #
02261 #  Represents the payload of the data message 'gear status'
02262 #  @warning Data should not be modified once created
02263 #
02264 #  @since 0.1
02265 #
02266 #  @pydoc
02267 class GearStatus(Payload):
02268     """Horizon Message Payload - Gear Status"""
02269     
02270     
02271     ## Create A Horizon Message Payload - Gear Status
02272     #
02273     #  Constructor for the Horizon Message Payload - Gear.                    \n
02274     #  The constructor can be called two different ways:
02275     #  - GearStatus(upshifting, downshifting,gear,raw=None,...)\n
02276     #    Create a command message payload to send.                            \n 
02277     #  - GearStatus(raw, version, timestamp)                   \n
02278     #    Parse raw data (most likely received) into payload variables.        \n 
02279     #
02280     #  @param  downshifting   Set the downshifting flag?
02281     #  @param  gear           The desired gear [-128,127]
02282     #  @param  raw            Raw Payload data byte list to parse
02283     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02284     #  @param  upshifting     Set the upshifting flag?
02285     #  @param  version        Horizon Protocol Version,
02286     #                         (-1,*) represents the newest version,
02287     #                         (0,0) auto-detect the version (if supported)
02288     #  @throws LookupError    If auto-detect version fails
02289     #  @throws ValueError     If values are out of range or if raw is invalid
02290     #
02291     #  @pydoc
02292     def __init__(self, downshifting = False, upshifting = False, gear = -1,
02293                  raw = None, 
02294                  timestamp = 0):
02295         """Create A Horizon Message Payload - Gear Status"""
02296         
02297         # Class Variables
02298         ## Downshifting
02299         self._down = False
02300         ## Gear
02301         self._gear = -1
02302         ## Upshifting
02303         self._up = False
02304         
02305         # Create Constructor
02306         if raw == None:
02307             data = []
02308             
02309             # test flags
02310             flags = 0x0
02311             if downshifting and upshifting:
02312                 raise ValueError("Cannot downshift and upshift!")
02313 
02314             self._down = downshifting
02315             self._up = upshifting
02316             if self._down: flags &= 0x01
02317             if self._up: flags &= 0x02
02318             data += utils.from_byte(flags)
02319             
02320             # test Gear
02321             if gear < -128 or gear > 127:
02322                 raise ValueError("Gear must be [-128,127]!")
02323 
02324             self._gear = gear
02325             data += utils.from_char(gear)
02326             
02327             # Pass on to super-class
02328             Payload.__init__(self, raw = data, timestamp = timestamp)
02329         
02330         # Parse Constructor
02331         else:
02332             
02333             # Verify Length
02334             if len(raw) != 2:
02335                 raise ValueError( "Bad length!")
02336 
02337                 
02338             # Pass on to super-class
02339             Payload.__init__(self, raw = raw, timestamp = timestamp)
02340             
02341             # Extract Flags
02342             flags = utils.to_byte(raw[0:1])
02343             self._down = (flags & 0x01) == 0x01
02344             self._up = (flags & 0x02) == 0x02
02345             logger.debug("%s downshifting: %s" % \
02346                          (self.__class__.__name__, str(self._down)))
02347             logger.debug("%s upshifting: %s" % \
02348                          (self.__class__.__name__, str(self._up)))
02349             
02350             # Extract Gear
02351             self._gear = utils.to_char(raw[1:2])
02352             logger.debug("%s gear: %d" % \
02353                          (self.__class__.__name__, self._gear))
02354     
02355     
02356     ## Human Readable Payload String
02357     def print_format(self):
02358         """Return the payload as a human readable string"""
02359         
02360         return "Downshifting: %s\nUpshifting: %s\nGear: %d" % (
02361                                     str(self._down), str(self._up), self._gear)
02362                 
02363                 
02364     ## Is Downshifting?
02365     #
02366     #  @return is the platform downshifting?
02367     #
02368     #  @pydoc
02369     def is_downshifting(self):
02370         """Is Downshifting?"""
02371         
02372         return self._down
02373                 
02374                 
02375     ## Is Upshifting?
02376     #
02377     #  @return is the platform upshifting?
02378     #
02379     #  @pydoc
02380     def is_upshifting(self):
02381         """Is Upshifting?"""
02382         
02383         return self._up
02384                 
02385                 
02386     ## Get Gear
02387     #
02388     #  @return the gear
02389     #
02390     #  @pydoc
02391     def get_gear(self):
02392         """Get Gear"""
02393         
02394         return self._gear
02395     
02396     
02397     # Class Properties
02398     ## Downshifting
02399     downshifting = property(fget=is_downshifting, doc="Downshifting")
02400     ## Upshifting
02401     upshifting = property(fget=is_upshifting, doc="Upshifting")
02402     ## Gear
02403     gear = property(fget=get_gear, doc="Gear")
02404 
02405 
02406 
02407 
02408 ## Horizon Message Payload - Distance
02409 #
02410 #  Represents the payload of the data message 'distance'
02411 #
02412 #  @warning Data should not be modified once created
02413 #
02414 #  @since 0.1
02415 #
02416 #  @pydoc
02417 class Distance(Payload):
02418     """Horizon Message Payload - Distance"""
02419     
02420     
02421     ## Create A Horizon Message Payload - Distance
02422     #
02423     #  Constructor for the Horizon Message Payload - Distance.                \n
02424     #  The constructor can be called two different ways:
02425     #  - Distance(distance, raw=None, timestamp, version, ...) \n
02426     #    Create a command message payload to send.                            \n 
02427     #  - Distance(raw, version, timestamp)                     \n
02428     #    Parse raw data (most likely received) into payload variables.        \n 
02429     #
02430     #  @param  distance       List of distances scaled to two bytes
02431     #  @param  raw            Raw Payload data byte list to parse
02432     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02433     #  @param  version        Horizon Protocol Version,
02434     #                         (-1,*) represents the newest version,
02435     #                         (0,0) auto-detect the version (if supported)
02436     #  @throws LookupError    If auto-detect version fails
02437     #  @throws ValueError     If values are out of range or if raw is invalid
02438     #
02439     #  @pydoc
02440     def __init__(self, distance = [], 
02441                  raw = None, timestamp = 0):
02442         """Create A Horizon Message Payload - Distance"""
02443         
02444         # Class Variables
02445         self.distances = []
02446         
02447         # Parse Constructor
02448         if raw != None:
02449             # Verify Length
02450             if len(raw) < 1 or len(raw) != raw[0]*2+1:
02451                 raise ValueError( "Bad length!")
02452 
02453                 
02454             # Pass on to super-class
02455             Payload.__init__(self, raw = raw, timestamp = timestamp)
02456             
02457             # Extract Distances
02458             for i in range(0, raw[0]):
02459                 self.distances += [utils.to_short(raw[i * 2 + 1:i * 2 + 3])]
02460             logger.debug("%s distances: %s" % (self.__class__.__name__, 
02461                      ' '.join(map(str,self.distances))))
02462     
02463     
02464     ## Human Readable Payload String
02465     def print_format(self):
02466         """Return the payload as a human readable string"""
02467         
02468         return "Distances: %s" % ''.join([string.rjust(str(s), 6) for s in self.distances])
02469         
02470 
02471 ## Horizon Message Payload - Distance & Timing
02472 #
02473 #  Represents the payload of the data message 'distance timing'
02474 #
02475 #  @warning Data should not be modified once created
02476 #
02477 #  @since 0.1
02478 #
02479 #
02480 #  @pydoc
02481 class DistanceTiming(Payload):
02482     """Horizon Message Payload - Distance & Timing"""
02483     
02484     
02485     ## Create A Horizon Message Payload - Distance & Timing
02486     #
02487     #  Constructor for the Horizon Message Payload - Distance & Timing.       \n
02488     #  The constructor can be called two different ways:
02489     #  - DistanceTiming(distance, timing, raw=None, ...)       \n
02490     #    Create a command message payload to send.                            \n 
02491     #  - DistanceTiming(raw, version, timestamp)               \n
02492     #    Parse raw data (most likely received) into payload variables.        \n 
02493     #
02494     #  @param  distance       List of distances scaled to two bytes
02495     #  @param  raw            Raw Payload data byte list to parse
02496     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02497     #  @param  timing         List of distance aquisition times (milliseconds)
02498     #  @param  version        Horizon Protocol Version,
02499     #                         (-1,*) represents the newest version,
02500     #                         (0,0) auto-detect the version (if supported)
02501     #  @throws LookupError    If auto-detect version fails
02502     #  @throws ValueError     If values are out of range or if raw is invalid
02503     #
02504     #  @pydoc
02505     def __init__(self, distance = [], timing = [], 
02506                  raw = None, timestamp = 0):
02507         """Create A Horizon Message Payload - Distance & Timing"""
02508         
02509         # Class Variables
02510         self.distances = []
02511         self.timings = []
02512         
02513         # Parse Constructor
02514         if raw != None:
02515 
02516             # Verify Length
02517             if len(raw) < 1 or len(raw) != raw[0] * 6 + 1:
02518                 raise ValueError( "Bad length!")
02519 
02520                 
02521             # Pass on to super-class
02522             Payload.__init__(self, raw = raw, timestamp = timestamp)
02523             
02524             # Extract Distances
02525             for i in range(0, raw[0]):
02526                 self.distances += [utils.to_short(raw[i*2+1:i*2+3])]
02527             logger.debug("%s distances: %s" % (self.__class__.__name__, 
02528                      ' '.join(map(hex,self.distances))))
02529             
02530             # Extract Timing
02531             for i in range(0,raw[0]):
02532                 self.timings += [utils.to_unsigned_int(
02533                                             raw[raw[0]*2+i*4+1:i*4+5+raw[0]*2])]
02534             logger.debug("%s times: %s" % (self.__class__.__name__, 
02535                      ' '.join(map(str,self.timings))))
02536     
02537     
02538     ## Human Readable Payload String
02539     def print_format(self):
02540         """Return the payload as a human readable string"""
02541         
02542         return "Distances: %s\nTiming:    %s" % (''.join([string.rjust(str(s), 6) for s in self.distances]),
02543                                               ''.join([string.rjust(str(s), 6) for s in self.timings]))
02544         
02545 
02546 ## Horizon Message Payload - Orientation
02547 #
02548 #  Represents the payload of the data message 'orientation'
02549 #  @warning Data should not be modified once created
02550 #
02551 #  @since 0.1
02552 #
02553 #  @pydoc
02554 class Orientation(Payload):
02555     """Horizon Message Payload - Orientation"""
02556     
02557     ## Create A Horizon Message Payload - Orientation
02558     #
02559     #  Constructor for the Horizon Message Payload - Orientation.             \n
02560     #  The constructor can be called two different ways:
02561     #  - Orientation(roll, pitch, yaw, raw=None, ...)          \n
02562     #    Create a command message payload to send.                            \n 
02563     #  - Orientation(raw, version, timestamp)                  \n
02564     #    Parse raw data (most likely received) into payload variables.        \n 
02565     #
02566     #  @param  pitch          The vehicle's pitch angle
02567     #  @param  raw            Raw Payload data byte list to parse
02568     #  @param  roll           The vehicle's roll angle
02569     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02570     #  @param  version        Horizon Protocol Version,
02571     #                         (-1,*) represents the newest version,
02572     #                         (0,0) auto-detect the version (if supported)
02573     #  @param  yaw            The vehicle's yaw (magnetic) angle
02574     #  @throws LookupError    If auto-detect version fails
02575     #  @throws ValueError     If values are out of range or if raw is invalid
02576     #
02577     #  @pydoc
02578     def __init__(self, roll = 0, pitch = 0, yaw = 0, 
02579                  raw = None, timestamp = 0):
02580         """Create A Horizon Message Payload - Orientation"""
02581         
02582         self.roll = 0
02583         self.pitch = 0
02584         self.yaw = 0
02585         
02586         # Create Constructor
02587         if raw == None:
02588             data = []
02589             
02590             if roll < -math.pi or roll > math.pi:
02591                 raise ValueError( "Roll must be [-π,π]!")
02592             self.roll = roll
02593             data += utils.from_short(int(roll*1000))
02594             
02595             if pitch < -math.pi or pitch > math.pi:
02596                 raise ValueError( "Pitch must be [-π,π]!")
02597             self.pitch = pitch
02598             data += utils.from_short(int(pitch*1000))
02599             
02600             if yaw < -math.pi or yaw > math.pi:
02601                 raise ValueError( "Yaw must be [-π,π]!")
02602             self.yaw = yaw
02603             data += utils.from_short(int(yaw*1000))
02604             
02605             Payload.__init__(self, raw = data, timestamp = timestamp)
02606         
02607         # Parse Constructor
02608         else:
02609             
02610             # Verify Length
02611             if len(raw) != 6:
02612                 raise ValueError( "Bad length!")
02613 
02614             Payload.__init__(self, raw = raw, timestamp = timestamp)
02615             self.roll = utils.to_short(raw[0:2]) / 1000.0
02616             self.pitch = utils.to_short(raw[2:4]) / 1000.0
02617             self.yaw = utils.to_short(raw[4:6]) / 1000.0
02618 
02619     
02620     ## Human Readable Payload String
02621     def print_format(self):
02622         """Return the payload as a human readable string"""
02623         
02624         return "Roll: %f\nPitch: %f\nYaw: %f" % (
02625                                         self.roll, self.pitch, self.yaw)
02626         
02627         
02628     
02629     
02630 ## Horizon Message Payload - Rotation
02631 #
02632 #  Represents the payload of the data message 'rotational rate'
02633 #
02634 #  @warning Data should not be modified once created
02635 #
02636 #  @since 0.1
02637 #
02638 #
02639 #  @pydoc
02640 class Rotation(Payload):
02641     """Horizon Message Payload - Rotation"""
02642     
02643     
02644     ## Create A Horizon Message Payload - Rotation
02645     #
02646     #  Constructor for the Horizon Message Payload - Rotation.                \n
02647     #  The constructor can be called two different ways:
02648     #  - Rotation(roll, pitch, yaw, raw=None, ...)             \n
02649     #    Create a command message payload to send.                            \n 
02650     #  - Rotation(raw, version, timestamp)                     \n
02651     #    Parse raw data (most likely received) into payload variables.        \n 
02652     #
02653     #  @param  raw            Raw Payload data byte list to parse
02654     #  @param  rot_pitch      The vehicle's pitch rotation rate
02655     #  @param  rot_roll       The vehicle's roll rotation rate
02656     #  @param  rot_yaw        The vehicle's yaw rotation rate
02657     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02658     #  @param  version        Horizon Protocol Version,
02659     #                         (-1,*) represents the newest version,
02660     #                         (0,0) auto-detect the version (if supported)
02661     #  @throws LookupError    If auto-detect version fails
02662     #  @throws ValueError     If values are out of range or if raw is invalid
02663     #
02664     #  @pydoc
02665     def __init__(self, roll = 0, pitch = 0, yaw = 0, 
02666                  raw = None, timestamp = 0):
02667         """Create A Horizon Message Payload - Rotation"""
02668         
02669         self.roll = 0
02670         self.pitch = 0
02671         self.yaw = 0
02672         
02673         # Create Constructor
02674         if raw == None:
02675             data = []
02676             
02677             # test roll
02678             if roll < -10*math.pi or roll > 10*math.pi:
02679                 raise ValueError( "Roll must be [-10π,10π]!")
02680 
02681             self.roll = roll
02682             data += utils.from_short(int(roll*1000))
02683             
02684             # test pitch
02685             if pitch < -10*math.pi or pitch > 10*math.pi:
02686                 raise ValueError( "Pitch must be [-10π,10π]!")
02687 
02688             self.pitch = pitch
02689             data += utils.from_short(int(pitch*1000))
02690             
02691             # test yaw
02692             if yaw < -10*math.pi or yaw > 10*math.pi:
02693                 raise ValueError( "Yaw must be [-10π,10π]!")
02694 
02695             self.yaw = yaw
02696             data += utils.from_short(int(yaw*1000))
02697             
02698             # Pass on to super-class
02699             Payload.__init__(self, raw = data, timestamp = timestamp)
02700         
02701         # Parse Constructor
02702         else:
02703             
02704             # Verify Length
02705             if len(raw) != 6:
02706                 raise ValueError( "Bad length!")
02707                 
02708             # Pass on to super-class
02709             Payload.__init__(self, raw = raw, timestamp = timestamp)
02710             self.roll = utils.to_short(raw[0:2])/1000.0
02711             self.pitch = utils.to_short(raw[2:4])/1000.0
02712             self.yaw = utils.to_short(raw[4:6])/1000.0
02713     
02714     
02715     ## Human Readable Payload String
02716     def print_format(self):
02717         """Return the payload as a human readable string"""
02718         
02719         return "Rotational Roll: %frad/s\nRotational Pitch: %frad/s\n"\
02720                "Rotational Yaw: %frad/s" % (
02721                           self.roll, self.pitch, self.yaw)
02722         
02723         
02724     
02725 
02726 class Acceleration(Payload):
02727     """Horizon Message Payload - Acceleration"""
02728     
02729     ## Create A Horizon Message Payload - Acceleration
02730     def __init__(self, x = 0, y = 0, z = 0, 
02731                  raw = None, timestamp = 0):
02732         """Create A Horizon Message Payload - Acceleration"""
02733         
02734         # Class Variables
02735         self.x = 0
02736         self.y = 0
02737         self.z = 0
02738 
02739         # Verify Length
02740         if raw == None or len(raw) != 6:
02741             raise ValueError( "Bad length!")
02742                 
02743         # Pass on to super-class
02744         Payload.__init__(self, raw = raw, timestamp = timestamp)
02745             
02746         # Extract X
02747         self.x = utils.to_short(raw[0:2]) / 1.0
02748         logger.debug("%s x: %f" % (self.__class__.__name__, self.x))
02749             
02750         # Extract Y
02751         self.y = utils.to_short(raw[2:4]) / 1.0
02752         logger.debug("%s y: %f" % (self.__class__.__name__, self.y))
02753             
02754         # Extract Z
02755         self.z = utils.to_short(raw[4:6]) / 1.0
02756         logger.debug("%s z: %f" % (self.__class__.__name__, self.z))
02757  
02758     
02759     ## Human Readable Payload String
02760     def print_format(self):
02761         """Return the payload as a human readable string"""
02762         
02763         return "X: %f\nY: %f\nZ: %f" % (self.x, self.y, self.z)
02764         
02765 
02766  
02767 class Magnetometer(Payload):
02768     """Horizon Message Payload - Magnetometer"""
02769     
02770     ## Create A Horizon Message Payload - Magnetometer
02771     def __init__(self, x = 0, y = 0, z = 0, 
02772                  raw = None, timestamp = 0):
02773         """Create A Horizon Message Payload - Magnetometer Data"""
02774         
02775         # Class Variables
02776         self.x = 0
02777         self.y = 0
02778         self.z = 0 
02779 
02780         # Verify Length
02781         if raw == None or len(raw) != 6:
02782             raise ValueError( "Bad length!")
02783                 
02784         # Pass on to super-class
02785         Payload.__init__(self, raw = raw, timestamp = timestamp)
02786             
02787         # Extract X
02788         self.x = utils.to_short(raw[0:2]) / 1.0
02789         logger.debug("%s x: %f" % (self.__class__.__name__, self.x))
02790             
02791         # Extract Y
02792         self.y = utils.to_short(raw[2:4]) / 1.0
02793         logger.debug("%s y: %f" % (self.__class__.__name__, self.y))
02794             
02795         # Extract Z
02796         self.z = utils.to_short(raw[4:6]) / 1.0
02797         logger.debug("%s z: %f" % (self.__class__.__name__, self.z))
02798  
02799     
02800     ## Human Readable Payload String
02801     def print_format(self):
02802         """Return the payload as a human readable string"""
02803         
02804         return "X: %f\nY: %f\nZ: %f" % (self.x, self.y, self.z)
02805  
02806 
02807 ## Horizon Message Payload - Encoders
02808 #
02809 #  Represents the payload of the data message 'encoders'
02810 #
02811 #  @warning Data should not be modified once created
02812 #
02813 #  @since 0.1
02814 #
02815 #  @pydoc
02816 class Encoders(Payload):
02817     """Horizon Message Payload - Encoders"""
02818     
02819     
02820     ## Create A Horizon Message Payload - Encoders
02821     #
02822     #  Constructor for the Horizon Message Payload - Encoders.                \n
02823     #  The constructor can be called two different ways:
02824     #  - Encoders(travel, speed, raw=None, ...)                \n
02825     #    Create a command message payload to send.                            \n 
02826     #  - Encoders(raw, version, timestamp)                     \n
02827     #    Parse raw data (most likely received) into payload variables.        \n 
02828     #
02829     #  @param  raw            Raw Payload data byte list to parse
02830     #  @param  speed          A list of encoder speeds
02831     #  @param  timestamp      Payload Send / Create Time (milliseconds)
02832     #  @param  travel         A list of encoder distances
02833     #  @param  version        Horizon Protocol Version,
02834     #                         (-1,*) represents the newest version,
02835     #                         (0,0) auto-detect the version (if supported)
02836     #  @throws LookupError    If auto-detect version fails
02837     #  @throws ValueError     If values are out of range or if raw is invalid
02838     #
02839     #  @pydoc
02840     def __init__(self, travel = [], speed = [], raw = None, 
02841                  timestamp = 0):
02842         """Create A Horizon Message Payload - Encoders"""
02843         
02844         # Class Variables
02845         ## Encoder Speeds
02846         self._speed = []
02847         ## Encoder Distances
02848         self._travel = []
02849         
02850         # Create Constructor
02851         if raw == None:
02852             data = []
02853             
02854             # test length
02855             if len(travel) != len(speed):
02856                 raise ValueError("Must have same number of distances and speeds!")
02857 
02858             if len(travel) > 255:
02859                 raise ValueError("Must have 0-255 encoders!")
02860 
02861             data = utils.from_byte(len(speed))
02862             
02863             # test distances
02864             for t in travel:
02865                 if t < -32 or t > 32:
02866                     raise ValueError("Distances must be [-2x10^6,2x10^6]!")
02867 
02868                 data += utils.from_int(int(t*1000))
02869             self._travel = travel
02870             
02871             # test speeds
02872             for s in speed:
02873                 if s < -32 or s > 32:
02874                     raise ValueError( "Speeds must be [-32,32]!")
02875 
02876                 data += utils.from_short(int(s*1000))
02877             self._speed = speed
02878             
02879             # Pass on to super-class
02880             Payload.__init__(self, raw = data, timestamp = timestamp)
02881         
02882         # Parse Constructor
02883         else:
02884             
02885             # Verify Length
02886             if len(raw) < 1 or len(raw) != raw[0]*6+1:
02887                 raise ValueError( "Bad length!")
02888 
02889                 
02890             # Pass on to super-class
02891             Payload.__init__(self, raw = raw, timestamp = timestamp)
02892             
02893             # Extract Distances
02894             self._travel = []
02895             for i in range(0,raw[0]):
02896                 self._travel.append(utils.to_int(
02897                                                 raw[i*4+1:(i+1)*4+1])/1000.0)
02898             logger.debug("%s distances: %s" % (self.__class__.__name__, 
02899                      self._travel))
02900             
02901             # Extract Speeds
02902             self._speed = []
02903             for i in range(0,raw[0]):
02904                 self._speed.append(utils.to_short(\
02905                                 raw[i*2+raw[0]*4+1:(i+1)*2+raw[0]*4+1])/1000.0)
02906             logger.debug("%s speeds: %s" % (self.__class__.__name__, 
02907                      self._speed))
02908     
02909     
02910     ## Human Readable Payload String
02911     def print_format(self):
02912         """Return the payload as a human readable string"""
02913         
02914         return "Distances: %s\nSpeeds: %s" % (\
02915                                         'm, '.join(map(str,self._travel))+'m', 
02916                                        'm/s, '.join(map(str,self._speed))+'m/s')
02917         
02918         
02919     ## Get the number of Encoders
02920     #
02921     #  @return number of encoders
02922     #
02923     #  @pydoc
02924     def get_count(self):
02925         """Get the number of Encoders."""
02926         
02927         return len(self._speed)
02928                 
02929                 
02930     ## Get Speed
02931     #
02932     #  @param  encoder  The encoder to get the speed of.
02933     #                   If -1 then it returns a list of all encoders
02934     #  @return the encoder speed (m/s), or all encoder speeds
02935     #
02936     #  @pydoc
02937     def get_speed(self, encoder = -1):
02938         """Get Encoder"""
02939         
02940         # all encoders
02941         if encoder < 0 or encoder >= len(self._speed):
02942             return self._speed
02943         
02944         # single encoder
02945         else:
02946             return self._speed[encoder]
02947                 
02948                 
02949     ## Get Distance
02950     #
02951     #  @param  encoder  The encoder to get the distance of.
02952     #                   If -1 then it returns a list of all encoders
02953     #  @return the distance of encoder (metres), or all encoder distances
02954     #
02955     #  @pydoc
02956     def get_travel(self, encoder = -1):
02957         """Get Distance"""
02958         
02959         # all encoders
02960         if encoder < 0 or encoder >= len(self._speed):
02961             return self._travel
02962         
02963         # single encoder
02964         else:
02965             return self._travel[encoder]
02966     
02967     
02968     # Class Properties
02969     ## Encoder Speed
02970     speed = property(fget=get_speed, doc="Encoder Speed")
02971     ## Encoder Distance
02972     travel = property(fget=get_travel, doc="Encoder Distance")
02973 
02974 
02975 ## Horizon Message Payload - Raw Encoders
02976 #
02977 #  Represents the payload of the data message 'raw encoders'
02978 #
02979 #  @warning Data should not be modified once created
02980 #
02981 #  @since 0.8
02982 #
02983 #  @pydoc
02984 class RawEncoders(Payload):
02985     """Horizon Message Payload - Raw Encoders"""
02986     
02987     
02988     
02989     ## Create A Horizon Message Payload - Raw Encoders
02990     #
02991     #  Constructor for the Horizon Message Payload - Raw Encoders.            \n
02992     #  The constructor can be called two different ways:
02993     #  - Encoders(ticks, raw=None, ...)                        \n
02994     #    Create a command message payload to send.                            \n 
02995     #  - Encoders(raw, version, timestamp)                     \n
02996     #    Parse raw data (most likely received) into payload variables.        \n 
02997     #
02998     #  @param  raw            Raw Payload data byte list to parse
02999     #  @param  ticks          A list of encoder ticks
03000     #  @param  timestamp      Payload Send / Create Time (milliseconds)
03001     #  @param  version        Horizon Protocol Version,
03002     #                         (-1,*) represents the newest version,
03003     #                         (0,0) auto-detect the version (if supported)
03004     #  @throws LookupError    If auto-detect version fails
03005     #  @throws ValueError     If values are out of range or if raw is invalid
03006     #
03007     #  @pydoc
03008     def __init__(self, ticks = [], raw = None, 
03009                  timestamp = 0):
03010         """Create A Horizon Message Payload - Raw Encoders"""
03011         
03012         self.ticks = []
03013         
03014         # Create Constructor
03015         if raw == None:
03016             data = []
03017             
03018             # test length
03019             if len(ticks) > 255:
03020                 raise ValueError("Must have 0-255 encoders!")
03021 
03022             data = utils.from_byte([len(ticks)])
03023             
03024             # test ticks
03025             for t in ticks:
03026                 if t < -math.pow(2, 31) or t > math.pow(2, 31)-1:
03027                     raise ValueError( "Ticks must be [-2^31,2^31-1]!")
03028 
03029                 data += utils.from_int(int(t))
03030             self.ticks = ticks
03031             
03032             # Pass on to super-class
03033             Payload.__init__(self, raw = data, timestamp = timestamp)
03034         
03035         # Parse Constructor
03036         else:
03037             
03038             # Verify Length
03039             if len(raw) < 1 or len(raw) != raw[0] * 4 + 1:
03040                 raise ValueError( "Bad length!")
03041 
03042             Payload.__init__(self, raw = raw, timestamp = timestamp)
03043             
03044             # Extract Ticks
03045             self.ticks = []
03046             for i in range(0,raw[0]):
03047                 self.ticks.append(utils.to_int(\
03048                                 raw[i*4+1:(i+1)*4+1]))
03049             logger.debug("%s ticks: %s" % (self.__class__.__name__, 
03050                      self.ticks))
03051     
03052     
03053     ## Human Readable Payload String
03054     def print_format(self):
03055         """Return the payload as a human readable string"""
03056         
03057         return "Ticks: %s" % (' '.join(map(str,self.ticks)))
03058         
03059 
03060 
03061 ## Horizon Message Payload - Reset
03062 #
03063 #  Represents the payload of the command message 'reset'
03064 #
03065 #  @warning Data should not be modified once created
03066 #
03067 #  @since 0.7
03068 #
03069 #  @pydoc
03070 class Reset(Payload):
03071     """Horizon Message Payload - Reset"""
03072     
03073     ## Create A Horizon Message Payload - Reset
03074     #
03075     #  Constructor for the Horizon Message Payload - Reset Class.             \n
03076     #  The constructor can be called two different ways:
03077     #  - PlatformTime(raw=None, version, timestamp)            \n
03078     #    Create a command message payload to send.                            \n 
03079     #  - PlatformTime(raw, version, timestamp)                 \n
03080     #    Parse raw data (most likely received) into payload variables.        \n 
03081     #
03082     #  @param  raw            Raw Payload data byte list to parse
03083     #  @param  timestamp      Payload Send / Create Time (milliseconds)
03084     #  @param  version        Horizon Protocol Version,
03085     #                         (-1,*) represents the newest version,
03086     #                         (0,0) auto-detect the version (if supported)
03087     #  @throws LookupError    If auto-detect version fails
03088     #  @throws ValueError     If values are out of range or if raw is invalid
03089     #
03090     #  @pydoc
03091     def __init__(self, raw = None, 
03092                  timestamp = 0):
03093         """Create A Horizon Message Payload - Reset"""
03094         
03095         # Class Variables
03096         ## Passcode
03097         self.passcode = 0x3A18
03098         
03099         # Create Constructor
03100         if raw == None:
03101             data = []
03102             
03103             # passcode
03104             data += utils.from_unsigned_short(self.passcode)
03105             
03106             # Pass on to super-class
03107             Payload.__init__(self, raw = data, timestamp = timestamp)
03108         
03109         # Parse Constructor
03110         else:
03111             
03112             # Verify Length
03113             if len(raw) != 2:
03114                 raise ValueError("Bad length!")
03115 
03116                 
03117             # Pass on to super-class
03118             Payload.__init__(self, raw = raw, timestamp = timestamp)
03119             
03120             # Extract Passcode
03121             self.passcode = utils.to_unsigned_short(raw)
03122             logger.debug("%s passcode: %d" % (self.__class__.__name__, self.passcode))
03123     
03124     
03125     ## Human Readable Payload String
03126     def print_format(self):
03127         """Return the payload as a human readable string"""
03128         
03129         return "Passcode: 0x%04X" % self.passcode
03130                 
03131 
03132 # Abstract class. This forms the basis for all the sensor config messages, as they are all
03133 # sequences of pairs of offset/scale values.
03134 class VariableSensorConfig(Payload):
03135     """Horizon Message Abstract Payload - Sensor Configuration"""
03136 
03137     def __init__(self, passcode = 0, offsets = [], scales = [], 
03138                  raw = None, timestamp = 0):
03139         
03140         # Class Variables
03141         self.passcode = passcode
03142         self.offsets = []
03143         self.scales = []
03144         
03145         # Create Constructor
03146         if raw == None:
03147             # Assume this is a config set going out.
03148 
03149             # Magic passcode
03150             data = utils.from_short(self.passcode)
03151             
03152             self.offsets = offsets
03153             self.scales = scales
03154             if len(self.offsets) != len(self.scales):
03155                 raise ValueError("Offsets and scales are not the same length!")
03156         
03157             for offset, scale in zip(self.offsets, self.scales):   
03158                 data += utils.from_short(int(offset * 1000))
03159                 data += utils.from_short(int(scale * 1000))
03160             
03161             # Pass on to super-class
03162             Payload.__init__(self, raw = data, timestamp = timestamp)
03163         
03164         # Parse Constructor
03165         else:
03166             # For now, assume this is a result coming back.
03167             # (Depending on the payload length, we could infer whether this
03168             # is a config set operation or a config request result.)
03169                 
03170             # Pass on to super-class
03171             Payload.__init__(self, raw = raw, timestamp = timestamp)
03172             
03173             for i in range(0, self._num_sensors(raw)):
03174                 base = self._base_for_value(i)
03175                 self.offsets.append(utils.to_short(raw[base:base + 2]) / 1000.0)
03176                 self.scales.append(utils.to_short(raw[base + 2:base + 4]) / 1000.0)
03177 
03178     def _num_sensors(self, raw):
03179         return raw[0]
03180 
03181     def _base_for_value(self, i):
03182         return i * 4 + 1
03183 
03184 
03185 class CurrentSensorConfig(VariableSensorConfig):
03186     """Horizon Message Payload - Current Sensor Configuration"""
03187 
03188     def print_format(self):
03189         lines = []
03190         for i in range(len(self.offsets)):
03191             lines.append("Current Sensor %d: Offset: %f,  Scale: %f" % (i + 1, self.offsets[i], self.scales[i]))
03192         return "\n".join(lines)
03193 
03194 
03195 class VoltageSensorConfig(VariableSensorConfig):
03196     """Horizon Message Payload - Voltage Sensor Configuration"""
03197 
03198     def print_format(self):
03199         lines = []
03200         for i in range(len(self.offsets)):
03201             lines.append("Voltage Sensor %d: Offset: %f,  Scale: %f" % (i + 1, self.offsets[i], self.scales[i]))
03202         return "\n".join(lines)
03203 
03204 
03205 class TemperatureSensorConfig(VariableSensorConfig):
03206     """Horizon Message Payload - Temperature Sensor Configuration"""
03207 
03208     def print_format(self):
03209         lines = []
03210         for i in range(len(self.offsets)):
03211             lines.append("Temperature Sensor %d: Offset: %f,  Scale: %f" % (i + 1, self.offsets[i], self.scales[i]))
03212         return "\n".join(lines)
03213 
03214 
03215 class OrientationSensorConfig(VariableSensorConfig):
03216     """Horizon Message Payload - Orientation Sensor Configuration"""
03217 
03218     def __init__(self, passcode = 0, roll_offset = 0, roll_scale = 0, 
03219                  pitch_offset = 0, pitch_scale = 0, yaw_offset = 0, 
03220                  yaw_scale = 0, raw = None, 
03221                  timestamp = 0):
03222         
03223         offsets = [roll_offset, pitch_offset, yaw_offset]
03224         scales = [roll_scale, pitch_scale, yaw_scale]
03225         
03226         VariableSensorConfig.__init__(self, passcode, offsets, scales, raw, timestamp)
03227 
03228         self.roll_offset, self.pitch_offset, self.yaw_offset = self.offsets
03229         self.roll_scale, self.pitch_scale, self.yaw_scale = self.scales
03230 
03231     def print_format(self):
03232         lines = []
03233         lines.append("Roll offset: %f,  Roll scale: %f" % (self.offsets[0], self.scales[0]))
03234         lines.append("Pitch offset: %f,  Pitch scale: %f" % (self.offsets[1], self.scales[1]))
03235         lines.append("Yaw offset: %f,  Yaw scale: %f" % (self.offsets[2], self.scales[2]))
03236         return "\n".join(lines)
03237 
03238     def _num_sensors(self, raw):
03239         return 3
03240 
03241     def _base_for_value(self, i):
03242         return i * 4
03243 
03244 
03245 class GyroConfig(OrientationSensorConfig):
03246     """Horizon Message Payload - Gyro Configuration, identical to orientation sensor"""
03247     pass
03248 
03249 
03250 class AccelerometerConfig(VariableSensorConfig):
03251     """Horizon Message Payload - Orientation Sensor Configuration"""
03252 
03253     def __init__(self, passcode = 0, x_offset = 0, x_scale = 0, y_offset = 0, 
03254                  y_scale = 0, z_offset = 0, z_scale = 0, 
03255                  raw = None, timestamp = 0):
03256         
03257         offsets = [x_offset, y_offset, z_offset]
03258         scales = [x_scale, y_scale, z_scale]
03259         
03260         VariableSensorConfig.__init__(self, passcode, offsets, scales, raw, timestamp)
03261 
03262         self.x_offset, self.y_offset, self.z_offset = self.offsets
03263         self.x_scale, self.y_scale, self.z_scale = self.scales
03264 
03265     def print_format(self):
03266         lines = []
03267         lines.append("x-axis offset: %f, scale: %f" % (self.offsets[0], self.scales[0]))
03268         lines.append("y-axis offset: %f, scale: %f" % (self.offsets[1], self.scales[1]))
03269         lines.append("z-axis offset: %f, scale: %f" % (self.offsets[2], self.scales[2]))
03270         return "\n".join(lines)
03271 
03272     def _num_sensors(self, raw):
03273         return 3
03274 
03275     def _base_for_value(self, i):
03276         return i * 4
03277 
03278 
03279 class MagnetometerConfig(AccelerometerConfig):
03280     """Horizon Message Payload - Magnetometer Configuration, identical to accelerometer"""
03281     pass
03282 
03283 
03284 class EncodersConfig(Payload):
03285     """Horizon Message Abstract Payload - Sensor Configuration"""
03286 
03287     def __init__(self, ppr = [], scales = [], 
03288                  raw = None, timestamp = 0):
03289         
03290         # Class Variables
03291         self.ppr = []
03292         self.scales = []
03293         
03294         # Create Constructor
03295         if raw == None:
03296             # Assume this is a config set going out.
03297 
03298             # Magic passcode
03299             data = []
03300             
03301             self.ppr = ppr
03302             self.scales = scales
03303 
03304             if len(self.ppr) != len(self.scales):
03305                 raise ValueError("PPR and scales are not the same length!")
03306         
03307             for ppr, scale in zip(self.ppr, self.scales):   
03308                 data += utils.from_short(int(ppr))
03309                 data += utils.from_short(int(scale * 1000))
03310              
03311             # Pass on to super-class
03312             Payload.__init__(self, raw = data, timestamp = timestamp)
03313         
03314         # Parse Constructor
03315         else:
03316             # For now, assume this is a result coming back.
03317             # (Depending on the payload length, we could infer whether this
03318             # is a config set operation or a config request result.)
03319                 
03320             # Pass on to super-class
03321             Payload.__init__(self, raw = raw, timestamp = timestamp)
03322             
03323             for i in range(0, raw[0]):
03324                 base = i * 4 + 1
03325                 self.ppr.append(utils.to_short(raw[base:base + 2]))
03326                 self.scales.append(utils.to_short(raw[base + 2:base + 4]) / 1000.0)
03327 
03328     def print_format(self):
03329         lines = []
03330         for i in range(len(self.ppr)):
03331             lines.append("Encoder %d: PPR: %f,  Scale: %f" % (i + 1, self.ppr[i], self.scales[i]))
03332         return "\n".join(lines)
03333 
03334 
03335 # Abstract class. This forms the basis for all the sensor config messages, as they are all
03336 # sequences of pairs of offset/scale values.
03337 class RawSensor(Payload):
03338     """Horizon Message Abstract Payload - Raw Sensor Data"""
03339 
03340     def __init__(self, raw = None, timestamp = 0):
03341         
03342         # Class Variables
03343         self.raw_values = []
03344         
03345         # Create Constructor
03346         if raw != None:
03347             # Result coming back from firmware            
03348                 
03349             # Pass on to super-class
03350             Payload.__init__(self, raw = raw, timestamp = timestamp)
03351             
03352             for i in range(0, self._num_sensors(raw)):
03353                 base = self._base_for_value(i)
03354                 self.raw_values.append(utils.to_short(raw[base:base + 2]))
03355 
03356     def _num_sensors(self, raw):
03357         return raw[0]
03358 
03359     def _base_for_value(self, i):
03360         return i * 2 + 1
03361 
03362 
03363 
03364 class RawCurrentSensor(RawSensor):
03365     """Horizon Message Payload - Raw Current Data"""
03366 
03367     def __init__(self, raw = None, timestamp = 0):
03368         RawSensor.__init__(self, raw, timestamp)
03369 
03370         # Class Variables
03371         self.raw_currents = self.raw_values
03372         
03373 
03374     def print_format(self):
03375         lines = []
03376         for i in range(len(self.raw_currents)):
03377             lines.append("Raw Current %d: %d" % (i + 1, self.raw_currents[i]))
03378         return "\n".join(lines)
03379 
03380 
03381 
03382 class RawVoltageSensor(RawSensor):
03383     """Horizon Message Payload - Raw Voltage Data"""
03384 
03385     def __init__(self, raw = None, timestamp = 0):
03386         RawSensor.__init__(self, raw, timestamp)
03387 
03388         # Class Variables
03389         self.raw_voltages = self.raw_values
03390         
03391 
03392     def print_format(self):
03393         lines = []
03394         for i in range(len(self.raw_voltages)):
03395             lines.append("Raw Voltage %d: %d" % (i + 1, self.raw_voltages[i]))
03396         return "\n".join(lines)
03397 
03398 
03399 class RawTemperatureSensor(RawSensor):
03400     """Horizon Message Payload - Raw Temperature Data"""
03401 
03402     def __init__(self, raw = None, timestamp = 0):
03403         RawSensor.__init__(self, raw, timestamp)
03404 
03405         # Class Variables
03406         self.raw_temperatures = self.raw_values
03407         
03408 
03409     def print_format(self):
03410         lines = []
03411         for i in range(len(self.raw_temperatures)):
03412             lines.append("Raw Temperature %d: %d" % (i + 1, self.raw_temperatures[i]))
03413         return "\n".join(lines)
03414 
03415 
03416 class RawOrientationSensor(RawSensor):
03417     """Horizon Message Payload - Raw Orientation Data"""
03418 
03419     def __init__(self, raw = None, timestamp = 0):
03420         RawSensor.__init__(self, raw, timestamp)
03421 
03422         # Class Variables
03423         self.raw_roll, self.raw_pitch, self.raw_yaw = self.raw_values
03424 
03425     def _num_sensors(self, raw):
03426         return 3
03427 
03428     def _base_for_value(self, i):
03429         return i * 2
03430 
03431     def print_format(self):
03432         lines = []
03433         lines.append("Raw Roll: %d" % self.raw_roll)
03434         lines.append("Raw Pitch: %d" % self.raw_pitch)
03435         lines.append("Raw Yaw: %d" % self.raw_yaw)
03436         return "\n".join(lines)
03437 
03438 
03439 class RawGyro(RawOrientationSensor):
03440     """Horizon Message Payload - Raw Gyro Data, same as raw orientation data"""
03441     pass
03442 
03443 
03444 class RawAccelerometer(RawSensor):
03445     """Horizon Message Payload - Raw Orientation Data"""
03446 
03447     def __init__(self, raw = None, timestamp = 0):
03448         RawSensor.__init__(self, raw, timestamp)
03449 
03450         # Class Variables
03451         self.raw_x, self.raw_y, self.raw_z = self.raw_values
03452 
03453     def _num_sensors(self, raw):
03454         return 3
03455 
03456     def _base_for_value(self, i):
03457         return i * 2
03458 
03459     def print_format(self):
03460         lines = []
03461         lines.append("Raw X: %d" % self.raw_x)
03462         lines.append("Raw Y: %d" % self.raw_y)
03463         lines.append("Raw Z: %d" % self.raw_z)
03464         return "\n".join(lines)
03465 
03466 
03467 class RawMagnetometer(RawSensor):
03468     """Horizon Message Payload - Raw Magnetometer Data"""
03469 
03470     def __init__(self, raw = None, timestamp = 0):
03471         RawSensor.__init__(self, raw, timestamp)
03472 
03473         # Class Variables
03474         self.raw_x, self.raw_y, self.raw_z = self.raw_values
03475 
03476     def _num_sensors(self, raw):
03477         return 3
03478 
03479     def _base_for_value(self, i):
03480         return i * 2
03481 
03482     def print_format(self):
03483         lines = []
03484         lines.append("Raw X: %d" % self.raw_x)
03485         lines.append("Raw Y: %d" % self.raw_y)
03486         lines.append("Raw Z: %d" % self.raw_z)
03487         return "\n".join(lines)
03488 
03489 
03490 
03491 class RestoreSystemConfig(Payload):
03492     """Horizon Message Payload - Restore System Configuration"""
03493 
03494     def __init__(self, passcode = 0x3A18, flags = 1, raw = None, 
03495                  timestamp = 0):
03496         """Create A Horizon Message Payload - Reset"""
03497         
03498         self.passcode = passcode
03499         self.flags = flags
03500 
03501         # Create Constructor
03502         if raw == None:
03503             data = utils.from_unsigned_short(self.passcode) + utils.from_byte(self.flags)
03504         
03505             Payload.__init__(self, raw = data, timestamp = timestamp)
03506         
03507         # Parse Constructor
03508         else:
03509             raise NotImplementedError('Payload %s cannot parse its response.' % self.__class__.__name__ )
03510     
03511     def print_format(self):
03512         """Return the payload as a human readable string"""
03513         return "Passcode: 0x%04X" % self.passcode
03514 
03515 
03516 
03517 
03518 class StoreSystemConfig(Payload):
03519     """Horizon Message Payload - Store System Configuration"""
03520 
03521     def __init__(self, passcode = 0x3A18, raw = None, 
03522                  timestamp = 0):
03523         """Create A Horizon Message Payload - Reset"""
03524         
03525         self.passcode = passcode
03526         
03527         # Create Constructor
03528         if raw == None:
03529             data = utils.from_unsigned_short(self.passcode)
03530         
03531             Payload.__init__(self, raw = data, timestamp = timestamp)
03532         
03533         # Parse Constructor
03534         else:
03535             raise NotImplementedError('Payload %s cannot parse its response.' % self.__class__.__name__ )
03536      
03537     def print_format(self):
03538         """Return the payload as a human readable string"""
03539         return "Passcode: 0x%04X" % self.passcode
03540 
03541 
03542 class ControlFlags(Payload):
03543     """Horizon Message Payload - Control Flags"""
03544 
03545     def __init__(self, passcode = 0, flags = 0, raw = None, timestamp = 0):
03546         """Create A Horizon Message Payload - Platform Information"""
03547         
03548         # Class Variables
03549         self.flags = 0x00000000
03550         self.passcode = passcode
03551         
03552         # Create Constructor - assume this is a set_platform_info payload
03553         if raw == None:
03554             data = utils.from_unsigned_short(self.passcode)
03555             self.flags = flags
03556             data += utils.from_unsigned_int(self.flags)
03557 
03558             # Pass on to super-class
03559             Payload.__init__(self, raw = data, 
03560                              timestamp = timestamp)
03561         
03562         # Parse Constructor
03563         else:
03564             # Pass on to super-class
03565             Payload.__init__(self, raw = raw, timestamp = timestamp)
03566 
03567             # Extract Serial
03568             self.flags = utils.to_unsigned_int(raw[:])
03569             logger.debug("%s flags: %d" % (self.__class__.__name__, self.flags))
03570     
03571 
03572     ## Human Readable Payload String
03573     def print_format(self):        
03574         return "Control Flags: %08X" % self.flags
03575 
03576 
03577 class BatteryEstimationConfig(VariableSensorConfig):
03578     """Horizon Message Payload - Battery Estimation Configuration"""
03579 
03580     def print_format(self):
03581         lines = []
03582         for i in range(len(self.offsets)):
03583             lines.append("Battery %d: Offset: %f,  Scale: %f" % (i + 1, self.offsets[i], self.scales[i]))
03584         return "\n".join(lines)
03585 
03586 
03587 class PlatformKinematics(Payload):
03588 
03589     def __init__(self, passcode = 0, track = 0, wheelbase = 0, 
03590                  raw = None, timestamp = 0):
03591         """Create A Horizon Message Payload - Platform Kinematics"""
03592         
03593         # Class Variables
03594         self.passcode = passcode
03595         self.track = track
03596         self.wheelbase = wheelbase
03597         
03598         # Create Constructor - assume this is a set_platform_kinematics payload
03599         if raw == None:
03600             data = utils.from_unsigned_short(self.passcode)
03601             data += utils.from_unsigned_short(int(self.track * 1000))
03602             data += utils.from_unsigned_short(int(self.wheelbase * 1000))
03603 
03604             # Pass on to super-class
03605             Payload.__init__(self, raw = data, 
03606                              timestamp = timestamp)
03607         
03608         # Parse Constructor
03609         else:
03610             # Pass on to super-class
03611             Payload.__init__(self, raw = raw, timestamp = timestamp)
03612 
03613             self.track = utils.to_unsigned_short(raw[0:2]) / 1000.0
03614             self.wheelbase = utils.to_unsigned_short(raw[2:4]) / 1000.0
03615 
03616     
03617     ## Human Readable Payload String
03618     def print_format(self):
03619         """Return the payload as a human readable string"""
03620         
03621         return "Platform Track: %f\nPlatform Wheelbase: %f" % (self.track, self.wheelbase)
03622 
03623 
03624 
03625 class Config(Payload):
03626     """Horizon Message Indexed Config Payload"""
03627 
03628     def __init__(self, index=0, value=0, raw=None, timestamp=0):
03629         
03630         # Class Variables
03631         self.index = None
03632         self.value = None
03633         
03634         # Create Constructor
03635         if raw == None:
03636             # This is an SET msg going out.
03637             self.index = index
03638             self.value = value
03639 
03640             int_part = math.floor(self.value)
03641             frac_part = self.value - int_part
03642             data = []
03643             data += utils.from_unsigned_short(self.index)
03644             data += utils.from_unsigned_int((1 << 32) * frac_part)
03645             data += utils.from_int(int_part)
03646              
03647             # Pass on to super-class
03648             Payload.__init__(self, raw = data, timestamp = timestamp)
03649         
03650         # Parse Constructor
03651         else:
03652             # This is a DATA msg coming back.
03653 
03654             # Pass on to super-class
03655             Payload.__init__(self, raw = raw, timestamp = timestamp)
03656 
03657             self.index = utils.to_unsigned_short(raw[0:2])
03658             frac_part = float(utils.to_unsigned_int(raw[2:6])) / (1 << 32)
03659             int_part = utils.to_int(raw[6:10])
03660             self.value = int_part + frac_part
03661 
03662     def print_format(self):
03663         return "Config #%d: %f" % (self.index, self.value)
03664 
03665 
03666 class ConfigRequest(Payload):
03667     """Horizon Message Indexed Config Payload"""
03668 
03669     def __init__(self, index=0, raw=None, timestamp=0):
03670         
03671         # Class Variables
03672         self.index = None
03673         
03674         # Create Constructor
03675         if raw == None:
03676             # This is an SET msg going out.
03677             self.index = index
03678             data = []
03679             data += utils.from_unsigned_short(self.index)
03680              
03681             # Pass on to super-class
03682             Payload.__init__(self, raw = data, timestamp = timestamp)
03683         
03684         # Parse Constructor
03685         else:
03686             raise NotImplementedError()
03687 
03688     def print_format(self):
03689         return "Request for config #%d" % self.index
03690 
03691 
03692 logger.debug("... clearpath.horizon.payloads loaded.")


clearpath_base
Author(s): Mike Purvis
autogenerated on Sun Oct 5 2014 22:52:08