From 7038aacdfee01178686a8a97804b663b9a52c743 Mon Sep 17 00:00:00 2001 From: padeler Date: Thu, 16 Feb 2017 02:48:37 +0200 Subject: [PATCH 1/6] LP-483 cherry-pick pyuavtalk code from amorales fork --- .gitignore | 20 +- Makefile | 21 ++ .../FlightPlan/lib/uavobject.pyt.template | 4 +- ground/pyuavtalk/examples/example.py | 218 ++++++++++++ ground/pyuavtalk/examples/example_readlog.py | 182 ++++++++++ ground/pyuavtalk/examples/example_tuning.py | 204 ++++++++++++ ground/pyuavtalk/openpilot/__init__.py | 0 .../openpilot/uavobjects/__init__.py | 0 .../pyuavtalk/openpilot/uavtalk/__init__.py | 0 .../openpilot/uavtalk/connectionManager.py | 102 ++++++ .../openpilot/uavtalk/flighttelemetrystats.py | 125 +++++++ .../openpilot/uavtalk/objectManager.py | 153 +++++++++ .../pyuavtalk/openpilot/uavtalk/uavobject.py | 294 +++++++++++++++++ ground/pyuavtalk/openpilot/uavtalk/uavtalk.py | 311 ++++++++++++++++++ ground/pyuavtalk/pyuavtalk.iml | 10 + ground/pyuavtalk/setup.py | 9 + 16 files changed, 1641 insertions(+), 12 deletions(-) create mode 100644 ground/pyuavtalk/examples/example.py create mode 100644 ground/pyuavtalk/examples/example_readlog.py create mode 100644 ground/pyuavtalk/examples/example_tuning.py create mode 100644 ground/pyuavtalk/openpilot/__init__.py create mode 100644 ground/pyuavtalk/openpilot/uavobjects/__init__.py create mode 100644 ground/pyuavtalk/openpilot/uavtalk/__init__.py create mode 100644 ground/pyuavtalk/openpilot/uavtalk/connectionManager.py create mode 100644 ground/pyuavtalk/openpilot/uavtalk/flighttelemetrystats.py create mode 100644 ground/pyuavtalk/openpilot/uavtalk/objectManager.py create mode 100644 ground/pyuavtalk/openpilot/uavtalk/uavobject.py create mode 100644 ground/pyuavtalk/openpilot/uavtalk/uavtalk.py create mode 100644 ground/pyuavtalk/pyuavtalk.iml create mode 100644 ground/pyuavtalk/setup.py 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'], + ) From 116ec93332c2154fde57cb114b6f9686f45c2379 Mon Sep 17 00:00:00 2001 From: padeler Date: Thu, 16 Feb 2017 02:51:50 +0200 Subject: [PATCH 2/6] LP-483 Moved python module outside ground. Renamed to librepilot Protocol bugs fixed in the metadata object packing Metadata fix for the connectionmanager. Connection manager keeps connection alive by sending an object periodically. Example scripts updated. --- .gitignore | 7 + Makefile | 18 +- .../FlightPlan/lib/uavobject.pyt.template | 2 +- .../pyuavtalk => python}/examples/example.py | 160 +++++++-------- .../examples/example_readlog.py | 18 +- .../examples/example_tuning.py | 18 +- .../librepilot}/__init__.py | 0 .../librepilot}/uavobjects/__init__.py | 0 .../librepilot}/uavtalk/__init__.py | 0 .../librepilot}/uavtalk/connectionManager.py | 32 +-- .../uavtalk/flighttelemetrystats.py | 2 +- .../librepilot}/uavtalk/objectManager.py | 2 +- .../librepilot}/uavtalk/uavobject.py | 190 ++++++++++++------ .../librepilot}/uavtalk/uavtalk.py | 71 +++---- {ground/pyuavtalk => python}/pyuavtalk.iml | 0 {ground/pyuavtalk => python}/setup.py | 0 16 files changed, 305 insertions(+), 215 deletions(-) rename {ground/pyuavtalk => python}/examples/example.py (71%) rename {ground/pyuavtalk => python}/examples/example_readlog.py (93%) rename {ground/pyuavtalk => python}/examples/example_tuning.py (95%) rename {ground/pyuavtalk/openpilot => python/librepilot}/__init__.py (100%) rename {ground/pyuavtalk/openpilot => python/librepilot}/uavobjects/__init__.py (100%) rename {ground/pyuavtalk/openpilot => python/librepilot}/uavtalk/__init__.py (100%) rename {ground/pyuavtalk/openpilot => python/librepilot}/uavtalk/connectionManager.py (88%) rename {ground/pyuavtalk/openpilot => python/librepilot}/uavtalk/flighttelemetrystats.py (98%) rename {ground/pyuavtalk/openpilot => python/librepilot}/uavtalk/objectManager.py (99%) rename {ground/pyuavtalk/openpilot => python/librepilot}/uavtalk/uavobject.py (56%) rename {ground/pyuavtalk/openpilot => python/librepilot}/uavtalk/uavtalk.py (85%) rename {ground/pyuavtalk => python}/pyuavtalk.iml (100%) rename {ground/pyuavtalk => python}/setup.py (100%) diff --git a/.gitignore b/.gitignore index d4e2d4fa7..672153faa 100644 --- a/.gitignore +++ b/.gitignore @@ -65,8 +65,15 @@ ground/gcs/.settings /.metadata /.settings /.pydevproject + +# python artifacts +/**/*.py* + /workspace +#pycharm project files +/python/.idea + # Ignore Eclipse temp folder, git plugin based? RemoteSystemsTempFiles diff --git a/Makefile b/Makefile index cb9aa4be0..c17f14d38 100644 --- a/Makefile +++ b/Makefile @@ -209,19 +209,19 @@ uavobjects_%: $(UAVOBJGENERATOR) $(UAVOBJGENERATOR) -$* $(UAVOBJ_XML_DIR) $(ROOT_DIR) ; \ ) -OBJECTCOUNT := $(shell find $(ROOT_DIR)/ground/pyuavtalk/openpilot/uavobjects/ -name '*.py' | wc -l) +OBJECTCOUNT := $(shell find $(ROOT_DIR)/python/librepilot/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) + $(V1) mkdir -p $(ROOT_DIR)/python/librepilot/uavobjects/ + $(V1) ( touch $(ROOT_DIR)/python/librepilot/uavobjects/__init__.py ) + $(V1) ( cp $(UAVOBJ_OUT_DIR)/python/* $(ROOT_DIR)/python/librepilot/uavobjects/ ) + $(V1) ( cd $(ROOT_DIR)/python/ && 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, $(ROOT_DIR)/python/librepilot/uavobjects/)" + $(V1) [ ! -d "$(ROOT_DIR)/python/librepilot/uavobjects/" ] || $(RM) -r "$(ROOT_DIR)/python/librepilot/uavobjects/" + @$(ECHO) " CLEAN $(call toprel, $(ROOT_DIR)/python/librepilot/build/)" + $(V1) [ ! -d "$(ROOT_DIR)/python/librepilot/build/" ] || sudo $(RM) -r "$(ROOT_DIR)/python/librepilot/build/" @$(ECHO) " CLEAN $(call toprel, $(UAVOBJ_OUT_DIR)/python/)" $(V1) [ ! -d "$(UAVOBJ_OUT_DIR)/python/" ] || $(RM) -r "$(UAVOBJ_OUT_DIR)/python/" diff --git a/flight/modules/FlightPlan/lib/uavobject.pyt.template b/flight/modules/FlightPlan/lib/uavobject.pyt.template index 942fb64fb..6d0fb5709 100644 --- a/flight/modules/FlightPlan/lib/uavobject.pyt.template +++ b/flight/modules/FlightPlan/lib/uavobject.pyt.template @@ -30,7 +30,7 @@ # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA # -from openpilot.uavtalk.uavobject import * +from librepilot.uavtalk.uavobject import * $(DATAFIELDS) diff --git a/ground/pyuavtalk/examples/example.py b/python/examples/example.py similarity index 71% rename from ground/pyuavtalk/examples/example.py rename to python/examples/example.py index 32e1583fb..efcdd442e 100644 --- a/ground/pyuavtalk/examples/example.py +++ b/python/examples/example.py @@ -23,7 +23,7 @@ # with this program; if not, write to the Free Software Foundation, Inc., # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA # - +# import logging @@ -31,132 +31,134 @@ 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 * - - +from librepilot.uavtalk.uavobject import * +from librepilot.uavtalk.uavtalk import * +from librepilot.uavtalk.objectManager import * +from librepilot.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 + _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)) - + + 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.telemetryUpdateMode = UAVMetaDataObject.UpdateMode.PERIODIC + self.objMan.AttitudeState.metadata.telemetryUpdatePeriod = 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) + + 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= " " - + + 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", - + print "RPY: %f %f %f " % (self.objMan.AttitudeState.Roll.value, self.objMan.AttitudeState.Pitch.value, + self.objMan.AttitudeState.Yaw.value) + " " + + # 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.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]) @@ -169,18 +171,19 @@ def printUsage(): print " s: Drive Servo" print print " for example: %s COM30 o" % appName - print - + 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"]: + if option not in ["o", "w", "g", "s"]: print "ERROR: Invalid option" printUsage() sys.exit(2) @@ -192,27 +195,26 @@ if __name__ == '__main__': 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 - + 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: + 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/python/examples/example_readlog.py similarity index 93% rename from ground/pyuavtalk/examples/example_readlog.py rename to python/examples/example_readlog.py index c13848e98..b8565071a 100644 --- a/ground/pyuavtalk/examples/example_readlog.py +++ b/python/examples/example_readlog.py @@ -31,10 +31,10 @@ 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 * +from librepilot.uavtalk.uavobject import * +from librepilot.uavtalk.uavtalk import * +from librepilot.uavtalk.objectManager import * +from librepilot.uavtalk.connectionManager import * def _hex02(value): @@ -150,14 +150,14 @@ def printUsage(): if __name__ == '__main__': if len(sys.argv) !=2: - print "ERROR: Incorrect number of arguments" - print len(sys.argv) - printUsage() - sys.exit(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]) + sys.exit('ERROR: Database %s was not found!' % sys.argv[1]) # Log everything, and send it to stderr. logging.basicConfig(level=logging.INFO) diff --git a/ground/pyuavtalk/examples/example_tuning.py b/python/examples/example_tuning.py similarity index 95% rename from ground/pyuavtalk/examples/example_tuning.py rename to python/examples/example_tuning.py index 8640b0fb9..ca193090a 100644 --- a/ground/pyuavtalk/examples/example_tuning.py +++ b/python/examples/example_tuning.py @@ -31,10 +31,10 @@ 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 * +from librepilot.uavtalk.uavobject import * +from librepilot.uavtalk.uavtalk import * +from librepilot.uavtalk.objectManager import * +from librepilot.uavtalk.connectionManager import * @@ -69,17 +69,17 @@ if __name__ == '__main__': import objectpersistence print "Getting Current Settings:" - for _ in range(2): # Try only twice to get the settings + for _ in range(2): # Try only twice to get the settings try: time.sleep(10) objMan.StabilizationSettingsBank1.getUpdate() except TimeoutException: print "Timeout" - pass + pass except KeyboardInterrupt: - os._exit(1) - else: - raise + os._exit(1) + else: + raise while True: while True: diff --git a/ground/pyuavtalk/openpilot/__init__.py b/python/librepilot/__init__.py similarity index 100% rename from ground/pyuavtalk/openpilot/__init__.py rename to python/librepilot/__init__.py diff --git a/ground/pyuavtalk/openpilot/uavobjects/__init__.py b/python/librepilot/uavobjects/__init__.py similarity index 100% rename from ground/pyuavtalk/openpilot/uavobjects/__init__.py rename to python/librepilot/uavobjects/__init__.py diff --git a/ground/pyuavtalk/openpilot/uavtalk/__init__.py b/python/librepilot/uavtalk/__init__.py similarity index 100% rename from ground/pyuavtalk/openpilot/uavtalk/__init__.py rename to python/librepilot/uavtalk/__init__.py diff --git a/ground/pyuavtalk/openpilot/uavtalk/connectionManager.py b/python/librepilot/uavtalk/connectionManager.py similarity index 88% rename from ground/pyuavtalk/openpilot/uavtalk/connectionManager.py rename to python/librepilot/uavtalk/connectionManager.py index e99bf1477..995ae0f8e 100644 --- a/ground/pyuavtalk/openpilot/uavtalk/connectionManager.py +++ b/python/librepilot/uavtalk/connectionManager.py @@ -30,7 +30,7 @@ import logging import time import objectManager -from openpilot.uavtalk import flighttelemetrystats +from librepilot.uavtalk import flighttelemetrystats import uavobject @@ -40,15 +40,12 @@ class ConnectionManager(object): 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") @@ -60,7 +57,7 @@ class ConnectionManager(object): self._onFtsChange() if self.connected: self.objMan.waitObjUpdate(self.ftsObj.metadata) - self.ftsObj.metadata.telemetryUpdateMode.value = uavobject.UAVMetaDataObject.UpdateMode.PERIODIC + self.ftsObj.metadata.telemetryUpdateMode = uavobject.UAVMetaDataObject.UpdateMode.PERIODIC self.ftsObj.metadata.telemetryUpdatePeriod.value = 1000 self.ftsObj.metadata.updated() self.objMan.regObjectObserver(self.ftsObj, self, "_onFtsChange") @@ -71,27 +68,32 @@ class ConnectionManager(object): self.connected = False logging.warning("Connecting TO") pass - logging.debug("Connected in %.1fs" % (time.clock()-startTime)) - - + 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: + + 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 - + try: + self.gcsObj.updated() # simulate update to the gcs stats. + # This will send a message and keep the connection from timeout + except objectManager.TimeoutException: + pass + if self.connected: if not connected: logging.warning("DISCONNECTED") diff --git a/ground/pyuavtalk/openpilot/uavtalk/flighttelemetrystats.py b/python/librepilot/uavtalk/flighttelemetrystats.py similarity index 98% rename from ground/pyuavtalk/openpilot/uavtalk/flighttelemetrystats.py rename to python/librepilot/uavtalk/flighttelemetrystats.py index 4c939a16c..216ae52ca 100644 --- a/ground/pyuavtalk/openpilot/uavtalk/flighttelemetrystats.py +++ b/python/librepilot/uavtalk/flighttelemetrystats.py @@ -30,7 +30,7 @@ # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA # -from openpilot.uavtalk.uavobject import * +from librepilot.uavtalk.uavobject import * # Field TxDataRate definition class TxDataRateField(UAVObjectField): diff --git a/ground/pyuavtalk/openpilot/uavtalk/objectManager.py b/python/librepilot/uavtalk/objectManager.py similarity index 99% rename from ground/pyuavtalk/openpilot/uavtalk/objectManager.py rename to python/librepilot/uavtalk/objectManager.py index 0849c57ce..a3549e7ef 100644 --- a/ground/pyuavtalk/openpilot/uavtalk/objectManager.py +++ b/python/librepilot/uavtalk/objectManager.py @@ -31,7 +31,7 @@ import sys import os import inspect -from openpilot.uavtalk.uavobject import * +from librepilot.uavtalk.uavobject import * class TimeoutException(Exception): diff --git a/ground/pyuavtalk/openpilot/uavtalk/uavobject.py b/python/librepilot/uavtalk/uavobject.py similarity index 56% rename from ground/pyuavtalk/openpilot/uavtalk/uavobject.py rename to python/librepilot/uavtalk/uavobject.py index 34ee28adc..7176e83e1 100644 --- a/ground/pyuavtalk/openpilot/uavtalk/uavobject.py +++ b/python/librepilot/uavtalk/uavobject.py @@ -43,8 +43,19 @@ import threading import struct +UAVOBJ_ACCESS_SHIFT = 0 +UAVOBJ_GCS_ACCESS_SHIFT = 1 +UAVOBJ_TELEMETRY_ACKED_SHIFT = 2 +UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT = 3 +UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT = 4 +UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT = 6 +UAVOBJ_LOGGING_UPDATE_MODE_SHIFT = 8 -class UAVObjectField: +def _set_bits(var,shift,value,mask): + return (var & ~(mask << shift)) | (value << shift) + + +class UAVObjectField(object): class FType: INT8 = 0 INT16 = 1 @@ -110,7 +121,7 @@ class UAVObjectField: ser += map(ord, apply(self.struct.pack, self.value)) def deserialize(self, data): - # DOTO: FIXME: This is getting very messy + # TODO: 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] @@ -127,7 +138,7 @@ class Observer(object): self.methodToCall(args) -class UAVObject: +class UAVObject(object): def __init__(self, objId, name=None): self.metadata = self # FIXME self.objId = objId @@ -201,63 +212,68 @@ class UAVObject: # pass # TODO - -class MetaFlightAccessField(UAVObjectField): +class MetaFlagsField(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) + UAVObjectField.__init__(self, UAVObjectField.FType.UINT16, 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) + UAVObjectField.__init__(self, UAVObjectField.FType.UINT16, 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) + UAVObjectField.__init__(self, UAVObjectField.FType.UINT16, 1) class MetaLoggingUpdatePeriod(UAVObjectField): def __init__(self): - UAVObjectField.__init__(self, UAVObjectField.FType.UINT32, 1) + UAVObjectField.__init__(self, UAVObjectField.FType.UINT16, 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 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 MetaLoggingUpdateMode(UAVObjectField): +# def __init__(self): +# UAVObjectField.__init__(self, UAVObjectField.FType.ENUM, 1) + + class UAVMetaDataObject(UAVObject): + + class UpdateMode: - PERIODIC = 0 - ONCHANGE = 1 - MANUAL = 2 - NEVER = 3 + MANUAL = 0 + PERIODIC = 1 + ONCHANGE = 2 + THROTTLED = 3 class Access: READWRITE = 0 @@ -265,30 +281,90 @@ class UAVMetaDataObject(UAVObject): def __init__(self, objId): UAVObject.__init__(self, objId) + self.flags = MetaFlagsField() + self.addField(self.flags) - self.access = MetaFlightAccessField() - self.addField(self.access) - self.gcsAccess = MetaGCSAccessField() - self.addField(self.gcsAccess) + # self.access = MetaFlightAccessField() + # self.addField(self.access) + # self.gcsAccess = MetaGCSAccessField() + # self.addField(self.gcsAccess) + # self.telemetryAcked = MetaFlightTelemetryAcked() + # self.addField(self.telemetryAcked) + + # self.gcsTelemetryAcked = MetaGCSTelemetryAcked() + # self.addField(self.gcsTelemetryAcked) + # self.telemetryUpdateMode = MetaFlightTelemetryUpdateMode() + # self.addField(self.telemetryUpdateMode) + # self.gcsTelemetryUpdateMode = MetaGCSTelemetryUpdateMode() + # self.addField(self.gcsTelemetryUpdateMode) + # self.loggingUpdateMode = MetaLoggingUpdateMode() + # self.addField(self.loggingUpdateMode) - 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) + @property + def access(self): + return (self.flags.value >> UAVOBJ_ACCESS_SHIFT) & 0x01 + + @property + def gcsAccess(self): + return (self.flags.value >> UAVOBJ_GCS_ACCESS_SHIFT) & 0x01 + + @property + def telemetryAcked(self): + return (self.flags.value >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 0x01 + + @property + def gcsTelemetryAcked(self): + return (self.flags.value >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 0x01 + + @property + def telemetryUpdateMode(self): + return (self.flags.value >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & 0x03 + + @property + def gcsTelemetryUpdateMode(self): + return (self.flags.value >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & 0x03 + + @property + def loggingUpdateMode(self): + return (self.flags.value >> UAVOBJ_LOGGING_UPDATE_MODE_SHIFT) & 0x03 + + @access.setter + def access(self, v): + self.flags.value = _set_bits(self.flags.value, UAVOBJ_ACCESS_SHIFT, v, 0x01) + + @gcsAccess.setter + def gcsAccess(self, v): + self.flags.value = _set_bits(self.flags.value, UAVOBJ_GCS_ACCESS_SHIFT, v, 0x01) + + @gcsTelemetryAcked.setter + def gcsTelemetryAcked(self, v): + self.flags.value = _set_bits(self.flags.value, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, v, 0x01) + + @telemetryUpdateMode.setter + def telemetryUpdateMode(self, v): + self.flags.value = _set_bits(self.flags.value, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, v, 0x03) + + @gcsTelemetryUpdateMode.setter + def gcsTelemetryUpdateMode(self, v): + self.flags.value = _set_bits(self.flags.value, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, v, 0x03) + + @loggingUpdateMode.setter + def loggingUpdateMode(self, v): + self.flags.value = _set_bits(self.flags.value, UAVOBJ_LOGGING_UPDATE_MODE_SHIFT, v, 0x03) + def isMetaData(self): return True + + def __str__(self): + return str(self.objId) + " " + self.name + " [" + format(self.flags.value,'016b') + " " + \ + str(self.telemetryUpdatePeriod.value) + " " + str(self.gcsTelemetryUpdatePeriod.value) + \ + " " + str(self.loggingUpdatePeriod.value) + "]" diff --git a/ground/pyuavtalk/openpilot/uavtalk/uavtalk.py b/python/librepilot/uavtalk/uavtalk.py similarity index 85% rename from ground/pyuavtalk/openpilot/uavtalk/uavtalk.py rename to python/librepilot/uavtalk/uavtalk.py index 72df60889..ab6996652 100644 --- a/ground/pyuavtalk/openpilot/uavtalk/uavtalk.py +++ b/python/librepilot/uavtalk/uavtalk.py @@ -28,7 +28,9 @@ import time import logging import threading - + +from librepilot.uavtalk.objectManager import TimeoutException + SYNC = 0x3C VERSION_MASK = 0xFC VERSION = 0x20 @@ -165,7 +167,8 @@ class UavTalkRecThread(threading.Thread): 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") + logging.error("INVALID Packet Size. Should be: "+str(HEADER_LENGTH + self.obj.getSerialisedSize())+ + " got "+str(self.rxSize) + " for obj "+str(self.obj.name)) self.rxState = UavTalkRecThread.STATE_SYNC else: self.rxCount = 0 @@ -198,7 +201,8 @@ class UavTalkRecThread(threading.Thread): self.rxDataSize = self.obj.getSerialisedSize() if HEADER_LENGTH + self.obj.getSerialisedSize() != self.rxSize: - logging.error("packet Size MISMATCH") + logging.error("packet Size MISMATCH. Should be: "+str(HEADER_LENGTH + self.obj.getSerialisedSize())+ + " got "+str(self.rxSize) + " for obj "+str(self.obj.name)) self.rxState = UavTalkRecThread.STATE_SYNC else: # print "Object: %s" % self.obj @@ -258,12 +262,12 @@ class UavTalk(object): 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 + # 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) + # logging.debug("Sending ACK for Obj %s", obj) self.sendObjectAck(obj) self.objMan.objUpdate(obj, rxData) @@ -274,38 +278,37 @@ class UavTalk(object): def sendObjectAck(self, obj): self._sendpacket(TYPE_ACK, obj.objId) - def sendObject(self, obj, reqAck=False): + 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): - + def _sendpacket(self, obj_type, obj_id, data=None): + # name = self.objMan.objs[obj_id].name + # logging.debug("Enter lock " + str(obj_type) + " " + name) 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() - + try: + header = [SYNC, obj_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] = obj_id & 0xff + obj_id >>= 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())) + finally: + self.txLock.release() + # logging.debug("Released lock " + str(obj_type) + " " + name) diff --git a/ground/pyuavtalk/pyuavtalk.iml b/python/pyuavtalk.iml similarity index 100% rename from ground/pyuavtalk/pyuavtalk.iml rename to python/pyuavtalk.iml diff --git a/ground/pyuavtalk/setup.py b/python/setup.py similarity index 100% rename from ground/pyuavtalk/setup.py rename to python/setup.py From 0ff0fe65321acf5fefa3f04ebdf01a09f9c72e58 Mon Sep 17 00:00:00 2001 From: padeler Date: Fri, 17 Feb 2017 20:37:47 +0200 Subject: [PATCH 3/6] LP-483 sets author to librepilot in all python files --- flight/modules/FlightPlan/lib/uavobject.pyt.template | 3 ++- python/examples/example.py | 3 ++- python/examples/example_readlog.py | 3 ++- python/examples/example_tuning.py | 3 ++- python/librepilot/uavtalk/connectionManager.py | 3 ++- python/librepilot/uavtalk/flighttelemetrystats.py | 3 ++- python/librepilot/uavtalk/objectManager.py | 3 ++- python/librepilot/uavtalk/uavobject.py | 3 ++- python/librepilot/uavtalk/uavtalk.py | 3 ++- python/setup.py | 8 ++++---- 10 files changed, 22 insertions(+), 13 deletions(-) diff --git a/flight/modules/FlightPlan/lib/uavobject.pyt.template b/flight/modules/FlightPlan/lib/uavobject.pyt.template index 6d0fb5709..f6d6f319d 100644 --- a/flight/modules/FlightPlan/lib/uavobject.pyt.template +++ b/flight/modules/FlightPlan/lib/uavobject.pyt.template @@ -2,7 +2,8 @@ ############################################################################## # # @file $(NAMELC).py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. # @brief Implementation of the $(NAME) object. This file has been # automatically generated by the UAVObjectGenerator. For use with # the PyMite VM of the FlightPlan module. diff --git a/python/examples/example.py b/python/examples/example.py index efcdd442e..5fb7f81be 100644 --- a/python/examples/example.py +++ b/python/examples/example.py @@ -2,7 +2,8 @@ ############################################################################## # # @file example.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # # @see The GNU Public License (GPL) Version 3 diff --git a/python/examples/example_readlog.py b/python/examples/example_readlog.py index b8565071a..3c82a65ae 100644 --- a/python/examples/example_readlog.py +++ b/python/examples/example_readlog.py @@ -2,7 +2,8 @@ ############################################################################## # # @file example_readlog.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # # @see The GNU Public License (GPL) Version 3 diff --git a/python/examples/example_tuning.py b/python/examples/example_tuning.py index ca193090a..ed82a3bc5 100644 --- a/python/examples/example_tuning.py +++ b/python/examples/example_tuning.py @@ -2,7 +2,8 @@ ############################################################################## # # @file example.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # # @see The GNU Public License (GPL) Version 3 diff --git a/python/librepilot/uavtalk/connectionManager.py b/python/librepilot/uavtalk/connectionManager.py index 995ae0f8e..7d835a94a 100644 --- a/python/librepilot/uavtalk/connectionManager.py +++ b/python/librepilot/uavtalk/connectionManager.py @@ -2,7 +2,8 @@ ############################################################################## # # @file connectionManager.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # # @see The GNU Public License (GPL) Version 3 diff --git a/python/librepilot/uavtalk/flighttelemetrystats.py b/python/librepilot/uavtalk/flighttelemetrystats.py index 216ae52ca..3ca675042 100644 --- a/python/librepilot/uavtalk/flighttelemetrystats.py +++ b/python/librepilot/uavtalk/flighttelemetrystats.py @@ -2,7 +2,8 @@ ############################################################################## # # @file flighttelemetrystats.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @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. diff --git a/python/librepilot/uavtalk/objectManager.py b/python/librepilot/uavtalk/objectManager.py index a3549e7ef..bdcf2604f 100644 --- a/python/librepilot/uavtalk/objectManager.py +++ b/python/librepilot/uavtalk/objectManager.py @@ -2,7 +2,8 @@ ############################################################################## # # @file objectManager.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # # @see The GNU Public License (GPL) Version 3 diff --git a/python/librepilot/uavtalk/uavobject.py b/python/librepilot/uavtalk/uavobject.py index 7176e83e1..561cc4564 100644 --- a/python/librepilot/uavtalk/uavobject.py +++ b/python/librepilot/uavtalk/uavobject.py @@ -2,7 +2,8 @@ ############################################################################## # # @file uavobject.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # # @see The GNU Public License (GPL) Version 3 diff --git a/python/librepilot/uavtalk/uavtalk.py b/python/librepilot/uavtalk/uavtalk.py index ab6996652..cd8f4b6af 100644 --- a/python/librepilot/uavtalk/uavtalk.py +++ b/python/librepilot/uavtalk/uavtalk.py @@ -2,7 +2,8 @@ ############################################################################## # # @file uavtalk.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # # @see The GNU Public License (GPL) Version 3 diff --git a/python/setup.py b/python/setup.py index 7e4c225e5..053b16931 100644 --- a/python/setup.py +++ b/python/setup.py @@ -1,9 +1,9 @@ from distutils.core import setup import glob -setup(name='OpenPilot UavTalk', +setup(name='LibrePilot UAVTalk', version='1.0', - description='OpenPilot UavTalk', - url='http://www.openpilot.org', - packages=['openpilot', 'openpilot.uavtalk', 'openpilot.uavobjects'], + description='LibrePilot UAVTalk', + url='http://www.librepilot.org', + packages=['librepilot', 'librepilot.uavtalk', 'librepilot.uavobjects'], ) From 8c0f29a4ea530af878298dc45bb6a8c07ac34ac6 Mon Sep 17 00:00:00 2001 From: padeler Date: Tue, 21 Feb 2017 01:45:39 +0200 Subject: [PATCH 4/6] LP-483 Fixed typo in header comments --- flight/modules/FlightPlan/lib/uavobject.pyt.template | 2 +- python/examples/example.py | 2 +- python/examples/example_readlog.py | 2 +- python/examples/example_tuning.py | 2 +- python/librepilot/uavtalk/connectionManager.py | 2 +- python/librepilot/uavtalk/flighttelemetrystats.py | 2 +- python/librepilot/uavtalk/objectManager.py | 2 +- python/librepilot/uavtalk/uavobject.py | 2 +- python/librepilot/uavtalk/uavtalk.py | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/flight/modules/FlightPlan/lib/uavobject.pyt.template b/flight/modules/FlightPlan/lib/uavobject.pyt.template index f6d6f319d..1326d46a8 100644 --- a/flight/modules/FlightPlan/lib/uavobject.pyt.template +++ b/flight/modules/FlightPlan/lib/uavobject.pyt.template @@ -2,7 +2,7 @@ ############################################################################## # # @file $(NAMELC).py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. # @brief Implementation of the $(NAME) object. This file has been # automatically generated by the UAVObjectGenerator. For use with diff --git a/python/examples/example.py b/python/examples/example.py index 5fb7f81be..3fc708512 100644 --- a/python/examples/example.py +++ b/python/examples/example.py @@ -2,7 +2,7 @@ ############################################################################## # # @file example.py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # diff --git a/python/examples/example_readlog.py b/python/examples/example_readlog.py index 3c82a65ae..70b5b9fcf 100644 --- a/python/examples/example_readlog.py +++ b/python/examples/example_readlog.py @@ -2,7 +2,7 @@ ############################################################################## # # @file example_readlog.py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # diff --git a/python/examples/example_tuning.py b/python/examples/example_tuning.py index ed82a3bc5..ff3d166a4 100644 --- a/python/examples/example_tuning.py +++ b/python/examples/example_tuning.py @@ -2,7 +2,7 @@ ############################################################################## # # @file example.py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # diff --git a/python/librepilot/uavtalk/connectionManager.py b/python/librepilot/uavtalk/connectionManager.py index 7d835a94a..58097f186 100644 --- a/python/librepilot/uavtalk/connectionManager.py +++ b/python/librepilot/uavtalk/connectionManager.py @@ -2,7 +2,7 @@ ############################################################################## # # @file connectionManager.py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # diff --git a/python/librepilot/uavtalk/flighttelemetrystats.py b/python/librepilot/uavtalk/flighttelemetrystats.py index 3ca675042..b223f1ddc 100644 --- a/python/librepilot/uavtalk/flighttelemetrystats.py +++ b/python/librepilot/uavtalk/flighttelemetrystats.py @@ -2,7 +2,7 @@ ############################################################################## # # @file flighttelemetrystats.py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Implementation of the FlightTelemetryStats object. This file has been # automatically generated by the UAVObjectGenerator. For use with diff --git a/python/librepilot/uavtalk/objectManager.py b/python/librepilot/uavtalk/objectManager.py index bdcf2604f..e3e22fb66 100644 --- a/python/librepilot/uavtalk/objectManager.py +++ b/python/librepilot/uavtalk/objectManager.py @@ -2,7 +2,7 @@ ############################################################################## # # @file objectManager.py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # diff --git a/python/librepilot/uavtalk/uavobject.py b/python/librepilot/uavtalk/uavobject.py index 561cc4564..799deceb4 100644 --- a/python/librepilot/uavtalk/uavobject.py +++ b/python/librepilot/uavtalk/uavobject.py @@ -2,7 +2,7 @@ ############################################################################## # # @file uavobject.py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # diff --git a/python/librepilot/uavtalk/uavtalk.py b/python/librepilot/uavtalk/uavtalk.py index cd8f4b6af..9bf78a73f 100644 --- a/python/librepilot/uavtalk/uavtalk.py +++ b/python/librepilot/uavtalk/uavtalk.py @@ -2,7 +2,7 @@ ############################################################################## # # @file uavtalk.py -# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) # 2017. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. # @brief Base classes for python UAVObject # From 364e24e59cfccb68fc92a1624d26589696ea5f1b Mon Sep 17 00:00:00 2001 From: padeler Date: Tue, 21 Mar 2017 23:48:22 +0200 Subject: [PATCH 5/6] LP-483 Updates example scripts, code cleanup --- python/examples/__init__.py | 0 python/examples/example.py | 8 +-- python/examples/example_readlog.py | 94 ++++------------------------ python/examples/example_tuning.py | 13 ++-- python/librepilot/uavtalk/uavtalk.py | 6 +- 5 files changed, 25 insertions(+), 96 deletions(-) create mode 100644 python/examples/__init__.py diff --git a/python/examples/__init__.py b/python/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/examples/example.py b/python/examples/example.py index 3fc708512..a22769f02 100644 --- a/python/examples/example.py +++ b/python/examples/example.py @@ -91,7 +91,7 @@ class UavtalkDemo(): def showAttitudeViaObserver(self): print "Request fast periodic updates for AttitudeState" - self.objMan.AttitudeState.metadata.telemetryUpdateMode.value = UAVMetaDataObject.UpdateMode.PERIODIC + self.objMan.AttitudeState.metadata.telemetryUpdateMode = UAVMetaDataObject.UpdateMode.PERIODIC self.objMan.AttitudeState.metadata.telemetryUpdatePeriod.value = 50 self.objMan.AttitudeState.metadata.updated() @@ -104,7 +104,7 @@ class UavtalkDemo(): def showAttitudeViaWait(self): print "Request fast periodic updates for AttitudeState" self.objMan.AttitudeState.metadata.telemetryUpdateMode = UAVMetaDataObject.UpdateMode.PERIODIC - self.objMan.AttitudeState.metadata.telemetryUpdatePeriod = 50 + self.objMan.AttitudeState.metadata.telemetryUpdatePeriod.value = 50 self.objMan.AttitudeState.metadata.updated() while True: @@ -146,10 +146,8 @@ class UavtalkDemo(): def driveServo(self): print "Taking control of self.actuatorCmd" - self.objMan.ActuatorCommand.metadata.access.value = UAVMetaDataObject.Access.READONLY + self.objMan.ActuatorCommand.metadata.access = 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 diff --git a/python/examples/example_readlog.py b/python/examples/example_readlog.py index 70b5b9fcf..56bbad901 100644 --- a/python/examples/example_readlog.py +++ b/python/examples/example_readlog.py @@ -36,12 +36,13 @@ from librepilot.uavtalk.uavobject import * from librepilot.uavtalk.uavtalk import * from librepilot.uavtalk.objectManager import * from librepilot.uavtalk.connectionManager import * - - +from example import UavtalkDemo + + def _hex02(value): return "%02X" % value -class UavtalkDemo(): +class UavtalkLogDemo(UavtalkDemo): def __init__(self): self.nbUpdates = 0 @@ -50,7 +51,7 @@ class UavtalkDemo(): self.objMan = None self.connMan = None - def setup(self, port, filename): + def setup(self, filename): print "Opening File \"%s\"" % filename file = open(filename,"rb") if file == None: @@ -58,86 +59,13 @@ class UavtalkDemo(): 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]) @@ -164,10 +92,12 @@ if __name__ == '__main__': logging.basicConfig(level=logging.INFO) try: - demo = UavtalkDemo() - demo.setup(None, filename) - + demo = UavtalkLogDemo() + demo.setup(filename) + while True: + demo.objMan.AttitudeState.waitUpdate() + demo._onAttitudeUpdate(demo.objMan.AttitudeState) except KeyboardInterrupt: pass diff --git a/python/examples/example_tuning.py b/python/examples/example_tuning.py index ff3d166a4..39f562f43 100644 --- a/python/examples/example_tuning.py +++ b/python/examples/example_tuning.py @@ -63,24 +63,25 @@ if __name__ == '__main__': serPort = serial.Serial(_port, 57600, timeout=.5) uavTalk = UavTalk(serPort, None) + + print "Starting ObjectManager" objMan = ObjManager(uavTalk) objMan.importDefinitions() + + print "Starting UavTalk" uavTalk.start() - - import objectpersistence + print "Getting Current Settings:" for _ in range(2): # Try only twice to get the settings try: - time.sleep(10) + time.sleep(1) objMan.StabilizationSettingsBank1.getUpdate() except TimeoutException: print "Timeout" pass except KeyboardInterrupt: - os._exit(1) - else: - raise + exit(1) while True: while True: diff --git a/python/librepilot/uavtalk/uavtalk.py b/python/librepilot/uavtalk/uavtalk.py index 9bf78a73f..6fe304b50 100644 --- a/python/librepilot/uavtalk/uavtalk.py +++ b/python/librepilot/uavtalk/uavtalk.py @@ -122,12 +122,12 @@ class UavTalkRecThread(threading.Thread): self.stop = False if self.uavTalk.logFile is not None: - file = open(self.uavTalk.logFile) + file = open(self.uavTalk.logFile, "rb") while not self.stop: rx = file.read(1) if len(rx) > 0: rx = ord(rx) - self._consumeByte(rx) + self._consumeByte(rx) elif self.uavTalk.serial is not None: while not self.stop: rx = self.uavTalk.serial.read(1) @@ -233,7 +233,7 @@ class UavTalkRecThread(threading.Thread): 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") + logging.debug("CRC ERROR") else: self.uavTalk._onRecevedPacket(self.obj, self.rxType, self.rxData) self.rxState = UavTalkRecThread.STATE_SYNC From 775e2a4063e0070e4029f1013509fe805bb3262a Mon Sep 17 00:00:00 2001 From: padeler Date: Thu, 23 Mar 2017 02:17:01 +0200 Subject: [PATCH 6/6] LP-483 Example tuning using accessory0 (by L Laurent) --- python/examples/example_tuning.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/examples/example_tuning.py b/python/examples/example_tuning.py index 39f562f43..8e014393e 100644 --- a/python/examples/example_tuning.py +++ b/python/examples/example_tuning.py @@ -177,11 +177,11 @@ if __name__ == '__main__': 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 + # get update of AccessoryDesired + objMan.AccessoryDesired.getUpdate(timeout=.5) + + # calculate value out of Accessory0 input (-1 ... +1) + txControl = objMan.AccessoryDesired.AccessoryVal.value value = tuneFrom + ((txControl+1)/2)*(tuneTo-tuneFrom) PI[PIIndex] = value objMan.StabilizationSettingsBank1.updated()