diff --git a/.gitignore b/.gitignore index 51bd0bdcf..d4e2d4fa7 100644 --- a/.gitignore +++ b/.gitignore @@ -66,14 +66,14 @@ ground/gcs/.settings /.settings /.pydevproject /workspace - -# Ignore Eclipse temp folder, git plugin based? -RemoteSystemsTempFiles - -# Ignore patch-related files -*.rej -*.orig -*.diff~ - -# ignore ccache storage + +# Ignore Eclipse temp folder, git plugin based? +RemoteSystemsTempFiles + +# Ignore patch-related files +*.rej +*.orig +*.diff~ + +# ignore ccache storage .ccache diff --git a/Makefile b/Makefile index 2b68e04dc..cb9aa4be0 100644 --- a/Makefile +++ b/Makefile @@ -209,6 +209,23 @@ uavobjects_%: $(UAVOBJGENERATOR) $(UAVOBJGENERATOR) -$* $(UAVOBJ_XML_DIR) $(ROOT_DIR) ; \ ) +OBJECTCOUNT := $(shell find $(ROOT_DIR)/ground/pyuavtalk/openpilot/uavobjects/ -name '*.py' | wc -l) +uavobjects_python_install: + $(V1) if [ $(OBJECTCOUNT) -gt 2 ]; then echo "UAVObjects already exist"; else make uavobjects_python; fi + $(V1) mkdir -p $(ROOT_DIR)/ground/pyuavtalk/openpilot/uavobjects/ + $(V1) ( touch $(ROOT_DIR)/ground/pyuavtalk/openpilot/uavobjects/__init__.py ) + $(V1) ( cp $(UAVOBJ_OUT_DIR)/python/* $(ROOT_DIR)/ground/pyuavtalk/openpilot/uavobjects/ ) + $(V1) ( cd $(ROOT_DIR)/ground/pyuavtalk/ && sudo python setup.py build && sudo python setup.py install) + +uavobjects_python_clean: + @$(ECHO) " CLEAN $(call toprel, $(ROOT_DIR)/ground/pyuavtalk/openpilot/uavobjects/)" + $(V1) [ ! -d "$(ROOT_DIR)/ground/pyuavtalk/openpilot/uavobjects/" ] || $(RM) -r "$(ROOT_DIR)/ground/pyuavtalk/openpilot/uavobjects/" + @$(ECHO) " CLEAN $(call toprel, $(ROOT_DIR)/ground/pyuavtalk/build/)" + $(V1) [ ! -d "$(ROOT_DIR)/ground/pyuavtalk/build/" ] || sudo $(RM) -r "$(ROOT_DIR)/ground/pyuavtalk/build/" + @$(ECHO) " CLEAN $(call toprel, $(UAVOBJ_OUT_DIR)/python/)" + $(V1) [ ! -d "$(UAVOBJ_OUT_DIR)/python/" ] || $(RM) -r "$(UAVOBJ_OUT_DIR)/python/" + + uavobjects_test: $(UAVOBJGENERATOR) $(V1) $(UAVOBJGENERATOR) -v $(UAVOBJ_XML_DIR) $(ROOT_DIR) @@ -711,6 +728,10 @@ help: @$(ECHO) " uavobjects_test - Parse xml-files - check for valid, duplicate ObjId's, ..." @$(ECHO) " uavobjects_ - Generate source files from a subset of the UAVObject definition XML files" @$(ECHO) " Supported groups are ($(UAVOBJ_TARGETS))" + @$(ECHO) " uavobjects_python_install" + @$(ECHO) " - Install generated python files as eggs for use with example Python scripts" + @$(ECHO) " uavobjects_python_clean" + @$(ECHO) " - Remove generated python UAVOs from build directory & pyuavtalk folder" @$(ECHO) @$(ECHO) " [Packaging]" @$(ECHO) " package - Build and package the platform-dependent package (no clean)" diff --git a/flight/modules/FlightPlan/lib/uavobject.pyt.template b/flight/modules/FlightPlan/lib/uavobject.pyt.template index ff6bab315..942fb64fb 100644 --- a/flight/modules/FlightPlan/lib/uavobject.pyt.template +++ b/flight/modules/FlightPlan/lib/uavobject.pyt.template @@ -30,14 +30,14 @@ # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA # -from uavobject import * +from openpilot.uavtalk.uavobject import * $(DATAFIELDS) # Object $(NAME) definition class $(NAME)(UAVObject): # Object constants - OBJID = $(UOBJID) + OBJID = $(OBJIDHEX) # Constructor def __init__(self): diff --git a/ground/pyuavtalk/examples/example.py b/ground/pyuavtalk/examples/example.py new file mode 100644 index 000000000..32e1583fb --- /dev/null +++ b/ground/pyuavtalk/examples/example.py @@ -0,0 +1,218 @@ +## +############################################################################## +# +# @file example.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @brief Base classes for python UAVObject +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + + + +import logging +import serial +import traceback +import sys + +from openpilot.uavtalk.uavobject import * +from openpilot.uavtalk.uavtalk import * +from openpilot.uavtalk.objectManager import * +from openpilot.uavtalk.connectionManager import * + + +def _hex02(value): + return "%02X" % value + +class UavtalkDemo(): + + def __init__(self): + self.nbUpdates = 0 + self.lastRateCalc = time.time() + self.updateRate = 0 + self.objMan = None + self.connMan = None + + def setup(self, port): + print "Opening Port \"%s\"" % port + if port[:3].upper() == "COM": + _port = int(port[3:])-1 + else: + _port = port + serPort = serial.Serial(_port, 57600, timeout=.5) + if not serPort.isOpen(): + raise IOError("Failed to open serial port") + + print "Creating UavTalk" + self.uavTalk = UavTalk(serPort, None) + + print "Starting ObjectManager" + self.objMan = ObjManager(self.uavTalk) + self.objMan.importDefinitions() + + print "Starting UavTalk" + self.uavTalk.start() + + #print "Starting ConnectionManager" + #self.connMan = ConnectionManager(self.uavTalk, self.objMan) + + #print "Connecting...", + #self.connMan.connect() + #print "Connected" + + #print "Getting all Data" + #self.objMan.requestAllObjUpdate() + + #print "SN:", + #sn = self.objMan.FirmwareIAPObj.CPUSerial.value + #print "".join(map(_hex02, sn)) + + def stop(self): + if self.uavTalk: + print "Stopping UavTalk" + self.uavTalk.stop() + + def showAttitudeViaObserver(self): + print "Request fast periodic updates for AttitudeState" + self.objMan.AttitudeState.metadata.telemetryUpdateMode.value = UAVMetaDataObject.UpdateMode.PERIODIC + self.objMan.AttitudeState.metadata.telemetryUpdatePeriod.value = 50 + self.objMan.AttitudeState.metadata.updated() + + print "Install Observer for AttitudeState updates\n" + self.objMan.regObjectObserver(self.objMan.AttitudeState, self, "_onAttitudeUpdate") + # Spin until we get interrupted + while True: + time.sleep(1) + + def showAttitudeViaWait(self): + print "Request fast periodic updates for AttitudeState" + self.objMan.AttitudeState.metadata.telemetryUpdateMode.value = UAVMetaDataObject.UpdateMode.PERIODIC + self.objMan.AttitudeState.metadata.telemetryUpdatePeriod.value = 50 + self.objMan.AttitudeState.metadata.updated() + + while True: + self.objMan.AttitudeState.waitUpdate() + self._onAttitudeUpdate(self.objMan.AttitudeState) + + def showAttitudeViaGet(self): + while True: + self.objMan.AttitudeState.getUpdate() + self._onAttitudeUpdate(self.objMan.AttitudeState) + + def _onAttitudeUpdate(self, args): + self.nbUpdates += 1 + + now = time.time() + if now-self.lastRateCalc > 1: + self.updateRate = self.nbUpdates/(now-self.lastRateCalc) + self.lastRateCalc = now + self.nbUpdates = 0 + + if self.nbUpdates & 1: + dot = "." + else: + dot= " " + + print " %s Rate: %02.1f Hz " % (dot, self.updateRate), + + roll = self.objMan.AttitudeState.Roll.value + print "RPY: %f %f %f " % (self.objMan.AttitudeState.Roll.value,self.objMan.AttitudeState.Pitch.value,self.objMan.AttitudeState.Yaw.value)+" \r", + return + print "Roll: %f " % roll, + i = roll/90 + if i<-1: i=-1 + if i>1: i= 1 + i = int((i+1)*15) + print "-"*i+"*"+"-"*(30-i)+" \r", + + def driveServo(self): + print "Taking control of self.actuatorCmd" + self.objMan.ActuatorCommand.metadata.access.value = UAVMetaDataObject.Access.READONLY + self.objMan.ActuatorCommand.metadata.updated() + self.objMan.ManualControlCommand.metadata.access.value = UAVMetaDataObject.Access.READONLY + self.objMan.ManualControlCommand.metadata.updated() + + while True: + self.objMan.ActuatorCommand.Channel.value[0] = 1000 + self.objMan.ActuatorCommand.updated() + time.sleep(1) + + self.objMan.ActuatorCommand.Channel.value[0] = 2000 + self.objMan.ActuatorCommand.updated() + time.sleep(1) + + +def printUsage(): + appName = os.path.basename(sys.argv[0]) + print + print "usage:" + print " %s port o|w|g|s" % appName + print " o: Show Attitude using an \"observer\"" + print " w: Show Attitude waiting for updates from flight" + print " g: Show Attitude performing get operations" + print " s: Drive Servo" + print + print " for example: %s COM30 o" % appName + print + +if __name__ == '__main__': + + if len(sys.argv) != 3: + print "ERROR: Incorrect number of arguments" + printUsage() + sys.exit(2) + + port, option = sys.argv[1:] + + if option not in ["o","w","g","s"]: + print "ERROR: Invalid option" + printUsage() + sys.exit(2) + + # Log everything, and send it to stderr. + logging.basicConfig(level=logging.INFO) + + try: + demo = UavtalkDemo() + demo.setup(port) + + if option == "o": + demo.showAttitudeViaObserver() # will not return + elif option == "w": + demo.showAttitudeViaWait() # will not return + if option == "g": + demo.showAttitudeViaGet() # will not return + if option == "s": + demo.driveServo() # will not return + + except KeyboardInterrupt: + pass + except Exception,e: + print + print "An error occured: ", e + print + traceback.print_exc() + + print + + try: + demo.stop() + except Exception: + pass + diff --git a/ground/pyuavtalk/examples/example_readlog.py b/ground/pyuavtalk/examples/example_readlog.py new file mode 100644 index 000000000..c13848e98 --- /dev/null +++ b/ground/pyuavtalk/examples/example_readlog.py @@ -0,0 +1,182 @@ +## +############################################################################## +# +# @file example_readlog.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @brief Base classes for python UAVObject +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + + + +import logging +import serial +import traceback +import sys + +from openpilot.uavtalk.uavobject import * +from openpilot.uavtalk.uavtalk import * +from openpilot.uavtalk.objectManager import * +from openpilot.uavtalk.connectionManager import * + + +def _hex02(value): + return "%02X" % value + +class UavtalkDemo(): + + def __init__(self): + self.nbUpdates = 0 + self.lastRateCalc = time.time() + self.updateRate = 0 + self.objMan = None + self.connMan = None + + def setup(self, port, filename): + print "Opening File \"%s\"" % filename + file = open(filename,"rb") + if file == None: + raise IOError("Failed to open file") + + print "Creating UavTalk" + self.uavTalk = UavTalk(None, filename) + + print "Starting ObjectManager" + self.objMan = ObjManager(self.uavTalk) + self.objMan.importDefinitions() + + print "Starting UavTalk" + self.uavTalk.start() + + def stop(self): + if self.uavTalk: + print "Stopping UavTalk" + self.uavTalk.stop() + + def showAttitudeViaObserver(self): + print "Request fast periodic updates for AttitudeActual" + self.objMan.AttitudeActual.metadata.telemetryUpdateMode.value = UAVMetaDataObject.UpdateMode.PERIODIC + self.objMan.AttitudeActual.metadata.telemetryUpdatePeriod.value = 50 + self.objMan.AttitudeActual.metadata.updated() + + print "Install Observer for AttitudeActual updates\n" + self.objMan.regObjectObserver(self.objMan.AttitudeActual, self, "_onAttitudeUpdate") + # Spin until we get interrupted + while True: + time.sleep(1) + + def showAttitudeViaWait(self): + print "Request fast periodic updates for AttitudeActual" + self.objMan.AttitudeActual.metadata.telemetryUpdateMode.value = UAVMetaDataObject.UpdateMode.PERIODIC + self.objMan.AttitudeActual.metadata.telemetryUpdatePeriod.value = 50 + self.objMan.AttitudeActual.metadata.updated() + + while True: + self.objMan.AttitudeActual.waitUpdate() + self._onAttitudeUpdate(self.objMan.AttitudeActual) + + def showAttitudeViaGet(self): + while True: + self.objMan.AttitudeActual.getUpdate() + self._onAttitudeUpdate(self.objMan.AttitudeActual) + + def _onAttitudeUpdate(self, args): + self.nbUpdates += 1 + + now = time.time() + if now-self.lastRateCalc > 1: + self.updateRate = self.nbUpdates/(now-self.lastRateCalc) + self.lastRateCalc = now + self.nbUpdates = 0 + + if self.nbUpdates & 1: + dot = "." + else: + dot= " " + + print " %s Rate: %02.1f Hz " % (dot, self.updateRate), + + roll = self.objMan.AttitudeActual.Roll.value + print "Roll: %-4d " % roll, + i = roll/90 + if i<-1: i=-1 + if i>1: i= 1 + i = int((i+1)*15) + print "-"*i+"*"+"-"*(30-i)+" \r", + + def driveServo(self): + print "Taking control of self.actuatorCmd" + self.objMan.ActuatorCommand.metadata.access.value = UAVMetaDataObject.Access.READONLY + self.objMan.ActuatorCommand.metadata.updated() + self.objMan.ManualControlCommand.metadata.access.value = UAVMetaDataObject.Access.READONLY + self.objMan.ManualControlCommand.metadata.updated() + + while True: + self.objMan.ActuatorCommand.Channel.value[0] = 1000 + self.objMan.ActuatorCommand.updated() + time.sleep(1) + + self.objMan.ActuatorCommand.Channel.value[0] = 2000 + self.objMan.ActuatorCommand.updated() + time.sleep(1) + + +def printUsage(): + appName = os.path.basename(sys.argv[0]) + print + print "usage:" + print " %s filename " % appName + print + print " for example: %s /tmp/OP-2015-04-28_23-16-33.opl" % appName + print + +if __name__ == '__main__': + + if len(sys.argv) !=2: + print "ERROR: Incorrect number of arguments" + print len(sys.argv) + printUsage() + sys.exit(2) + + script, filename = sys.argv + if not os.path.exists(sys.argv[1]): + sys.exit('ERROR: Database %s was not found!' % sys.argv[1]) + + # Log everything, and send it to stderr. + logging.basicConfig(level=logging.INFO) + + try: + demo = UavtalkDemo() + demo.setup(None, filename) + + + + except KeyboardInterrupt: + pass + except Exception,e: + print + print "An error occured: ", e + print + traceback.print_exc() + + print + + + diff --git a/ground/pyuavtalk/examples/example_tuning.py b/ground/pyuavtalk/examples/example_tuning.py new file mode 100644 index 000000000..8640b0fb9 --- /dev/null +++ b/ground/pyuavtalk/examples/example_tuning.py @@ -0,0 +1,204 @@ +## +############################################################################## +# +# @file example.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @brief Base classes for python UAVObject +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + + + +import logging +import serial +import traceback +import sys + +from openpilot.uavtalk.uavobject import * +from openpilot.uavtalk.uavtalk import * +from openpilot.uavtalk.objectManager import * +from openpilot.uavtalk.connectionManager import * + + + +def printUsage(): + appName = os.path.basename(sys.argv[0]) + print + print "usage:" + print " %s port " % appName + print " for example: %s COM30 " % appName + print + +if __name__ == '__main__': + try: + if len(sys.argv) != 2: + print "ERROR: Incorrect number of arguments" + printUsage() + sys.exit(2) + + port = sys.argv[1] + + if port[:3].upper() == "COM": + _port = int(port[3:])-1 + else: + _port = port + + serPort = serial.Serial(_port, 57600, timeout=.5) + uavTalk = UavTalk(serPort, None) + objMan = ObjManager(uavTalk) + objMan.importDefinitions() + uavTalk.start() + + import objectpersistence + + print "Getting Current Settings:" + for _ in range(2): # Try only twice to get the settings + try: + time.sleep(10) + objMan.StabilizationSettingsBank1.getUpdate() + except TimeoutException: + print "Timeout" + pass + except KeyboardInterrupt: + os._exit(1) + else: + raise + + while True: + while True: + print + print + print "q. Quit" + print "s. Save settings" + print + print "1. Tune Roll Rate %2.4f %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.RollRatePID.value) + print "2. Tune Pitch Rate %2.4f %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.PitchRatePID.value) + print "3. Tune Yaw Rate %2.4f %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.YawRatePID.value) + print + print "4. Tune Roll Attitude %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.RollPI.value) + print "5. Tune Pitch Attitude %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.PitchPI.value) + print "6. Tune Yaw Attitude %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.YawPI.value) + print + sel = raw_input() + if len(sel) != 1: + continue + elif sel == "q": + exit(0) + elif sel == "s": + print "Saving ", + objMan.ObjectPersistence.Operation.value = objectpersistence.OperationField.SAVE + objMan.ObjectPersistence.Selection.value = objectpersistence.SelectionField.SINGLEOBJECT + objMan.ObjectPersistence.ObjectID.value = objMan.StabilizationSettingsBank1.objId + objMan.ObjectPersistence.InstanceID.value = objMan.StabilizationSettingsBank1.instId + objMan.ObjectPersistence.updated() + for i in range(10): + print ".", + objMan.ObjectPersistence.getUpdate(timeout=1) + if objMan.ObjectPersistence.Operation.value == objectpersistence.OperationField.COMPLETED: + print "Done" + break + print + elif sel == "1": + PI = objMan.StabilizationSettingsBank1.RollRatePID.value + break + elif sel == "2": + PI = objMan.StabilizationSettingsBank1.PitchRatePI.value + break + elif sel == "3": + PI = objMan.StabilizationSettingsBank1.YawRatePI.value + break + elif sel == "4": + PI = objMan.StabilizationSettingsBank1.RollPI.value + break + elif sel == "5": + PI = objMan.StabilizationSettingsBank1.PitchPI.value + break + elif sel == "6": + PI = objMan.StabilizationSettingsBank1.YawPI.value + break + + while True: + print + print + print "q. Quit" + print + print "1. tune K %2.4f" % PI[0] + print "2. tune I %2.4f" % PI[1] + print "3. tune I Limit %2.4f" % PI[2] + sel = raw_input() + if len(sel) != 1: + continue + elif sel == "q": + exit(0) + elif sel == "1": + PIIndex = 0 + break + elif sel == "2": + PIIndex = 1 + break + elif sel == "3": + PIIndex = 2 + break + + print + + while True: + try: + print "Current value: %2.4f" % PI[PIIndex] + print "Tune-range from", + tuneFrom = float(raw_input()) + print "Tune-range to", + tuneTo = float(raw_input()) + break + except Exception: + pass + + + print + print + while True: + try: + # get update of ManualControlCommand + objMan.ManualControlCommand.getUpdate(timeout=.5) + + # calculate value out of Accessory1 input (-1 ... +1) + txControl = objMan.ManualControlCommand.Accessory1.value + value = tuneFrom + ((txControl+1)/2)*(tuneTo-tuneFrom) + PI[PIIndex] = value + objMan.StabilizationSettingsBank1.updated() + + print "\r%-1.2f => %2.4f" % (txControl, value), + time.sleep(.1) + + except TimeoutException: + print "Timeout \a" + print + except Exception,e: + print + print "An error occured: ", e + print + traceback.print_exc() + + try: + print "Stop" + uavTalk.stop() + except Exception: + pass + diff --git a/ground/pyuavtalk/openpilot/__init__.py b/ground/pyuavtalk/openpilot/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ground/pyuavtalk/openpilot/uavobjects/__init__.py b/ground/pyuavtalk/openpilot/uavobjects/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ground/pyuavtalk/openpilot/uavtalk/__init__.py b/ground/pyuavtalk/openpilot/uavtalk/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ground/pyuavtalk/openpilot/uavtalk/connectionManager.py b/ground/pyuavtalk/openpilot/uavtalk/connectionManager.py new file mode 100644 index 000000000..e99bf1477 --- /dev/null +++ b/ground/pyuavtalk/openpilot/uavtalk/connectionManager.py @@ -0,0 +1,102 @@ +## +############################################################################## +# +# @file connectionManager.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @brief Base classes for python UAVObject +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + + + +import logging +import time + +import objectManager +from openpilot.uavtalk import flighttelemetrystats +import uavobject + + +class ConnectionManager(object): + + def __init__(self, uavTalk, objMan): + self.uavTalk = uavTalk + self.objMan = objMan + self.connected = False + + + + self.ftsObj = self.objMan.FlightTelemetryStats + self.gcsObj = self.objMan.GCSTelemetryStats + + self.statusFieldClss = flighttelemetrystats.StatusField + + + def connect(self): + timeout = True + logging.debug("Connecting") + startTime = time.clock() + while not self.connected: + try: + self.objMan.waitObjUpdate(self.ftsObj, request=timeout, timeout=2) + timeout = False + self._onFtsChange() + if self.connected: + self.objMan.waitObjUpdate(self.ftsObj.metadata) + self.ftsObj.metadata.telemetryUpdateMode.value = uavobject.UAVMetaDataObject.UpdateMode.PERIODIC + self.ftsObj.metadata.telemetryUpdatePeriod.value = 1000 + self.ftsObj.metadata.updated() + self.objMan.regObjectObserver(self.ftsObj, self, "_onFtsChange") + else: + pass + except objectManager.TimeoutException: + timeout = True + self.connected = False + logging.warning("Connecting TO") + pass + logging.debug("Connected in %.1fs" % (time.clock()-startTime)) + + + def _onFtsChange(self, args=None): + connected = False + logging.debug("FTS State=%d TxFail=%3d RxFail=%3d TxRetry=%3d" % \ + (self.ftsObj.Status.value, self.ftsObj.TxFailures.value, self.ftsObj.RxFailures.value, self.ftsObj.TxRetries.value)) + + if self.ftsObj.Status.value == self.statusFieldClss.DISCONNECTED: + logging.debug(" Handshake REQ") + self.gcsObj.Status.value = self.statusFieldClss.HANDSHAKEREQ + self.gcsObj.updated() + + elif self.ftsObj.Status.value == self.statusFieldClss.HANDSHAKEACK: + logging.debug(" Got Handshake ACK") + self.gcsObj.Status.value = self.statusFieldClss.CONNECTED + self.gcsObj.updated() + + elif self.ftsObj.Status.value == self.statusFieldClss.CONNECTED: + connected = True + + if self.connected: + if not connected: + logging.warning("DISCONNECTED") + else: + if connected: + logging.debug("CONNECTED") + + self.connected = connected diff --git a/ground/pyuavtalk/openpilot/uavtalk/flighttelemetrystats.py b/ground/pyuavtalk/openpilot/uavtalk/flighttelemetrystats.py new file mode 100644 index 000000000..4c939a16c --- /dev/null +++ b/ground/pyuavtalk/openpilot/uavtalk/flighttelemetrystats.py @@ -0,0 +1,125 @@ +## +############################################################################## +# +# @file flighttelemetrystats.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# @brief Implementation of the FlightTelemetryStats object. This file has been +# automatically generated by the UAVObjectGenerator. For use with +# the PyMite VM of the FlightPlan module. +# +# @note Object definition file: flighttelemetrystats.xml. +# This is an automatically generated file. +# DO NOT modify manually. +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + +from openpilot.uavtalk.uavobject import * + +# Field TxDataRate definition +class TxDataRateField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 6, 1) + +# Field TxBytes definition +class TxBytesField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 5, 1) + +# Field TxFailures definition +class TxFailuresField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 5, 1) + +# Field TxRetries definition +class TxRetriesField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 5, 1) + +# Field RxDataRate definition +class RxDataRateField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 6, 1) + +# Field RxBytes definition +class RxBytesField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 5, 1) + +# Field RxFailures definition +class RxFailuresField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 5, 1) + +# Field RxSyncErrors definition +class RxSyncErrorsField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 5, 1) + +# Field RxCrcErrors definition +class RxCrcErrorsField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, 5, 1) + +# Field Status definition +class StatusField(UAVObjectField): + # Enumeration options + DISCONNECTED = 0 + HANDSHAKEREQ = 1 + HANDSHAKEACK = 2 + CONNECTED = 3 + def __init__(self): + UAVObjectField.__init__(self, 7, 1) + + + +# Object FlightTelemetryStats definition +class FlightTelemetryStats(UAVObject): + # Object constants + OBJID = 1731705690 + + # Constructor + def __init__(self): + UAVObject.__init__(self, FlightTelemetryStats.OBJID) + + # Create object fields + self.TxDataRate = TxDataRateField() + self.addField(self.TxDataRate) + self.TxBytes = TxBytesField() + self.addField(self.TxBytes) + self.TxFailures = TxFailuresField() + self.addField(self.TxFailures) + self.TxRetries = TxRetriesField() + self.addField(self.TxRetries) + self.RxDataRate = RxDataRateField() + self.addField(self.RxDataRate) + self.RxBytes = RxBytesField() + self.addField(self.RxBytes) + self.RxFailures = RxFailuresField() + self.addField(self.RxFailures) + self.RxSyncErrors = RxSyncErrorsField() + self.addField(self.RxSyncErrors) + self.RxCrcErrors = RxCrcErrorsField() + self.addField(self.RxCrcErrors) + self.Status = StatusField() + self.addField(self.Status) + + # Read field data + self.read() + self.metadata.read() diff --git a/ground/pyuavtalk/openpilot/uavtalk/objectManager.py b/ground/pyuavtalk/openpilot/uavtalk/objectManager.py new file mode 100644 index 000000000..0849c57ce --- /dev/null +++ b/ground/pyuavtalk/openpilot/uavtalk/objectManager.py @@ -0,0 +1,153 @@ +## +############################################################################## +# +# @file objectManager.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @brief Base classes for python UAVObject +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + + + +import logging +import sys +import os +import inspect + +from openpilot.uavtalk.uavobject import * + + +class TimeoutException(Exception): + pass + +class ObjManager(object): + + def __init__(self, uavTalk): + self.objs = {} + self.uavTalk = uavTalk + uavTalk.setObjMan(self) + + + def addObj(self, obj): + obj.objMan = self + self.objs[obj.objId] = obj + + def getObj(self, objId): + try: + return self.objs[objId] + except KeyError: + return None + + def getObjByName(self, name): + for objId, obj in self.objs.items(): + if obj.name == name: + return obj + return None + + def importDefinitions(self, uavObjDefPath=None): + # when the uavObjDefPath is nor defined, assume it is installed together with this module + if uavObjDefPath == None: + currModPath = os.path.dirname(sys.modules[__name__].__file__) + uavObjDefPath = os.path.join(currModPath, "..", "uavobjects") + + logging.info("Importing UAVObject definitions from %s" % uavObjDefPath) + sys.path.append(uavObjDefPath) + for fileName in os.listdir(uavObjDefPath): + if fileName[-3:] == ".py": + logging.debug("Importing from file %s", fileName) + module = __import__(fileName.replace(".py","")) + for name in dir(module): + klass = getattr(module, name) + obj = getattr(module, name) + if inspect.isclass(obj): + if name != "UAVObject" and name != "UAVMetaDataObject" and name != "UAVDataObject" and issubclass(klass, UAVObject): + logging.debug("Importing class %s", name) + obj = klass() + obj.name = name + setattr(self, name, obj) + self.addObj(obj) + metaObj = UAVMetaDataObject(obj.getMetaObjId()) + obj.metadata = metaObj + metaObj.name = "Meta[%s]" % name + self.addObj(metaObj) + + def regObjectObserver(self, obj, observerObj, observerMethod): + o = Observer(observerObj, observerMethod) + obj.observers.append(o) + + def objUpdate(self, obj, rxData): + obj.deserialize(rxData) + obj.updateCnt += 1 + for observer in obj.observers: + observer.call(obj) + obj.updateEvent.acquire() + obj.updateEvent.notifyAll() + obj.updateEvent.release() + + def requestObjUpdate(self, obj): + logging.debug("Requesting %s" % obj) + self.uavTalk.sendObjReq(obj) + + def waitObjUpdate(self, obj, request=True, timeout=.5): + logging.debug("Waiting for %s " % obj) + cnt = obj.updateCnt + if request: + self.requestObjUpdate(obj) + obj.updateEvent.acquire() + obj.updateEvent.wait(timeout) + obj.updateEvent.release() + timeout = (cnt == obj.updateCnt) + logging.debug("-> Waiting for %s Done. " % (obj)) + if timeout: + s = "Timeout waiting for %s" % obj + logging.debug(s) + raise TimeoutException(s) + + def objLocallyUpdated(self, obj): + # TODO: should check meta-data what to do + self.uavTalk.sendObject(obj) + + def requestAllObjUpdate(self): + for objId, obj in self.objs.items(): + if not obj.isMetaData(): + #print "GetMeta %s" % obj + try: + logging.debug("Getting %s" % obj) + self.waitObjUpdate(obj, request=True, timeout=.1) + logging.debug(" Getting %s" % obj.metadata) + self.waitObjUpdate(obj.metadata, request=True, timeout=.1) + except TimeoutException: + logging.debug(" TIMEOUT") + pass + + def disableAllAutomaticUpdates(self): + + objsToExclude = [self.getObjByName("GCSTelemetryStats"), self.getObjByName("FlightTelemetryStats"), self.getObjByName("ObjectPersistence")] + for i in xrange(len(objsToExclude)): + objsToExclude[i] = objsToExclude[i].metadata.objId + + for objId, obj in self.objs.items(): + if obj.isMetaData() and obj.updateCnt>0: + if obj.objId not in objsToExclude: + #print "Disabling automatic updates for %s" % (obj) + #print obj.telemetryUpdateMode.value + obj.telemetryUpdateMode.value = UAVMetaDataObject.UpdateMode.MANUAL + self.uavTalk.sendObject(obj) + diff --git a/ground/pyuavtalk/openpilot/uavtalk/uavobject.py b/ground/pyuavtalk/openpilot/uavtalk/uavobject.py new file mode 100644 index 000000000..34ee28adc --- /dev/null +++ b/ground/pyuavtalk/openpilot/uavtalk/uavobject.py @@ -0,0 +1,294 @@ +## +############################################################################## +# +# @file uavobject.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# @brief Base classes for python UAVObject +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + +"""__NATIVE__ +#include "openpilot.h" + +#define TYPE_INT8 0 +#define TYPE_INT16 1 +#define TYPE_INT32 2 +#define TYPE_UINT8 3 +#define TYPE_UINT16 4 +#define TYPE_UINT32 5 +#define TYPE_FLOAT32 6 +#define TYPE_ENUM 7 + +""" + +#from list import append + +import threading +import struct + + +class UAVObjectField: + class FType: + INT8 = 0 + INT16 = 1 + INT32 = 2 + UINT8 = 3 + UINT16 = 4 + UINT32 = 5 + FLOAT32 = 6 + ENUM = 7 + + def __init__(self, ftype, numElements): + + self.ftype = ftype + self.numElements = numElements + + if self.ftype == UAVObjectField.FType.INT8: + vfmt = "b" + self.rawSizePerElem = 1 + elif self.ftype == UAVObjectField.FType.UINT8 or self.ftype == UAVObjectField.FType.ENUM: + vfmt = "B" + self.rawSizePerElem = 1 + elif self.ftype == UAVObjectField.FType.INT16: + vfmt = "h" + self.rawSizePerElem = 2 + elif self.ftype == UAVObjectField.FType.UINT16: + vfmt = "H" + self.rawSizePerElem = 2 + elif self.ftype == UAVObjectField.FType.INT32: + vfmt = "i" + self.rawSizePerElem = 4 + elif self.ftype == UAVObjectField.FType.UINT32: + vfmt = "I" + self.rawSizePerElem = 4 + elif self.ftype == UAVObjectField.FType.FLOAT32: + vfmt = "f" + self.rawSizePerElem = 4 + else: + raise ValueError() + fmt = "<" + vfmt * numElements + self.struct = struct.Struct(fmt) + + self.fmt = fmt + + if ftype == UAVObjectField.FType.FLOAT32: + baseValue = 0.0 + else: + baseValue = 0 + + if numElements == 1: + self.value = baseValue + else: + self.value = [baseValue] * numElements + + self.rawSize = self.rawSizePerElem * self.numElements + + def getRawSize(self): + return self.rawSize + + def serialize(self, ser): + if self.numElements == 1: + ser += map(ord, self.struct.pack(self.value)) + else: + ser += map(ord, apply(self.struct.pack, self.value)) + + def deserialize(self, data): + # DOTO: FIXME: This is getting very messy + values = list(self.struct.unpack("".join(map(chr, data[:self.rawSize])))) + if self.numElements == 1: + self.value = values[0] + else: + self.value = values + return self.rawSize + + +class Observer(object): + def __init__(self, obj, method): + self.methodToCall = getattr(obj, method) + + def call(self, *args): + self.methodToCall(args) + + +class UAVObject: + def __init__(self, objId, name=None): + self.metadata = self # FIXME + self.objId = objId + self.instId = 0 + self.fields = [] + self.observers = [] + self.updateEvent = threading.Condition() + self.updateCnt = 0 + self.name = name + self.objMan = None + + def updated(self): + self.objMan.objLocallyUpdated(self) + + + def waitUpdate(self, timeout=5): + self.objMan.waitObjUpdate(self, request=False, timeout=timeout) + + def getUpdate(self, timeout=.1): + self.objMan.waitObjUpdate(self, request=True, timeout=timeout) + + def addField(self, field): + self.fields.append(field) + + def getSerialisedSize(self): + size = 0 + for field in self.fields: + size += field.getRawSize() + return size + + def serialize(self): + ser = [] + for field in self.fields: + field.serialize(ser) + return ser + + def deserialize(self, data): + p = 0 + for field in self.fields: + p += field.deserialize(data[p:]) + + def getName(self): + pass + + def read(self): + pass + + def write(self): + pass + + def getMetaObjId(self): + return self.objId + 1 # FIXME, should be in UAVDataObject + + def isMetaData(self): + return False + + def __str__(self): + if self.name != None: + if self.isMetaData(): + return "%s" % self.name + else: + return "%s" % self.name + else: + if self.isMetaData(): + return "UAVMetaObj of %08x" % (self.objId - 1) + else: + return "UAVObj %08x" % self.objId + + +#class UAVDataObject(UAVObject): +# pass # TODO + + + +class MetaFlightAccessField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.ENUM, 1) + + +class MetaGCSAccessField(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.ENUM, 1) + + +class MetaFlightTelemetryAcked(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.ENUM, 1) + + +class MetaFlightTelemetryUpdateMode(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.ENUM, 1) + + +class MetaFlightTelemetryUpdatePeriod(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.UINT32, 1) + + +class MetaGCSTelemetryAcked(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.ENUM, 1) + + +class MetaGCSTelemetryUpdateMode(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.ENUM, 1) + + +class MetaGCSTelemetryUpdatePeriod(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.UINT32, 1) + + +class MetaLoggingUpdateMode(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.ENUM, 1) + + +class MetaLoggingUpdatePeriod(UAVObjectField): + def __init__(self): + UAVObjectField.__init__(self, UAVObjectField.FType.UINT32, 1) + + +class UAVMetaDataObject(UAVObject): + class UpdateMode: + PERIODIC = 0 + ONCHANGE = 1 + MANUAL = 2 + NEVER = 3 + + class Access: + READWRITE = 0 + READONLY = 1 + + def __init__(self, objId): + UAVObject.__init__(self, objId) + + self.access = MetaFlightAccessField() + self.addField(self.access) + self.gcsAccess = MetaGCSAccessField() + self.addField(self.gcsAccess) + + self.telemetryAcked = MetaFlightTelemetryAcked() + self.addField(self.telemetryAcked) + self.telemetryUpdateMode = MetaFlightTelemetryUpdateMode() + self.addField(self.telemetryUpdateMode) + self.telemetryUpdatePeriod = MetaFlightTelemetryUpdatePeriod() + self.addField(self.telemetryUpdatePeriod) + + self.gcsTelemetryAcked = MetaGCSTelemetryAcked() + self.addField(self.gcsTelemetryAcked) + self.gcsTelemetryUpdateMode = MetaGCSTelemetryUpdateMode() + self.addField(self.gcsTelemetryUpdateMode) + self.gcsTelemetryUpdatePeriod = MetaGCSTelemetryUpdatePeriod() + self.addField(self.gcsTelemetryUpdatePeriod) + + self.loggingUpdateMode = MetaLoggingUpdateMode() + self.addField(self.loggingUpdateMode) + self.loggingUpdatePeriod = MetaLoggingUpdatePeriod() + self.addField(self.loggingUpdatePeriod) + + def isMetaData(self): + return True diff --git a/ground/pyuavtalk/openpilot/uavtalk/uavtalk.py b/ground/pyuavtalk/openpilot/uavtalk/uavtalk.py new file mode 100644 index 000000000..72df60889 --- /dev/null +++ b/ground/pyuavtalk/openpilot/uavtalk/uavtalk.py @@ -0,0 +1,311 @@ +## +############################################################################## +# +# @file uavtalk.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @brief Base classes for python UAVObject +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + + +import time +import logging +import threading + +SYNC = 0x3C +VERSION_MASK = 0xFC +VERSION = 0x20 +TYPE_MASK = 0x03 +TYPE_OBJ = 0x00 +TYPE_OBJ_REQ = 0x01 +TYPE_OBJ_ACK = 0x02 +TYPE_ACK = 0x03 +TYPE_NACK = 0x04 + +HEADER_LENGTH = 10 # sync(1), type (1), size(2), object ID (4), instance ID(2 ) + +MAX_PAYLOAD_LENGTH = 255 +CHECKSUM_LENGTH = 1 +MAX_PACKET_LENGTH = (HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH) + + + + +class Crc(object): + + crcTable = ( 0x00, 0x07, 0x0e, 0x09, 0x1c, + 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, + 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, + 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, 0x90, + 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, + 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, + 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, + 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, + 0x94, 0x9d, 0x9a, 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, + 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, + 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, + 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, + 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, 0xf9, 0xfe, 0xf7, 0xf0, + 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, + 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, + 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, 0x05, + 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, + 0x7f, 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, + 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, + 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, + 0x8a, 0x8d, 0x84, 0x83, 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, + 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 ) + + def __init__(self): + self.reset() + + def reset(self, firstValue=None): + self.crc = 0 + if firstValue != None: + self.add(firstValue) + + def read(self): + return self.crc + + def add(self, value): + try: + self.crc = Crc.crcTable[self.crc ^ (value & 0xff)] + except TypeError: + print "Likely End Of File" + raise SystemExit + + def addList(self, values): + for v in values: + self.add(v) + + +class UavTalkRecThread(threading.Thread): + STATE_SYNC = 0 + STATE_TYPE = 1 + STATE_SIZE = 2 + STATE_OBJID = 3 + STATE_INSTID = 4 + STATE_DATA = 5 + STATE_CS = 6 + + def __init__(self, uavTalk): + threading.Thread.__init__(self) + self.uavTalk = uavTalk + self.rxState = self.STATE_SYNC + self.rxCrc = Crc() + self.stop = False + + def run(self): + #self.uavTalk.serial.open() + self.stop = False + + if self.uavTalk.logFile is not None: + file = open(self.uavTalk.logFile) + while not self.stop: + rx = file.read(1) + if len(rx) > 0: + rx = ord(rx) + self._consumeByte(rx) + elif self.uavTalk.serial is not None: + while not self.stop: + rx = self.uavTalk.serial.read(1) + if len(rx) > 0: + rx = ord(rx) + # if (rx == SYNC): + # print + # print hex(rx), + self._consumeByte(rx) + else: + print "Nothing to do!" + + + def _consumeByte(self, rx): + self.rxCrc.add(rx) + + if self.rxState == UavTalkRecThread.STATE_SYNC: + if rx == SYNC: + self.rxCrc.reset(rx) + self.rxState += 1 +# else: +# logging.warning("NoSync") + + elif self.rxState == UavTalkRecThread.STATE_TYPE: + if (rx & VERSION_MASK != VERSION): + self.rxState == UavTalkRecThread.STATE_SYNC + else: + self.rxType = rx & TYPE_MASK + self.rxCount = 0 + self.rxSize = 0 + self.rxState += 1 + + elif self.rxState == UavTalkRecThread.STATE_SIZE: + self.rxSize >>= 8 + self.rxSize |= (rx<<8) + self.rxCount += 1 + + if self.rxCount == 2: + # Received complete packet size, check for valid packet size + if (self.rxSize < HEADER_LENGTH) or (self.rxSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH): + logging.error("INVALID Packet Size") + self.rxState = UavTalkRecThread.STATE_SYNC + else: + self.rxCount = 0 + self.rxObjId = 0 + self.rxState = UavTalkRecThread.STATE_OBJID + + elif self.rxState == UavTalkRecThread.STATE_OBJID: + self.rxObjId >>= 8 + self.rxObjId |= (rx<<24) + self.rxCount += 1 + + if self.rxCount < 4: + return + + self.rxCount = 0 + self.rxInstId = 0 + self.rxState += 1 + + elif self.rxState == UavTalkRecThread.STATE_INSTID: + self.rxCount += 1 + + if self.rxCount < 2: + return + + self.rxCount = 0 + + # Received complete ObjID + self.obj = self.uavTalk.objMan.getObj(self.rxObjId) + if self.obj is not None: + self.rxDataSize = self.obj.getSerialisedSize() + + if HEADER_LENGTH + self.obj.getSerialisedSize() != self.rxSize: + logging.error("packet Size MISMATCH") + self.rxState = UavTalkRecThread.STATE_SYNC + else: +# print "Object: %s" % self.obj + self.rxCount = 0 + self.rxData = [] + + if self.rxType == TYPE_OBJ_REQ | self.rxType == TYPE_ACK | self.rxType == TYPE_NACK: + self.rxDataSize = 0 + + if self.rxDataSize > 0: + self.rxState = UavTalkRecThread.STATE_DATA + else: + self.rxState = UavTalkRecThread.STATE_DATA+1 + else: + logging.warning("Rec UNKNOWN Obj %x", self.rxObjId) + self.rxState = self.STATE_SYNC + + + elif self.rxState == UavTalkRecThread.STATE_DATA: + self.rxData.append(rx) + self.rxCount += 1 + if self.rxCount == self.rxDataSize: + self.rxState += 1 + #else: + # logging.error("Obj %x INVALID SIZE", self.rxObjId) + + elif self.rxState == UavTalkRecThread.STATE_CS: + # by now, the CS has been added to the CRC calc, so now the CRC calc should read 0 + if self.rxCrc.read() != 0: + logging.error("CRC ERROR") + else: + self.uavTalk._onRecevedPacket(self.obj, self.rxType, self.rxData) + self.rxState = UavTalkRecThread.STATE_SYNC + + else: + logging.error("INVALID STATE") + self.rxState = UavTalkRecThread.STATE_SYNC + + +class UavTalk(object): + + def __init__(self, serial, logFile): + self.logFile = logFile + self.serial = serial + self.objMan = None + self.txLock = threading.Lock() + + def setObjMan(self, objMan): + self.objMan = objMan + + def start(self): + self.recThread = UavTalkRecThread(self) + self.recThread.start() + + def stop(self): + self.recThread.stop = True; + self.recThread.join() + + def _onRecevedPacket(self, obj, rxType, rxData): + logging.debug("REC Obj %20s type %x cnt %d", obj, rxType, obj.updateCnt+1) +# for i in rxData: print hex(i), +# print + + if rxType == TYPE_OBJ_ACK: + logging.debug("Sending ACK for Obj %s", obj) + self.sendObjectAck(obj) + + self.objMan.objUpdate(obj, rxData) + + def sendObjReq(self, obj): + self._sendpacket(TYPE_OBJ_REQ, obj.objId) + + def sendObjectAck(self, obj): + self._sendpacket(TYPE_ACK, obj.objId) + + def sendObject(self, obj, reqAck=False): + if reqAck: + type = TYPE_OBJ_ACK + else: + type = TYPE_OBJ + self._sendpacket(type, obj.objId, obj.serialize()) + + def _sendpacket(self, type, objId, data=None): + + self.txLock.acquire() + + header = [SYNC, type | VERSION, 0, 0, 0, 0, 0, 0, 0, 0] + + length = HEADER_LENGTH + if data is not None: + length += len(data) + header[2] = length & 0xFF + header[3] = (length >>8) & 0xFF + for i in xrange(4,8): + header[i] = objId & 0xff + objId >>= 8 + + crc = Crc() + crc.addList(header) + self.serial.write("".join(map(chr,header))) + + if data is not None: + crc.addList(data) + self.serial.write("".join(map(chr,data))) + + self.serial.write(chr(crc.read())) + + self.txLock.release() + + diff --git a/ground/pyuavtalk/pyuavtalk.iml b/ground/pyuavtalk/pyuavtalk.iml new file mode 100644 index 000000000..7d65805bc --- /dev/null +++ b/ground/pyuavtalk/pyuavtalk.iml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/ground/pyuavtalk/setup.py b/ground/pyuavtalk/setup.py new file mode 100644 index 000000000..7e4c225e5 --- /dev/null +++ b/ground/pyuavtalk/setup.py @@ -0,0 +1,9 @@ +from distutils.core import setup +import glob + +setup(name='OpenPilot UavTalk', + version='1.0', + description='OpenPilot UavTalk', + url='http://www.openpilot.org', + packages=['openpilot', 'openpilot.uavtalk', 'openpilot.uavobjects'], + )